From fb8bf92c5f0ab55b6613115eaf9b196cd10e99ea Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Mon, 25 Nov 2024 12:53:47 +0100 Subject: [PATCH 01/18] mobile_manipulation_config: revisit favorites --- .../objectives/00_solution_-_move_to_initial_pose.xml | 3 +-- .../objectives/01_solution_-_create_a_pose.xml | 3 +-- .../objectives/02_solution_-_visualize_a_pose.xml | 3 +-- .../objectives/03_solution_-_pose_2m_above_the_robot.xml | 3 +-- .../objectives/05_solution_-_define_a_path.xml | 3 +-- .../objectives/06_solution_-_visualize_a_path.xml | 3 +-- .../objectives/07_solution_-_plan_motion_for_a_path.xml | 3 +-- .../objectives/08_solution_-_execute_path_plan.xml | 3 +-- .../objectives/09_solution_-_an_infeasible_path.xml | 3 +-- .../objectives/10_solution_-_visualize_path_error.xml | 3 +-- .../objectives/12_solution_-_constrain_orientation.xml | 3 +-- .../objectives/13_solution_-_load_a_path_from_file.xml | 3 +-- .../objectives/15_solution_-_a_path_that_collides.xml | 3 +-- .../objectives/16_solution_-_checking_for_collisions.xml | 3 +-- .../objectives/17_solution_-_execute_the_feasible_path.xml | 3 +-- .../objectives/18_solution_-_define_a_coverage_path.xml | 3 +-- .../objectives/19_solution_-_execute_coverage_path.xml | 3 +-- .../objectives/20_solution_-_look_at_the_airfoil.xml | 3 +-- .../objectives/21_solution_-_find_the_airfoil.xml | 3 +-- .../objectives/22_solution_-_visualize_airfoil_frame.xml | 3 +-- .../objectives/23_solution_-_airfoil_coverage_origin.xml | 3 +-- .../objectives/24_solution_-_airfoil_coverage_path.xml | 3 +-- .../objectives/25_solution_-_execute_airfoil_coverage.xml | 3 +-- .../mobile_manipulation_config/objectives/take_snapshot.xml | 3 +-- 24 files changed, 24 insertions(+), 48 deletions(-) diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml index a3218ad5..331cc32b 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml @@ -1,4 +1,3 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml index 864e0a27..92d0ad90 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml @@ -1,10 +1,9 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml index 80c42b30..6d12c405 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml @@ -1,4 +1,3 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml index a2b766dd..dcd90294 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml @@ -1,10 +1,9 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml index a8234385..e7f4b46f 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml @@ -1,4 +1,3 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml index 304f77fe..0c2a75ef 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml @@ -1,4 +1,3 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml index 1ee19ff6..882109b5 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml @@ -1,4 +1,3 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml index 890ff21c..0e34cd50 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml @@ -1,4 +1,3 @@ - diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml index 4b1c1bca..e2e8723d 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/take_snapshot.xml @@ -1,10 +1,9 @@ - From 9ece954a2cea0aa5dcb2ff9de03e19adb9365958 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Mon, 2 Dec 2024 11:00:31 -0800 Subject: [PATCH 02/18] Syncing main to 6.5 (#25) * updating picknik_accesories * patching fanuc (#22) * added dependency (#21) --- src/fanuc_sim/CMakeLists.txt | 12 +++++++ src/fanuc_sim/description/scene.xml | 12 ++++--- src/fanuc_sim/package.xml | 1 + .../kinova_gen3_base_config/package.xml | 2 +- .../description/picknik_kinova_gen3.xacro | 32 +++++++++---------- src/picknik_accessories | 2 +- 6 files changed, 38 insertions(+), 23 deletions(-) diff --git a/src/fanuc_sim/CMakeLists.txt b/src/fanuc_sim/CMakeLists.txt index 98ed68c5..ace747cd 100644 --- a/src/fanuc_sim/CMakeLists.txt +++ b/src/fanuc_sim/CMakeLists.txt @@ -2,7 +2,19 @@ cmake_minimum_required(VERSION 3.22) project(fanuc_sim) find_package(ament_cmake REQUIRED) +find_package(picknik_accessories REQUIRED) +# Install all XML files in directory +set(PICKNIK_ACCESSORIES_SHARE_DIR +"${CMAKE_INSTALL_PREFIX}/../picknik_accessories/share/picknik_accessories/mujoco_assets/" +) +# Destination directory +set(DEST_DIR "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/description/") + +install(DIRECTORY "${PICKNIK_ACCESSORIES_SHARE_DIR}" + DESTINATION "${DEST_DIR}" + FILES_MATCHING PATTERN "*") + install( DIRECTORY config diff --git a/src/fanuc_sim/description/scene.xml b/src/fanuc_sim/description/scene.xml index aec8caad..d55cd798 100644 --- a/src/fanuc_sim/description/scene.xml +++ b/src/fanuc_sim/description/scene.xml @@ -6,12 +6,12 @@ - + - - + + - + @@ -117,6 +117,8 @@ resolution="640 480" euler="-0.9 0 3.1415" /> - + + + diff --git a/src/fanuc_sim/package.xml b/src/fanuc_sim/package.xml index ab8ac55c..3e125fd9 100644 --- a/src/fanuc_sim/package.xml +++ b/src/fanuc_sim/package.xml @@ -11,6 +11,7 @@ ament_cmake + picknik_accessories admittance_controller kinematics_interface_kdl moveit_planners_stomp diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml index dcee35f1..40703c64 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/package.xml @@ -13,8 +13,8 @@ ament_cmake moveit_studio_kinova_pstop_manager + kortex_description - kortex_description moveit_ros_perception moveit_studio_agent moveit_studio_behavior diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro index 33379894..a00a589e 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/description/picknik_kinova_gen3.xacro @@ -52,7 +52,7 @@ - + @@ -61,7 +61,7 @@ - + @@ -74,7 +74,7 @@ - + @@ -83,7 +83,7 @@ - + @@ -103,7 +103,7 @@ - + @@ -112,7 +112,7 @@ - + @@ -132,7 +132,7 @@ - + @@ -141,7 +141,7 @@ - + @@ -161,7 +161,7 @@ - + @@ -170,7 +170,7 @@ - + @@ -190,7 +190,7 @@ - + @@ -199,7 +199,7 @@ - + @@ -219,7 +219,7 @@ - + @@ -228,7 +228,7 @@ - + @@ -248,7 +248,7 @@ - + @@ -257,7 +257,7 @@ - + diff --git a/src/picknik_accessories b/src/picknik_accessories index 18b1e9f5..6ddf6570 160000 --- a/src/picknik_accessories +++ b/src/picknik_accessories @@ -1 +1 @@ -Subproject commit 18b1e9f50f8f566b979e7251baf8744fa6387b29 +Subproject commit 6ddf657050f678b9eac282ade0940d827a8ea166 From 63f19d8d528d2ea5e3a953287d28b15f78a19608 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Mon, 2 Dec 2024 21:43:37 -0800 Subject: [PATCH 03/18] Ci fix (#28) * removing whitespace --- src/fanuc_sim/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fanuc_sim/CMakeLists.txt b/src/fanuc_sim/CMakeLists.txt index ace747cd..e3d195d7 100644 --- a/src/fanuc_sim/CMakeLists.txt +++ b/src/fanuc_sim/CMakeLists.txt @@ -14,7 +14,6 @@ set(DEST_DIR "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/description/") install(DIRECTORY "${PICKNIK_ACCESSORIES_SHARE_DIR}" DESTINATION "${DEST_DIR}" FILES_MATCHING PATTERN "*") - install( DIRECTORY config From d46cfbaf259ba0ea859002d4a00a1aa8ff85d979 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Mon, 2 Dec 2024 21:44:23 -0800 Subject: [PATCH 04/18] fixing-kinovas (#26) --- .../description/kortex_robot.xacro | 100 ++++++++++++++++++ .../description/picknik_kinova_gen3.xacro | 2 +- .../description/robotiq_2f_85_macro.xacro | 38 +++++++ 3 files changed, 139 insertions(+), 1 deletion(-) create mode 100644 src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro create mode 100644 src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro new file mode 100644 index 00000000..5f34da47 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro index 57f5aaae..fdb64cd8 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro @@ -26,7 +26,7 @@ - + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro new file mode 100644 index 00000000..3f7e6316 --- /dev/null +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + From 8d44152d2bc393be3a2297a07cbc7f7a4130c518 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Mon, 2 Dec 2024 21:45:19 -0800 Subject: [PATCH 05/18] Colcon ignoring example behaviors (#27) *colcon ignoring example behaviors --- src/example_behaviors/COLCON_IGNORE | 0 src/lab_sim/objectives/pick_and_place_example.xml | 2 ++ src/lab_sim/objectives/plan_and_save_trajectory.xml | 1 + 3 files changed, 3 insertions(+) create mode 100644 src/example_behaviors/COLCON_IGNORE diff --git a/src/example_behaviors/COLCON_IGNORE b/src/example_behaviors/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml index 4529916f..49c844c9 100644 --- a/src/lab_sim/objectives/pick_and_place_example.xml +++ b/src/lab_sim/objectives/pick_and_place_example.xml @@ -24,6 +24,7 @@ task="{mtc_pick_task}" /> + + + Date: Mon, 2 Dec 2024 21:59:46 -0800 Subject: [PATCH 06/18] Fixes for kinova (#30) --- .../kinova_gen3_base_config/description/kortex_robot.xacro | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro index 5f34da47..f0533e2e 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/kortex_robot.xacro @@ -55,14 +55,11 @@ sim_gazebo="${sim_gazebo}" sim_ignition="${sim_ignition}" sim_isaac="${sim_isaac}" - isaac_joint_commands="${isaac_joint_commands}" - isaac_joint_states="${isaac_joint_states}" gripper_joint_name="${gripper_joint_name}" gripper_max_velocity="${gripper_max_velocity}" gripper_max_force="${gripper_max_force}" use_external_cable="${use_external_cable}" - initial_positions="${initial_positions}" - moveit_active="${moveit_active}"> + initial_positions="${initial_positions}"> From 77e4dcd025be1b7a0084094aeca300f1ab311b9f Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Mon, 2 Dec 2024 22:08:51 -0800 Subject: [PATCH 07/18] removing ignored dependency --- src/lab_sim/config/config.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lab_sim/config/config.yaml b/src/lab_sim/config/config.yaml index 538cc7d9..f784a7d9 100644 --- a/src/lab_sim/config/config.yaml +++ b/src/lab_sim/config/config.yaml @@ -42,7 +42,6 @@ objectives: - "moveit_studio::behaviors::MTCCoreBehaviorsLoader" - "moveit_studio::behaviors::ServoBehaviorsLoader" - "moveit_studio::behaviors::VisionBehaviorsLoader" - - "example_behaviors::ExampleBehaviorsLoader" # Specify source folder for objectives # [Required] objective_library_paths: From 6f98b9e4a94df0ecec0a9524ff7b1241b1bdcb49 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Wed, 4 Dec 2024 01:00:17 -0800 Subject: [PATCH 08/18] Added categories for more objectives --- src/fanuc_sim/objectives/interpolate_to_joint_state.xml | 3 +++ src/fanuc_sim/objectives/move_to_joint_state.xml | 3 +++ src/fanuc_sim/objectives/move_to_pose.xml | 3 +++ src/fanuc_sim/objectives/move_to_waypoint.xml | 3 +++ src/lab_sim/objectives/add_waypoints_to_mtc_task.xml | 3 +++ src/lab_sim/objectives/apriltag_pick_object.xml | 6 +++++- src/lab_sim/objectives/create_pose_vector.xml | 3 +++ src/lab_sim/objectives/force_relaxation.xml | 3 +++ src/lab_sim/objectives/get_april_tag_pose_from_image.xml | 3 +++ src/lab_sim/objectives/load_mesh_as_green_poincloud.xml | 3 +++ src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml | 3 +++ src/lab_sim/objectives/pick_first_object_in_vector.xml | 6 +++++- src/lab_sim/objectives/place_object.xml | 6 +++++- src/lab_sim/objectives/plan_and_save_trajectory.xml | 6 +++++- src/lab_sim/objectives/plan_move_to_pose.xml | 3 +++ src/lab_sim/objectives/plan_to_pose.xml | 3 +++ .../objectives/save_and_execute_a_trajectory_from_disk.xml | 6 +++++- src/lab_sim/objectives/take_snap.xml | 6 +++++- .../objectives/get_imarker_pose_from_mesh_visualization.xml | 3 +++ .../kinova_gen3_base_config/objectives/move_to_waypoint.xml | 3 +++ .../kinova_gen3_mujoco_config/objectives/move_to_pose.xml | 3 +++ .../objectives/move_to_waypoint.xml | 3 +++ .../objectives/re_zero_force_torque_sensors.xml | 6 +++++- .../objectives/sample_april_tag.xml | 3 +++ .../objectives/estimate_object_pose.xml | 6 +++++- .../objectives/move_to_a_stampedpose.xml | 3 +++ .../picknik_ur_mobile_config/objectives/take_snap.xml | 6 +++++- .../picknik_ur_mobile_config/objectives/wrist_snap.xml | 6 +++++- .../arm_on_rail_sim/objectives/pick_object.xml | 6 +++++- .../objectives/interpolate_to_joint_state.xml | 3 +++ .../objectives/move_to_joint_state.xml | 3 +++ .../picknik_ur_multi_arm_config/objectives/move_to_pose.xml | 3 +++ .../objectives/retreat_to_initial_pose.xml | 3 +++ 33 files changed, 121 insertions(+), 11 deletions(-) diff --git a/src/fanuc_sim/objectives/interpolate_to_joint_state.xml b/src/fanuc_sim/objectives/interpolate_to_joint_state.xml index 892d8e83..e4f99ff4 100644 --- a/src/fanuc_sim/objectives/interpolate_to_joint_state.xml +++ b/src/fanuc_sim/objectives/interpolate_to_joint_state.xml @@ -34,6 +34,9 @@ + + + diff --git a/src/fanuc_sim/objectives/move_to_joint_state.xml b/src/fanuc_sim/objectives/move_to_joint_state.xml index 60b6c5d1..63ab4a8e 100644 --- a/src/fanuc_sim/objectives/move_to_joint_state.xml +++ b/src/fanuc_sim/objectives/move_to_joint_state.xml @@ -32,6 +32,9 @@ + + + diff --git a/src/fanuc_sim/objectives/move_to_pose.xml b/src/fanuc_sim/objectives/move_to_pose.xml index 1e58a85e..4009dca1 100644 --- a/src/fanuc_sim/objectives/move_to_pose.xml +++ b/src/fanuc_sim/objectives/move_to_pose.xml @@ -36,6 +36,9 @@ + + + diff --git a/src/fanuc_sim/objectives/move_to_waypoint.xml b/src/fanuc_sim/objectives/move_to_waypoint.xml index ca42e19e..ad9ec51d 100644 --- a/src/fanuc_sim/objectives/move_to_waypoint.xml +++ b/src/fanuc_sim/objectives/move_to_waypoint.xml @@ -47,6 +47,9 @@ name="controller_names" default="/joint_trajectory_controller" /> + + + diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml index a8b2f957..9359a4b0 100644 --- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml +++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml @@ -41,6 +41,9 @@ + + + diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml index df3f814f..90dc1d9a 100644 --- a/src/lab_sim/objectives/apriltag_pick_object.xml +++ b/src/lab_sim/objectives/apriltag_pick_object.xml @@ -90,6 +90,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml index 0a5ccce0..fe951010 100644 --- a/src/lab_sim/objectives/create_pose_vector.xml +++ b/src/lab_sim/objectives/create_pose_vector.xml @@ -111,6 +111,9 @@ + + + diff --git a/src/lab_sim/objectives/force_relaxation.xml b/src/lab_sim/objectives/force_relaxation.xml index a62429fe..f137daea 100644 --- a/src/lab_sim/objectives/force_relaxation.xml +++ b/src/lab_sim/objectives/force_relaxation.xml @@ -50,6 +50,9 @@ + + + diff --git a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml index 302e2f6f..6a223a9e 100644 --- a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml +++ b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml @@ -40,6 +40,9 @@ + + + diff --git a/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml b/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml index 28acf149..cce854ba 100644 --- a/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml +++ b/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml @@ -31,6 +31,9 @@ + + + diff --git a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml b/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml index 4984c7c5..e7492b15 100644 --- a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml +++ b/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml @@ -31,6 +31,9 @@ + + + diff --git a/src/lab_sim/objectives/pick_first_object_in_vector.xml b/src/lab_sim/objectives/pick_first_object_in_vector.xml index 30bd2bf4..b153b8a1 100644 --- a/src/lab_sim/objectives/pick_first_object_in_vector.xml +++ b/src/lab_sim/objectives/pick_first_object_in_vector.xml @@ -43,6 +43,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml index 21a7eaa5..fec0fd73 100644 --- a/src/lab_sim/objectives/place_object.xml +++ b/src/lab_sim/objectives/place_object.xml @@ -30,6 +30,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml index fe26aee1..ae458e45 100644 --- a/src/lab_sim/objectives/plan_and_save_trajectory.xml +++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml @@ -51,6 +51,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml index 9f1da373..c280015e 100644 --- a/src/lab_sim/objectives/plan_move_to_pose.xml +++ b/src/lab_sim/objectives/plan_move_to_pose.xml @@ -35,6 +35,9 @@ name="move_to_pose_solution" default="{move_to_pose_solution}" /> + + + diff --git a/src/lab_sim/objectives/plan_to_pose.xml b/src/lab_sim/objectives/plan_to_pose.xml index 13d39ec9..3bb7c76b 100644 --- a/src/lab_sim/objectives/plan_to_pose.xml +++ b/src/lab_sim/objectives/plan_to_pose.xml @@ -49,6 +49,9 @@ + + + diff --git a/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml b/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml index ddeac5a6..33d02a77 100644 --- a/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml +++ b/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml @@ -15,6 +15,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/take_snap.xml b/src/lab_sim/objectives/take_snap.xml index ebadf701..0fdb917b 100644 --- a/src/lab_sim/objectives/take_snap.xml +++ b/src/lab_sim/objectives/take_snap.xml @@ -14,6 +14,10 @@ - + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml index 9be4f855..70fa45ba 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml @@ -26,6 +26,9 @@ + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml index dd1dac8a..15132ea6 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml @@ -38,6 +38,9 @@ + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml index 51a3f183..fca077c6 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml @@ -63,6 +63,9 @@ + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml index 4733f815..c81b5485 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml @@ -45,6 +45,9 @@ name="controller_names" default="/joint_trajectory_controller" /> + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml index 9c4f1ec4..5fbbf52e 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml @@ -18,6 +18,10 @@ - + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml index 78f8523c..0f26bbba 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml @@ -56,6 +56,9 @@ + + + diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml index fede7df6..1414ad50 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml @@ -71,6 +71,10 @@ - + + + + + diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml index 03509637..60cfa019 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml @@ -57,6 +57,9 @@ default="/joint_trajectory_controller" /> + + + diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml index 07878d8d..89f2873c 100644 --- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml +++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml @@ -14,6 +14,10 @@ - + + + + + diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml index 97c4f73b..324be61b 100644 --- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml +++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml @@ -13,6 +13,10 @@ - + + + + + diff --git a/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml b/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml index b92faa44..8fa1bfc1 100644 --- a/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml +++ b/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml @@ -30,6 +30,10 @@ - + + + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml index 9687de28..67b02a66 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/interpolate_to_joint_state.xml @@ -33,6 +33,9 @@ + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml index 1e0c2a5e..82d378cb 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml @@ -31,6 +31,9 @@ + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml index cda5743f..c00a3227 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml @@ -35,6 +35,9 @@ + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml index ee4c82a9..24964ffa 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml @@ -59,6 +59,9 @@ default="{pre_approach_robot_state}" /> + + + From 45cf19fa7fc2129bcbd96fa1e244ea64539376e7 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Wed, 4 Dec 2024 01:00:34 -0800 Subject: [PATCH 09/18] Add 'Example' prefix to all behavior classnames and filenames in example_behaviors repository (#33) Co-authored-by: Nathan Brooks --- src/example_behaviors/CMakeLists.txt | 24 +++++----- ...> example_add_two_ints_service_client.hpp} | 4 +- ...vert_mtc_solution_to_joint_trajectory.hpp} | 4 +- ...essage.hpp => example_delayed_message.hpp} | 6 +-- ...pp => example_fibonacci_action_client.hpp} | 4 +- ....hpp => example_get_string_from_topic.hpp} | 4 +- ...ello_world.hpp => example_hello_world.hpp} | 10 ++-- ...ation.hpp => example_ndt_registration.hpp} | 4 +- ...gba.hpp => example_publish_color_rgba.hpp} | 4 +- ...on.hpp => example_ransac_registration.hpp} | 4 +- ...p => example_setup_mtc_pick_from_pose.hpp} | 4 +- ... => example_setup_mtc_place_from_pose.hpp} | 4 +- ...nd.hpp => example_setup_mtc_wave_hand.hpp} | 14 +++--- ...> example_add_two_ints_service_client.cpp} | 14 +++--- ...vert_mtc_solution_to_joint_trajectory.cpp} | 14 +++--- ...essage.cpp => example_delayed_message.cpp} | 14 +++--- ...pp => example_fibonacci_action_client.cpp} | 16 +++---- ....cpp => example_get_string_from_topic.cpp} | 8 ++-- ...ello_world.cpp => example_hello_world.cpp} | 12 ++--- ...ation.cpp => example_ndt_registration.cpp} | 10 ++-- ...gba.cpp => example_publish_color_rgba.cpp} | 10 ++-- ...on.cpp => example_ransac_registration.cpp} | 10 ++-- ...p => example_setup_mtc_pick_from_pose.cpp} | 12 ++--- ... => example_setup_mtc_place_from_pose.cpp} | 12 ++--- ...nd.cpp => example_setup_mtc_wave_hand.cpp} | 12 ++--- .../src/register_behaviors.cpp | 48 +++++++++---------- .../test/test_behavior_plugins.cpp | 22 ++++----- 27 files changed, 152 insertions(+), 152 deletions(-) rename src/example_behaviors/include/example_behaviors/{add_two_ints_service_client.hpp => example_add_two_ints_service_client.hpp} (91%) rename src/example_behaviors/include/example_behaviors/{convert_mtc_solution_to_joint_trajectory.hpp => example_convert_mtc_solution_to_joint_trajectory.hpp} (89%) rename src/example_behaviors/include/example_behaviors/{delayed_message.hpp => example_delayed_message.hpp} (90%) rename src/example_behaviors/include/example_behaviors/{fibonacci_action_client.hpp => example_fibonacci_action_client.hpp} (92%) rename src/example_behaviors/include/example_behaviors/{get_string_from_topic.hpp => example_get_string_from_topic.hpp} (86%) rename src/example_behaviors/include/example_behaviors/{hello_world.hpp => example_hello_world.hpp} (84%) rename src/example_behaviors/include/example_behaviors/{ndt_registration.hpp => example_ndt_registration.hpp} (91%) rename src/example_behaviors/include/example_behaviors/{publish_color_rgba.hpp => example_publish_color_rgba.hpp} (78%) rename src/example_behaviors/include/example_behaviors/{ransac_registration.hpp => example_ransac_registration.hpp} (91%) rename src/example_behaviors/include/example_behaviors/{setup_mtc_pick_from_pose.hpp => example_setup_mtc_pick_from_pose.hpp} (83%) rename src/example_behaviors/include/example_behaviors/{setup_mtc_place_from_pose.hpp => example_setup_mtc_place_from_pose.hpp} (83%) rename src/example_behaviors/include/example_behaviors/{setup_mtc_wave_hand.hpp => example_setup_mtc_wave_hand.hpp} (86%) rename src/example_behaviors/src/{add_two_ints_service_client.cpp => example_add_two_ints_service_client.cpp} (76%) rename src/example_behaviors/src/{convert_mtc_solution_to_joint_trajectory.cpp => example_convert_mtc_solution_to_joint_trajectory.cpp} (89%) rename src/example_behaviors/src/{delayed_message.cpp => example_delayed_message.cpp} (85%) rename src/example_behaviors/src/{fibonacci_action_client.cpp => example_fibonacci_action_client.cpp} (80%) rename src/example_behaviors/src/{get_string_from_topic.cpp => example_get_string_from_topic.cpp} (78%) rename src/example_behaviors/src/{hello_world.cpp => example_hello_world.cpp} (66%) rename src/example_behaviors/src/{ndt_registration.cpp => example_ndt_registration.cpp} (95%) rename src/example_behaviors/src/{publish_color_rgba.cpp => example_publish_color_rgba.cpp} (70%) rename src/example_behaviors/src/{ransac_registration.cpp => example_ransac_registration.cpp} (97%) rename src/example_behaviors/src/{setup_mtc_pick_from_pose.cpp => example_setup_mtc_pick_from_pose.cpp} (96%) rename src/example_behaviors/src/{setup_mtc_place_from_pose.cpp => example_setup_mtc_place_from_pose.cpp} (96%) rename src/example_behaviors/src/{setup_mtc_wave_hand.cpp => example_setup_mtc_wave_hand.cpp} (92%) diff --git a/src/example_behaviors/CMakeLists.txt b/src/example_behaviors/CMakeLists.txt index 21be9500..4eabcd35 100644 --- a/src/example_behaviors/CMakeLists.txt +++ b/src/example_behaviors/CMakeLists.txt @@ -19,18 +19,18 @@ endforeach() add_library( example_behaviors SHARED - src/add_two_ints_service_client.cpp - src/convert_mtc_solution_to_joint_trajectory.cpp - src/delayed_message.cpp - src/get_string_from_topic.cpp - src/fibonacci_action_client.cpp - src/hello_world.cpp - src/publish_color_rgba.cpp - src/setup_mtc_pick_from_pose.cpp - src/setup_mtc_place_from_pose.cpp - src/setup_mtc_wave_hand.cpp - src/ndt_registration.cpp - src/ransac_registration.cpp + src/example_add_two_ints_service_client.cpp + src/example_convert_mtc_solution_to_joint_trajectory.cpp + src/example_delayed_message.cpp + src/example_get_string_from_topic.cpp + src/example_fibonacci_action_client.cpp + src/example_hello_world.cpp + src/example_publish_color_rgba.cpp + src/example_setup_mtc_pick_from_pose.cpp + src/example_setup_mtc_place_from_pose.cpp + src/example_setup_mtc_wave_hand.cpp + src/example_ndt_registration.cpp + src/example_ransac_registration.cpp src/register_behaviors.cpp) target_include_directories( example_behaviors diff --git a/src/example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp b/src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp similarity index 91% rename from src/example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp rename to src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp index a79f76e8..9840ee58 100644 --- a/src/example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp +++ b/src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp @@ -9,10 +9,10 @@ using AddTwoInts = example_interfaces::srv::AddTwoInts; namespace example_behaviors { -class AddTwoIntsServiceClient final : public ServiceClientBehaviorBase +class ExampleAddTwoIntsServiceClient final : public ServiceClientBehaviorBase { public: - AddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config, + ExampleAddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */ diff --git a/src/example_behaviors/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp b/src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp similarity index 89% rename from src/example_behaviors/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp rename to src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp index 1c0a95c7..c8712d5a 100644 --- a/src/example_behaviors/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp +++ b/src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp @@ -29,10 +29,10 @@ namespace example_behaviors * | sampling_rate | Input | int | * | joint_trajectory | Output | trajectory_msgs::msg::JointTrajectory | */ -class ConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode +class ExampleConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode { public: - ConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config, + ExampleConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/src/example_behaviors/include/example_behaviors/delayed_message.hpp b/src/example_behaviors/include/example_behaviors/example_delayed_message.hpp similarity index 90% rename from src/example_behaviors/include/example_behaviors/delayed_message.hpp rename to src/example_behaviors/include/example_behaviors/example_delayed_message.hpp index 6e95c173..b6648f93 100644 --- a/src/example_behaviors/include/example_behaviors/delayed_message.hpp +++ b/src/example_behaviors/include/example_behaviors/example_delayed_message.hpp @@ -12,9 +12,9 @@ namespace example_behaviors { /** - * @brief DelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port + * @brief ExampleDelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port */ -class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode +class ExampleDelayedMessage : public moveit_studio::behaviors::SharedResourcesNode { private: std::chrono::time_point start_time_; @@ -33,7 +33,7 @@ class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode& shared_resources); /** diff --git a/src/example_behaviors/include/example_behaviors/fibonacci_action_client.hpp b/src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp similarity index 92% rename from src/example_behaviors/include/example_behaviors/fibonacci_action_client.hpp rename to src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp index 06f70b12..19e4e2f9 100644 --- a/src/example_behaviors/include/example_behaviors/fibonacci_action_client.hpp +++ b/src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp @@ -9,10 +9,10 @@ using Fibonacci = example_interfaces::action::Fibonacci; namespace example_behaviors { -class FibonacciActionClient final : public ActionClientBehaviorBase +class ExampleFibonacciActionClient final : public ActionClientBehaviorBase { public: - FibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config, + ExampleFibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */ diff --git a/src/example_behaviors/include/example_behaviors/get_string_from_topic.hpp b/src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp similarity index 86% rename from src/example_behaviors/include/example_behaviors/get_string_from_topic.hpp rename to src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp index 7c6e6662..5ea2d711 100644 --- a/src/example_behaviors/include/example_behaviors/get_string_from_topic.hpp +++ b/src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp @@ -8,10 +8,10 @@ using moveit_studio::behaviors::GetMessageFromTopicBehaviorBase; namespace example_behaviors { -class GetStringFromTopic final : public GetMessageFromTopicBehaviorBase +class ExampleGetStringFromTopic final : public GetMessageFromTopicBehaviorBase { public: - GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, + ExampleGetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */ diff --git a/src/example_behaviors/include/example_behaviors/hello_world.hpp b/src/example_behaviors/include/example_behaviors/example_hello_world.hpp similarity index 84% rename from src/example_behaviors/include/example_behaviors/hello_world.hpp rename to src/example_behaviors/include/example_behaviors/example_hello_world.hpp index 02886219..524f650c 100644 --- a/src/example_behaviors/include/example_behaviors/hello_world.hpp +++ b/src/example_behaviors/include/example_behaviors/example_hello_world.hpp @@ -8,9 +8,9 @@ namespace example_behaviors { /** - * @brief The HelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS + * @brief The ExampleHelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS */ -class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode +class ExampleHelloWorld : public moveit_studio::behaviors::SharedResourcesNode { public: /** @@ -25,7 +25,7 @@ class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode& shared_resources); /** @@ -46,9 +46,9 @@ class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode& shared_resources); + ExampleNDTRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** * @brief Implementation of the required providedPorts() function for the Behavior. diff --git a/src/example_behaviors/include/example_behaviors/publish_color_rgba.hpp b/src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp similarity index 78% rename from src/example_behaviors/include/example_behaviors/publish_color_rgba.hpp rename to src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp index 38531347..11c08809 100644 --- a/src/example_behaviors/include/example_behaviors/publish_color_rgba.hpp +++ b/src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp @@ -12,10 +12,10 @@ namespace example_behaviors { -class PublishColorRGBA final : public moveit_studio::behaviors::SharedResourcesNode +class ExamplePublishColorRGBA final : public moveit_studio::behaviors::SharedResourcesNode { public: - PublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, + ExamplePublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/src/example_behaviors/include/example_behaviors/ransac_registration.hpp b/src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp similarity index 91% rename from src/example_behaviors/include/example_behaviors/ransac_registration.hpp rename to src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp index b3bd141c..a786b733 100644 --- a/src/example_behaviors/include/example_behaviors/ransac_registration.hpp +++ b/src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp @@ -12,7 +12,7 @@ namespace example_behaviors /** * @brief TODO(...) */ -class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase +class ExampleRANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase { public: /** @@ -22,7 +22,7 @@ class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase * @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. */ - RANSACRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + ExampleRANSACRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** * @brief Implementation of the required providedPorts() function for the Behavior. diff --git a/src/example_behaviors/include/example_behaviors/setup_mtc_pick_from_pose.hpp b/src/example_behaviors/include/example_behaviors/example_setup_mtc_pick_from_pose.hpp similarity index 83% rename from src/example_behaviors/include/example_behaviors/setup_mtc_pick_from_pose.hpp rename to src/example_behaviors/include/example_behaviors/example_setup_mtc_pick_from_pose.hpp index e271bd23..3ce38834 100644 --- a/src/example_behaviors/include/example_behaviors/setup_mtc_pick_from_pose.hpp +++ b/src/example_behaviors/include/example_behaviors/example_setup_mtc_pick_from_pose.hpp @@ -17,10 +17,10 @@ namespace example_behaviors * | task | Bidirectional | std::shared_ptr | * | grasp_pose | Input | geometry_msgs::msg::PoseStamped | */ -class SetupMtcPickFromPose final : public moveit_studio::behaviors::SharedResourcesNode +class ExampleSetupMtcPickFromPose final : public moveit_studio::behaviors::SharedResourcesNode { public: - SetupMtcPickFromPose(const std::string& name, const BT::NodeConfiguration& config, + ExampleSetupMtcPickFromPose(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/src/example_behaviors/include/example_behaviors/setup_mtc_place_from_pose.hpp b/src/example_behaviors/include/example_behaviors/example_setup_mtc_place_from_pose.hpp similarity index 83% rename from src/example_behaviors/include/example_behaviors/setup_mtc_place_from_pose.hpp rename to src/example_behaviors/include/example_behaviors/example_setup_mtc_place_from_pose.hpp index c06431cb..9f81ae9a 100644 --- a/src/example_behaviors/include/example_behaviors/setup_mtc_place_from_pose.hpp +++ b/src/example_behaviors/include/example_behaviors/example_setup_mtc_place_from_pose.hpp @@ -17,10 +17,10 @@ namespace example_behaviors * | task | Bidirectional | std::shared_ptr | * | grasp_pose | Input | geometry_msgs::msg::PoseStamped | */ -class SetupMtcPlaceFromPose final : public moveit_studio::behaviors::SharedResourcesNode +class ExampleSetupMtcPlaceFromPose final : public moveit_studio::behaviors::SharedResourcesNode { public: - SetupMtcPlaceFromPose(const std::string& name, const BT::NodeConfiguration& config, + ExampleSetupMtcPlaceFromPose(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/src/example_behaviors/include/example_behaviors/setup_mtc_wave_hand.hpp b/src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp similarity index 86% rename from src/example_behaviors/include/example_behaviors/setup_mtc_wave_hand.hpp rename to src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp index 4d635b08..756b3555 100644 --- a/src/example_behaviors/include/example_behaviors/setup_mtc_wave_hand.hpp +++ b/src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp @@ -11,7 +11,7 @@ namespace example_behaviors { /** - * @brief The SetupMTCWaveHand behavior makes any robot move the end of its arm back and forth. + * @brief The ExampleSetupMTCWaveHand behavior makes any robot move the end of its arm back and forth. * @details This is an example of a behavior that uses MoveIt Task Constructor to configure an MTC task, * which can be planned and executed by MoveIt Pro's core PlanMTCTask and ExecuteMTCTask Behaviors. * It is derived from AsyncBehaviorBase, an extension of the templated SharedResourcesNode type, @@ -24,11 +24,11 @@ namespace example_behaviors * if the process completed successfully or was canceled, * or an error message if the process failed unexpectedly. */ -class SetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBase +class ExampleSetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBase { public: /** - * @brief Constructor for the SetupMTCWaveHand behavior. + * @brief Constructor for the ExampleSetupMTCWaveHand 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 shared_resources A shared_ptr to a BehaviorContext that is shared among all @@ -38,17 +38,17 @@ class SetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBas * 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. */ - SetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, + ExampleSetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** - * @brief Implementation of the required providedPorts() function for the SetupMTCWaveHand Behavior. + * @brief Implementation of the required providedPorts() function for the ExampleSetupMTCWaveHand 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 SetupMTCWaveHand has one data port: a bidirectional port named "task", which is a shared_ptr + * @return ExampleSetupMTCWaveHand has one data port: a bidirectional port named "task", which is a shared_ptr * to an MTC task object. This function returns a BT::PortsList that declares this single port. */ static BT::PortsList providedPorts(); @@ -62,7 +62,7 @@ class SetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBas private: /** - * @brief Async thread for SetupMTCWaveHand. Adds MTC stages to an MTC task provided on a data port. + * @brief Async thread for ExampleSetupMTCWaveHand. Adds MTC stages to an MTC task provided on a data port. * @details This function is where the Behavior performs its work asynchronously while the behavior tree ticks. * It is very important that behaviors return from being ticked very quickly because if it blocks before returning * it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors diff --git a/src/example_behaviors/src/add_two_ints_service_client.cpp b/src/example_behaviors/src/example_add_two_ints_service_client.cpp similarity index 76% rename from src/example_behaviors/src/add_two_ints_service_client.cpp rename to src/example_behaviors/src/example_add_two_ints_service_client.cpp index 9ade021b..48f9dc0b 100644 --- a/src/example_behaviors/src/add_two_ints_service_client.cpp +++ b/src/example_behaviors/src/example_add_two_ints_service_client.cpp @@ -1,17 +1,17 @@ -#include +#include // Include the template implementation for GetMessageFromTopicBehaviorBase. #include namespace example_behaviors { -AddTwoIntsServiceClient::AddTwoIntsServiceClient( +ExampleAddTwoIntsServiceClient::ExampleAddTwoIntsServiceClient( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : ServiceClientBehaviorBase(name, config, shared_resources) { } -BT::PortsList AddTwoIntsServiceClient::providedPorts() +BT::PortsList ExampleAddTwoIntsServiceClient::providedPorts() { // This node has three input ports and one output port return BT::PortsList({ @@ -22,13 +22,13 @@ BT::PortsList AddTwoIntsServiceClient::providedPorts() }); } -BT::KeyValueVector AddTwoIntsServiceClient::metadata() +BT::KeyValueVector ExampleAddTwoIntsServiceClient::metadata() { return { { "subcategory", "Example" }, { "description", "Calls a service to add two integers and makes the result available on an output port." } }; } -tl::expected AddTwoIntsServiceClient::getServiceName() +tl::expected ExampleAddTwoIntsServiceClient::getServiceName() { const auto service_name = getInput("service_name"); if (const auto error = moveit_studio::behaviors::maybe_error(service_name)) @@ -38,7 +38,7 @@ tl::expected AddTwoIntsServiceClient::getServiceName() return service_name.value(); } -tl::expected AddTwoIntsServiceClient::createRequest() +tl::expected ExampleAddTwoIntsServiceClient::createRequest() { const auto a = getInput("addend1"); const auto b = getInput("addend2"); @@ -49,7 +49,7 @@ tl::expected AddTwoIntsServiceClient::createRe return example_interfaces::build().a(a.value()).b(b.value()); } -tl::expected AddTwoIntsServiceClient::processResponse(const AddTwoInts::Response& response) +tl::expected ExampleAddTwoIntsServiceClient::processResponse(const AddTwoInts::Response& response) { setOutput("result", response.sum); return { true }; diff --git a/src/example_behaviors/src/convert_mtc_solution_to_joint_trajectory.cpp b/src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp similarity index 89% rename from src/example_behaviors/src/convert_mtc_solution_to_joint_trajectory.cpp rename to src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp index 6cb5cd28..51dd0077 100644 --- a/src/example_behaviors/src/convert_mtc_solution_to_joint_trajectory.cpp +++ b/src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp @@ -1,8 +1,8 @@ -#include +#include namespace { -const auto kLogger = rclcpp::get_logger("ConvertMtcSolutionToJointTrajectory"); +const auto kLogger = rclcpp::get_logger("ExampleConvertMtcSolutionToJointTrajectory"); // Port names for input and output ports. constexpr auto kPortIDSolution = "solution"; @@ -15,14 +15,14 @@ constexpr auto kPortIDSamplingRate = "sampling_rate"; namespace example_behaviors { -ConvertMtcSolutionToJointTrajectory::ConvertMtcSolutionToJointTrajectory( +ExampleConvertMtcSolutionToJointTrajectory::ExampleConvertMtcSolutionToJointTrajectory( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList ConvertMtcSolutionToJointTrajectory::providedPorts() +BT::PortsList ExampleConvertMtcSolutionToJointTrajectory::providedPorts() { return { BT::InputPort(kPortIDSolution, "{mtc_solution}", @@ -40,13 +40,13 @@ BT::PortsList ConvertMtcSolutionToJointTrajectory::providedPorts() }; } -BT::KeyValueVector ConvertMtcSolutionToJointTrajectory::metadata() +BT::KeyValueVector ExampleConvertMtcSolutionToJointTrajectory::metadata() { return { { "subcategory", "Motion Planning" }, { "description", "Extracts joint space trajectories from an MTC solution and adds time parameterization using TOTG." } }; } -BT::NodeStatus ConvertMtcSolutionToJointTrajectory::tick() +BT::NodeStatus ExampleConvertMtcSolutionToJointTrajectory::tick() { using namespace moveit_studio::behaviors; @@ -108,7 +108,7 @@ BT::NodeStatus ConvertMtcSolutionToJointTrajectory::tick() return BT::NodeStatus::SUCCESS; } -const std::vector& ConvertMtcSolutionToJointTrajectory::extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution) +const std::vector& ExampleConvertMtcSolutionToJointTrajectory::extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution) { static std::vector waypoints; waypoints.clear(); diff --git a/src/example_behaviors/src/delayed_message.cpp b/src/example_behaviors/src/example_delayed_message.cpp similarity index 85% rename from src/example_behaviors/src/delayed_message.cpp rename to src/example_behaviors/src/example_delayed_message.cpp index 87c4c826..584f7994 100644 --- a/src/example_behaviors/src/delayed_message.cpp +++ b/src/example_behaviors/src/example_delayed_message.cpp @@ -1,27 +1,27 @@ -#include +#include namespace example_behaviors { -DelayedMessage::DelayedMessage(const std::string& name, const BT::NodeConfiguration& config, +ExampleDelayedMessage::ExampleDelayedMessage(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList DelayedMessage::providedPorts() +BT::PortsList ExampleDelayedMessage::providedPorts() { // delay_duration: Number of seconds to wait before logging a message return BT::PortsList( { BT::InputPort("delay_duration", "5", "The duration, in seconds, to wait before logging a message.") }); } -BT::KeyValueVector DelayedMessage::metadata() +BT::KeyValueVector ExampleDelayedMessage::metadata() { return { { "subcategory", "Example" }, { "description", "After some time, log a message that says \"Hello, world!\"." } }; } -BT::NodeStatus DelayedMessage::onStart() +BT::NodeStatus ExampleDelayedMessage::onStart() { // Store the time at which this node was first ticked start_time_ = std::chrono::steady_clock::now(); @@ -53,7 +53,7 @@ BT::NodeStatus DelayedMessage::onStart() return BT::NodeStatus::RUNNING; } -BT::NodeStatus DelayedMessage::onRunning() +BT::NodeStatus ExampleDelayedMessage::onRunning() { // If the delay duration has not elapsed since this node was started, return RUNNING if (std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time_).count() < @@ -70,7 +70,7 @@ BT::NodeStatus DelayedMessage::onRunning() } } -void DelayedMessage::onHalted() +void ExampleDelayedMessage::onHalted() { } diff --git a/src/example_behaviors/src/fibonacci_action_client.cpp b/src/example_behaviors/src/example_fibonacci_action_client.cpp similarity index 80% rename from src/example_behaviors/src/fibonacci_action_client.cpp rename to src/example_behaviors/src/example_fibonacci_action_client.cpp index cf3255e9..d51a4e67 100644 --- a/src/example_behaviors/src/fibonacci_action_client.cpp +++ b/src/example_behaviors/src/example_fibonacci_action_client.cpp @@ -1,18 +1,18 @@ -#include +#include // Include the template implementation for ActionClientBehaviorBase. #include namespace example_behaviors { -FibonacciActionClient::FibonacciActionClient( +ExampleFibonacciActionClient::ExampleFibonacciActionClient( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : ActionClientBehaviorBase(name, config, shared_resources) { } -BT::PortsList FibonacciActionClient::providedPorts() +BT::PortsList ExampleFibonacciActionClient::providedPorts() { // This node has two input ports and two output ports return BT::PortsList({ @@ -26,14 +26,14 @@ BT::PortsList FibonacciActionClient::providedPorts() }); } -BT::KeyValueVector FibonacciActionClient::metadata() +BT::KeyValueVector ExampleFibonacciActionClient::metadata() { return { { "subcategory", "Example" }, { "description", "Calls an action to get a Fibonacci sequence and makes the result available on an output port." } }; } -tl::expected FibonacciActionClient::getActionName() +tl::expected ExampleFibonacciActionClient::getActionName() { const auto action_name = getInput("action_name"); if (const auto error = moveit_studio::behaviors::maybe_error(action_name)) @@ -43,7 +43,7 @@ tl::expected FibonacciActionClient::getActionName() return action_name.value(); } -tl::expected FibonacciActionClient::createGoal() +tl::expected ExampleFibonacciActionClient::createGoal() { const auto order = getInput("order"); @@ -55,7 +55,7 @@ tl::expected FibonacciActionClient::createGoal() return example_interfaces::build().order(order.value()); } -tl::expected FibonacciActionClient::processResult(const std::shared_ptr result) +tl::expected ExampleFibonacciActionClient::processResult(const std::shared_ptr result) { std::stringstream stream; for (const auto& value : result->sequence) @@ -72,7 +72,7 @@ tl::expected FibonacciActionClient::processResult(const std:: return { true }; } -void FibonacciActionClient::processFeedback(const std::shared_ptr feedback) +void ExampleFibonacciActionClient::processFeedback(const std::shared_ptr feedback) { std::stringstream stream; for (const auto& value : feedback->sequence) diff --git a/src/example_behaviors/src/get_string_from_topic.cpp b/src/example_behaviors/src/example_get_string_from_topic.cpp similarity index 78% rename from src/example_behaviors/src/get_string_from_topic.cpp rename to src/example_behaviors/src/example_get_string_from_topic.cpp index 1ee63726..5655436f 100644 --- a/src/example_behaviors/src/get_string_from_topic.cpp +++ b/src/example_behaviors/src/example_get_string_from_topic.cpp @@ -1,17 +1,17 @@ -#include +#include // Include the template implementation for GetMessageFromTopicBehaviorBase. #include namespace example_behaviors { -GetStringFromTopic::GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, +ExampleGetStringFromTopic::ExampleGetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : GetMessageFromTopicBehaviorBase(name, config, shared_resources) { } -BT::PortsList GetStringFromTopic::providedPorts() +BT::PortsList ExampleGetStringFromTopic::providedPorts() { // This node has one input port and one output port return BT::PortsList({ @@ -20,7 +20,7 @@ BT::PortsList GetStringFromTopic::providedPorts() }); } -BT::KeyValueVector GetStringFromTopic::metadata() +BT::KeyValueVector ExampleGetStringFromTopic::metadata() { return { { "subcategory", "Example" }, { "description", "Captures a string message and makes it available on an output port." } }; diff --git a/src/example_behaviors/src/hello_world.cpp b/src/example_behaviors/src/example_hello_world.cpp similarity index 66% rename from src/example_behaviors/src/hello_world.cpp rename to src/example_behaviors/src/example_hello_world.cpp index 596d6b95..5a8531fb 100644 --- a/src/example_behaviors/src/hello_world.cpp +++ b/src/example_behaviors/src/example_hello_world.cpp @@ -1,27 +1,27 @@ -#include +#include namespace example_behaviors { -HelloWorld::HelloWorld(const std::string& name, const BT::NodeConfiguration& config, +ExampleHelloWorld::ExampleHelloWorld(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList HelloWorld::providedPorts() +BT::PortsList ExampleHelloWorld::providedPorts() { // This node has no input or output ports return BT::PortsList({}); } -BT::KeyValueVector HelloWorld::metadata() +BT::KeyValueVector ExampleHelloWorld::metadata() { return { { "subcategory", "Example" }, { "description", "Log a message that says \"Hello, world!\"." } }; } -BT::NodeStatus HelloWorld::tick() +BT::NodeStatus ExampleHelloWorld::tick() { - // Do HelloWorld's useful work. + // Do ExampleHelloWorld's useful work. // Setting the third argument to false ensures the message will be shown immediately shared_resources_->logger->publishInfoMessage(name(), "Hello, world!"); diff --git a/src/example_behaviors/src/ndt_registration.cpp b/src/example_behaviors/src/example_ndt_registration.cpp similarity index 95% rename from src/example_behaviors/src/ndt_registration.cpp rename to src/example_behaviors/src/example_ndt_registration.cpp index 3b8c6e63..f8e538b5 100644 --- a/src/example_behaviors/src/ndt_registration.cpp +++ b/src/example_behaviors/src/example_ndt_registration.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -43,7 +43,7 @@ getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) } // namespace -NDTRegistration::NDTRegistration( +ExampleNDTRegistration::ExampleNDTRegistration( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) @@ -51,7 +51,7 @@ NDTRegistration::NDTRegistration( } -BT::PortsList NDTRegistration::providedPorts() +BT::PortsList ExampleNDTRegistration::providedPorts() { return { BT::InputPort("base_point_cloud", "{point_cloud}", "Point cloud to align with the target point cloud."), @@ -73,13 +73,13 @@ BT::PortsList NDTRegistration::providedPorts() "The pose of the target point cloud relative to the frame " "of the base point cloud.") }; } -BT::KeyValueVector NDTRegistration::metadata() +BT::KeyValueVector ExampleNDTRegistration::metadata() { return { {"subcategory", "Perception - 3D Point Cloud"}, {"description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algorithm"} }; } -tl::expected NDTRegistration::doWork() +tl::expected ExampleNDTRegistration::doWork() { const auto base_point_cloud_msg = getInput("base_point_cloud"); const auto target_point_cloud_msg = getInput("target_point_cloud"); diff --git a/src/example_behaviors/src/publish_color_rgba.cpp b/src/example_behaviors/src/example_publish_color_rgba.cpp similarity index 70% rename from src/example_behaviors/src/publish_color_rgba.cpp rename to src/example_behaviors/src/example_publish_color_rgba.cpp index b227746f..ecd21b74 100644 --- a/src/example_behaviors/src/publish_color_rgba.cpp +++ b/src/example_behaviors/src/example_publish_color_rgba.cpp @@ -1,26 +1,26 @@ -#include +#include namespace example_behaviors { -PublishColorRGBA::PublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, +ExamplePublishColorRGBA::ExamplePublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) , publisher_{ shared_resources_->node->create_publisher("/my_topic", rclcpp::QoS(1)) } { } -BT::PortsList PublishColorRGBA::providedPorts() +BT::PortsList ExamplePublishColorRGBA::providedPorts() { return {}; } -BT::KeyValueVector PublishColorRGBA::metadata() +BT::KeyValueVector ExamplePublishColorRGBA::metadata() { return { { "subcategory", "Example" }, { "description", "Publishes a fixed std_msgs::msg::ColorRGBA message to a topic named \"/my_topic\"" } }; } -BT::NodeStatus PublishColorRGBA::tick() +BT::NodeStatus ExamplePublishColorRGBA::tick() { std_msgs::msg::ColorRGBA color_msg; color_msg.r = 0.5; diff --git a/src/example_behaviors/src/ransac_registration.cpp b/src/example_behaviors/src/example_ransac_registration.cpp similarity index 97% rename from src/example_behaviors/src/ransac_registration.cpp rename to src/example_behaviors/src/example_ransac_registration.cpp index c25f81ea..b85d69c0 100644 --- a/src/example_behaviors/src/ransac_registration.cpp +++ b/src/example_behaviors/src/example_ransac_registration.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -43,7 +43,7 @@ getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) } // namespace -RANSACRegistration::RANSACRegistration( +ExampleRANSACRegistration::ExampleRANSACRegistration( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) @@ -51,7 +51,7 @@ RANSACRegistration::RANSACRegistration( } -BT::PortsList RANSACRegistration::providedPorts() +BT::PortsList ExampleRANSACRegistration::providedPorts() { return { BT::InputPort("base_point_cloud", "{point_cloud}", "Point cloud to align with the target point cloud."), @@ -80,13 +80,13 @@ BT::PortsList RANSACRegistration::providedPorts() "of the base point cloud.") }; } -BT::KeyValueVector RANSACRegistration::metadata() +BT::KeyValueVector ExampleRANSACRegistration::metadata() { return { {"subcategory", "Perception - 3D Point Cloud"}, {"description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algorithm"} }; } -tl::expected RANSACRegistration::doWork() +tl::expected ExampleRANSACRegistration::doWork() { const auto base_point_cloud_msg = getInput("base_point_cloud"); diff --git a/src/example_behaviors/src/setup_mtc_pick_from_pose.cpp b/src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp similarity index 96% rename from src/example_behaviors/src/setup_mtc_pick_from_pose.cpp rename to src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp index f83267f9..02dee6bd 100644 --- a/src/example_behaviors/src/setup_mtc_pick_from_pose.cpp +++ b/src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -14,7 +14,7 @@ namespace { -const auto kLogger = rclcpp::get_logger("SetupMTCPickFromPose"); +const auto kLogger = rclcpp::get_logger("ExampleSetupMtcPickFromPose"); using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes; // Port names for input and output ports. @@ -37,14 +37,14 @@ constexpr auto kSceneObjectNameOctomap = ""; namespace example_behaviors { -SetupMtcPickFromPose::SetupMtcPickFromPose( +ExampleSetupMtcPickFromPose::ExampleSetupMtcPickFromPose( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList SetupMtcPickFromPose::providedPorts() +BT::PortsList ExampleSetupMtcPickFromPose::providedPorts() { return { BT::BidirectionalPort(kPortIDTask, "{mtc_task}", "MoveIt Task Constructor task."), @@ -53,13 +53,13 @@ BT::PortsList SetupMtcPickFromPose::providedPorts() }; } -BT::KeyValueVector SetupMtcPickFromPose::metadata() +BT::KeyValueVector ExampleSetupMtcPickFromPose::metadata() { return { { "subcategory", "Example" }, { "description", "Adds the stages to describe a pick motion to the MTC task." } }; } -BT::NodeStatus SetupMtcPickFromPose::tick() +BT::NodeStatus ExampleSetupMtcPickFromPose::tick() { using namespace moveit_studio::behaviors; diff --git a/src/example_behaviors/src/setup_mtc_place_from_pose.cpp b/src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp similarity index 96% rename from src/example_behaviors/src/setup_mtc_place_from_pose.cpp rename to src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp index dd24d5ba..e067b398 100644 --- a/src/example_behaviors/src/setup_mtc_place_from_pose.cpp +++ b/src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -14,7 +14,7 @@ namespace { -const auto kLogger = rclcpp::get_logger("SetupMTCPickFromPose"); +const auto kLogger = rclcpp::get_logger("ExampleSetupMtcPickFromPose"); using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes; // Port names for input and output ports. @@ -38,14 +38,14 @@ constexpr auto kSceneObjectNameOctomap = ""; namespace example_behaviors { -SetupMtcPlaceFromPose::SetupMtcPlaceFromPose( +ExampleSetupMtcPlaceFromPose::ExampleSetupMtcPlaceFromPose( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList SetupMtcPlaceFromPose::providedPorts() +BT::PortsList ExampleSetupMtcPlaceFromPose::providedPorts() { return { BT::BidirectionalPort(kPortIDTask, "{mtc_task}", "MoveIt Task Constructor task."), @@ -54,13 +54,13 @@ BT::PortsList SetupMtcPlaceFromPose::providedPorts() }; } -BT::KeyValueVector SetupMtcPlaceFromPose::metadata() +BT::KeyValueVector ExampleSetupMtcPlaceFromPose::metadata() { return { { "subcategory", "Example" }, { "description", "Adds the stages to describe a place motion to the MTC task." } }; } -BT::NodeStatus SetupMtcPlaceFromPose::tick() +BT::NodeStatus ExampleSetupMtcPlaceFromPose::tick() { using namespace moveit_studio::behaviors; diff --git a/src/example_behaviors/src/setup_mtc_wave_hand.cpp b/src/example_behaviors/src/example_setup_mtc_wave_hand.cpp similarity index 92% rename from src/example_behaviors/src/setup_mtc_wave_hand.cpp rename to src/example_behaviors/src/example_setup_mtc_wave_hand.cpp index 3f00581c..6b436f96 100644 --- a/src/example_behaviors/src/setup_mtc_wave_hand.cpp +++ b/src/example_behaviors/src/example_setup_mtc_wave_hand.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -9,7 +9,7 @@ namespace // Define a constant for the ID of the Behavior's data port. constexpr auto kPortIDTask = "task"; -// Define constants for the names of the behavior parameters used by the SetupMTCWaveHand behavior. +// Define constants for the names of the behavior parameters used by the ExampleSetupMTCWaveHand behavior. // These defaults are set to match the UR5e robot defined in the moveit_studio_custom_site_config_example package. constexpr auto kPrimaryGroupName = "manipulator"; constexpr auto kHandFrameName = "grasp_link"; @@ -20,13 +20,13 @@ const rclcpp::Logger kLogger = rclcpp::get_logger("WaveHello"); namespace example_behaviors { -SetupMTCWaveHand::SetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, +ExampleSetupMTCWaveHand::ExampleSetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) { } -BT::PortsList SetupMTCWaveHand::providedPorts() +BT::PortsList ExampleSetupMTCWaveHand::providedPorts() { return { BT::BidirectionalPort>(kPortIDTask, "{mtc_task}", @@ -34,12 +34,12 @@ BT::PortsList SetupMTCWaveHand::providedPorts() }; } -BT::KeyValueVector SetupMTCWaveHand::metadata() +BT::KeyValueVector ExampleSetupMTCWaveHand::metadata() { return { { "subcategory", "Example" }, { "description", "Wave hello with the end effector." } }; } -tl::expected SetupMTCWaveHand::doWork() +tl::expected ExampleSetupMTCWaveHand::doWork() { // Retrieve the "task" data port const auto task = getInput(kPortIDTask); diff --git a/src/example_behaviors/src/register_behaviors.cpp b/src/example_behaviors/src/register_behaviors.cpp index c3d26677..80662314 100644 --- a/src/example_behaviors/src/register_behaviors.cpp +++ b/src/example_behaviors/src/register_behaviors.cpp @@ -2,18 +2,18 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -25,21 +25,21 @@ class ExampleBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesN void registerBehaviors(BT::BehaviorTreeFactory& factory, const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "HelloWorld", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "ConvertMtcSolutionToJointTrajectory", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "DelayedMessage", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "SetupMTCWaveHand", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "GetStringFromTopic", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "AddTwoIntsServiceClient", + moveit_studio::behaviors::registerBehavior(factory, "ExampleHelloWorld", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleConvertMtcSolutionToJointTrajectory", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleDelayedMessage", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleSetupMTCWaveHand", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleGetStringFromTopic", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleAddTwoIntsServiceClient", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "FibonacciActionClient", + moveit_studio::behaviors::registerBehavior(factory, "ExampleFibonacciActionClient", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "PublishColorRGBA", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "SetupMtcPickFromPose", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "SetupMtcPlaceFromPose", + moveit_studio::behaviors::registerBehavior(factory, "ExamplePublishColorRGBA", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleSetupMtcPickFromPose", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleSetupMtcPlaceFromPose", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "NDTRegistration", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "RANSACRegistration", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleNDTRegistration", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleRANSACRegistration", shared_resources); } }; } // namespace example_behaviors diff --git a/src/example_behaviors/test/test_behavior_plugins.cpp b/src/example_behaviors/test/test_behavior_plugins.cpp index c6432ea5..56dae5a8 100644 --- a/src/example_behaviors/test/test_behavior_plugins.cpp +++ b/src/example_behaviors/test/test_behavior_plugins.cpp @@ -24,23 +24,23 @@ TEST(BehaviorTests, test_load_behavior_plugins) } // 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", "AddTwoIntsServiceClient", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "DelayedMessage", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleAddTwoIntsServiceClient", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExampleDelayedMessage", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "FibonacciActionClient", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleFibonacciActionClient", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "GetStringFromTopic", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "HelloWorld", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "PublishColorRGBA", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleGetStringFromTopic", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExampleHelloWorld", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExamplePublishColorRGBA", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "SetupMtcPickFromPose", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleSetupMtcPickFromPose", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "SetupMtcPlaceFromPose", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "SetupMTCWaveHand", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleSetupMtcPlaceFromPose", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExampleSetupMTCWaveHand", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "NDTRegistration", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleNDTRegistration", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "RANSACRegistration", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleRANSACRegistration", BT::NodeConfiguration())); } int main(int argc, char** argv) From bee947939e6ace0d29793c07262e9ae053586059 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Wed, 4 Dec 2024 01:02:46 -0800 Subject: [PATCH 10/18] Updated commentsin example objectives --- src/lab_sim/objectives/pick_and_place_example.xml | 4 ++-- src/lab_sim/objectives/plan_and_save_trajectory.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml index 49c844c9..456b378b 100644 --- a/src/lab_sim/objectives/pick_and_place_example.xml +++ b/src/lab_sim/objectives/pick_and_place_example.xml @@ -24,7 +24,7 @@ task="{mtc_pick_task}" /> - + - + - + Date: Wed, 4 Dec 2024 22:09:40 -0700 Subject: [PATCH 11/18] fix(workspace): categories readded for lab_sim (#35) * fix(workspace): categories readded for lab_sim --- .../src/example_add_two_ints_service_client.cpp | 2 +- src/example_behaviors/src/example_delayed_message.cpp | 2 +- .../src/example_fibonacci_action_client.cpp | 2 +- .../src/example_get_string_from_topic.cpp | 2 +- src/example_behaviors/src/example_hello_world.cpp | 2 +- src/example_behaviors/src/example_publish_color_rgba.cpp | 2 +- .../src/example_setup_mtc_pick_from_pose.cpp | 2 +- .../src/example_setup_mtc_place_from_pose.cpp | 2 +- src/example_behaviors/src/example_setup_mtc_wave_hand.cpp | 2 +- src/lab_sim/objectives/3_waypoint_pick_and_place.xml | 7 +++++++ src/lab_sim/objectives/constrained_pick_place.xml | 7 +++++++ src/lab_sim/objectives/cycle_between_waypoints.xml | 7 +++++++ src/lab_sim/objectives/get_april_tag_pose_from_image.xml | 2 +- src/lab_sim/objectives/joint_diagnostic.xml | 7 +++++++ .../objectives/load_and_execute_joint_trajectory.xml | 7 +++++++ src/lab_sim/objectives/look_at_table.xml | 7 +++++++ src/lab_sim/objectives/move_with_velocity_and_force.xml | 7 +++++++ src/lab_sim/objectives/pick_and_place_example.xml | 7 +++++++ .../objectives/pick_april_tag_object_with_approval.xml | 7 +++++++ src/lab_sim/objectives/pick_object.xml | 7 +++++++ src/lab_sim/objectives/pick_up_cube.xml | 7 +++++++ src/lab_sim/objectives/place_first_object_in_vector.xml | 7 +++++++ src/lab_sim/objectives/place_object.xml | 2 +- src/lab_sim/objectives/plan_move_to_pose.xml | 5 +++-- src/lab_sim/objectives/plan_to_pose.xml | 2 +- src/lab_sim/objectives/register_cad_part.xml | 7 +++++++ src/lab_sim/objectives/take_snap.xml | 6 +++--- src/lab_sim/objectives/visualize_pose.xml | 7 +++++++ .../picknik_ur_base_config/objectives/clear_snapshot.xml | 2 +- .../picknik_ur_base_config/objectives/close_gripper.xml | 6 +++++- 30 files changed, 122 insertions(+), 19 deletions(-) diff --git a/src/example_behaviors/src/example_add_two_ints_service_client.cpp b/src/example_behaviors/src/example_add_two_ints_service_client.cpp index 48f9dc0b..5e90fa05 100644 --- a/src/example_behaviors/src/example_add_two_ints_service_client.cpp +++ b/src/example_behaviors/src/example_add_two_ints_service_client.cpp @@ -24,7 +24,7 @@ BT::PortsList ExampleAddTwoIntsServiceClient::providedPorts() BT::KeyValueVector ExampleAddTwoIntsServiceClient::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Calls a service to add two integers and makes the result available on an output port." } }; } diff --git a/src/example_behaviors/src/example_delayed_message.cpp b/src/example_behaviors/src/example_delayed_message.cpp index 584f7994..34818fae 100644 --- a/src/example_behaviors/src/example_delayed_message.cpp +++ b/src/example_behaviors/src/example_delayed_message.cpp @@ -17,7 +17,7 @@ BT::PortsList ExampleDelayedMessage::providedPorts() BT::KeyValueVector ExampleDelayedMessage::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "After some time, log a message that says \"Hello, world!\"." } }; } diff --git a/src/example_behaviors/src/example_fibonacci_action_client.cpp b/src/example_behaviors/src/example_fibonacci_action_client.cpp index d51a4e67..e8c0bdd1 100644 --- a/src/example_behaviors/src/example_fibonacci_action_client.cpp +++ b/src/example_behaviors/src/example_fibonacci_action_client.cpp @@ -28,7 +28,7 @@ BT::PortsList ExampleFibonacciActionClient::providedPorts() BT::KeyValueVector ExampleFibonacciActionClient::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Calls an action to get a Fibonacci sequence and makes the result available on an output port." } }; } diff --git a/src/example_behaviors/src/example_get_string_from_topic.cpp b/src/example_behaviors/src/example_get_string_from_topic.cpp index 5655436f..48495ed3 100644 --- a/src/example_behaviors/src/example_get_string_from_topic.cpp +++ b/src/example_behaviors/src/example_get_string_from_topic.cpp @@ -22,7 +22,7 @@ BT::PortsList ExampleGetStringFromTopic::providedPorts() BT::KeyValueVector ExampleGetStringFromTopic::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Captures a string message and makes it available on an output port." } }; } diff --git a/src/example_behaviors/src/example_hello_world.cpp b/src/example_behaviors/src/example_hello_world.cpp index 5a8531fb..c214c389 100644 --- a/src/example_behaviors/src/example_hello_world.cpp +++ b/src/example_behaviors/src/example_hello_world.cpp @@ -16,7 +16,7 @@ BT::PortsList ExampleHelloWorld::providedPorts() BT::KeyValueVector ExampleHelloWorld::metadata() { - return { { "subcategory", "Example" }, { "description", "Log a message that says \"Hello, world!\"." } }; + return { { "subcategory", "Example Behaviors" }, { "description", "Log a message that says \"Hello, world!\"." } }; } BT::NodeStatus ExampleHelloWorld::tick() diff --git a/src/example_behaviors/src/example_publish_color_rgba.cpp b/src/example_behaviors/src/example_publish_color_rgba.cpp index ecd21b74..5a6e68ed 100644 --- a/src/example_behaviors/src/example_publish_color_rgba.cpp +++ b/src/example_behaviors/src/example_publish_color_rgba.cpp @@ -16,7 +16,7 @@ BT::PortsList ExamplePublishColorRGBA::providedPorts() BT::KeyValueVector ExamplePublishColorRGBA::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Publishes a fixed std_msgs::msg::ColorRGBA message to a topic named \"/my_topic\"" } }; } diff --git a/src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp b/src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp index 02dee6bd..67f56007 100644 --- a/src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp +++ b/src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp @@ -55,7 +55,7 @@ BT::PortsList ExampleSetupMtcPickFromPose::providedPorts() BT::KeyValueVector ExampleSetupMtcPickFromPose::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Adds the stages to describe a pick motion to the MTC task." } }; } diff --git a/src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp b/src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp index e067b398..3f5e44ae 100644 --- a/src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp +++ b/src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp @@ -56,7 +56,7 @@ BT::PortsList ExampleSetupMtcPlaceFromPose::providedPorts() BT::KeyValueVector ExampleSetupMtcPlaceFromPose::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Adds the stages to describe a place motion to the MTC task." } }; } diff --git a/src/example_behaviors/src/example_setup_mtc_wave_hand.cpp b/src/example_behaviors/src/example_setup_mtc_wave_hand.cpp index 6b436f96..5867b1b0 100644 --- a/src/example_behaviors/src/example_setup_mtc_wave_hand.cpp +++ b/src/example_behaviors/src/example_setup_mtc_wave_hand.cpp @@ -36,7 +36,7 @@ BT::PortsList ExampleSetupMTCWaveHand::providedPorts() BT::KeyValueVector ExampleSetupMTCWaveHand::metadata() { - return { { "subcategory", "Example" }, { "description", "Wave hello with the end effector." } }; + return { { "subcategory", "Example Behaviors" }, { "description", "Wave hello with the end effector." } }; } tl::expected ExampleSetupMTCWaveHand::doWork() diff --git a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml index 9693c3f9..60490ad4 100644 --- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml +++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml @@ -119,4 +119,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml index 9b680044..838533b1 100644 --- a/src/lab_sim/objectives/constrained_pick_place.xml +++ b/src/lab_sim/objectives/constrained_pick_place.xml @@ -152,4 +152,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/cycle_between_waypoints.xml b/src/lab_sim/objectives/cycle_between_waypoints.xml index 71ff91b6..e6c79ac7 100644 --- a/src/lab_sim/objectives/cycle_between_waypoints.xml +++ b/src/lab_sim/objectives/cycle_between_waypoints.xml @@ -25,4 +25,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml index 6a223a9e..69637f54 100644 --- a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml +++ b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml @@ -41,7 +41,7 @@ - + diff --git a/src/lab_sim/objectives/joint_diagnostic.xml b/src/lab_sim/objectives/joint_diagnostic.xml index 1bbb7714..aebb7879 100644 --- a/src/lab_sim/objectives/joint_diagnostic.xml +++ b/src/lab_sim/objectives/joint_diagnostic.xml @@ -35,4 +35,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml index 989a9fbb..b0b64db3 100644 --- a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml +++ b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml @@ -22,4 +22,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/look_at_table.xml b/src/lab_sim/objectives/look_at_table.xml index 8aeed171..70626c00 100644 --- a/src/lab_sim/objectives/look_at_table.xml +++ b/src/lab_sim/objectives/look_at_table.xml @@ -17,4 +17,11 @@ /> + + + + + + + diff --git a/src/lab_sim/objectives/move_with_velocity_and_force.xml b/src/lab_sim/objectives/move_with_velocity_and_force.xml index 0026831a..ded7c72e 100644 --- a/src/lab_sim/objectives/move_with_velocity_and_force.xml +++ b/src/lab_sim/objectives/move_with_velocity_and_force.xml @@ -72,4 +72,11 @@ /> + + + + + + + diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml index 456b378b..7548b37d 100644 --- a/src/lab_sim/objectives/pick_and_place_example.xml +++ b/src/lab_sim/objectives/pick_and_place_example.xml @@ -67,4 +67,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml index 6a932157..6ca263f1 100644 --- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml +++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml @@ -50,4 +50,11 @@ /> + + + + + + + diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml index 2aa4018c..9ecb7981 100644 --- a/src/lab_sim/objectives/pick_object.xml +++ b/src/lab_sim/objectives/pick_object.xml @@ -29,4 +29,11 @@ /> + + + + + + + diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml index fc22a88e..c405250c 100644 --- a/src/lab_sim/objectives/pick_up_cube.xml +++ b/src/lab_sim/objectives/pick_up_cube.xml @@ -14,4 +14,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/place_first_object_in_vector.xml b/src/lab_sim/objectives/place_first_object_in_vector.xml index 0e058131..efde08c0 100644 --- a/src/lab_sim/objectives/place_first_object_in_vector.xml +++ b/src/lab_sim/objectives/place_first_object_in_vector.xml @@ -42,4 +42,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml index fec0fd73..548e71f5 100644 --- a/src/lab_sim/objectives/place_object.xml +++ b/src/lab_sim/objectives/place_object.xml @@ -30,7 +30,7 @@ - + diff --git a/src/lab_sim/objectives/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml index c280015e..0d3be8df 100644 --- a/src/lab_sim/objectives/plan_move_to_pose.xml +++ b/src/lab_sim/objectives/plan_move_to_pose.xml @@ -1,5 +1,6 @@ + - + - + diff --git a/src/lab_sim/objectives/plan_to_pose.xml b/src/lab_sim/objectives/plan_to_pose.xml index 3bb7c76b..066fac12 100644 --- a/src/lab_sim/objectives/plan_to_pose.xml +++ b/src/lab_sim/objectives/plan_to_pose.xml @@ -50,7 +50,7 @@ - + diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml index 4c56b729..9393a831 100644 --- a/src/lab_sim/objectives/register_cad_part.xml +++ b/src/lab_sim/objectives/register_cad_part.xml @@ -58,4 +58,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/take_snap.xml b/src/lab_sim/objectives/take_snap.xml index 0fdb917b..f6fb49d9 100644 --- a/src/lab_sim/objectives/take_snap.xml +++ b/src/lab_sim/objectives/take_snap.xml @@ -1,7 +1,7 @@ - + - + diff --git a/src/lab_sim/objectives/visualize_pose.xml b/src/lab_sim/objectives/visualize_pose.xml index 524070c5..b8a8d771 100644 --- a/src/lab_sim/objectives/visualize_pose.xml +++ b/src/lab_sim/objectives/visualize_pose.xml @@ -15,4 +15,11 @@ + + + + + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml index b4f943f0..07a7e5c8 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml @@ -8,7 +8,7 @@ - + diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml index e96d3dba..6687772d 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml @@ -1,6 +1,10 @@ - + Date: Thu, 5 Dec 2024 08:25:26 -0800 Subject: [PATCH 12/18] Updating favourites (#36) --- src/lab_sim/objectives/add_waypoints_to_mtc_task.xml | 5 +++-- src/lab_sim/objectives/apriltag_pick_object.xml | 1 - src/lab_sim/objectives/move_with_velocity_and_force.xml | 2 +- src/lab_sim/objectives/pick_and_place_example.xml | 3 +-- src/lab_sim/objectives/plan_and_save_trajectory.xml | 3 ++- src/lab_sim/objectives/register_cad_part.xml | 2 +- .../objectives/save_and_execute_a_trajectory_from_disk.xml | 2 +- .../picknik_ur_base_config/objectives/clear_snapshot.xml | 2 +- .../picknik_ur_base_config/objectives/close_gripper.xml | 2 +- .../picknik_ur_base_config/objectives/open_gripper.xml | 7 +++++-- 10 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml index 9359a4b0..4ede36b5 100644 --- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml +++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml @@ -1,3 +1,4 @@ + diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml index be333a75..db138a7a 100644 --- a/src/lab_sim/objectives/plan_and_save_trajectory.xml +++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml @@ -1,3 +1,4 @@ + - + diff --git a/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml b/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml index 33d02a77..caafc2a5 100644 --- a/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml +++ b/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml @@ -6,7 +6,7 @@ diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml index 07a7e5c8..7c2e83b3 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml @@ -1,9 +1,9 @@ - diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml index 6687772d..d280cbb4 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml @@ -1,8 +1,8 @@ - diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml index f2d84d47..91194afe 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml @@ -1,7 +1,10 @@ - - + Date: Wed, 11 Dec 2024 19:29:06 -0700 Subject: [PATCH 13/18] Improve lab sim example objectives with story telling --- .../objectives/3_waypoint_pick_and_place.xml | 11 +- src/lab_sim/objectives/_scan_scene.xml | 33 ++ .../objectives/constrained_pick_place.xml | 2 +- src/lab_sim/objectives/joint_diagnostic.xml | 2 +- .../load_and_execute_joint_trajectory.xml | 2 +- src/lab_sim/objectives/look_at_table.xml | 2 +- .../move_with_velocity_and_force.xml | 23 +- .../pick_april_tag_object_with_approval.xml | 1 - .../objectives/plan_and_save_trajectory.xml | 1 - src/lab_sim/objectives/take_snap.xml | 2 +- src/lab_sim/objectives/wrist_snap.xml | 2 +- src/lab_sim/waypoints/ur_waypoints.yaml | 401 ++++++++++-------- .../objectives/clear_snapshot.xml | 2 +- .../objectives/close_gripper.xml | 2 +- .../objectives/open_gripper.xml | 2 +- 15 files changed, 282 insertions(+), 206 deletions(-) create mode 100644 src/lab_sim/objectives/_scan_scene.xml diff --git a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml index 60490ad4..d3605cc2 100644 --- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml +++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml @@ -1,13 +1,12 @@ + - - - - - - - + diff --git a/src/lab_sim/objectives/_scan_scene.xml b/src/lab_sim/objectives/_scan_scene.xml new file mode 100644 index 00000000..b67089b7 --- /dev/null +++ b/src/lab_sim/objectives/_scan_scene.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml index 838533b1..1f90df87 100644 --- a/src/lab_sim/objectives/constrained_pick_place.xml +++ b/src/lab_sim/objectives/constrained_pick_place.xml @@ -3,7 +3,7 @@ diff --git a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml index b0b64db3..8d3ea8f3 100644 --- a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml +++ b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml @@ -3,7 +3,7 @@ diff --git a/src/lab_sim/objectives/look_at_table.xml b/src/lab_sim/objectives/look_at_table.xml index 70626c00..e7d6a131 100644 --- a/src/lab_sim/objectives/look_at_table.xml +++ b/src/lab_sim/objectives/look_at_table.xml @@ -3,7 +3,7 @@ diff --git a/src/lab_sim/objectives/move_with_velocity_and_force.xml b/src/lab_sim/objectives/move_with_velocity_and_force.xml index 32c099e5..13109d70 100644 --- a/src/lab_sim/objectives/move_with_velocity_and_force.xml +++ b/src/lab_sim/objectives/move_with_velocity_and_force.xml @@ -1,11 +1,14 @@ - + + + + - - - - - + diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml index 6ca263f1..9196fe4a 100644 --- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml +++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml @@ -1,4 +1,3 @@ - diff --git a/src/lab_sim/objectives/wrist_snap.xml b/src/lab_sim/objectives/wrist_snap.xml index 82b4878c..81d3c072 100644 --- a/src/lab_sim/objectives/wrist_snap.xml +++ b/src/lab_sim/objectives/wrist_snap.xml @@ -3,7 +3,7 @@ diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml index 0bec6a94..ed9275b1 100644 --- a/src/lab_sim/waypoints/ur_waypoints.yaml +++ b/src/lab_sim/waypoints/ur_waypoints.yaml @@ -1,9 +1,52 @@ +- description: Move out of view of scene camera + 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.00780498767879874 + - -1.7207208458780023 + - -2.9355287074907658e-05 + - -1.8849275826089145 + - 1.102019084998803 + - -1.2563366868253756 + - -1.5707679924921147 + - 1.5931846964135707e-07 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Park Far Right - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -12,23 +55,23 @@ 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 + - 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.039808309949994805 - - -0.5839327290315586 - - -0.2687381616263423 - - -1.4142103820838365 - - 1.1521752505644343 - - -1.3067562163163808 - - -1.5708040696131618 - - -0.2687661213277525 + - 0.06656710337674145 + - 0.1162925924535442 + - 0.2054240147028693 + - -0.987070953194143 + - 1.3146849599773849 + - -1.8698622612624394 + - -1.5707050706852668 + - 0.20529114323213696 velocity: [] multi_dof_joint_state: header: @@ -40,13 +83,13 @@ transforms: [] twist: [] wrench: [] - name: Workspace Right + name: Above Place Cube - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -55,23 +98,23 @@ 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 + - 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.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - 3.14 - - -0.0803135811027271 + - 0.06656709575770764 + - 0.2544520984236789 + - 0.00297097958947393 + - -0.5454836524482858 + - 1.028930149174636 + - -2.02088483587436 + - -1.5709145918870968 + - 0.002734726536987722 velocity: [] multi_dof_joint_state: header: @@ -83,13 +126,13 @@ transforms: [] twist: [] wrench: [] - name: Wrist 2 Max + name: Pick Cube - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -98,23 +141,23 @@ 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 + - 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.02639307417885356 - - -0.0368978637919646 - - -0.05133733759107817 - - -1.0646673321083444 - - 1.6085719305391477 - - -2.116413476868559 - - -1.5570186007251205 - - -0.04927977068008667 + - 0.06656709465883658 + - 0.31742962464478663 + - -0.07518824419393247 + - -0.7207568718596512 + - 0.8665668564352057 + - -1.6837956156664982 + - -1.5707507158288945 + - -0.07517349313281775 velocity: [] multi_dof_joint_state: header: @@ -126,13 +169,13 @@ transforms: [] twist: [] wrench: [] - name: VFC Start + name: Above Pick Cube - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -141,23 +184,23 @@ 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 + - 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.06656710453529632 - - -0.026311129005491703 - - 0.391188076851874 - - -0.6421557305136849 - - 1.2200227002982904 - - -2.1172063445999307 - - -1.5707036597762196 - - 0.39119709035689026 + - 0.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - -1.5575126426436692 + - -0.0803135811027271 velocity: [] multi_dof_joint_state: header: @@ -169,13 +212,13 @@ transforms: [] twist: [] wrench: [] - name: Place Cube + name: Look at Table - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -184,23 +227,23 @@ 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 + - 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.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - -3.14 - - -0.0803135811027271 + - 0.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - -3.14 + - -0.0803135811027271 velocity: [] multi_dof_joint_state: header: @@ -216,9 +259,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -227,23 +270,23 @@ 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 + - 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.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - -1.5575126426436692 - - -0.0803135811027271 + - 0.06656710453529632 + - -0.026311129005491703 + - 0.391188076851874 + - -0.6421557305136849 + - 1.2200227002982904 + - -2.1172063445999307 + - -1.5707036597762196 + - 0.39119709035689026 velocity: [] multi_dof_joint_state: header: @@ -255,13 +298,13 @@ transforms: [] twist: [] wrench: [] - name: Look at Table + name: Place Cube - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -270,23 +313,23 @@ 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 + - 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.06656709465883658 - - 0.31742962464478663 - - -0.07518824419393247 - - -0.7207568718596512 - - 0.8665668564352057 - - -1.6837956156664982 - - -1.5707507158288945 - - -0.07517349313281775 + - 0.7639616943900628 + - 0.5467729038282597 + - -1.1106700374717813 + - -0.6197663519343527 + - 1.2680653231682326 + - -2.801491408619309 + - -1.2801931991207176 + - -0.8417481239202634 velocity: [] multi_dof_joint_state: header: @@ -298,13 +341,13 @@ transforms: [] twist: [] wrench: [] - name: Above Pick Cube + name: VFC Start - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -313,23 +356,23 @@ 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 + - 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.06656709575770764 - - 0.2544520984236789 - - 0.00297097958947393 - - -0.5454836524482858 - - 1.028930149174636 - - -2.02088483587436 - - -1.5709145918870968 - - 0.002734726536987722 + - 0.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - 3.14 + - -0.0803135811027271 velocity: [] multi_dof_joint_state: header: @@ -341,13 +384,13 @@ transforms: [] twist: [] wrench: [] - name: Pick Cube + name: Wrist 2 Max - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -356,23 +399,23 @@ 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 + - 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.06656710337674145 - - 0.1162925924535442 - - 0.2054240147028693 - - -0.987070953194143 - - 1.3146849599773849 - - -1.8698622612624394 - - -1.5707050706852668 - - 0.20529114323213696 + - 0.039808309949994805 + - -0.5839327290315586 + - -0.2687381616263423 + - -1.4142103820838365 + - 1.1521752505644343 + - -1.3067562163163808 + - -1.5708040696131618 + - -0.2687661213277525 velocity: [] multi_dof_joint_state: header: @@ -384,4 +427,4 @@ transforms: [] twist: [] wrench: [] - name: Above Place Cube + name: Workspace Right diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml index 7c2e83b3..aeb30fe6 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml @@ -3,7 +3,7 @@ diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml index d280cbb4..9d1f99ed 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml @@ -2,7 +2,7 @@ diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml index 91194afe..14921bf6 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/open_gripper.xml @@ -3,7 +3,7 @@ Date: Wed, 11 Dec 2024 21:34:16 -0700 Subject: [PATCH 14/18] Added move beakers to burners --- src/lab_sim/description/picknik_ur.xacro | 1 + src/lab_sim/objectives/_scan_scene.xml | 2 +- .../constrained_pick_and_place_subtree.xml | 44 ++ .../objectives/constrained_pick_place.xml | 172 +----- src/lab_sim/waypoints/ur_waypoints.yaml | 549 ++++++++++++++++-- .../objectives/close_gripper.xml | 4 +- .../objectives/request_teleoperation.xml | 3 - 7 files changed, 588 insertions(+), 187 deletions(-) create mode 100644 src/lab_sim/objectives/constrained_pick_and_place_subtree.xml diff --git a/src/lab_sim/description/picknik_ur.xacro b/src/lab_sim/description/picknik_ur.xacro index 73a1f420..51ef9b86 100644 --- a/src/lab_sim/description/picknik_ur.xacro +++ b/src/lab_sim/description/picknik_ur.xacro @@ -1202,6 +1202,7 @@ 10 60 10 + true diff --git a/src/lab_sim/objectives/_scan_scene.xml b/src/lab_sim/objectives/_scan_scene.xml index b67089b7..1a345139 100644 --- a/src/lab_sim/objectives/_scan_scene.xml +++ b/src/lab_sim/objectives/_scan_scene.xml @@ -1,4 +1,4 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml index 1f90df87..20b445ed 100644 --- a/src/lab_sim/objectives/constrained_pick_place.xml +++ b/src/lab_sim/objectives/constrained_pick_place.xml @@ -1,9 +1,11 @@ - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - - + diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml index ed9275b1..cc75d18d 100644 --- a/src/lab_sim/waypoints/ur_waypoints.yaml +++ b/src/lab_sim/waypoints/ur_waypoints.yaml @@ -107,14 +107,14 @@ - wrist_2_joint - wrist_3_joint position: - - 0.06656709575770764 - - 0.2544520984236789 - - 0.00297097958947393 - - -0.5454836524482858 - - 1.028930149174636 - - -2.02088483587436 - - -1.5709145918870968 - - 0.002734726536987722 + - 0.4780667534172703 + - 0.3608351793573486 + - 0.2721933774717241 + - -0.650306028957599 + - 1.0215947637261753 + - -1.9877894219896062 + - -1.5762525416073718 + - 0.27265413670456373 velocity: [] multi_dof_joint_state: header: @@ -126,7 +126,136 @@ transforms: [] twist: [] wrench: [] - name: Pick Cube + name: Beaker 2b - Pick +- 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.43237578803980076 + - -0.19566474197031553 + - -0.10784891898904193 + - -0.5986453710405081 + - 0.836859710487763 + - -1.7815258791956854 + - -1.5640308592317707 + - -0.10710417727092728 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 2d - Place +- 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.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - 3.14 + - -0.0803135811027271 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Wrist 2 Max +- 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.3761774522488565 + - 0.43870516665145437 + - 0.27714879591368174 + - -0.7918551706608584 + - 1.3504894426360203 + - -2.118845440126674 + - -1.5602152969527976 + - 0.27792978541346414 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 1b - Pick - description: '' favorite: false joint_group_names: @@ -193,14 +322,14 @@ - wrist_2_joint - wrist_3_joint position: - - 0.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - -1.5575126426436692 - - -0.0803135811027271 + - 0.052588692632339035 + - 0.508478092656867 + - 0.09612551025010993 + - -0.9188673150202135 + - 0.7618566476251475 + - -1.4613747847293725 + - -1.5678602328067865 + - 0.09674250994554384 velocity: [] multi_dof_joint_state: header: @@ -212,7 +341,7 @@ transforms: [] twist: [] wrench: [] - name: Look at Table + name: Beaker 2a - Pre Pick - description: '' favorite: false joint_group_names: @@ -236,14 +365,14 @@ - wrist_2_joint - wrist_3_joint position: - - 0.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - -3.14 - - -0.0803135811027271 + - 0.052588724578849555 + - -0.220083115405952 + - -0.08486731500120634 + - -0.8090985472843552 + - 0.9401733527993958 + - -1.6870026013070798 + - -1.5638428818154486 + - -0.08408482009988492 velocity: [] multi_dof_joint_state: header: @@ -255,7 +384,50 @@ transforms: [] twist: [] wrench: [] - name: Wrist 2 Min + name: Beaker 1c - Pre Place +- 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.0525887259974598 + - 0.4843873562040059 + - 0.21286143674800728 + - -0.997345198223852 + - 1.232324799017508 + - -1.7964734087232395 + - -1.560638066013017 + - 0.21364257295602904 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 1a - Pre Pick - description: '' favorite: false joint_group_names: @@ -342,6 +514,264 @@ twist: [] wrench: [] name: VFC Start +- 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.039808309949994805 + - -0.5839327290315586 + - -0.2687381616263423 + - -1.4142103820838365 + - 1.1521752505644343 + - -1.3067562163163808 + - -1.5708040696131618 + - -0.2687661213277525 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Workspace Right +- 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.06656709575770764 + - 0.2544520984236789 + - 0.00297097958947393 + - -0.5454836524482858 + - 1.028930149174636 + - -2.02088483587436 + - -1.5709145918870968 + - 0.002734726536987722 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Pick Cube +- 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.05258871514731412 + - 0.6440121485168355 + - 0.16714441471809105 + - -0.6755456990514144 + - 1.1370276946741866 + - -2.002323479338112 + - -1.5611578708620235 + - 0.1685214966810631 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 3b - Pick +- 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.40327064214293146 + - 0.7682399980631509 + - 0.003716857923731027 + - -0.8146293536895327 + - 0.8180450803596784 + - -1.5468411626100742 + - -1.5636018761151254 + - 0.004528274675283207 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 3a - Pre Pick +- 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.38931073396967025 + - -0.290254322341971 + - 0.006658034411070151 + - -0.834082695846227 + - 1.3320548836178794 + - -2.0476903105098367 + - -1.5619927978151986 + - 0.007429431951791563 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 1d - Place +- 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.0525887106342127 + - -0.33400852907508416 + - -0.08923058620254333 + - -0.8882001373906337 + - 1.097074500743269 + - -1.7479934890388062 + - -1.5633601438176188 + - -0.08852801461312204 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 3c - Pre Place - description: '' favorite: false joint_group_names: @@ -371,7 +801,7 @@ - -1.1118986430431266 - 0.7735698103006698 - -1.2414135949062157 - - 3.14 + - -1.5575126426436692 - -0.0803135811027271 velocity: [] multi_dof_joint_state: @@ -384,7 +814,7 @@ transforms: [] twist: [] wrench: [] - name: Wrist 2 Max + name: Look at Table - description: '' favorite: false joint_group_names: @@ -408,14 +838,14 @@ - wrist_2_joint - wrist_3_joint position: - - 0.039808309949994805 - - -0.5839327290315586 - - -0.2687381616263423 - - -1.4142103820838365 - - 1.1521752505644343 - - -1.3067562163163808 - - -1.5708040696131618 - - -0.2687661213277525 + - 0.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - -3.14 + - -0.0803135811027271 velocity: [] multi_dof_joint_state: header: @@ -427,4 +857,47 @@ transforms: [] twist: [] wrench: [] - name: Workspace Right + name: Wrist 2 Min +- 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.51310923283404 + - -0.17182905886893637 + - -0.13540665247278597 + - -0.5979264893484634 + - 0.5671539457505681 + - -1.5175955738665268 + - -1.56464052041591 + - -0.13473683789696786 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Beaker 2c - Pre Place diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml index 9d1f99ed..35ba2fcd 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml @@ -1,3 +1,4 @@ + - - - diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml index 330d4fc6..59524894 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml @@ -152,9 +152,6 @@ - - - From 15f1393ffb6a3bc2c10058f2ab522ce6600956be Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Wed, 11 Dec 2024 22:20:11 -0800 Subject: [PATCH 15/18] Updating pick orientation --- src/lab_sim/description/picknik_ur.xacro | 2 +- .../constrained_pick_and_place_subtree.xml | 96 ++++- .../objectives/constrained_pick_place.xml | 4 +- src/lab_sim/objectives/my_constraints.yaml | 4 +- src/lab_sim/waypoints/ur_waypoints.yaml | 403 ++++++++++-------- src/picknik_accessories | 2 +- 6 files changed, 308 insertions(+), 203 deletions(-) diff --git a/src/lab_sim/description/picknik_ur.xacro b/src/lab_sim/description/picknik_ur.xacro index 51ef9b86..ba480db6 100644 --- a/src/lab_sim/description/picknik_ur.xacro +++ b/src/lab_sim/description/picknik_ur.xacro @@ -1202,7 +1202,7 @@ 10 60 10 - true + false diff --git a/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml index 8c3d3b07..5f17ebd0 100644 --- a/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml +++ b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml @@ -1,44 +1,106 @@ - + + - + - - + + - + - + - + - + - + - + - + - + - - - - + + + + - \ No newline at end of file + diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml index 20b445ed..78c15ca2 100644 --- a/src/lab_sim/objectives/constrained_pick_place.xml +++ b/src/lab_sim/objectives/constrained_pick_place.xml @@ -1,4 +1,4 @@ - + Date: Sat, 14 Dec 2024 16:27:02 -0800 Subject: [PATCH 16/18] updated waypoints and formatting --- src/lab_sim/waypoints/ur_waypoints.yaml | 869 +++++++++++------------- 1 file changed, 413 insertions(+), 456 deletions(-) diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml index e4cdd211..d4fc1c23 100644 --- a/src/lab_sim/waypoints/ur_waypoints.yaml +++ b/src/lab_sim/waypoints/ur_waypoints.yaml @@ -1,52 +1,9 @@ -- description: Move out of view of scene camera - 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.06656709575770764 - - 0.2544520984236789 - - 0.00297097958947393 - - -0.5454836524482858 - - 1.028930149174636 - - -2.02088483587436 - - -1.5709145918870968 - - 0.002734726536987722 - velocity: [] - multi_dof_joint_state: - header: - frame_id: '' - stamp: - nanosec: 0 - sec: 0 - joint_names: [] - transforms: [] - twist: [] - wrench: [] - name: Park Far Right - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -55,23 +12,23 @@ 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 + - 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.012213921836805967 - - -0.7406882588595464 - - 0.6961937479975829 - - 0.5000455178412782 - - -0.5449085497306905 - - -3.1415116041916686 - - -2.267353935370992 - - -0.026816314160348173 + - 0.5527237261550169 + - -0.8095633402493997 + - 0.8041978448752184 + - 0.3469595920821291 + - -0.4759133036391534 + - -3.0449635096222445 + - -2.3752901974217586 + - -0.03355586908682823 velocity: [] multi_dof_joint_state: header: @@ -87,9 +44,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -98,23 +55,23 @@ 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 + - 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.017720147765548155 - - -0.9435245955841107 - - 0.7892402388643184 - - -0.1531014838504317 - - 0.2798450400699416 - - -3.1415506151869534 - - -2.3563334644432694 - - 0.0981269858098807 + - 0.7639667711919623 + - -0.8205919083342896 + - 0.6106411457210819 + - 0.09629022558040691 + - -0.4356003871725801 + - -2.8163100209411844 + - -2.181786596029688 + - -0.027837851359850115 velocity: [] multi_dof_joint_state: header: @@ -130,9 +87,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -141,23 +98,23 @@ 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 + - 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.052558135449284636 - - 0.1015560185661144 - - 0.8742197383524934 - - 0.596069253342475 - - -0.5146934305221308 - - -3.1414852154720943 - - -2.4442469657377055 - - 0.06294310366684684 + - 0.052558135449284636 + - 0.1015560185661144 + - 0.8742197383524934 + - 0.596069253342475 + - -0.5146934305221308 + - -3.1414852154720943 + - -2.4442469657377055 + - 0.06294310366684684 velocity: [] multi_dof_joint_state: header: @@ -173,9 +130,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -184,23 +141,23 @@ 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 + - 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.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - -1.5575126426436692 - - -0.0803135811027271 + - 0.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - -3.14 + - -0.0803135811027271 velocity: [] multi_dof_joint_state: header: @@ -212,13 +169,13 @@ transforms: [] twist: [] wrench: [] - name: Look at Table + name: Wrist 2 Min - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -227,23 +184,23 @@ 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 + - 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.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - -3.14 - - -0.0803135811027271 + - 0.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - -1.5575126426436692 + - -0.0803135811027271 velocity: [] multi_dof_joint_state: header: @@ -255,13 +212,13 @@ transforms: [] twist: [] wrench: [] - name: Wrist 2 Min + name: Look at Table - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -270,23 +227,23 @@ 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 + - 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.7890780201148069 - - -0.8368030240080745 - - 0.7283319945923006 - - 0.22410851632014514 - - -0.8069932079887898 - - -2.58109618541478 - - -2.299721589861034 - - -0.009177004622370385 + - 0.5527279024765758 + - -0.9244326009773784 + - 0.8128532763795221 + - 0.19375908538144387 + - -0.4813430484021708 + - -2.882600666994581 + - -2.3839438626762592 + - -0.033962625761634384 velocity: [] multi_dof_joint_state: header: @@ -302,9 +259,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -313,23 +270,23 @@ 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 + - 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.012213833413987712 - - -0.7844649671754594 - - 0.7438765562939323 - - 0.22097141717523036 - - -0.4666236680564175 - - -2.9461361616730652 - - -2.315072230759138 - - -0.03003280763257616 + - 0.552721299565486 + - -0.8026817877894441 + - 0.7832655701857458 + - 0.2347983960726958 + - -0.47185366935901807 + - -2.944152059324353 + - -2.3543647658190983 + - -0.0324954245702678 velocity: [] multi_dof_joint_state: header: @@ -345,9 +302,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -356,23 +313,23 @@ 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 + - 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.7892070473993079 - - -0.8180372827842187 - - 0.7063304300241853 - - 0.6291518873938855 - - -0.7821086134090082 - - -3.0075222136655575 - - -2.2777806699439096 - - -0.00862197883061698 + - 0.017721560439610148 + - 0.2691509193711033 + - 0.7727615348146952 + - 0.5552397114893021 + - -0.5137325863770072 + - -3.0003468873078876 + - -2.335831248791311 + - 0.12529289653514364 velocity: [] multi_dof_joint_state: header: @@ -384,13 +341,13 @@ transforms: [] twist: [] wrench: [] - name: Beaker 2d - Place + name: Beaker 2b - Pick - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -399,23 +356,23 @@ 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 + - 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.017721560439610148 - - 0.2691509193711033 - - 0.7727615348146952 - - 0.5552397114893021 - - -0.5137325863770072 - - -3.0003468873078876 - - -2.335831248791311 - - 0.12529289653514364 + - 0.5527294577060091 + - -0.9178831799453755 + - 0.8032232530956076 + - 0.36305116959502753 + - -0.4822710241456316 + - -3.0466827988573497 + - -2.3743200968681815 + - -0.03356569648918818 velocity: [] multi_dof_joint_state: header: @@ -427,13 +384,13 @@ transforms: [] twist: [] wrench: [] - name: Beaker 2b - Pick + name: Beaker 2d - Place - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -442,23 +399,23 @@ 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 + - 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.5527346771001045 - - -0.9516220149816192 - - 0.8066871989063058 - - 0.14742263116956433 - - -0.010043045270869088 - - -3.141472609823535 - - -2.373450674734549 - - 0.10130553366836863 + - 0.36093425387337075 + - 0.3468469487674742 + - -0.08122232656648681 + - -1.1118986430431266 + - 0.7735698103006698 + - -1.2414135949062157 + - 3.14 + - -0.0803135811027271 velocity: [] multi_dof_joint_state: header: @@ -470,13 +427,13 @@ transforms: [] twist: [] wrench: [] - name: Beaker 3d - Place + name: Wrist 2 Max - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -485,23 +442,23 @@ 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 + - 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.36093425387337075 - - 0.3468469487674742 - - -0.08122232656648681 - - -1.1118986430431266 - - 0.7735698103006698 - - -1.2414135949062157 - - 3.14 - - -0.0803135811027271 + - 0.7639656639313784 + - -0.8204325716971375 + - 0.6051040370509869 + - 0.33021173155820605 + - -0.441945944944294 + - -3.047095890623255 + - -2.176250244653335 + - -0.02772141144783419 velocity: [] multi_dof_joint_state: header: @@ -513,13 +470,13 @@ transforms: [] twist: [] wrench: [] - name: Wrist 2 Max -- description: '' + name: Beaker 3d - Place +- description: Move out of view of scene camera favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -528,23 +485,23 @@ 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 + - 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.06656710337674145 - - 0.1162925924535442 - - 0.2054240147028693 - - -0.987070953194143 - - 1.3146849599773849 - - -1.8698622612624394 - - -1.5707050706852668 - - 0.20529114323213696 + - 0.00780498767879874 + - -1.7207208458780023 + - -2.9355287074907658e-05 + - -1.8849275826089145 + - 1.102019084998803 + - -1.2563366868253756 + - -1.5707679924921147 + - 1.5931846964135707e-07 velocity: [] multi_dof_joint_state: header: @@ -556,13 +513,13 @@ transforms: [] twist: [] wrench: [] - name: Above Place Cube -- description: Move out of view of scene camera + name: Park Far Right +- description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -571,23 +528,23 @@ 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 + - 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.00780498767879874 - - -1.7207208458780023 - - -2.9355287074907658e-05 - - -1.8849275826089145 - - 1.102019084998803 - - -1.2563366868253756 - - -1.5707679924921147 - - 1.5931846964135707e-07 + - 0.06656710337674145 + - 0.1162925924535442 + - 0.2054240147028693 + - -0.987070953194143 + - 1.3146849599773849 + - -1.8698622612624394 + - -1.5707050706852668 + - 0.20529114323213696 velocity: [] multi_dof_joint_state: header: @@ -599,13 +556,13 @@ transforms: [] twist: [] wrench: [] - name: Park Far Right + name: Above Place Cube - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -614,23 +571,23 @@ 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 + - 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.017721719550362367 - - 0.10614201950655074 - - 0.6667921047157305 - - -0.020173732725037447 - - -0.48719718634786974 - - -2.46144527046315 - - -2.2314189585794835 - - 0.10080448035214111 + - 0.01772162964006715 + - 0.07592482714796696 + - 0.7452091128812429 + - 0.5731708863903833 + - -0.5272977039075434 + - -3.0065755351034587 + - -2.308742635428776 + - 0.11856214137460191 velocity: [] multi_dof_joint_state: header: @@ -642,13 +599,13 @@ transforms: [] twist: [] wrench: [] - name: Beaker 3a - Pre Pick + name: Beaker 3b - Pick - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -657,23 +614,23 @@ 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 + - 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.01772162964006715 - - 0.07592482714796696 - - 0.7452091128812429 - - 0.5731708863903833 - - -0.5272977039075434 - - -3.0065755351034587 - - -2.308742635428776 - - 0.11856214137460191 + - 0.017721719550362367 + - 0.10614201950655074 + - 0.6667921047157305 + - -0.020173732725037447 + - -0.48719718634786974 + - -2.46144527046315 + - -2.2314189585794835 + - 0.10080448035214111 velocity: [] multi_dof_joint_state: header: @@ -685,13 +642,13 @@ transforms: [] twist: [] wrench: [] - name: Beaker 3b - Pick + name: Beaker 3a - Pre Pick - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -700,23 +657,23 @@ 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 + - 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.039808309949994805 - - -0.5839327290315586 - - -0.2687381616263423 - - -1.4142103820838365 - - 1.1521752505644343 - - -1.3067562163163808 - - -1.5708040696131618 - - -0.2687661213277525 + - 0.039808309949994805 + - -0.5839327290315586 + - -0.2687381616263423 + - -1.4142103820838365 + - 1.1521752505644343 + - -1.3067562163163808 + - -1.5708040696131618 + - -0.2687661213277525 velocity: [] multi_dof_joint_state: header: @@ -732,9 +689,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -743,23 +700,23 @@ 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 + - 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.7639616943900628 - - 0.5467729038282597 - - -1.1106700374717813 - - -0.6197663519343527 - - 1.2680653231682326 - - -2.801491408619309 - - -1.2801931991207176 - - -0.8417481239202634 + - 0.7639616943900628 + - 0.5467729038282597 + - -1.1106700374717813 + - -0.6197663519343527 + - 1.2680653231682326 + - -2.801491408619309 + - -1.2801931991207176 + - -0.8417481239202634 velocity: [] multi_dof_joint_state: header: @@ -775,9 +732,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -786,23 +743,23 @@ 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 + - 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.06656710453529632 - - -0.026311129005491703 - - 0.391188076851874 - - -0.6421557305136849 - - 1.2200227002982904 - - -2.1172063445999307 - - -1.5707036597762196 - - 0.39119709035689026 + - 0.06656710453529632 + - -0.026311129005491703 + - 0.391188076851874 + - -0.6421557305136849 + - 1.2200227002982904 + - -2.1172063445999307 + - -1.5707036597762196 + - 0.39119709035689026 velocity: [] multi_dof_joint_state: header: @@ -818,9 +775,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -829,23 +786,23 @@ 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 + - 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.012216800212355133 - - 0.04970573711721682 - - 0.8984514823861834 - - 0.1063125865651509 - - -0.41508272007941566 - - -2.751881697859383 - - -2.4680827148226405 - - 0.06614325541815676 + - 0.012216800212355133 + - 0.04970573711721682 + - 0.8984514823861834 + - 0.1063125865651509 + - -0.41508272007941566 + - -2.751881697859383 + - -2.4680827148226405 + - 0.06614325541815676 velocity: [] multi_dof_joint_state: header: @@ -861,9 +818,9 @@ - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -872,23 +829,23 @@ 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 + - 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.7888251346894957 - - 0.28410448263455473 - - 0.7522809111415887 - - 0.14258472176098788 - - -0.6567553362551677 - - -2.6535637271089603 - - -2.3237504598801015 - - -0.010010484715838389 + - 0.06656709465883658 + - 0.31742962464478663 + - -0.07518824419393247 + - -0.7207568718596512 + - 0.8665668564352057 + - -1.6837956156664982 + - -1.5707507158288945 + - -0.07517349313281775 velocity: [] multi_dof_joint_state: header: @@ -900,13 +857,13 @@ transforms: [] twist: [] wrench: [] - name: Beaker 2a - Pre Pick + name: Above Pick Cube - description: '' favorite: false joint_group_names: - - gripper - - linear_actuator - - manipulator + - gripper + - linear_actuator + - manipulator joint_state: effort: [] header: @@ -915,23 +872,23 @@ 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 + - 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.06656709465883658 - - 0.31742962464478663 - - -0.07518824419393247 - - -0.7207568718596512 - - 0.8665668564352057 - - -1.6837956156664982 - - -1.5707507158288945 - - -0.07517349313281775 + - 0.7888251346894957 + - 0.28410448263455473 + - 0.7522809111415887 + - 0.14258472176098788 + - -0.6567553362551677 + - -2.6535637271089603 + - -2.3237504598801015 + - -0.010010484715838389 velocity: [] multi_dof_joint_state: header: @@ -943,4 +900,4 @@ transforms: [] twist: [] wrench: [] - name: Above Pick Cube + name: Beaker 2a - Pre Pick From a8d310aad30e5ccd60d22b37f888dea21b1ec1dd Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Tue, 31 Dec 2024 14:06:23 -0800 Subject: [PATCH 17/18] added links to SRDF to fix objective --- docker-compose.yaml | 106 ---------------------- src/lab_sim/config/moveit/picknik_ur.srdf | 1 + 2 files changed, 1 insertion(+), 106 deletions(-) delete mode 100644 docker-compose.yaml diff --git a/docker-compose.yaml b/docker-compose.yaml deleted file mode 100644 index 2c94baf9..00000000 --- a/docker-compose.yaml +++ /dev/null @@ -1,106 +0,0 @@ -# Docker Compose file for user overlays of the MoveIt Pro images. - -services: - # Sets common properties for other services. Should not be instantiated directly. - base: - # Extend the installed MoveIt Pro docker compose file. - # Change this to match your environment, if MoveIt Pro was installed to a different location. - extends: - file: /opt/moveit_pro/docker-compose.yaml - service: base - image: moveit-pro-overlay - build: - context: . - target: user-overlay - args: - - USER_UID=${STUDIO_USER_UID:-1000} - - USER_GID=${STUDIO_USER_GID:-1000} - - USERNAME=${STUDIO_USERNAME:-studio-user} - - MOVEIT_STUDIO_BASE_IMAGE=picknikciuser/moveit-studio:${STUDIO_DOCKER_TAG:-main} - - # Starts the MoveIt Pro Agent and the Bridge between the Agent and the Web UI - agent_bridge: - extends: base - privileged: true - # This service relies on cgroup_rules defined in base which allow the user to use the host's network video4linux and usb_device devices. - depends_on: - rest_api: - condition: service_healthy - volumes: - # Allow the user to run graphical programs from within the docker container. - - /tmp/.X11-unix:/tmp/.X11-unix:ro - # Allow access to host hardware e.g. RealSense cameras - - /dev:/dev - deploy: - restart_policy: - condition: any - delay: 2s - command: agent_bridge.app - - # Starts the REST API for the Web UI. - rest_api: - extends: base - healthcheck: - test: "curl -f http://localhost:3200/objectives" - interval: 5s - timeout: 1m - command: rest_api.app - - # Starts the robot drivers. - drivers: - extends: base - privileged: true - # Ensures the drivers container has RT priority - ulimits: - rtprio: 99 - devices: - - "/dev/ttyUSB0:/dev/ttyUSB0" # Allow access to the gripper. - command: robot.app - - # Starts the web UI frontend. - web_ui: - image: picknikciuser/moveit-studio-frontend:${STUDIO_DOCKER_TAG:-main} - ports: - - "80:80" - network_mode: host - - # Starts RViz for visualization. - rviz: - extends: base - profiles: - - rviz - command: bash -c "ros2 launch moveit_studio_agent developer_rviz.launch.py" - - # Starts MoveIt Setup Assistant for creating MoveIt configurations. - setup_assistant: - extends: base - profiles: - - setup_assistant - command: bash -c "ros2 launch moveit_setup_assistant setup_assistant.launch.py" - - # Developer specific configuration - dev: - extends: base - build: - target: user-overlay-dev - image: moveit-studio-overlay-dev - stdin_open: true - tty: true - privileged: true - volumes: - # Mount the source code, colcon generated artifacts, and ccache - - ./src/:/home/${STUDIO_USERNAME:-studio-user}/user_ws/src:rw - - ./build/:/home/${STUDIO_USERNAME:-studio-user}/user_ws/build:rw - - ./install/:/home/${STUDIO_USERNAME:-studio-user}/user_ws/install:rw - - ./log/:/home/${STUDIO_USERNAME:-studio-user}/user_ws/log:rw - - ./.ccache/:/home/${STUDIO_USERNAME:-studio-user}/.ccache:rw - - ${HOME}/.ros/log_moveit_pro:/home/${STUDIO_USERNAME:-studio-user}/.ros/log - # Allow access to host hardware e.g. RealSense cameras - - /dev:/dev - command: sleep infinity - # Making a separate profile prevents this service from being built when using `docker compose build` - # and launched when using `docker compose up`. - profiles: ["dev"] - -volumes: - ignition_resources: diff --git a/src/lab_sim/config/moveit/picknik_ur.srdf b/src/lab_sim/config/moveit/picknik_ur.srdf index f0440417..0b6b1b2b 100644 --- a/src/lab_sim/config/moveit/picknik_ur.srdf +++ b/src/lab_sim/config/moveit/picknik_ur.srdf @@ -52,6 +52,7 @@ + From 651cc2cfd768699d9ef2f4780232869e70081b68 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Tue, 31 Dec 2024 16:54:35 -0800 Subject: [PATCH 18/18] updating metadata --- .../picknik_ur_base_config/objectives/close_gripper.xml | 5 ++++- .../objectives/request_teleoperation.xml | 3 +++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml index 7b8387e0..d280cbb4 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml @@ -2,7 +2,7 @@ @@ -16,5 +16,8 @@ + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml index 59524894..330d4fc6 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/request_teleoperation.xml @@ -152,6 +152,9 @@ + + +