add rrt-connect test Objective (#289)

PickNikRobotics · Jun 19, 2024 · bd793a1 · bd793a1
1 parent 8f56fb0
commit bd793a1
Showing 1 changed file with 19 additions and 0 deletions.
diff --git a/src/picknik_ur_mock_hw_config/objectives/rrt_connect_test.xml b/src/picknik_ur_mock_hw_config/objectives/rrt_connect_test.xml
@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="RRTConnect Test">
+  <!--//////////-->
+  <BehaviorTree ID="RRTConnect Test" _description="A test to move between waypoints using RRTConnect" _favorite="true" _hardcoded="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="ActivateControllers" controller_names="joint_trajectory_controller"/>
+      <Decorator ID="Repeat" num_cycles="10">
+        <Control ID="Sequence">
+          <Action ID="RetrieveWaypoint" waypoint_name="Grasp Left" waypoint_joint_state="{grasp_left}" joint_group_name="manipulator"/>
+          <Action ID="PlanToJointGoal" joint_goal="{grasp_left}"/>
+          <Action ID="ExecuteFollowJointTrajectory"/>
+          <Action ID="RetrieveWaypoint" waypoint_joint_state="{grasp_right}" joint_group_name="manipulator" waypoint_name="Grasp Right"/>
+          <Action ID="PlanToJointGoal" joint_goal="{grasp_right}"/>
+          <Action ID="ExecuteFollowJointTrajectory"/>
+        </Control>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>