move all MTC stage initializating into ForEach block

PickNikRobotics · Apr 22, 2024 · ee07fa0 · ee07fa0
1 parent 4dbd574
commit ee07fa0
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/src/picknik_ur_site_config/objectives/pick_object.xml b/src/picknik_ur_site_config/objectives/pick_object.xml
@@ -2,18 +2,18 @@
 <root BTCPP_format="4" main_tree_to_execute="Pick Object">
     <BehaviorTree ID="Pick Object" _description="Pick up and lift a small object" _favorite="true">
         <Control ID="Sequence" name="root">
+            <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
             <SubTree ID="Open Gripper"/>
             <Control ID="Sequence">
                 <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
                 <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
                 <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
                 <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
             </Control>
-            <Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
-            <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
-            <Action ID="SetupMTCCurrentState" task="{pick_object_task}"/>
             <Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{grasp_pose}">
                 <Control ID="Sequence">
+                    <Action ID="InitializeMTCTask" task_id="pick_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{pick_object_task}"/>
+                    <Action ID="SetupMTCCurrentState" task="{pick_object_task}"/>
                     <Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{pick_object_task}" parameters="{parameters}"/>
                     <Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
                     <SubTree ID="Wait for Trajectory Approval if User Available" solution="{pick_object_solution}"/>