diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 215a8013..e21a7419 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,6 +18,7 @@ jobs: uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@main with: image_tag: ${{ github.event.inputs.image_tag || null }} + colcon_test_args: "--executor sequential" secrets: inherit ensure-no-ssh-in-gitmodules: diff --git a/src/external_dependencies/gz_ros2_control b/src/external_dependencies/gz_ros2_control index c23e09cd..1cffeb4f 160000 --- a/src/external_dependencies/gz_ros2_control +++ b/src/external_dependencies/gz_ros2_control @@ -1 +1 @@ -Subproject commit c23e09cd0fc78f631e11115c56de59f513a9d779 +Subproject commit 1cffeb4f986ac6f45dddb3560e187813b775aead diff --git a/src/picknik_ur_multi_arm_gazebo_config/CMakeLists.txt b/src/mobile_manipulation_config/CMakeLists.txt similarity index 84% rename from src/picknik_ur_multi_arm_gazebo_config/CMakeLists.txt rename to src/mobile_manipulation_config/CMakeLists.txt index fe5914de..d549f941 100644 --- a/src/picknik_ur_multi_arm_gazebo_config/CMakeLists.txt +++ b/src/mobile_manipulation_config/CMakeLists.txt @@ -1,12 +1,11 @@ cmake_minimum_required(VERSION 3.22) -project(picknik_ur_multi_arm_gazebo_config) +project(mobile_manipulation_config) find_package(ament_cmake REQUIRED) install( DIRECTORY config - description launch objectives waypoints diff --git a/src/picknik_ur_multi_arm_gazebo_config/LICENSE b/src/mobile_manipulation_config/LICENSE similarity index 100% rename from src/picknik_ur_multi_arm_gazebo_config/LICENSE rename to src/mobile_manipulation_config/LICENSE diff --git a/src/mobile_manipulation_config/README.md b/src/mobile_manipulation_config/README.md new file mode 100644 index 00000000..192ddae3 --- /dev/null +++ b/src/mobile_manipulation_config/README.md @@ -0,0 +1,5 @@ +# mobile_manipulation_config + +A MoveIt Pro MuJoCo simulation of a UR5 arm on a mobile Ridgeback base. + +For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/) diff --git a/src/mobile_manipulation_config/config/config.yaml b/src/mobile_manipulation_config/config/config.yaml new file mode 100644 index 00000000..ec60c802 --- /dev/null +++ b/src/mobile_manipulation_config/config/config.yaml @@ -0,0 +1,83 @@ +# +# This contains information for a unique instance of a robot. +# + +# Name of the package to specialize +based_on_package: "picknik_ur_mobile_config" +hardware: + # Parameters used to configure the robot description through XACRO. + # A URDF and SRDF are both required. + # [Required] + robot_description: + urdf: + package: "picknik_ur_mobile_config" + path: "description/picknik_ur.xacro" + srdf: + package: "picknik_ur_mobile_config" + path: "config/moveit/picknik_ur.srdf" + urdf_params: + - joint_limits_parameters_file: + package: "picknik_ur_mobile_config" + path: "config/moveit/joint_limits.yaml" + +moveit_params: + servo: + package: "picknik_ur_mobile_config" + path: "config/moveit/ur_servo.yaml" + joint_limits: + package: "picknik_ur_mobile_config" + path: "config/moveit/joint_limits.yaml" + servo_joint_limits: + package: "picknik_ur_mobile_config" + path: "config/moveit/hard_joint_limits.yaml" + +# 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: + sim_objectives: + package_name: "mobile_manipulation_config" + relative_path: "objectives" + # Specify the location of the saved waypoints file. + # [Required] + waypoints_file: + package_name: "mobile_manipulation_config" + relative_path: "waypoints/ur_waypoints.yaml" + + +# Configuration for launching ros2_control processes. +# [Required, if using ros2_control] +ros2_control: + config: + package: "picknik_ur_mobile_config" + path: "config/control/picknik_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: + - "force_torque_sensor_broadcaster" + - "robotiq_gripper_controller" + - "joint_state_broadcaster" + - "servo_controller" + # Load but do not start these controllers so they can be activated later if needed. + controllers_inactive_at_startup: + - "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: [] diff --git a/src/picknik_ur_multi_arm_gazebo_config/launch/agent_bridge.launch.xml b/src/mobile_manipulation_config/launch/agent_bridge.launch.xml similarity index 100% rename from src/picknik_ur_multi_arm_gazebo_config/launch/agent_bridge.launch.xml rename to src/mobile_manipulation_config/launch/agent_bridge.launch.xml diff --git a/src/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml b/src/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml new file mode 100644 index 00000000..c3fa5002 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml b/src/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml new file mode 100644 index 00000000..9596c213 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml b/src/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml new file mode 100644 index 00000000..8cebe15e --- /dev/null +++ b/src/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml b/src/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml new file mode 100644 index 00000000..dd34c4c0 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml b/src/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml new file mode 100644 index 00000000..4aadc8d3 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml b/src/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml new file mode 100644 index 00000000..280487fa --- /dev/null +++ b/src/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml b/src/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml new file mode 100644 index 00000000..06a4795e --- /dev/null +++ b/src/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml b/src/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml new file mode 100644 index 00000000..2f149479 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml b/src/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml new file mode 100644 index 00000000..978f367c --- /dev/null +++ b/src/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml b/src/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml new file mode 100644 index 00000000..940c673f --- /dev/null +++ b/src/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml b/src/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml new file mode 100644 index 00000000..08b57d34 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml b/src/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml new file mode 100644 index 00000000..3786e5c1 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml b/src/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml new file mode 100644 index 00000000..2e7a4a03 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml b/src/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml new file mode 100644 index 00000000..b8584c34 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml b/src/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml new file mode 100644 index 00000000..ede9bc94 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml b/src/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml new file mode 100644 index 00000000..f63a2a60 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml b/src/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml new file mode 100644 index 00000000..3fccaa71 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml b/src/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml new file mode 100644 index 00000000..b2ef670d --- /dev/null +++ b/src/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml b/src/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml new file mode 100644 index 00000000..0a983d32 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/19_solution_-_execute_coverage_path.xml b/src/mobile_manipulation_config/objectives/19_solution_-_execute_coverage_path.xml new file mode 100644 index 00000000..deee3a5c --- /dev/null +++ b/src/mobile_manipulation_config/objectives/19_solution_-_execute_coverage_path.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/estimate_object_pose.xml b/src/mobile_manipulation_config/objectives/estimate_object_pose.xml new file mode 100644 index 00000000..64a1739f --- /dev/null +++ b/src/mobile_manipulation_config/objectives/estimate_object_pose.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml b/src/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml new file mode 100644 index 00000000..2775db31 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/mobile_manipulation_config/objectives/path.yaml b/src/mobile_manipulation_config/objectives/path.yaml new file mode 100644 index 00000000..ad3afad2 --- /dev/null +++ b/src/mobile_manipulation_config/objectives/path.yaml @@ -0,0 +1,2288 @@ +--- +header: + frame_id: local +pose: + position: + x: 0.0288413925 + y: 0.01169471275 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.0288413925 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.042870013171365205 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.05689863384273042 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.07173567861179339 + y: 0.012665550237283206 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.08322048287199012 + y: 0.01868961245082696 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.08866611688762616 + y: 0.03108875832357643 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.08633203732387187 + y: 0.044982102640739124 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.07797587858610382 + y: 0.05603897429408606 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.06578994831237304 + y: 0.06171672083712903 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.05135575429316438 + y: 0.0629648125 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.03871761700637697 + y: 0.0646494025446498 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.03605848607902924 + y: 0.07842369819460152 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.03175323393896217 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.017724613267596976 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.01542234641457947 + y: 0.08054225555788634 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.018106155631663578 + y: 0.06677274699630907 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.02078996484874768 + y: 0.053003238434731814 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.023473774065831784 + y: 0.039233729873154564 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.02615758328291589 + y: 0.02546422131157728 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.0288413925 + y: 0.011694712750000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.0288413925 + y: 0.011694712750000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.0462995175 + y: 0.026365850000000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.0462995175 + y: 0.026365850000000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.04545907389575817 + y: 0.03063981211947943 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.04461863029151635 + y: 0.034913774238958854 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.043778186687274526 + y: 0.03918773635843828 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.04293774308303269 + y: 0.0434616984779177 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.042097299478790866 + y: 0.04773566059739713 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.045774681111083695 + y: 0.048293675 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.05013049305039605 + y: 0.048293675 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.05473349253768283 + y: 0.04825225346850195 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.05931338583742742 + y: 0.04750796633095318 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.06296063019818732 + y: 0.04583103094723683 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.06576021902046622 + y: 0.04308668747253468 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.06744434350819487 + y: 0.03927950118984562 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.06789624878694002 + y: 0.034515685888428564 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.06668238501122158 + y: 0.030333801206558833 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.06378326051277153 + y: 0.02768905690540692 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.05968946870699657 + y: 0.026494364725245686 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.055011141378624714 + y: 0.026365850000000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.050655329439312356 + y: 0.026365850000000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.0462995175 + y: 0.026365850000000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.0462995175 + y: 0.026365850000000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10777105000000001 + y: 0.01169471275 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10777105000000001 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.1183217305123276 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.12785235695939606 + y: 0.012535143699317845 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.1258339110314766 + y: 0.0228909505394088 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.12381546510355713 + y: 0.033246757379499746 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.12179701917563765 + y: 0.0436025642195907 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.11977857324771816 + y: 0.05395837105968166 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.1177601273197987 + y: 0.06431417789977262 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.11574168139187921 + y: 0.07466998473986355 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.11372323546395974 + y: 0.0850257915799545 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10743867224383621 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.09688799173150862 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.09364192850456368 + y: 0.08418536063063668 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.09566037443248317 + y: 0.07382955379054573 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.09767882036040262 + y: 0.06347374695045477 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.09969726628832211 + y: 0.053117940110363804 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10171571221624157 + y: 0.04276213327027287 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10373415814416105 + y: 0.03240632643018192 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10575260407208052 + y: 0.022050519590090963 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10777105000000001 + y: 0.011694712750000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.10777105000000001 + y: 0.011694712750000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.195534945 + y: 0.08652276499999999 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.195534945 + y: 0.08652276499999999 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.17771818104251466 + y: 0.0910754165839387 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.15896037696502555 + y: 0.09073872903853274 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.14355336894859672 + y: 0.08297816182324531 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.13493082662342823 + y: 0.06773737381670904 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.13527475402100594 + y: 0.0490438464138701 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.14206585002068836 + y: 0.032406942622364036 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.15457177798737778 + y: 0.01921556786973803 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.17097718261584338 + y: 0.011747692843507873 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.18915942703124852 + y: 0.010591299282233969 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.206666345046646 + y: 0.015396559098655898 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.20499384903120682 + y: 0.032355684644813365 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.18854441070372543 + y: 0.025242405928509803 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.17090415648515955 + y: 0.028231864200311464 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.1588526923953411 + y: 0.04137674231933609 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.15542085720616539 + y: 0.059642140177169255 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.16369361960470713 + y: 0.07448017504858914 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.18151055410851982 + y: 0.07625850276315374 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.1985054226589077 + y: 0.06939231382385545 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.195534945 + y: 0.08652276499999999 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.195534945 + y: 0.08652276499999999 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.22687543000000002 + y: 0.01169471275 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.22687543000000002 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.24667904796643422 + y: 0.013952666539273456 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.24235257338486496 + y: 0.036079484198058835 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.2558165261699729 + y: 0.027909508595456635 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.2730318173118219 + y: 0.013351067988321655 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.29497117656426974 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.2856499063312995 + y: 0.0233763950280311 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.26844559100904913 + y: 0.037947804579020986 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.25711712001616316 + y: 0.05301838770300754 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.27161683956905414 + y: 0.0702831661521395 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.28611655912194517 + y: 0.08754794460127144 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.26926925204250785 + y: 0.09020369500000001 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.25337566710918613 + y: 0.07630036209962063 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.23922962770312836 + y: 0.058744621980435674 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.2342011875738227 + y: 0.07793526610630296 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.22177024688961972 + y: 0.09020369500000001 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.21393573241551775 + y: 0.07808292107431812 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.2182489649436785 + y: 0.05595351829954545 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.22256219747183928 + y: 0.03382411552477272 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.22687543000000002 + y: 0.011694712750000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.22687543000000002 + y: 0.011694712750000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.31032740000000003 + y: 0.01169471275 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.31032740000000003 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.333587078175536 + y: 0.013654635894324425 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3413358449640447 + y: 0.0370821915807485 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.34908461175255345 + y: 0.060509747267172564 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3545144109543099 + y: 0.04759105719796958 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.35922301189525946 + y: 0.023368695022449096 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.37427559388318027 + y: 0.011694712750000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3771287008268542 + y: 0.02967458733824614 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3724079906532211 + y: 0.05389459245054438 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.36768728047958804 + y: 0.07811459756284261 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.35297181520484167 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.33819025022199706 + y: 0.07650977287760335 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3304414834656032 + y: 0.05308221718055722 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3235001770418951 + y: 0.04221823019943254 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3188141509771638 + y: 0.06644496980474997 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3137419611292364 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.2961652471884344 + y: 0.08435472374222756 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3008859647922896 + y: 0.060134720078151745 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.3056066823961448 + y: 0.03591471641407587 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.31032740000000003 + y: 0.011694712750000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.31032740000000003 + y: 0.011694712750000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4004576 + y: 0.01169471275 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4004576 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.41100828151461455 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.42053891851478 + y: 0.012535135856594536 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4185204727128093 + y: 0.022890943742381927 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4165020269108386 + y: 0.03324675162816932 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.414483581108868 + y: 0.04360255951395672 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4124651353068973 + y: 0.05395836739974412 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.41044668950492674 + y: 0.06431417528553152 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.40842824370295605 + y: 0.0746699831713189 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4064097979009853 + y: 0.08502579105710631 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.40012523424269275 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.38957455272807817 + y: 0.090203695 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.38632847938620546 + y: 0.08418536795051176 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.38834692518817604 + y: 0.0738295600647244 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.39036537099014673 + y: 0.06347375217893698 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.39238381679211737 + y: 0.053117944293149574 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.394402262594088 + y: 0.0427621364073622 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.39642070839605875 + y: 0.032406328521574816 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.39843915419802933 + y: 0.022050520635787406 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4004576 + y: 0.011694712750000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4004576 + y: 0.011694712750000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.44052715 + y: 0.01169471275 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.44052715 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4603307494675481 + y: 0.013952680849204953 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.45600429249378127 + y: 0.03607949862470184 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.46946825591326125 + y: 0.027909495582450955 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.48668353918830887 + y: 0.013351050728148496 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.5086229011494898 + y: 0.01169471275 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.49930159006171615 + y: 0.023376408807757744 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4820972772304196 + y: 0.03794781624900318 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.47076882952936533 + y: 0.05301839903035277 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.48526854698289174 + y: 0.07028317497977662 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.49976826443641803 + y: 0.08754795092920047 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4829209470432883 + y: 0.09020369500000001 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4670273673072691 + y: 0.07630035971199492 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4528813315739715 + y: 0.05874462082557477 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4478528928896693 + y: 0.07793526448008453 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.4354219568105598 + y: 0.09020369500000001 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.42758745632567197 + y: 0.07808291185916139 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.43190068755044797 + y: 0.055953512156107624 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.436213918775224 + y: 0.03382411245305381 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.44052715 + y: 0.011694712750000003 + z: 0.0 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 +--- +header: + frame_id: local +pose: + position: + x: 0.44052715 + y: 0.011694712750000003 + z: -0.01 + orientation: + x: 0 + y: 0 + z: 1 + w: 0 diff --git a/src/picknik_ur_multi_arm_gazebo_config/package.xml b/src/mobile_manipulation_config/package.xml similarity index 72% rename from src/picknik_ur_multi_arm_gazebo_config/package.xml rename to src/mobile_manipulation_config/package.xml index bd7e860d..6afa8bd5 100644 --- a/src/picknik_ur_multi_arm_gazebo_config/package.xml +++ b/src/mobile_manipulation_config/package.xml @@ -1,9 +1,9 @@ - picknik_ur_multi_arm_gazebo_config - 6.0.0 + mobile_manipulation_config + 5.1.0 - Experimental configuration package for dual UR arms. + MuJoCo simulation configuration package for scan and spray application MoveIt Pro Maintainer @@ -11,24 +11,23 @@ ament_cmake - picknik_ur_base_config - picknik_ur_multi_arm_config 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_camera realsense2_description - robotiq_description - robotiq_controllers trac_ik_kinematics_plugin ur_description - ros_gz + ur_robot_driver + picknik_mujoco_ros + ridgeback_description ament_lint_auto + ament_clang_format ament_clang_tidy ament_cmake_copyright diff --git a/src/mobile_manipulation_config/waypoints/ur_waypoints.yaml b/src/mobile_manipulation_config/waypoints/ur_waypoints.yaml new file mode 100644 index 00000000..c2e2210b --- /dev/null +++ b/src/mobile_manipulation_config/waypoints/ur_waypoints.yaml @@ -0,0 +1,94 @@ +- description: '' + favorite: true + joint_group_names: + - gripper + - linear_actuator + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - robotiq_85_left_knuckle_joint + - linear_x_joint + - linear_y_joint + - rotational_yaw_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + position: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 4.758818847682003e-05 + - -1.5341106548131833 + - 0.8197512382602479 + - -2.268639495454643 + - -1.5707673068531498 + - -9.811780847989905e-05 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Look at Wall +- description: '' + favorite: false + joint_group_names: + - gripper + - linear_actuator + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - robotiq_85_left_knuckle_joint + - linear_x_joint + - linear_y_joint + - rotational_yaw_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + position: + - 0.0 + - 0.4 + - 0.4 + - 1.0 + - 0.0 + - -1.5708 + - 0.785398 + - -1.2948195437205516 + - -1.5708 + - 0.0 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Look at Table diff --git a/src/picknik_ur_mobile_config/config/config.yaml b/src/picknik_ur_mobile_config/config/config.yaml index c330b194..8af6c561 100644 --- a/src/picknik_ur_mobile_config/config/config.yaml +++ b/src/picknik_ur_mobile_config/config/config.yaml @@ -81,9 +81,3 @@ ros2_control: # 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_camera/points" diff --git a/src/picknik_ur_mobile_config/package.xml b/src/picknik_ur_mobile_config/package.xml index 9487462c..9e8a0e10 100644 --- a/src/picknik_ur_mobile_config/package.xml +++ b/src/picknik_ur_mobile_config/package.xml @@ -3,7 +3,7 @@ picknik_ur_mobile_config 6.0.0 - MuJoCo simulation configuration package for Picknik's UR robot on a linear rail + MuJoCo simulation configuration package for Picknik's UR robot on a mobile base MoveIt Pro Maintainer diff --git a/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml index c08e1f6c..5c09bf5b 100644 --- a/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml +++ b/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml @@ -1,11 +1,11 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/src/picknik_ur_mujoco_config/config/config.yaml b/src/picknik_ur_mujoco_config/config/config.yaml index c14a7c48..38c0285a 100644 --- a/src/picknik_ur_mujoco_config/config/config.yaml +++ b/src/picknik_ur_mujoco_config/config/config.yaml @@ -42,6 +42,7 @@ objectives: - "moveit_studio::behaviors::MTCCoreBehaviorsLoader" - "moveit_studio::behaviors::ServoBehaviorsLoader" - "moveit_studio::behaviors::VisionBehaviorsLoader" + - "moveit_studio::behaviors::ConverterBehaviorsLoader" # Specify source folder for objectives # [Required] objective_library_paths: @@ -73,6 +74,8 @@ ros2_control: # Load but do not start these controllers so they can be activated later if needed. controllers_inactive_at_startup: - "joint_trajectory_controller" + - "joint_trajectory_admittance_controller" + - "velocity_force_controller" # Any controllers here will not be spawned by MoveIt Pro. # [Optional, default=[]] controllers_not_managed: [] diff --git a/src/picknik_ur_mujoco_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_mujoco_config/config/control/picknik_ur.ros2_control.yaml index e41e5c96..9b47d9aa 100644 --- a/src/picknik_ur_mujoco_config/config/control/picknik_ur.ros2_control.yaml +++ b/src/picknik_ur_mujoco_config/config/control/picknik_ur.ros2_control.yaml @@ -9,9 +9,12 @@ controller_manager: type: position_controllers/GripperActionController servo_controller: type: joint_trajectory_controller/JointTrajectoryController - force_torque_sensor_broadcaster: type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + joint_trajectory_admittance_controller: + type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController + velocity_force_controller: + type: velocity_force_controller/VelocityForceController joint_state_broadcaster: ros__parameters: @@ -151,3 +154,75 @@ force_torque_sensor_broadcaster: - torque.y - torque.z frame_id: fts_link + + +joint_trajectory_admittance_controller: + ros__parameters: + joints: + - linear_rail_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + base_frame: base_link + sensor_frame: tool0 + ee_frame: grasp_link + ft_sensor_name: robotiq_ft_sensor + # Joint accelerations chosen to match MoveIt configs. + stop_accelerations: + - 30.0 + - 30.0 + - 30.0 + - 30.0 + - 30.0 + - 30.0 + - 30.0 + +velocity_force_controller: + ros__parameters: + joints: + - linear_rail_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + base_frame: base_link + sensor_frame: tool0 + ee_frame: grasp_link + ft_sensor_name: robotiq_ft_sensor + ft_force_deadband: 2.0 + ft_torque_deadband: 1.0 + max_joint_velocity: + - 0.524 + - 0.524 + - 0.524 + - 0.524 + - 1.047 + - 1.047 + - 1.047 + max_joint_acceleration: + - 0.524 + - 0.524 + - 0.524 + - 0.524 + - 0.524 + - 0.524 + - 0.524 + max_cartesian_velocity: + - 0.25 + - 0.25 + - 0.25 + - 1.5707 + - 1.5707 + - 1.5707 + max_cartesian_acceleration: + - 2.0 + - 2.0 + - 2.0 + - 4.0 + - 4.0 + - 4.0 diff --git a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro index 33905a51..de03a457 100644 --- a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro +++ b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro @@ -790,12 +790,6 @@ - - - - - - @@ -880,20 +874,20 @@ - + - + - + diff --git a/src/picknik_ur_mujoco_config/description/scene.xml b/src/picknik_ur_mujoco_config/description/scene.xml index 073c6a57..1d2da458 100644 --- a/src/picknik_ur_mujoco_config/description/scene.xml +++ b/src/picknik_ur_mujoco_config/description/scene.xml @@ -43,24 +43,44 @@ - - + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/src/picknik_ur_mujoco_config/description/ur5e.xml b/src/picknik_ur_mujoco_config/description/ur5e.xml index 4300f7c6..df4617f3 100644 --- a/src/picknik_ur_mujoco_config/description/ur5e.xml +++ b/src/picknik_ur_mujoco_config/description/ur5e.xml @@ -1,5 +1,5 @@ - + diff --git a/src/picknik_ur_mujoco_config/objectives/command_vfc.xml b/src/picknik_ur_mujoco_config/objectives/command_vfc.xml new file mode 100644 index 00000000..86254a50 --- /dev/null +++ b/src/picknik_ur_mujoco_config/objectives/command_vfc.xml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_mujoco_config/objectives/force_relaxation.xml b/src/picknik_ur_mujoco_config/objectives/force_relaxation.xml new file mode 100644 index 00000000..f3e0ffe8 --- /dev/null +++ b/src/picknik_ur_mujoco_config/objectives/force_relaxation.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_mujoco_config/objectives/move_until_contact_example.xml b/src/picknik_ur_mujoco_config/objectives/move_until_contact_example.xml new file mode 100644 index 00000000..ad393051 --- /dev/null +++ b/src/picknik_ur_mujoco_config/objectives/move_until_contact_example.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_mujoco_config/objectives/move_with_velocity_and_distance_example.xml b/src/picknik_ur_mujoco_config/objectives/move_with_velocity_and_distance_example.xml new file mode 100644 index 00000000..b60d0755 --- /dev/null +++ b/src/picknik_ur_mujoco_config/objectives/move_with_velocity_and_distance_example.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/picknik_ur_mujoco_config/objectives/velocity_force_control_example.xml b/src/picknik_ur_mujoco_config/objectives/velocity_force_control_example.xml new file mode 100644 index 00000000..1c043bcd --- /dev/null +++ b/src/picknik_ur_mujoco_config/objectives/velocity_force_control_example.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_mujoco_config/package.xml b/src/picknik_ur_mujoco_config/package.xml index b4024b61..c0193a82 100644 --- a/src/picknik_ur_mujoco_config/package.xml +++ b/src/picknik_ur_mujoco_config/package.xml @@ -12,6 +12,7 @@ ament_cmake admittance_controller + joint_trajectory_admittance_controller moveit_planners_stomp moveit_ros_perception moveit_studio_agent @@ -26,6 +27,7 @@ ur_description ur_robot_driver picknik_mujoco_ros + velocity_force_controller ament_lint_auto diff --git a/src/picknik_ur_mujoco_config/waypoints/ur_waypoints.yaml b/src/picknik_ur_mujoco_config/waypoints/ur_waypoints.yaml index 706c3227..95f7d47e 100644 --- a/src/picknik_ur_mujoco_config/waypoints/ur_waypoints.yaml +++ b/src/picknik_ur_mujoco_config/waypoints/ur_waypoints.yaml @@ -21,14 +21,14 @@ - wrist_2_joint - wrist_3_joint position: - - 0.0 + - 0 - 0.29608248868337067 - - 0.0 + - 0 - -1.5708 - 0.785398 - -1.2948195437205516 - -1.5708 - - 0.0 + - 0 velocity: [] multi_dof_joint_state: header: @@ -41,3 +41,46 @@ twist: [] wrench: [] name: Look at Table +- description: '' + favorite: false + joint_group_names: + - gripper + - linear_actuator + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - robotiq_85_left_knuckle_joint + - linear_rail_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + position: + - 0.020916781481167455 + - -0.22610599344464546 + - 0.38969451825539325 + - -0.7463048667584851 + - 1.4104637547514023 + - -2.2350037021105393 + - -1.5646945626735103 + - 0.3796920004270002 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: VFC Start diff --git a/src/picknik_ur_multi_arm_config/config/config.yaml b/src/picknik_ur_multi_arm_config/config/config.yaml index 4eb77e35..c56f3a6d 100644 --- a/src/picknik_ur_multi_arm_config/config/config.yaml +++ b/src/picknik_ur_multi_arm_config/config/config.yaml @@ -1,11 +1,6 @@ # 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. # This allows users to switch between mock and real hardware by changing a single parameter with config inheritance. @@ -26,9 +21,6 @@ hardware: 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. @@ -36,9 +28,6 @@ hardware: 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" # Parameters used to configure the robot description through XACRO. # A URDF and SRDF are both required. @@ -79,10 +68,7 @@ hardware: # Load visual_parameters.yaml from ur_description/config/ package: "ur_description" path: "config/ur5e/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] @@ -138,13 +124,6 @@ moveit_params: 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: "first_grasp_link" # Configuration for launching ros2_control processes. # [Required, if using ros2_control] @@ -182,12 +161,6 @@ ros2_control: # [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: diff --git a/src/picknik_ur_multi_arm_config/config/moveit/multi_arm_ur.srdf b/src/picknik_ur_multi_arm_config/config/moveit/multi_arm_ur.srdf index 44e58209..b306e8a3 100644 --- a/src/picknik_ur_multi_arm_config/config/moveit/multi_arm_ur.srdf +++ b/src/picknik_ur_multi_arm_config/config/moveit/multi_arm_ur.srdf @@ -182,27 +182,6 @@ - - - - - - - - - - - - - - - - - - - - - @@ -331,27 +310,6 @@ - - - - - - - - - - - - - - - - - - - - - @@ -480,27 +438,6 @@ - - - - - - - - - - - - - - - - - - - - - @@ -629,27 +566,6 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_config/description/multi_arm_ur.xacro b/src/picknik_ur_multi_arm_config/description/multi_arm_ur.xacro index b6c6294e..18b02d84 100644 --- a/src/picknik_ur_multi_arm_config/description/multi_arm_ur.xacro +++ b/src/picknik_ur_multi_arm_config/description/multi_arm_ur.xacro @@ -23,6 +23,23 @@ + + + + + + + + + + + + + + + + + @@ -38,17 +55,16 @@ - - - - - - - - - - - - - - - - - - - - - + - + - - - - - - - - - - - - - - - - + + + + + + diff --git a/src/picknik_ur_multi_arm_config/waypoints/ur_waypoints.yaml b/src/picknik_ur_multi_arm_config/waypoints/ur_waypoints.yaml index 7b31e104..f7a0bb55 100644 --- a/src/picknik_ur_multi_arm_config/waypoints/ur_waypoints.yaml +++ b/src/picknik_ur_multi_arm_config/waypoints/ur_waypoints.yaml @@ -1,3 +1,92 @@ +- description: '' + favorite: false + joint_group_names: + - first_manipulator + - fourth_gripper + - fourth_manipulator + - gripper + - multi_arm_manipulator + - second_gripper + - second_manipulator + - third_gripper + - third_manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - first_shoulder_pan_joint + - first_shoulder_lift_joint + - first_elbow_joint + - first_wrist_1_joint + - first_wrist_2_joint + - first_wrist_3_joint + - fourth_robotiq_85_left_knuckle_joint + - fourth_shoulder_pan_joint + - fourth_shoulder_lift_joint + - fourth_elbow_joint + - fourth_wrist_1_joint + - fourth_wrist_2_joint + - fourth_wrist_3_joint + - first_robotiq_85_left_knuckle_joint + - second_shoulder_pan_joint + - second_shoulder_lift_joint + - second_elbow_joint + - second_wrist_1_joint + - second_wrist_2_joint + - second_wrist_3_joint + - third_shoulder_pan_joint + - third_shoulder_lift_joint + - third_elbow_joint + - third_wrist_1_joint + - third_wrist_2_joint + - third_wrist_3_joint + - second_robotiq_85_left_knuckle_joint + - third_robotiq_85_left_knuckle_joint + position: + - 3.027358056711766 + - -2.027755258226139 + - 0.8567979964335799 + - -1.199517195007012 + - -1.4851165271515387 + - 0.00019073125240392984 + - 0.7929 + - -3.141561346270125 + - -1.71359599286716 + - 0.4855188646456952 + - -0.6283185307179586 + - -1.5993562600093492 + - 3.672383190132678e-05 + - 0.7929 + - 3.055875023715748 + - -1.770715859296065 + - 0.7711181967902214 + - -0.3712791317878848 + - -1.5707963267948966 + - 0.00013646876746788622 + - -3.1415909278466643 + - -1.71359599286716 + - 0.48551886464569516 + - -0.7500245148129295 + - -1.513676460365991 + - -5.523976171389222e-05 + - 0.7929 + - 0.7929 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Flip - description: '' favorite: true joint_group_names: @@ -37,6 +126,16 @@ - 1.6699864011902827 - -9.210125892423094e-05 velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] name: Two Arms Down - description: Home Flipped favorite: true @@ -104,7 +203,106 @@ - -1.669 - 3.141 velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] name: Home Flipped +- description: '' + favorite: true + joint_group_names: + - first_manipulator + - fourth_gripper + - fourth_manipulator + - gripper + - multi_arm_manipulator + - second_gripper + - second_manipulator + - third_gripper + - third_manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - first_shoulder_pan_joint + - first_shoulder_lift_joint + - first_elbow_joint + - first_wrist_1_joint + - first_wrist_2_joint + - first_wrist_3_joint + - fourth_robotiq_85_left_knuckle_joint + - fourth_shoulder_pan_joint + - fourth_shoulder_lift_joint + - fourth_elbow_joint + - fourth_wrist_1_joint + - fourth_wrist_2_joint + - fourth_wrist_3_joint + - first_robotiq_85_left_knuckle_joint + - second_shoulder_pan_joint + - second_shoulder_lift_joint + - second_elbow_joint + - second_wrist_1_joint + - second_wrist_2_joint + - second_wrist_3_joint + - third_shoulder_pan_joint + - third_shoulder_lift_joint + - third_elbow_joint + - third_wrist_1_joint + - third_wrist_2_joint + - third_wrist_3_joint + - second_robotiq_85_left_knuckle_joint + - third_robotiq_85_left_knuckle_joint + position: + - 3.027352920731983 + - -1.0129336116452234 + - -1.7989371590030379 + - -0.7500431305404287 + - 1.6689824679556302 + - 9.53144878614694e-05 + - 0.7929 + - -3.141592653589793 + - -1.0129416442132089 + - -1.7130895491734148 + - -0.7499239977449179 + - 1.6689872079757508 + - 8.899450297467411e-05 + - 0.7929 + - 3.0559128539464346 + - -1.0130466124989557 + - -1.712902314238716 + - -0.7499221115582158 + - 1.6690666370701976 + - 4.438869529403747e-05 + - -3.141592653589793 + - -1.3137569278648227 + - -0.9139178628624852 + - -0.7499328942831606 + - 1.6689088345160707 + - 1.0323978960514069e-05 + - 0.7929 + - 0.7929 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Back - description: Arms down favorite: true joint_group_names: @@ -171,6 +369,16 @@ - 1.6699516917224972 - 0 velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] name: Arms Down - description: Home twisted favorite: true @@ -238,6 +446,16 @@ - 1.669 - 0 velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] name: Home Twisted - description: Home favorite: true @@ -305,4 +523,14 @@ - 1.669 - 0 velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] name: Home diff --git a/src/picknik_ur_multi_arm_gazebo_config/README.md b/src/picknik_ur_multi_arm_gazebo_config/README.md deleted file mode 100644 index 71574513..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# picknik_multi_ur_gazebo_config - -A MoveIt Pro example configuration package for using multiple UR5e arms using gazebo ignition. - -For further documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/) diff --git a/src/picknik_ur_multi_arm_gazebo_config/config/cameras.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/cameras.yaml deleted file mode 100644 index 565f04ec..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/cameras.yaml +++ /dev/null @@ -1,67 +0,0 @@ -# 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 - - # 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/depth_registered/camera_info" - registered_image: "/scene_camera/depth_registered/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/color/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/color/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_gazebo_config/config/config.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/config.yaml deleted file mode 100644 index 6702f230..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/config.yaml +++ /dev/null @@ -1,207 +0,0 @@ -# -# This contains information for a unique instance of a robot. -# - -# Optional parameters that can be read in your launch files for specific functionality -optional_feature_params: - gazebo_world_name: "space_station_blocks_world.sdf" - gazebo_gui: False - gazebo_verbose: True - -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. - type: "ur5e" - - # This is the only option for this site_config - simulated: True - - # Gazebo starts its own controller manager through the ros2_control plugin, so set this to False - launch_control_node: False - - # 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. - launch_cameras_when_simulated: True - - # 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_hardware_launch_file: - package: "picknik_ur_multi_arm_gazebo_config" - path: "launch/sim/hardware_sim.launch.py" - - # Configuration details for cameras and scene planning. - # [Required] - camera_config_file: - package: "picknik_ur_multi_arm_gazebo_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_gazebo_config" - path: "description/dual_arm_ur.xacro" - srdf: - package: "picknik_ur_multi_arm_gazebo_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: "false" - - gripper_name: "robotiq_85" - - use_pinch_links: "true" - - simulation: "gazebo" - - fake_sensor_commands: "false" - - headless_mode: "true" - - robot_ip: "0.0.0.0" - - 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/ur5e/default_kinematics.yaml" - - physical_parameters_file: - # Load physical_parameters.yaml from ur_description/config/ - package: "ur_description" - path: "config/ur5e/physical_parameters.yaml" - - visual_parameters_file: - # Load visual_parameters.yaml from ur_description/config/ - package: "ur_description" - path: "config/ur5e/visual_parameters.yaml" - -# Sets ROS global params for launch. -# [Optional] -ros_global_params: - # Whether or not to use simulated time. - # [Optional, default=False] - use_sim_time: True - -# 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: "manipulator_right" - - ompl_planning: - package: "picknik_ur_multi_arm_gazebo_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_gazebo_config" - path: "config/moveit/trac_ik_kinematics_distance.yaml" - servo: - package: "picknik_ur_multi_arm_gazebo_config" - path: "config/moveit/ur_servo.yaml" - sensors_3d: - package: "picknik_ur_base_config" - path: "config/moveit/sensors_3d.yaml" - servo_kinematics: - package: "picknik_ur_multi_arm_gazebo_config" - path: "config/moveit/trac_ik_kinematics_speed.yaml" - joint_limits: - package: "picknik_ur_multi_arm_gazebo_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" - - -# This configures what controllers gets run at startup -ros2_control: - config: - package: "picknik_ur_multi_arm_gazebo_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" - - "servo_controller" - - "left_robotiq_gripper_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: "/left_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_gazebo_config" - relative_path: "objectives" - # Specify the location of the saved waypoints file. - # [Required] - waypoints_file: - package_name: "picknik_ur_multi_arm_gazebo_config" - relative_path: "waypoints/waypoints.yaml" diff --git a/src/picknik_ur_multi_arm_gazebo_config/config/control/picknik_dual_ur.ros2_control.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/control/picknik_dual_ur.ros2_control.yaml deleted file mode 100644 index 5675518c..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/control/picknik_dual_ur.ros2_control.yaml +++ /dev/null @@ -1,241 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 200 # Hz - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - 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 - right_robotiq_gripper_controller: - type: position_controllers/GripperActionController - servo_controller: - type: joint_trajectory_controller/JointTrajectoryController - -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 - -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 - - 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: - - right_shoulder_pan_joint - - right_shoulder_lift_joint - - right_elbow_joint - - right_wrist_1_joint - - right_wrist_2_joint - - right_wrist_3_joint - - 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 - 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_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 - -left_robotiq_gripper_controller: - ros__parameters: - default: true - joint: left_robotiq_85_left_knuckle_joint - allow_stalling: true - stall_velocity_threshold: 0.075 - stall_timeout: 0.2 - -right_robotiq_gripper_controller: - ros__parameters: - default: true - joint: right_robotiq_85_left_knuckle_joint - allow_stalling: true - stall_velocity_threshold: 0.075 - stall_timeout: 0.2 diff --git a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/dual_arm_joint_limits.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/dual_arm_joint_limits.yaml deleted file mode 100644 index 6c453078..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/dual_arm_joint_limits.yaml +++ /dev/null @@ -1,242 +0,0 @@ -# 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 - -# UR robots use hard coded values that ignore prefixes - - 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 - 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 - 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 - 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 - 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 - 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 - 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_gazebo_config/config/moveit/dual_arm_ur.srdf b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/dual_arm_ur.srdf deleted file mode 100644 index c6732b4d..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/dual_arm_ur.srdf +++ /dev/null @@ -1,349 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/ompl_planning.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/ompl_planning.yaml deleted file mode 100644 index f7ec7771..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/ompl_planning.yaml +++ /dev/null @@ -1,112 +0,0 @@ -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 -gripper: - 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_gazebo_config/config/moveit/pilz_cartesian_limits.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/pilz_cartesian_limits.yaml deleted file mode 100644 index 1633938e..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/pilz_cartesian_limits.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# 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_gazebo_config/config/moveit/stomp_planning.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/stomp_planning.yaml deleted file mode 100644 index dc6fd444..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/stomp_planning.yaml +++ /dev/null @@ -1,22 +0,0 @@ -planning_plugins: - - stomp_moveit/StompPlanner -# 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 - -stomp_moveit: - num_timesteps: 60 - num_iterations: 40 - num_iterations_after_valid: 0 - num_rollouts: 30 - max_rollouts: 30 - exponentiated_cost_sensitivity: 0.8 - control_cost_weight: 0.1 - delta_t: 0.1 diff --git a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/trac_ik_kinematics_distance.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/trac_ik_kinematics_distance.yaml deleted file mode 100644 index afdc636c..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/trac_ik_kinematics_distance.yaml +++ /dev/null @@ -1,12 +0,0 @@ -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_gazebo_config/config/moveit/trac_ik_kinematics_speed.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/trac_ik_kinematics_speed.yaml deleted file mode 100644 index fe311e4a..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/trac_ik_kinematics_speed.yaml +++ /dev/null @@ -1,12 +0,0 @@ -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_gazebo_config/config/moveit/ur_servo.yaml b/src/picknik_ur_multi_arm_gazebo_config/config/moveit/ur_servo.yaml deleted file mode 100644 index b26594ad..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/config/moveit/ur_servo.yaml +++ /dev/null @@ -1,60 +0,0 @@ -############################################### -# 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: trajectory_msgs/JointTrajectory - -# 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: /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_gazebo_config/description/camera_and_gripper.xacro b/src/picknik_ur_multi_arm_gazebo_config/description/camera_and_gripper.xacro deleted file mode 100644 index 2dd4becc..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/description/camera_and_gripper.xacro +++ /dev/null @@ -1,135 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.047 - - 640 - 480 - RGB_INT8 - - - 0.1 - 5 - - - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 -
0.5 0.5
- - - - 554.25469 - 554.25469 - 320.5 - 240.5 - 0 - - - - 554.25469 - 554.25469 - 320.5 - 240.5 - 0 - 0 - - - - gaussian - 0 - 0.00 - - - - 0.25 - 5 - - - ${prefix_}wrist_mounted_camera_color_optical_frame - - ${prefix_}wrist_mounted_camera_color_frame - 1 - 6 - false - ${prefix_}wrist_mounted_camera - false - - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/description/dual_arm_ur.xacro b/src/picknik_ur_multi_arm_gazebo_config/description/dual_arm_ur.xacro deleted file mode 100644 index 10328d78..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/description/dual_arm_ur.xacro +++ /dev/null @@ -1,222 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - 50 - 0 - left_tcp_fts_sensor/ft_data - 0 0 0 0 0 0 - - child - parent_to_child - - - - 0 - 0 - - - - - 0 - 0 - - - - - 0 - 0 - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 0 - 0 - - - - - - - - - - - $(find picknik_ur_multi_arm_gazebo_config)/config/control/picknik_dual_ur.ros2_control.yaml - - /left_servo_controller/commands:=/left_robot_controllers/commands - /left_servo_controller/joint_trajectory:=/left_robot_controllers/joint_trajectory - /right_servo_controller/commands:=/right_robot_controllers/commands - /right_servo_controller/joint_trajectory:=/right_robot_controllers/joint_trajectory - /servo_controller/commands:=/robot_controllers/commands - /servo_controller/joint_trajectory:=/robot_controllers/joint_trajectory - - - - - - - ${gazebo_renderer} - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/description/simulation_worlds/space_station_blocks_world.sdf b/src/picknik_ur_multi_arm_gazebo_config/description/simulation_worlds/space_station_blocks_world.sdf deleted file mode 100644 index 5912c6a1..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/description/simulation_worlds/space_station_blocks_world.sdf +++ /dev/null @@ -1,441 +0,0 @@ - - - - - 0.005 - 0.005 - 1.0 - - - - - - - - - - - - - - - 0.0 0.0 0.0 1.0 - 0 - - - - false - 0 0 100 0 0 0 - 0.6 0.6 0.6 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.5 -1.0 - - - false - 0 0 100 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 -0.5 -1.0 - - - false - 0 0 -100 0 0 0 - 0.5 0.5 0.5 1 - 0.5 0.5 0.5 1 - - 1000 - 0.9 - 0.01 - 0.001 - - 0.0 0.0 1.0 - - - false - 100 75 1.5 0 0 0 - 0.4 0.4 0.4 1 - 0.4 0.4 0.4 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -1.0 -0.75 0.0 - - - false - 100 -75 1.5 0 0 0 - 0.4 0.4 0.4 - 0.4 0.4 0.4 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -1.0 0.75 0.0 - - - - true - - - - - 0 0 1 - 100 100 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - - true - - - - - package://picknik_accessories/descriptions/furniture/space_station/space_booth.dae - - - - - - - package://picknik_accessories/descriptions/furniture/space_station/space_booth.dae - - - - -1 0 0 0 0 0 - - false - - - - - true - 0 0 1 0 0 0 - - - - - 1.5 1.0 0.05 - - - - - - 0.6 - 0.6 - - - - - 1e+5 - 1 - 0 - 0.2 - 0.001 - - - - - - - - - 1.5 1.0 0.05 - - - - 0.93 0.89 0.75 1.0 - 0.93 0.89 0.75 1.0 - - - - - - - - -0.6 -0.3 1.1 0 0 0 - - - - 0.0001875 - 0 - 0 - 0.0001875 - 0 - 0.0001875 - - 0.2 - - - - - 0.05 0.05 0.05 - - - - - - 1000000.0 - 1000000.0 - - - - - 1e+5 - 1 - 0 - 0.2 - 0.002 - 0 - - - - - - - - - 0.05 0.05 0.05 - - - - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 - - - - - - - -0.6 0.0 1.1 0 0 0 - - - - 0.0001875 - 0 - 0 - 0.0001875 - 0 - 0.0001875 - - 0.2 - - - - - 0.05 0.05 0.05 - - - - - - 1000000.0 - 1000000.0 - - - - - 1e+5 - 1 - 0 - 0.2 - 0.002 - 0 - - - - - - - - - 0.05 0.05 0.05 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - - - - - -0.6 0.3 1.1 0 0 0 - - - - 0.0001875 - 0 - 0 - 0.0001875 - 0 - 0.0001875 - - 0.2 - - - - - 0.05 0.05 0.05 - - - - - - 1000000.0 - 1000000.0 - - - - - 1e+5 - 1 - 0 - 0.2 - 0.002 - 0 - - - - - - - - - 0.05 0.05 0.05 - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - - - - - - - -0.5 -0.3 1.03 0 0 0 - - - - - 1 1 1 - package://picknik_ur_gazebo_config/description/apriltags/tag36_11_00001.dae - - - - - - 0.0001 - 0 - 0 - 0.0001 - 0 - 0.0001 - - 0.01 - - - 1 - - - - -0.5 0.0 1.03 0 0 0 - - - - - 1 1 1 - package://picknik_ur_gazebo_config/description/apriltags/tag36_11_00002.dae - - - - - - 0.0001 - 0 - 0 - 0.0001 - 0 - 0.0001 - - 0.01 - - - 1 - - - - -0.5 0.3 1.03 0 0 0 - - - - - 1 1 1 - package://picknik_ur_gazebo_config/description/apriltags/tag36_11_00003.dae - - - - - - 0.0001 - 0 - 0 - 0.0001 - 0 - 0.0001 - - 0.01 - - - 1 - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/launch/robot_drivers_to_persist.launch.py b/src/picknik_ur_multi_arm_gazebo_config/launch/robot_drivers_to_persist.launch.py deleted file mode 100644 index 72aa95a2..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/launch/robot_drivers_to_persist.launch.py +++ /dev/null @@ -1,75 +0,0 @@ -# Copyright 2023 PickNik Inc. -# -# 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 PickNik Inc. 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. - - -from launch import LaunchDescription -from launch_ros.actions import Node - -from moveit_studio_utils_py.launch_common import empty_gen -from moveit_studio_utils_py.system_config import ( - SystemConfigParser, -) - - -def generate_launch_description(): - system_config_parser = SystemConfigParser() - hardware_config = system_config_parser.get_hardware_config() - controller_config = system_config_parser.get_ros2_control_config() - - dashboard_client_node = Node( - package="ur_robot_driver", - executable="dashboard_client", - name="dashboard_client", - output="both", - emulate_tty=True, - parameters=[{"robot_ip": hardware_config["ip"]}], - ) - - protective_stop_manager_node = Node( - package="moveit_studio_ur_pstop_manager", - executable="protective_stop_manager_node", - name="protective_stop_manager_node", - output="both", - parameters=[ - { - "controllers_default_active": controller_config.get( - "controllers_active_at_startup", empty_gen() - ), - "controllers_default_not_active": controller_config.get( - "controllers_inactive_at_startup", empty_gen() - ), - } - ], - ) - - nodes_to_launch = [ - dashboard_client_node, - protective_stop_manager_node, - ] - - return LaunchDescription(nodes_to_launch) diff --git a/src/picknik_ur_multi_arm_gazebo_config/launch/sim/hardware_sim.launch.py b/src/picknik_ur_multi_arm_gazebo_config/launch/sim/hardware_sim.launch.py deleted file mode 100644 index d2715245..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/launch/sim/hardware_sim.launch.py +++ /dev/null @@ -1,352 +0,0 @@ -# Copyright 2023 PickNik Inc. -# -# 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 PickNik Inc. 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. - - -import os -import re -import shlex - -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, OpaqueFunction -from launch_ros.actions import Node - -from moveit_studio_utils_py.launch_common import get_launch_file, get_ros_path -from moveit_studio_utils_py.system_config import get_config_folder, SystemConfigParser - - -def path_pattern_change_for_gazebo(urdf_string): - """ - Replaces strings in a URDF file such as - package://package_name/path/to/file - to the actual full path of the file. - """ - data = urdf_string - package_expressions = re.findall("(package://([^//]*))", data) - for expr in set(package_expressions): - data = data.replace(expr[0], get_ros_path(expr[1])) - return data - - -def generate_simulation_description(context, *args, **settings): - world_path = settings.get( - "gazebo_world_path", - "description/simulation_worlds/space_station_blocks_world.sdf", - ) - use_gui = settings.get("gazebo_gui", False) - is_verbose = settings.get("gazebo_verbose", False) - - # Create a Gazebo world file that swaps out package:// paths with absolute path. - original_world_file = get_ros_path( - settings.get("gazebo_world_package_name", "picknik_ur_multi_arm_gazebo_config"), - world_path, - ) - modified_world_file = os.path.join( - get_config_folder(), "auto_created", "gazebo_world.sdf" - ) - with open(original_world_file, "r") as file: - world_sdf = path_pattern_change_for_gazebo(file.read()) - with open(modified_world_file, "w") as file: - file.write(world_sdf) - - # Launch Gazebo. - print(f"Starting Gazebo with world at {world_path}") - print(f"GUI: {use_gui}, Verbose: {is_verbose}") - - sim_args = "-r --render-engine ogre" - if is_verbose: - sim_args += " -v 4" - if not use_gui: - sim_args += " -s --headless-rendering" - - gazebo = IncludeLaunchDescription( - get_launch_file("ros_gz_sim", "launch/gz_sim.launch.py"), - launch_arguments=[("gz_args", [f"{sim_args} {modified_world_file}"])], - ) - return [gazebo] - - -def generate_launch_description(): - system_config_parser = SystemConfigParser() - optional_feature_setting = system_config_parser.get_optional_feature_configs() - - # The path to the auto_created urdf files - robot_urdf = system_config_parser.get_processed_urdf() - robot_urdf_ignition = path_pattern_change_for_gazebo(robot_urdf) - - # Launch Gazebo - gazebo = OpaqueFunction( - function=generate_simulation_description, kwargs=optional_feature_setting - ) - - init_pose_args = shlex.split("-x 0.0 -y 0.0 -z 1.03 -R 0.0 -P 0.0 -Y 0.0") - spawn_robot = Node( - package="ros_gz_sim", - executable="create", - output="both", - arguments=[ - "-string", - robot_urdf_ignition, - "-name", - "robot", - "-allow_renaming", - "true", - ] - + init_pose_args, - ) - - ######################## - # Camera Topic Bridges # - ######################## - # For the scene camera, enable RGB image topics only. - scene_image_rgb_ignition_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - name="scene_image_rgb_ignition_bridge", - arguments=[ - "/scene_camera/image", - ], - remappings=[ - ("/scene_camera/image", "/scene_camera/color/image_raw"), - ], - output="both", - ) - scene_image_depth_ignition_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - name="scene_image_depth_ignition_bridge", - arguments=[ - "/scene_camera/depth_image", - ], - remappings=[ - ( - "/scene_camera/depth_image", - "/scene_camera/depth/image_rect_raw", - ), - ], - output="both", - ) - - scene_camera_info_ignition_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="scene_camera_info_ignition_bridge", - arguments=[ - "/scene_camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", - ], - remappings=[ - ("/scene_camera/camera_info", "/scene_camera/color/camera_info"), - ], - output="both", - ) - - scene_camera_pointcloud_ignition_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="scene_camera_pointcloud_ignition_bridge", - arguments=[ - "/scene_mounted_camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", - ], - remappings=[ - ( - "/scene_mounted_camera/points", - "/scene_mounted_camera/depth/color/points", - ), - ], - output="both", - ) - - # For the wrist mounted camera, enable RGB and depth topics. - left_wrist_image_rgb_ignition_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - name="left_wrist_image_rgb_ignition_bridge", - arguments=[ - "/left_wrist_mounted_camera/image", - ], - remappings=[ - ( - "/left_wrist_mounted_camera/image", - "/left_wrist_mounted_camera/color/image_raw", - ), - ], - output="both", - ) - left_wrist_image_depth_ignition_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - name="left_wrist_image_depth_ignition_bridge", - arguments=[ - "/left_wrist_mounted_camera/depth_image", - ], - remappings=[ - ( - "/left_wrist_mounted_camera/depth_image", - "/left_wrist_mounted_camera/depth/image_rect_raw", - ), - ], - output="both", - ) - left_wrist_camera_pointcloud_ignition_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="left_wrist_camera_pointcloud_ignition_bridge", - arguments=[ - "/left_wrist_mounted_camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", - ], - remappings=[ - ( - "/left_wrist_mounted_camera/points", - "/left_wrist_mounted_camera/depth/color/points", - ), - ], - output="both", - ) - left_wrist_camera_info_ignition_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="left_wrist_camera_info_ignition_bridge", - arguments=[ - "/left_wrist_mounted_camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", - ], - remappings=[ - ( - "/left_wrist_mounted_camera/camera_info", - "/left_wrist_mounted_camera/color/camera_info", - ), - ], - output="both", - ) - - right_wrist_image_rgb_ignition_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - name="right_wrist_image_rgb_ignition_bridge", - arguments=[ - "/right_wrist_mounted_camera/image", - ], - remappings=[ - ( - "/right_wrist_mounted_camera/image", - "/right_wrist_mounted_camera/color/image_raw", - ), - ], - output="both", - ) - right_wrist_image_depth_ignition_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - name="right_wrist_image_depth_ignition_bridge", - arguments=[ - "/right_wrist_mounted_camera/depth_image", - ], - remappings=[ - ( - "/right_wrist_mounted_camera/depth_image", - "/right_wrist_mounted_camera/depth/image_rect_raw", - ), - ], - output="both", - ) - right_wrist_camera_pointcloud_ignition_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="right_wrist_camera_pointcloud_ignition_bridge", - arguments=[ - "/right_wrist_mounted_camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", - ], - remappings=[ - ( - "/right_wrist_mounted_camera/points", - "/right_wrist_mounted_camera/depth/color/points", - ), - ], - output="both", - ) - right_wrist_camera_info_ignition_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="right_wrist_camera_info_ignition_bridge", - arguments=[ - "/right_wrist_mounted_camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", - ], - remappings=[ - ( - "/right_wrist_mounted_camera/camera_info", - "/right_wrist_mounted_camera/color/camera_info", - ), - ], - output="both", - ) - - ####################### - # Force Torque Sensor # - ####################### - fts_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="fts_bridge", - arguments=[ - "/tcp_fts_sensor/ft_data@geometry_msgs/msg/WrenchStamped[ignition.msgs.Wrench", - ], - remappings=[ - ( - "/tcp_fts_sensor/ft_data", - "/force_torque_sensor_broadcaster/wrench", - ), - ], - output="both", - ) - - clock_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="clock_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], - output="both", - ) - - return LaunchDescription( - [ - scene_image_rgb_ignition_bridge, - scene_image_depth_ignition_bridge, - scene_camera_info_ignition_bridge, - scene_camera_pointcloud_ignition_bridge, - left_wrist_image_rgb_ignition_bridge, - left_wrist_camera_info_ignition_bridge, - left_wrist_image_depth_ignition_bridge, - left_wrist_camera_pointcloud_ignition_bridge, - right_wrist_image_rgb_ignition_bridge, - right_wrist_camera_info_ignition_bridge, - right_wrist_image_depth_ignition_bridge, - right_wrist_camera_pointcloud_ignition_bridge, - clock_bridge, - fts_bridge, - gazebo, - spawn_robot, - ] - ) diff --git a/src/picknik_ur_multi_arm_gazebo_config/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/picknik_ur_multi_arm_gazebo_config/launch/sim/robot_drivers_to_persist_sim.launch.py deleted file mode 100644 index c6596305..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/launch/sim/robot_drivers_to_persist_sim.launch.py +++ /dev/null @@ -1,43 +0,0 @@ -# Copyright 2023 PickNik Inc. -# -# 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 PickNik Inc. 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. - - -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - # Mock the UR Dashboard Client - mock_dashboard_client = Node( - package="moveit_studio_ur_pstop_manager", - executable="mock_ur_dashboard_client_node", - name="dashboard_client", - output="both", - ) - - return LaunchDescription([mock_dashboard_client]) diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/clear_snapshot.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/clear_snapshot.xml deleted file mode 100644 index 68c52c49..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/clear_snapshot.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/close_left_gripper.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/close_left_gripper.xml deleted file mode 100644 index ef168026..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/close_left_gripper.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/close_right_gripper.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/close_right_gripper.xml deleted file mode 100644 index 7e8268eb..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/close_right_gripper.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/interpolate_to_joint_state.xml deleted file mode 100644 index 811a5829..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/interpolate_to_joint_state.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_joint_state.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_joint_state.xml deleted file mode 100644 index 158cb6b5..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_joint_state.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_named_pose.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_named_pose.xml deleted file mode 100644 index 7d907dc7..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_named_pose.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_pose.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_pose.xml deleted file mode 100644 index 3a5086c3..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_to_pose.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_two_arms.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/move_two_arms.xml deleted file mode 100644 index abd6ca73..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/move_two_arms.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/open_left_gripper.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/open_left_gripper.xml deleted file mode 100644 index 90e72edb..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/open_left_gripper.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/open_right_gripper.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/open_right_gripper.xml deleted file mode 100644 index 879aee52..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/open_right_gripper.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object.xml deleted file mode 100644 index 7bd28350..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object_config.yaml b/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object_config.yaml deleted file mode 100644 index 1697754c..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object_config.yaml +++ /dev/null @@ -1,11 +0,0 @@ -SetupMTCPickObject: - # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame. - world_frame_name: "world" - arm_group_name: "manipulator_right" - end_effector_group_name: "gripper" - end_effector_name: "right_ee" - hand_frame_name: "right_grasp_link" - end_effector_closed_pose_name: "close" - - approach_distance: 0.1 - lift_distance: 0.1 diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object_left.yaml b/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object_left.yaml deleted file mode 100644 index dae9e59a..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/pick_object_left.yaml +++ /dev/null @@ -1,11 +0,0 @@ -SetupMTCPickObject: - # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame. - world_frame_name: "world" - arm_group_name: "manipulator_left" - end_effector_group_name: "gripper_left" - end_effector_name: "left_ee" - hand_frame_name: "left_grasp_link" - end_effector_closed_pose_name: "close_left" - - approach_distance: 0.1 - lift_distance: 0.1 diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/reactivate_gripper.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/reactivate_gripper.xml deleted file mode 100644 index a9666a46..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/reactivate_gripper.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/request_teleoperation.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/request_teleoperation.xml deleted file mode 100644 index 5565817b..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/request_teleoperation.xml +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/reset_planning_scene.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/reset_planning_scene.xml deleted file mode 100644 index 8e43ac5a..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/reset_planning_scene.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/take_snapshot.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/take_snapshot.xml deleted file mode 100644 index de177011..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/take_snapshot.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/teleoperate.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/teleoperate.xml deleted file mode 100644 index 23614685..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/teleoperate.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/objectives/wait_for_trajectory_approval_if_user_available.xml b/src/picknik_ur_multi_arm_gazebo_config/objectives/wait_for_trajectory_approval_if_user_available.xml deleted file mode 100644 index 7abb4108..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/objectives/wait_for_trajectory_approval_if_user_available.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/src/picknik_ur_multi_arm_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_multi_arm_gazebo_config/waypoints/waypoints.yaml deleted file mode 100644 index a21c8807..00000000 --- a/src/picknik_ur_multi_arm_gazebo_config/waypoints/waypoints.yaml +++ /dev/null @@ -1,241 +0,0 @@ -- description: '' - favorite: false - joint_group_names: - - gripper - - manipulator_right - joint_state: - effort: [] - header: - frame_id: '' - stamp: - nanosec: 0 - sec: 0 - name: - - right_robotiq_85_left_knuckle_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: - - -1.1967178672123605e-06 - - 1.6294922237286515 - - -1.4075014169855327 - - -0.7172292331392662 - - -1.02642347033837 - - 1.6641204475784201 - - 0.0021283164147193555 - velocity: [] - name: Inspect Right -- description: '' - favorite: false - joint_group_names: - - gripper - joint_state: - effort: [] - header: - frame_id: '' - stamp: - nanosec: 0 - sec: 0 - name: - - right_robotiq_85_left_knuckle_joint - position: - - 0.7930685959474917 - velocity: [] - name: Right Closed Gripper -- description: Both arms are in a down position - favorite: true - joint_group_names: - - dual_arm_manipulator - - gripper - - 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 -- description: '' - 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: - - -1.7095832296029843 - - -1.0500604029207294 - - -1.240154802498822 - - -0.8781638544771856 - - 1.6648887726908908 - - 0.0015182654301393564 - - 1.6294921509134237 - - -1.4075015036987286 - - -0.7172273447373503 - - -1.0264368393178898 - - 1.6641152815836038 - - 0.002125560625236223 - - 0.592406376256699 - - 0.592813686273142 - velocity: [] - name: Inspect -- description: '' - favorite: false - joint_group_names: - - gripper_left - - manipulator_left - joint_state: - effort: [] - header: - frame_id: '' - stamp: - nanosec: 0 - sec: 0 - name: - - left_robotiq_85_left_knuckle_joint - - left_shoulder_pan_joint - - left_shoulder_lift_joint - - left_elbow_joint - - left_wrist_1_joint - - left_wrist_2_joint - - left_wrist_3_joint - position: - - -2.0035421747362765e-06 - - -0.09976038781468007 - - -2.070732278116712 - - -1.7232074073646642 - - -0.9237947543320453 - - 1.5614126435536537 - - -0.057252780894965 - velocity: [] - name: Pick Red Left -- description: '' - favorite: false - joint_group_names: - - gripper_left - - manipulator_left - joint_state: - effort: [] - header: - frame_id: '' - stamp: - nanosec: 0 - sec: 0 - name: - - left_robotiq_85_left_knuckle_joint - - left_shoulder_pan_joint - - left_shoulder_lift_joint - - left_elbow_joint - - left_wrist_1_joint - - left_wrist_2_joint - - left_wrist_3_joint - position: - - -1.4413813976896227e-06 - - -1.71154795406197 - - -1.0535278711221066 - - -1.2324300583621988 - - -0.8837549907194114 - - 1.6672861150624345 - - 0.0013212085431139491 - velocity: [] - name: Inspect Left diff --git a/src/picknik_ur_site_config/CMakeLists.txt b/src/picknik_ur_site_config/CMakeLists.txt index 0fd7ee9b..db083761 100644 --- a/src/picknik_ur_site_config/CMakeLists.txt +++ b/src/picknik_ur_site_config/CMakeLists.txt @@ -2,6 +2,11 @@ cmake_minimum_required(VERSION 3.22) project(picknik_ur_site_config) find_package(ament_cmake REQUIRED) +find_package(moveit_studio_common REQUIRED) +moveit_studio_package() + +# add custom behaviors +add_subdirectory(behaviors) install( DIRECTORY @@ -20,4 +25,10 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface behaviors/trigger_pstop_reset_service_plugin_description.xml) + ament_package() diff --git a/src/picknik_ur_site_config/behaviors/CMakeLists.txt b/src/picknik_ur_site_config/behaviors/CMakeLists.txt new file mode 100644 index 00000000..cbeeeb27 --- /dev/null +++ b/src/picknik_ur_site_config/behaviors/CMakeLists.txt @@ -0,0 +1,40 @@ +set(THIS_PACKAGE_INCLUDE_DEPENDS + moveit_studio_behavior_interface + pluginlib + std_srvs + ) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + trigger_pstop_reset_service + SHARED + src/trigger_pstop_reset_service.cpp + src/register_behaviors.cpp) +target_include_directories( + trigger_pstop_reset_service + PUBLIC $ + $) +ament_target_dependencies(trigger_pstop_reset_service + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +if(BUILD_TESTING) + add_subdirectory(test) + # Add coverage flags behavior + target_compile_options(trigger_pstop_reset_service PRIVATE -fprofile-arcs -ftest-coverage) + target_link_options(trigger_pstop_reset_service PRIVATE -fprofile-arcs -ftest-coverage) +endif() + +# Install Libraries +install( + TARGETS trigger_pstop_reset_service + EXPORT trigger_pstop_reset_serviceTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +ament_export_targets(trigger_pstop_reset_serviceTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/picknik_ur_site_config/behaviors/include/trigger_pstop_reset_service/trigger_pstop_reset_service.hpp b/src/picknik_ur_site_config/behaviors/include/trigger_pstop_reset_service/trigger_pstop_reset_service.hpp new file mode 100644 index 00000000..03a26ebe --- /dev/null +++ b/src/picknik_ur_site_config/behaviors/include/trigger_pstop_reset_service/trigger_pstop_reset_service.hpp @@ -0,0 +1,78 @@ +#pragma once + +#include +#include + +namespace trigger_pstop_reset_service +{ +/** + * @brief Resets the UR P-stop status by calling the reset service named /recover_from_protective_stop. + */ +using PStopService = std_srvs::srv::Trigger; +class TriggerPStopResetService : public moveit_studio::behaviors::ServiceClientBehaviorBase +{ +public: +/** + * @brief Constructor for the trigger_pstop_reset_service behavior. + * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. + */ + TriggerPStopResetService(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + /** + * @brief Implementation of the required providedPorts() function for the trigger_pstop_reset_service Behavior. + * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList. + * This function returns a list of ports with their names and port info, which is used internally by the behavior tree. + * @return trigger_pstop_reset_service does not use expose any ports, so this function returns an empty list. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and + * subcategory, in the MoveIt Studio Developer Tool. + * @return A BT::KeyValueVector containing the Behavior metadata. + */ + static BT::KeyValueVector metadata(); + +private: + /** + * @brief Specify the name of the service that will be called by the behaviors + * @return Returns the name of the service. If not successful, returns an error message. + */ + tl::expected getServiceName() override; + /** + * @brief Sets the timeout for the service response. + * @details If the timeout expires before a response is received, the behavior will fail. A negative duration indicates no timeout. + * @return Returns the service response timeout duration. + */ + tl::expected, std::string> getResponseTimeout() override; + /** + * @brief Create a service request. + * @return Returns a service request message. If not successful, returns an error message. + */ + tl::expected createRequest() override; + /** + * @brief Determines if the service request succeeded or failed based on the response message. + * @param response Response message received from the service server. + * @return Returns true if the response indicates success. If not successful, returns an error message. + */ + tl::expected processResponse(const std_srvs::srv::Trigger::Response&) override; + + /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */ + std::shared_future>& getFuture() override; + + /** + * @brief Holds a copy of the service name specified by the input port. + */ + std::string service_name_; + + /** + * @brief Holds the result of calling the service asynchronously. + */ + std::shared_future> future_; + + +}; +} // namespace trigger_pstop_reset_service diff --git a/src/picknik_ur_site_config/behaviors/src/register_behaviors.cpp b/src/picknik_ur_site_config/behaviors/src/register_behaviors.cpp new file mode 100644 index 00000000..e5ecd0e9 --- /dev/null +++ b/src/picknik_ur_site_config/behaviors/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace trigger_pstop_reset_service +{ +class TriggerPStopResetServiceBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "TriggerPStopResetService", shared_resources); + + } +}; +} // namespace trigger_pstop_reset_service + +PLUGINLIB_EXPORT_CLASS(trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/picknik_ur_site_config/behaviors/src/trigger_pstop_reset_service.cpp b/src/picknik_ur_site_config/behaviors/src/trigger_pstop_reset_service.cpp new file mode 100644 index 00000000..624f7491 --- /dev/null +++ b/src/picknik_ur_site_config/behaviors/src/trigger_pstop_reset_service.cpp @@ -0,0 +1,80 @@ +#include +#include + +namespace +{ + constexpr auto kDefaultPStopServiceName = "/recover_from_protective_stop"; + constexpr auto kPortServiceName = "service_name"; +}// namespace + +namespace trigger_pstop_reset_service +{ + TriggerPStopResetService::TriggerPStopResetService(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& + shared_resources) + : ServiceClientBehaviorBase(name, config, shared_resources) + { + } + + tl::expected TriggerPStopResetService::getServiceName() + { + // Get service name from the input port. + const auto service_name = getInput(kPortServiceName); + // Check that the port has a value on it, if not, return an error. + if (const auto error = moveit_studio::behaviors::maybe_error(service_name)) + { + return tl::make_unexpected("Failed to get required value from input data port: " + error.value()); + } + + service_name_ = service_name.value(); + return service_name_; + } + + tl::expected, std::string> TriggerPStopResetService::getResponseTimeout() + { + // Create timeout for service call. + return std::chrono::duration{2.0}; + } + + tl::expected TriggerPStopResetService::createRequest() + { + // Create request message. + return PStopService::Request{}; + } + + tl::expected TriggerPStopResetService::processResponse( + const std_srvs::srv::Trigger::Response& trigger_response) + { + // If the service response could not be processed, returns an error message, otherwise, return true to indicate success. + return trigger_response.success + ? tl::expected(true) + : tl::make_unexpected( + "Failed to call P-Stop service `" + service_name_ + "`: " + trigger_response.message); + } + + std::shared_future>& TriggerPStopResetService::getFuture() + { + return future_; + } + + + BT::PortsList TriggerPStopResetService::providedPorts() + { + return { + BT::InputPort(kPortServiceName, kDefaultPStopServiceName, + "Name of the service to send a request to.") + }; + } + + BT::KeyValueVector TriggerPStopResetService::metadata() + { + return { + { + "description", + std::string( + "Resets the UR P-stop status by calling the reset service named service name specified by the input port named `") + .append(kPortServiceName).append("`.") + } + }; + } +} // namespace trigger_pstop_reset_service diff --git a/src/picknik_ur_site_config/behaviors/test/CMakeLists.txt b/src/picknik_ur_site_config/behaviors/test/CMakeLists.txt new file mode 100644 index 00000000..2780a607 --- /dev/null +++ b/src/picknik_ur_site_config/behaviors/test/CMakeLists.txt @@ -0,0 +1,6 @@ +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake_gmock REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + +ament_auto_add_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/picknik_ur_site_config/behaviors/test/test_behavior_plugins.cpp b/src/picknik_ur_site_config/behaviors/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..0d688f2b --- /dev/null +++ b/src/picknik_ur_site_config/behaviors/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("test_node"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "TriggerPStopResetService", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml b/src/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml new file mode 100644 index 00000000..5574332f --- /dev/null +++ b/src/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/picknik_ur_site_config/config/config.yaml b/src/picknik_ur_site_config/config/config.yaml index 43e2d260..575b8836 100644 --- a/src/picknik_ur_site_config/config/config.yaml +++ b/src/picknik_ur_site_config/config/config.yaml @@ -1,7 +1,9 @@ based_on_package: "picknik_ur_base_config" objectives: - behavior_loader_plugins: {} + behavior_loader_plugins: + trigger_pstop_reset_service: + - "trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader" objective_library_paths: # You must use a unique key for each package. # The picknik_ur_base_config uses "core" diff --git a/src/picknik_ur_site_config/package.xml b/src/picknik_ur_site_config/package.xml index 33bb0a49..0d0e92d1 100644 --- a/src/picknik_ur_site_config/package.xml +++ b/src/picknik_ur_site_config/package.xml @@ -11,6 +11,9 @@ ament_cmake + moveit_studio_common + + moveit_studio_behavior_interface picknik_ur_base_config diff --git a/src/picknik_ur_studio_integration_testing/picknik_ur_studio_integration_testing/generate_agent_plus_drivers_launch_description.py b/src/picknik_ur_studio_integration_testing/picknik_ur_studio_integration_testing/generate_agent_plus_drivers_launch_description.py index 8889bb65..aec5f34d 100644 --- a/src/picknik_ur_studio_integration_testing/picknik_ur_studio_integration_testing/generate_agent_plus_drivers_launch_description.py +++ b/src/picknik_ur_studio_integration_testing/picknik_ur_studio_integration_testing/generate_agent_plus_drivers_launch_description.py @@ -121,7 +121,9 @@ def generate_agent_plus_drivers_launch_description( # Additionally, we must convert this list of locations to a comma separated list # because standard lists cannot be used as launch arguments. objective_library_directories = ",".join( - system_config_parser.get_objective_library_paths() + [str(test_objective_path)] + str(path) + for path in system_config_parser.get_objective_library_paths() + + [test_objective_path] ) # Get launch description for MoveIt Pro Agent nodes