Merge branch 'main' into ml-mtc-updates

PickNikRobotics · Mar 8, 2024 · a2a0e8a · a2a0e8a
2 parents b5aa748 + cdbcb9d
commit a2a0e8a
Show file tree

Hide file tree

Showing 24 changed files with 143 additions and 61 deletions.
diff --git a/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,28 +5,28 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+            <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
                 <Control ID="Sequence">
                     <!-- Pick object from "Grab", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Pick Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Place Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Grab", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Place Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Pick Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                 </Control>
             </Decorator>

diff --git a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
@@ -3,8 +3,8 @@
     <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
         <Decorator ID="KeepRunningUntilFailure">
             <Control ID="Sequence">
-                <Action ID="MoveToWaypoint" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
-                <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                <SubTree ID="Move to Named Pose" waypoint_name="Right Corner" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                <SubTree ID="Move to Named Pose" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             </Control>
         </Decorator>
     </BehaviorTree>

diff --git a/src/picknik_ur_base_config/objectives/move_to_named_pose.xml b/src/picknik_ur_base_config/objectives/move_to_named_pose.xml
@@ -2,10 +2,10 @@
 <root BTCPP_format="4" main_tree_to_execute="Move to Named Pose">
     <BehaviorTree ID="Move to Named Pose" _description="This Objective is used when moving to one of the saved waypoints in your site configuration">
         <Control ID="Sequence" name="root">
-            <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Home"/>
-            <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
+            <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
+            <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>
             <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-            <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="manipulator" task="{move_to_waypoint_task}"/>
+            <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" use_all_planners="{use_all_planners}" constraints="{constraints}" task="{move_to_waypoint_task}"/>
             <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
             <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
         </Control>

diff --git a/src/picknik_ur_base_config/waypoints/ur_waypoints.yaml b/src/picknik_ur_base_config/waypoints/ur_waypoints.yaml
@@ -1,6 +1,8 @@
 - name: Grab
   description: ""
   favorite: true
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -13,6 +15,8 @@
     effort: []
 - name: Look at Table
   favorite: true
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -25,6 +29,8 @@
     effort: []
 - name: Look at Pick and Place Zone
   favorite: true
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -37,6 +43,8 @@
     effort: []
 - name: Forward Down
   favorite: true
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -49,6 +57,8 @@
     effort: []
 - name: Right Corner
   favorite: true
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -61,6 +71,8 @@
     effort: []
 - name: Left Corner
   favorite: true
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -73,6 +85,8 @@
     effort: []
 - name: Home
   favorite: true
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -84,6 +98,8 @@
     velocity: []
     effort: []
 - name: Place
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -95,6 +111,8 @@
     velocity: []
     effort: []
 - name: Right Wall
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -106,6 +124,8 @@
     velocity: []
     effort: []
 - name: Left Wall
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -117,6 +137,8 @@
     velocity: []
     effort: []
 - name: Left Shelf
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -128,6 +150,8 @@
     velocity: []
     effort: []
 - name: Right Shelf
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -139,6 +163,8 @@
     velocity: []
     effort: []
 - name: Drop
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -150,6 +176,8 @@
     velocity: []
     effort: []
 - name: Place Left
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -161,6 +189,8 @@
     velocity: []
     effort: []
 - name: Place Right
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -172,6 +202,8 @@
     velocity: []
     effort: []
 - name: Place Block
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -183,6 +215,8 @@
     velocity: []
     effort: []
 - name: Pick Block
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -194,6 +228,8 @@
     velocity: []
     effort: []
 - name: Wrist 2 Min
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world
@@ -205,6 +241,8 @@
     velocity: []
     effort: []
 - name: Wrist 2 Max
+  joint_group_names:
+    - manipulator
   joint_state:
     header:
       frame_id: world

diff --git a/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
@@ -1,32 +1,32 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
     <!-- ////////// -->
-    <BehaviorTree ID="3 Waypoints Pick and Place" _description="Repeatedly pick up a small object, place it at desired destination, and then navigate to specified waypoint until failure">
+    <BehaviorTree ID="3 Waypoints Pick and Place" _description="Repeatedly grab a small object, place it at desired destination, and then navigate to specified waypoint until failure" _favorite="true">
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+            <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
                 <Control ID="Sequence">
                     <!-- Pick object from "Pick", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Pick" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Pick", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Pick" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <SubTree ID="Move to Named Pose" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                 </Control>
             </Decorator>

diff --git a/src/picknik_ur_gazebo_config/objectives/object_segmentation_2d.xml b/src/picknik_ur_gazebo_config/objectives/object_segmentation_2d.xml
@@ -3,7 +3,7 @@
   <!-- ////////// -->
   <BehaviorTree ID="Object Segmentation 2D" _description="Detect objects using a 2D image segmentation model." _favorite="false" _hardcoded="false">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" />
+      <SubTree ID="Move to Named Pose" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
       <Action ID="GetMasks2DAction" image="{image}" action_name="/get_masks_2d_maskrcnn" valid_classes="" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="-1" masks2d="{masks2d}"/>
     </Control>

diff --git a/src/picknik_ur_gazebo_config/objectives/object_segmentation_3d.xml b/src/picknik_ur_gazebo_config/objectives/object_segmentation_3d.xml
@@ -5,7 +5,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="ClearSnapshot"/>
       <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
-      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" />
+      <SubTree ID="Move to Named Pose" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
       <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
       <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>

diff --git a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
@@ -5,7 +5,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper"/>
-        <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+        <SubTree ID="Move to Named Pose" waypoint_name="Look at Pick and Place Zone" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       </Control>
       <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
       <Action ID="LoadObjectiveParameters" config_file_name="apriltag1_grasp_offset.yaml" parameters="{pose_parameters}"/>