Create move_to_pose.xml (#17)

PickNikRobotics · May 22, 2023 · 9ffea4a · 9ffea4a
1 parent 3599463
commit 9ffea4a
Showing 1 changed file with 18 additions and 0 deletions.
diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Move to Pose">
+    <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
+        <Sequence>
+            <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
+            <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
+            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{move_to_pose.target_pose}" task="{move_to_pose_task}"/>
+            <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
+            <Fallback name="wait_for_approval_if_user_available">
+                <Inverter>
+                    <Action ID="IsUserAvailable" />
+                </Inverter>
+                <Action ID="WaitForUserTrajectoryApproval" solution="{move_to_pose_solution}" />
+            </Fallback>
+            <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+        </Sequence>
+    </BehaviorTree>
+</root>