diff --git a/README.md b/README.md index ef59aea5..7b51316a 100644 --- a/README.md +++ b/README.md @@ -31,4 +31,5 @@ Specifically: * `picknik_ur_site_config` extends the base configuration with capabilities for robots with physics and perception. * `picknik_ur_gazebo_config` extends the site configuration with support for the Gazebo simulator. * `picknik_ur_gazebo_scan_and_plan_config` extends the Gazebo configuration with an alternate environment for 3D object scanning. +* `picknik_ur_multi_arm_config` is an example configuration for using multiple arms. * Other hardware-specific configurations, such as the ones used on PickNik's UR arms, inherit from `picknik_ur_site_config`. diff --git a/src/picknik_ur_base_config/description/picknik_ur_attachments_macro.xacro b/src/picknik_ur_base_config/description/picknik_ur_attachments_macro.xacro index 8df4acca..f29deb38 100644 --- a/src/picknik_ur_base_config/description/picknik_ur_attachments_macro.xacro +++ b/src/picknik_ur_base_config/description/picknik_ur_attachments_macro.xacro @@ -1,33 +1,36 @@ - + + + + + + + + + - - - - - - - - + + - + - + - + + @@ -37,8 +40,8 @@ - @@ -46,7 +49,7 @@ - + diff --git a/src/picknik_ur_base_config/objectives/reset_plannning_scene.xml b/src/picknik_ur_base_config/objectives/reset_planning_scene.xml similarity index 100% rename from src/picknik_ur_base_config/objectives/reset_plannning_scene.xml rename to src/picknik_ur_base_config/objectives/reset_planning_scene.xml diff --git a/src/picknik_ur_multi_arm_config/CMakeLists.txt b/src/picknik_ur_multi_arm_config/CMakeLists.txt new file mode 100644 index 00000000..25d08c1b --- /dev/null +++ b/src/picknik_ur_multi_arm_config/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.22) +project(picknik_ur_multi_arm_config) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY + config + description + objectives + waypoints + DESTINATION + share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/picknik_ur_multi_arm_config/LICENSE b/src/picknik_ur_multi_arm_config/LICENSE new file mode 100644 index 00000000..574ef079 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/LICENSE @@ -0,0 +1,25 @@ +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/picknik_ur_multi_arm_config/README.md b/src/picknik_ur_multi_arm_config/README.md new file mode 100644 index 00000000..4935cabb --- /dev/null +++ b/src/picknik_ur_multi_arm_config/README.md @@ -0,0 +1,5 @@ +# picknik_ur_multi_arm_config + +A MoveIt Pro example configuration package for using multiple UR5e arms using mock hardware. + +For further documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/) diff --git a/src/picknik_ur_multi_arm_config/config/cameras.yaml b/src/picknik_ur_multi_arm_config/config/cameras.yaml new file mode 100644 index 00000000..606d2c3d --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/cameras.yaml @@ -0,0 +1,68 @@ +# Placeholders for camera configuration. +# Cameras are not a requirement for running MoveIt Pro, but this is provided as an +# example for plugging in Realsense D415 camera topics to the application. + +- scene_camera: + camera_name: "scene_camera" + type: "sim" + use: True + serial_no: "0" + device_type: "D415" + framerate: 6 + image_width: 640 + image_height: 480 + enable_pointcloud: False + + # information about the topics the camera publishes the raw image and info + rgb_info: "/scene_camera/color/camera_info" + rgb_image: "/scene_camera/color/image_raw" + + registered_rgb_depth_pair: + depth_info: "/scene_camera/color/camera_info" + depth_image: "/scene_camera/depth/image_rect_raw" + registered_info: "/scene_camera/color/camera_info" + registered_image: "/scene_camera/depth/image_rect_raw" + +- left_wrist_mounted_camera: + camera_name: "left_wrist_mounted_camera" + type: "sim" + use: True + serial_no: "0" + device_type: "D415" + framerate: 6 + image_width: 640 + image_height: 480 + enable_pointcloud: True + + # information about the topics the camera publishes the raw image and info + rgb_info: "/left_wrist_mounted_camera/color/camera_info" + rgb_image: "/left_wrist_mounted_camera/color/image_raw" + + # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click" + registered_rgb_depth_pair: + depth_info: "/left_wrist_mounted_camera/depth/camera_info" + depth_image: "/left_wrist_mounted_camera/depth/image_rect_raw" + registered_info: "/left_wrist_mounted_camera/depth_registered/camera_info" + registered_image: "/left_wrist_mounted_camera/depth_registered/image_rect" + +- right_wrist_mounted_camera: + camera_name: "right_wrist_mounted_camera" + type: "sim" + use: True + serial_no: "0" + device_type: "D415" + framerate: 6 + image_width: 640 + image_height: 480 + enable_pointcloud: True + + # information about the topics the camera publishes the raw image and info + rgb_info: "/right_wrist_mounted_camera/color/camera_info" + rgb_image: "/right_wrist_mounted_camera/color/image_raw" + + # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click" + registered_rgb_depth_pair: + depth_info: "/right_wrist_mounted_camera/depth/camera_info" + depth_image: "/right_wrist_mounted_camera/depth/image_rect_raw" + registered_info: "/right_wrist_mounted_camera/depth_registered/camera_info" + registered_image: "/right_wrist_mounted_camera/depth_registered/image_rect" diff --git a/src/picknik_ur_multi_arm_config/config/config.yaml b/src/picknik_ur_multi_arm_config/config/config.yaml new file mode 100644 index 00000000..7fa1ea44 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/config.yaml @@ -0,0 +1,227 @@ +# Baseline hardware configuration parameters for MoveIt Pro. +# [Required] +hardware: + # Used by the ur_description package to set kinematics and geometry for a specific robot type. + # You can change this to another UR model but you must update any configuration affected by the different arm size. + # UR models in the ur_description package are ur3, ur3e, ur5, ur5e, ur10, ur10e, and ur16e. + # [Required] + type: ur5e + + # Set simulated to false if you are using this as a configuration for real hardware. + # Override-able by setting the $MOCK_HARDWARE environment variable to 'True'. + # This allows users to switch between mock and real hardware using the same configuration. + # [Required] + simulated: ${MOCK_HARDWARE:-true} + + # If the MoveIt Pro Agent should launch the ros2 controller node. + # [Optional, default=True] + launch_control_node: True + + # If the MoveIt Pro Agent should launch the robot state publisher. + # This should be false if you are launching the robot state publisher as part of drivers. + # [Optional, default=True] + launch_robot_state_publisher: True + + # If the MoveIt Pro Agent should launch cameras when simulated. + # This must be False when using mock hardware, since there are no cameras simulated. + # [Optional, default=True] + launch_cameras_when_simulated: False + + # The robot's IP address. + # [Required] + ip: "0.0.0.0" + + # Specify additional launch files for running the robot with real hardware. + # [Optional, defaults to a blank launch file if not specified] + robot_driver_persist_launch_file: + package: "picknik_ur_base_config" + path: "launch/robot_drivers_to_persist.launch.py" + hardware_launch_file: + package: "moveit_studio_agent" + path: "launch/blank.launch.py" + + # Specify any additional launch files for running the robot in simulation mode. + # Used when hardware.simulated is True. + # [Optional, defaults to a blank launch file if not specified] + simulated_robot_driver_persist_launch_file: + package: "picknik_ur_base_config" + path: "launch/sim/robot_drivers_to_persist_sim.launch.py" + simulated_hardware_launch_file: + package: "moveit_studio_agent" + path: "launch/blank.launch.py" + + # Configuration details for cameras and scene planning. + # [Required] + camera_config_file: + package: "picknik_ur_multi_arm_config" + path: "config/cameras.yaml" + + # Parameters used to configure the robot description through XACRO. + # A URDF and SRDF are both required. + # [Required] + robot_description: + urdf: + package: "picknik_ur_multi_arm_config" + path: "description/dual_arm_ur.xacro" + srdf: + package: "picknik_ur_multi_arm_config" + path: "config/moveit/dual_arm_ur.srdf" + # Specify any additional parameters required for the URDF. + # Many of these are specific to the UR descriptions packages, and can be customized as needed. + # [Optional] + urdf_params: + - name: "dual_arm_ur" + - prefix: "" + - use_fake_hardware: "%>> hardware.simulated" + - use_pinch_links: "true" + - fake_sensor_commands: "false" + - headless_mode: "true" + - robot_ip: "%>> hardware.ip" + - joint_limits_parameters_file: + package: "picknik_ur_base_config" + path: "config/moveit/joint_limits.yaml" + # The following files are loaded based on the ur_description package layout. + # To use parameters from a different package, place them in a config/ROBOT_NAME/ directory, + # replace ROBOT_NAME with the value used for hardware.type in this file. + - kinematics_parameters_file: + # Load default_kinematics.yaml from ur_description/config/ + package: "ur_description" + path: "config/%>> hardware.type <<%/default_kinematics.yaml" + - physical_parameters_file: + # Load physical_parameters.yaml from ur_description/config/ + package: "ur_description" + path: "config/%>> hardware.type <<%/physical_parameters.yaml" + - visual_parameters_file: + # Load visual_parameters.yaml from ur_description/config/ + package: "ur_description" + path: "config/%>> hardware.type <<%/visual_parameters.yaml" + # Set advanced camera settings to improve point cloud accuracy + json_file_path: + package: "picknik_ur_base_config" + path: "config/realsense_config_high_accuracy.json" + +# Sets ROS global params for launch. +# [Optional] +ros_global_params: + # Whether or not to use simulated time. + # [Optional, default=False] + use_sim_time: False + +# Configuration files for MoveIt. +# For more information, refer to https://moveit.picknik.ai/main/doc/how_to_guides/moveit_configuration/moveit_configuration_tutorial.html +# [Required] +moveit_params: + # Used by the Waypoint Manager to save joint states from this joint group. + joint_group_name: "dual_arm_manipulator" + + ompl_planning: + package: "picknik_ur_multi_arm_config" + path: "config/moveit/ompl_planning.yaml" + pilz_planning: + package: "picknik_ur_base_config" + path: "config/moveit/pilz_industrial_motion_planner_planning.yaml" + stomp_planning: + package: "picknik_ur_base_config" + path: "config/moveit/stomp_planning.yaml" + kinematics: + package: "picknik_ur_multi_arm_config" + path: "config/moveit/trac_ik_kinematics_distance.yaml" + servo: + package: "picknik_ur_multi_arm_config" + path: "config/moveit/right_ur_servo.yaml" + sensors_3d: + package: "picknik_ur_base_config" + path: "config/moveit/sensors_3d.yaml" + servo_kinematics: + package: "picknik_ur_multi_arm_config" + path: "config/moveit/trac_ik_kinematics_speed.yaml" + joint_limits: + package: "picknik_ur_multi_arm_config" + path: "config/moveit/dual_arm_joint_limits.yaml" + pilz_cartesian_limits: + package: "picknik_ur_base_config" + path: "config/moveit/pilz_cartesian_limits.yaml" + + publish: + planning_scene: True + geometry_updates: True + state_updates: True + transforms_updates: True + + trajectory_execution: + manage_controllers: True + allowed_execution_duration_scaling: 2.0 + allowed_goal_duration_margin: 5.0 + allowed_start_tolerance: 0.01 + +# Additional configurable parameters for the MoveIt Pro user interface. +# [Required] +ui_params: + # By default, MoveIt Pro uses a frame called "grasp_link" for tool grasp pose rendering + # and planning. + # [Required] + servo_endpoint_frame_id: "right_grasp_link" + +# Configuration for launching ros2_control processes. +# [Required, if using ros2_control] +ros2_control: + config: + package: "picknik_ur_multi_arm_config" + path: "config/control/picknik_dual_ur.ros2_control.yaml" + # MoveIt Pro will load and activate these controllers at start up to ensure they are available. + # If not specified, it is up to the user to ensure the appropriate controllers are active and available + # for running the application. + # [Optional, default=[]] + controllers_active_at_startup: + - "joint_state_broadcaster" + - "left_servo_controller" + - "right_servo_controller" + - "left_io_and_status_controller" + - "right_io_and_status_controller" + - "left_robotiq_activation_controller" + - "left_robotiq_gripper_controller" + - "right_robotiq_activation_controller" + - "right_robotiq_gripper_controller" + # Load but do not start these controllers so they can be activated later if needed. + # [Optional, default=[]] + controllers_inactive_at_startup: + - "left_joint_trajectory_controller" + - "right_joint_trajectory_controller" + - "dual_arm_joint_trajectory_controller" + # Any controllers here will not be spawned by MoveIt Pro. + # [Optional, default=[]] + controllers_not_managed: [] + # Optionally configure remapping rules to let multiple controllers receive commands on the same topic. + # [Optional, default=[]] + controller_shared_topics: [] + +# Octomap manager configuration parameters +octomap_manager: + # Input point cloud topic name. The *output* point cloud topic published by + # the Octomap manager node is defined in sensors_3d.yaml. + input_point_cloud_topic: "/wrist_mounted_camera/depth/color/points" + +# Configuration for loading behaviors and objectives. +# [Required] +objectives: + # List of plugins for loading custom behaviors. + # [Required] + behavior_loader_plugins: + # This plugin will load the core MoveIt Pro behaviors. + # Add additional plugin loaders as needed. + core: + - "moveit_studio::behaviors::CoreBehaviorsLoader" + - "moveit_studio::behaviors::MTCCoreBehaviorsLoader" + - "moveit_studio::behaviors::ServoBehaviorsLoader" + - "moveit_studio::behaviors::VisionBehaviorsLoader" + # Specify source folder for objectives + # [Required] + objective_library_paths: + core: + package_name: "picknik_ur_multi_arm_config" + relative_path: "objectives" + # Specify the location of the saved waypoints file. + # [Required] + waypoints_file: + package_name: "picknik_ur_multi_arm_config" + relative_path: "waypoints/ur_waypoints.yaml" diff --git a/src/picknik_ur_multi_arm_config/config/control/picknik_dual_ur.ros2_control.yaml b/src/picknik_ur_multi_arm_config/config/control/picknik_dual_ur.ros2_control.yaml new file mode 100644 index 00000000..708b321c --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/control/picknik_dual_ur.ros2_control.yaml @@ -0,0 +1,265 @@ +controller_manager: + ros__parameters: + update_rate: 600 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + left_io_and_status_controller: + type: ur_controllers/GPIOController + right_io_and_status_controller: + type: ur_controllers/GPIOController + left_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + right_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + dual_arm_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + left_robotiq_gripper_controller: + type: position_controllers/GripperActionController + left_robotiq_activation_controller: + type: robotiq_controllers/RobotiqActivationController + right_robotiq_gripper_controller: + type: position_controllers/GripperActionController + right_robotiq_activation_controller: + type: joint_trajectory_controller/JointTrajectoryController + left_servo_controller: + type: joint_trajectory_controller/JointTrajectoryController + right_servo_controller: + type: position_controllers/JointGroupPositionController + +left_io_and_status_controller: + ros__parameters: + tf_prefix: "left_" + +right_io_and_status_controller: + ros__parameters: + tf_prefix: "right_" + +left_joint_trajectory_controller: + ros__parameters: + joints: + - left_shoulder_pan_joint + - left_shoulder_lift_joint + - left_elbow_joint + - left_wrist_1_joint + - left_wrist_2_joint + - left_wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + command_joints: + - left_shoulder_pan_joint + - left_shoulder_lift_joint + - left_elbow_joint + - left_wrist_1_joint + - left_wrist_2_joint + - left_wrist_3_joint + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + left_shoulder_pan_joint: + goal: 0.05 + left_shoulder_lift_joint: + goal: 0.05 + left_elbow_joint: + goal: 0.05 + left_wrist_1_joint: + goal: 0.05 + left_wrist_2_joint: + goal: 0.05 + left_wrist_3_joint: + goal: 0.05 + +right_joint_trajectory_controller: + ros__parameters: + joints: + - right_shoulder_pan_joint + - right_shoulder_lift_joint + - right_elbow_joint + - right_wrist_1_joint + - right_wrist_2_joint + - right_wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + command_joints: + - right_shoulder_pan_joint + - right_shoulder_lift_joint + - right_elbow_joint + - right_wrist_1_joint + - right_wrist_2_joint + - right_wrist_3_joint + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + right_shoulder_pan_joint: + goal: 0.05 + right_shoulder_lift_joint: + goal: 0.05 + right_elbow_joint: + goal: 0.05 + right_wrist_1_joint: + goal: 0.05 + right_wrist_2_joint: + goal: 0.05 + right_wrist_3_joint: + goal: 0.05 + +dual_arm_joint_trajectory_controller: + ros__parameters: + joints: + - left_shoulder_pan_joint + - left_shoulder_lift_joint + - left_elbow_joint + - left_wrist_1_joint + - left_wrist_2_joint + - left_wrist_3_joint + - right_shoulder_pan_joint + - right_shoulder_lift_joint + - right_elbow_joint + - right_wrist_1_joint + - right_wrist_2_joint + - right_wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + command_joints: + - left_shoulder_pan_joint + - left_shoulder_lift_joint + - left_elbow_joint + - left_wrist_1_joint + - left_wrist_2_joint + - left_wrist_3_joint + - right_shoulder_pan_joint + - right_shoulder_lift_joint + - right_elbow_joint + - right_wrist_1_joint + - right_wrist_2_joint + - right_wrist_3_joint + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: true + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + left_shoulder_pan_joint: + goal: 0.05 + left_shoulder_lift_joint: + goal: 0.05 + left_elbow_joint: + goal: 0.05 + left_wrist_1_joint: + goal: 0.05 + left_wrist_2_joint: + goal: 0.05 + left_wrist_3_joint: + goal: 0.05 + right_shoulder_pan_joint: + goal: 0.05 + right_shoulder_lift_joint: + goal: 0.05 + right_elbow_joint: + goal: 0.05 + right_wrist_1_joint: + goal: 0.05 + right_wrist_2_joint: + goal: 0.05 + right_wrist_3_joint: + goal: 0.05 + +left_servo_controller: + ros__parameters: + joints: + - left_shoulder_pan_joint + - left_shoulder_lift_joint + - left_elbow_joint + - left_wrist_1_joint + - left_wrist_2_joint + - left_wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + shoulder_pan_joint: + goal: 0.05 + shoulder_lift_joint: + goal: 0.05 + elbow_joint: + goal: 0.05 + wrist_1_joint: + goal: 0.05 + wrist_2_joint: + goal: 0.05 + wrist_3_joint: + goal: 0.05 + +right_servo_controller: + ros__parameters: + joints: + - right_shoulder_pan_joint + - right_shoulder_lift_joint + - right_elbow_joint + - right_wrist_1_joint + - right_wrist_2_joint + - right_wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + shoulder_pan_joint: + goal: 0.05 + shoulder_lift_joint: + goal: 0.05 + elbow_joint: + goal: 0.05 + wrist_1_joint: + goal: 0.05 + wrist_2_joint: + goal: 0.05 + wrist_3_joint: + goal: 0.05 + +left_robotiq_gripper_controller: + ros__parameters: + default: true + joint: left_robotiq_85_left_knuckle_joint + allow_stalling: true + +left_robotiq_activation_controller: + ros__parameters: + default: true + +right_robotiq_gripper_controller: + ros__parameters: + default: true + joint: right_robotiq_85_left_knuckle_joint + allow_stalling: true + +right_robotiq_activation_controller: + ros__parameters: + default: true diff --git a/src/picknik_ur_multi_arm_config/config/initial_positions.yaml b/src/picknik_ur_multi_arm_config/config/initial_positions.yaml new file mode 100644 index 00000000..0d8b33f7 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/initial_positions.yaml @@ -0,0 +1,6 @@ +shoulder_pan_joint: 0.0 +shoulder_lift_joint: -1.41 +elbow_joint: -0.7 +wrist_1_joint: -2.12 +wrist_2_joint: 1.67 +wrist_3_joint: 0.0 diff --git a/src/picknik_ur_multi_arm_config/config/moveit/dual_arm_joint_limits.yaml b/src/picknik_ur_multi_arm_config/config/moveit/dual_arm_joint_limits.yaml new file mode 100644 index 00000000..6022de2f --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/dual_arm_joint_limits.yaml @@ -0,0 +1,164 @@ +# Joints limits +# +# Sources: +# +# - Universal Robots e-Series, User Manual, UR5e, Version 5.8 +# https://s3-eu-west-1.amazonaws.com/ur-support-site/69091/99404_UR5e_User_Manual_en_Global.pdf +# - Support > Articles > UR articles > Max. joint torques +# https://www.universal-robots.com/articles/ur-articles/max-joint-torques +# retrieved: 2020-06-16, last modified: 2020-06-09 +# +# NOTE: Acceleration limits are not publicly available so we have picked a reasonable upper limit +joint_limits: + left_shoulder_pan_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 150.0 + max_position: !degrees 180.0 + max_velocity: !degrees 30.0 + min_position: !degrees -180.0 + left_shoulder_lift_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 150.0 + max_position: !degrees 90.0 + max_velocity: !degrees 30.0 + min_position: !degrees -270.0 + left_elbow_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 150.0 + # we artificially limit this joint to half its actual joint position limit + # to avoid (MoveIt/OMPL) planning problems, as due to the physical + # construction of the robot, it's impossible to rotate the 'elbow_joint' + # over more than approx +- 1 pi (the shoulder lift joint gets in the way). + # + # This leads to planning problems as the search space will be divided into + # two sections, with no connections from one to the other. + # + # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for + # more information. + max_position: !degrees 180.0 + max_velocity: !degrees 30.0 + min_position: !degrees -180.0 + left_wrist_1_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 28.0 + max_position: !degrees 180.0 + max_velocity: !degrees 60.0 + min_position: !degrees -180.0 + left_wrist_2_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 28.0 + max_position: !degrees 180.0 + max_velocity: !degrees 60.0 + min_position: !degrees -180.0 + left_wrist_3_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 28.0 + max_position: !degrees 360.0 + max_velocity: !degrees 60.0 + min_position: !degrees -360.0 + left_robotiq_85_left_knuckle_joint: + has_velocity_limits: true + max_velocity: 0.5 + has_acceleration_limits: true + max_acceleration: 1.0 + + + right_shoulder_pan_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 150.0 + max_position: !degrees 180.0 + max_velocity: !degrees 30.0 + min_position: !degrees -180.0 + right_shoulder_lift_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 150.0 + max_position: !degrees 90.0 + max_velocity: !degrees 30.0 + min_position: !degrees -270.0 + right_elbow_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 150.0 + # we artificially limit this joint to half its actual joint position limit + # to avoid (MoveIt/OMPL) planning problems, as due to the physical + # construction of the robot, it's impossible to rotate the 'elbow_joint' + # over more than approx +- 1 pi (the shoulder lift joint gets in the way). + # + # This leads to planning problems as the search space will be divided into + # two sections, with no connections from one to the other. + # + # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for + # more information. + max_position: !degrees 180.0 + max_velocity: !degrees 30.0 + min_position: !degrees -180.0 + right_wrist_1_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 28.0 + max_position: !degrees 180.0 + max_velocity: !degrees 60.0 + min_position: !degrees -180.0 + right_wrist_2_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 28.0 + max_position: !degrees 180.0 + max_velocity: !degrees 60.0 + min_position: !degrees -180.0 + right_wrist_3_joint: + has_acceleration_limits: true + has_effort_limits: true + has_position_limits: true + has_velocity_limits: true + max_acceleration: !degrees 30.0 + max_effort: 28.0 + max_position: !degrees 360.0 + max_velocity: !degrees 60.0 + min_position: !degrees -360.0 + right_robotiq_85_left_knuckle_joint: + has_velocity_limits: true + max_velocity: 0.5 + has_acceleration_limits: true + max_acceleration: 1.0 diff --git a/src/picknik_ur_multi_arm_config/config/moveit/dual_arm_ur.srdf b/src/picknik_ur_multi_arm_config/config/moveit/dual_arm_ur.srdf new file mode 100644 index 00000000..3f3aefce --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/dual_arm_ur.srdf @@ -0,0 +1,397 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/config/moveit/left_ur_servo.yaml b/src/picknik_ur_multi_arm_config/config/moveit/left_ur_servo.yaml new file mode 100644 index 00000000..510ac57b --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/left_ur_servo.yaml @@ -0,0 +1,60 @@ +############################################### +# Modify all parameters related to servoing here +############################################### + +# Enable dynamic parameter updates +enable_parameter_update: true + +## Properties of incoming commands +command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 2.0 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 3.0 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 2.0 + +# Default to driving the arm at 50% maximum speed. +override_velocity_scaling_factor: 0.5 + +## Properties of outgoing commands +publish_period: 0.002 # 1/Nominal publish rate [seconds] + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: std_msgs/Float64MultiArray + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +use_smoothing: true +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" +low_pass_filter_coeff: 1.5 # Larger --> trust the filtered data more, trust the measurements less. + +## MoveIt properties +move_group_name: manipulator_left # Often 'manipulator' or 'arm' +is_primary_planning_scene_monitor: false # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there. + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /left_servo_controller/commands # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +self_collision_proximity_threshold: 0.006 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/src/picknik_ur_multi_arm_config/config/moveit/ompl_planning.yaml b/src/picknik_ur_multi_arm_config/config/moveit/ompl_planning.yaml new file mode 100644 index 00000000..97979bdc --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/ompl_planning.yaml @@ -0,0 +1,104 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + +planner_configs: + APSConfigDefault: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # Compute hybrid solution trajectories + max_hybrid_paths: 32 # Number of hybrid paths generated per iteration + num_planners: 16 # The number of default planners to use for planning + planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + +manipulator_left: + default_planner_config: RRTConnectkConfigDefault + planner_configs: + - APSConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + enforce_constrained_state_space: true + longest_valid_segment_fraction: 0.01 +gripper_left: + planner_configs: + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + +manipulator_right: + default_planner_config: RRTConnectkConfigDefault + planner_configs: + - APSConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + enforce_constrained_state_space: true + longest_valid_segment_fraction: 0.01 +gripper_right: + planner_configs: + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + +dual_arm_manipulator: + default_planner_config: RRTConnectkConfigDefault + planner_configs: + - APSConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + enforce_constrained_state_space: true + longest_valid_segment_fraction: 0.01 diff --git a/src/picknik_ur_multi_arm_config/config/moveit/pilz_cartesian_limits.yaml b/src/picknik_ur_multi_arm_config/config/moveit/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..1633938e --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Cartesian limits for the Pilz planner +cartesian_limits: + max_trans_vel: 0.1 + max_trans_acc: 0.1 + max_trans_dec: -0.1 + max_rot_vel: 0.1 diff --git a/src/picknik_ur_multi_arm_config/config/moveit/right_ur_servo.yaml b/src/picknik_ur_multi_arm_config/config/moveit/right_ur_servo.yaml new file mode 100644 index 00000000..a56532c2 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/right_ur_servo.yaml @@ -0,0 +1,60 @@ +############################################### +# Modify all parameters related to servoing here +############################################### + +# Enable dynamic parameter updates +enable_parameter_update: true + +## Properties of incoming commands +command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 2.0 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 3.0 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 2.0 + +# Default to driving the arm at 50% maximum speed. +override_velocity_scaling_factor: 0.5 + +## Properties of outgoing commands +publish_period: 0.002 # 1/Nominal publish rate [seconds] + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: std_msgs/Float64MultiArray + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +use_smoothing: true +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" +low_pass_filter_coeff: 1.5 # Larger --> trust the filtered data more, trust the measurements less. + +## MoveIt properties +move_group_name: manipulator_right # Often 'manipulator' or 'arm' +is_primary_planning_scene_monitor: false # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there. + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /right_servo_controller/commands # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +self_collision_proximity_threshold: 0.006 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/src/picknik_ur_multi_arm_config/config/moveit/trac_ik_kinematics_distance.yaml b/src/picknik_ur_multi_arm_config/config/moveit/trac_ik_kinematics_distance.yaml new file mode 100644 index 00000000..afdc636c --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/trac_ik_kinematics_distance.yaml @@ -0,0 +1,12 @@ +manipulator_left: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + epsilon: 0.0001 + solve_type: "Distance" +manipulator_right: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + epsilon: 0.0001 + solve_type: "Distance" diff --git a/src/picknik_ur_multi_arm_config/config/moveit/trac_ik_kinematics_speed.yaml b/src/picknik_ur_multi_arm_config/config/moveit/trac_ik_kinematics_speed.yaml new file mode 100644 index 00000000..fe311e4a --- /dev/null +++ b/src/picknik_ur_multi_arm_config/config/moveit/trac_ik_kinematics_speed.yaml @@ -0,0 +1,12 @@ +manipulator_left: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + epsilon: 0.0001 + solve_type: "Speed" +manipulator_right: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 + epsilon: 0.0001 + solve_type: "Speed" diff --git a/src/picknik_ur_multi_arm_config/description/dual_arm_ur.xacro b/src/picknik_ur_multi_arm_config/description/dual_arm_ur.xacro new file mode 100644 index 00000000..72374602 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/description/dual_arm_ur.xacro @@ -0,0 +1,139 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/clear_snapshot.xml b/src/picknik_ur_multi_arm_config/objectives/clear_snapshot.xml new file mode 100644 index 00000000..2550ff7d --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/clear_snapshot.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/close_left_gripper.xml b/src/picknik_ur_multi_arm_config/objectives/close_left_gripper.xml new file mode 100644 index 00000000..d9958fe6 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/close_left_gripper.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/close_right_gripper.xml b/src/picknik_ur_multi_arm_config/objectives/close_right_gripper.xml new file mode 100644 index 00000000..42ae9812 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/close_right_gripper.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml new file mode 100644 index 00000000..811a5829 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml b/src/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml new file mode 100644 index 00000000..03d4728a --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/move_to_named_pose.xml b/src/picknik_ur_multi_arm_config/objectives/move_to_named_pose.xml new file mode 100644 index 00000000..d44d7305 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/move_to_named_pose.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/move_to_pose.xml b/src/picknik_ur_multi_arm_config/objectives/move_to_pose.xml new file mode 100644 index 00000000..f30159c8 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/move_to_pose.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/move_two_arms.xml b/src/picknik_ur_multi_arm_config/objectives/move_two_arms.xml new file mode 100644 index 00000000..0b5ccfe0 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/move_two_arms.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/open_left_gripper.xml b/src/picknik_ur_multi_arm_config/objectives/open_left_gripper.xml new file mode 100644 index 00000000..12b33726 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/open_left_gripper.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/open_right_gripper.xml b/src/picknik_ur_multi_arm_config/objectives/open_right_gripper.xml new file mode 100644 index 00000000..47c13753 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/open_right_gripper.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/reactivate_gripper.xml b/src/picknik_ur_multi_arm_config/objectives/reactivate_gripper.xml new file mode 100644 index 00000000..a9666a46 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/reactivate_gripper.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/request_teleoperation.xml b/src/picknik_ur_multi_arm_config/objectives/request_teleoperation.xml new file mode 100644 index 00000000..3c29ae5e --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/request_teleoperation.xml @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/reset_planning_scene.xml b/src/picknik_ur_multi_arm_config/objectives/reset_planning_scene.xml new file mode 100644 index 00000000..8e43ac5a --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/reset_planning_scene.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/teleoperate.xml b/src/picknik_ur_multi_arm_config/objectives/teleoperate.xml new file mode 100644 index 00000000..23614685 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/teleoperate.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/picknik_ur_multi_arm_config/objectives/wait_for_trajectory_approval_if_user_available.xml new file mode 100644 index 00000000..7abb4108 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/objectives/wait_for_trajectory_approval_if_user_available.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/src/picknik_ur_multi_arm_config/package.xml b/src/picknik_ur_multi_arm_config/package.xml new file mode 100644 index 00000000..eb5d9e90 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/package.xml @@ -0,0 +1,41 @@ + + + picknik_ur_multi_arm_config + 5.0.0 + + Example configuration package for multiple UR arms. + + MoveIt Pro Maintainer + + BSD 3-Clause + + ament_cmake + + admittance_controller + kinematics_interface_kdl + moveit_planners_stomp + moveit_ros_perception + moveit_studio_agent + moveit_studio_behavior + moveit_studio_ur_pstop_manager + picknik_accessories + realsense2_description + robotiq_description + robotiq_controllers + trac_ik_kinematics_plugin + ur_description + ur_robot_driver + + ament_lint_auto + + ament_clang_format + ament_clang_tidy + ament_cmake_copyright + ament_cmake_lint_cmake + picknik_ament_copyright + ament_flake8 + + + ament_cmake + + diff --git a/src/picknik_ur_multi_arm_config/waypoints/ur_waypoints.yaml b/src/picknik_ur_multi_arm_config/waypoints/ur_waypoints.yaml new file mode 100644 index 00000000..c7049e84 --- /dev/null +++ b/src/picknik_ur_multi_arm_config/waypoints/ur_waypoints.yaml @@ -0,0 +1,88 @@ +- description: 'Both arms are in a down position' + favorite: true + joint_group_names: + - dual_arm_manipulator + - gripper + - gripper_left + - manipulator_left + - manipulator_right + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - left_shoulder_pan_joint + - left_shoulder_lift_joint + - left_elbow_joint + - left_wrist_1_joint + - left_wrist_2_joint + - left_wrist_3_joint + - right_shoulder_pan_joint + - right_shoulder_lift_joint + - right_elbow_joint + - right_wrist_1_joint + - right_wrist_2_joint + - right_wrist_3_joint + - right_robotiq_85_left_knuckle_joint + - left_robotiq_85_left_knuckle_joint + position: + - 5.12061324901879e-05 + - -1.4100637137602083 + - -0.6999914125130512 + - -2.1199174221931956 + - 1.6699393665105104 + - -4.32235198095441e-06 + - 3.109852643683551e-05 + - -1.410057083617756 + - -0.7000249972289894 + - -2.1200145056689625 + - 1.669979084378574 + - 1.3941351324319836e-05 + - 0 + - 0.7929 + velocity: [] + name: Home +- description: Both arms are in a down position + favorite: true + joint_group_names: + - dual_arm_manipulator + - manipulator_left + - manipulator_right + joint_state: + effort: [] + header: + frame_id: world + stamp: + nanosec: 0 + sec: 0 + name: + - left_shoulder_pan_joint + - left_shoulder_lift_joint + - left_elbow_joint + - left_wrist_1_joint + - left_wrist_2_joint + - left_wrist_3_joint + - right_shoulder_pan_joint + - right_shoulder_lift_joint + - right_elbow_joint + - right_wrist_1_joint + - right_wrist_2_joint + - right_wrist_3_joint + position: + - 8.581502470187843e-05 + - -1.5136764603659916 + - -1.799275792510518 + - -1.5993562600093492 + - 1.6699146867751609 + - -1.3363372068852196e-06 + - -6.064204568974674e-05 + - -1.5136764603659916 + - -1.71359599286716 + - -1.6564761264382546 + - 1.6699516917224972 + - -4.144793883897364e-05 + velocity: [] + name: Arms Down