Remove local PickObject subtree

PickNikRobotics · Nov 13, 2023 · 08e5171 · 08e5171
1 parent 6718a4b
commit 08e5171
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 11 deletions.
diff --git a/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
@@ -13,7 +13,7 @@
         <input_port name="max_rotation" default="0.2">The maximum angle allow between the average pose and the new sample (shortest angle between two pose quaternions).</input_port>
         <output_port name="avg_pose" default="{avg_pose}">The calculated average pose.</output_port>
     </SubTree>
-    <SubTree ID="PickObject">
+    <SubTree ID="Pick Object">
         <description>
           <p>
               Picks an object at the specified grasp pose.

diff --git a/src/picknik_ur_site_config/objectives/pick_object.xml b/src/picknik_ur_site_config/objectives/pick_object.xml
@@ -2,13 +2,11 @@
 <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">
-            <SubTree ID="OpenGripper"/>
-            <Action ID="GetPoseFromUser" parameter_name="pick_object.grasp_pose" parameter_value="{grasp_pose}" />
-            <SubTree ID="PickObject" grasp_pose="{grasp_pose}"/>
-        </Control>
-    </BehaviorTree>
-    <BehaviorTree ID="PickObject">
-        <Control ID="Sequence" name="pick_object_main">
+            <SubTree ID="Open Gripper"/>
+            <!-- Wrap in a ForceSuccess decorator so this can be used as both a top-level tree and subtree -->
+            <Decorator ID="ForceSuccess">
+                <Action ID="GetPoseFromUser" parameter_name="pick_object.grasp_pose" parameter_value="{grasp_pose}" />
+            </Decorator>
             <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}"/>
@@ -23,7 +21,4 @@
             <Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
         </Control>
     </BehaviorTree>
-    <BehaviorTree ID="OpenGripper">
-        <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-    </BehaviorTree>
 </root>