Merge main changes into v6.0

.github/workflows/ci.yaml

@@ -18,6 +18,7 @@ jobs:
     uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@main
     with:
       image_tag: ${{ github.event.inputs.image_tag || null }}
+      colcon_test_args: "--executor sequential"
     secrets: inherit
 
   ensure-no-ssh-in-gitmodules:

src/picknik_ur_multi_arm_gazebo_config/CMakeLists.txt → src/mobile_manipulation_config/CMakeLists.txt

@@ -1,12 +1,11 @@
 cmake_minimum_required(VERSION 3.22)
-project(picknik_ur_multi_arm_gazebo_config)
+project(mobile_manipulation_config)
 
 find_package(ament_cmake REQUIRED)
 
 install(
   DIRECTORY
     config
-    description
     launch
     objectives
     waypoints

src/mobile_manipulation_config/README.md

@@ -0,0 +1,5 @@
+# mobile_manipulation_config
+
+A MoveIt Pro MuJoCo simulation of a UR5 arm on a mobile Ridgeback base.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/mobile_manipulation_config/config/config.yaml

@@ -0,0 +1,83 @@
+#
+#  This contains information for a unique instance of a robot.
+#
+
+# Name of the package to specialize
+based_on_package: "picknik_ur_mobile_config"
+hardware:
+  # Parameters used to configure the robot description through XACRO.
+  # A URDF and SRDF are both required.
+  # [Required]
+  robot_description:
+    urdf:
+      package: "picknik_ur_mobile_config"
+      path: "description/picknik_ur.xacro"
+    srdf:
+      package: "picknik_ur_mobile_config"
+      path: "config/moveit/picknik_ur.srdf"
+    urdf_params:
+      - joint_limits_parameters_file:
+          package: "picknik_ur_mobile_config"
+          path: "config/moveit/joint_limits.yaml"
+
+moveit_params:
+  servo:
+    package: "picknik_ur_mobile_config"
+    path: "config/moveit/ur_servo.yaml"
+  joint_limits:
+    package: "picknik_ur_mobile_config"
+    path: "config/moveit/joint_limits.yaml"
+  servo_joint_limits:
+    package: "picknik_ur_mobile_config"
+    path: "config/moveit/hard_joint_limits.yaml"
+
+# Configuration for loading behaviors and objectives.
+# [Required]
+objectives:
+  # List of plugins for loading custom behaviors.
+  # [Required]
+  behavior_loader_plugins:
+    # This plugin will load the core MoveIt Pro Behaviors.
+    # Add additional plugin loaders as needed.
+    core:
+      - "moveit_studio::behaviors::CoreBehaviorsLoader"
+      - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
+      - "moveit_studio::behaviors::ServoBehaviorsLoader"
+      - "moveit_studio::behaviors::VisionBehaviorsLoader"
+  # Specify source folder for objectives
+  # [Required]
+  objective_library_paths:
+    sim_objectives:
+      package_name: "mobile_manipulation_config"
+      relative_path: "objectives"
+  # Specify the location of the saved waypoints file.
+  # [Required]
+  waypoints_file:
+    package_name: "mobile_manipulation_config"
+    relative_path: "waypoints/ur_waypoints.yaml"
+
+
+# Configuration for launching ros2_control processes.
+# [Required, if using ros2_control]
+ros2_control:
+  config:
+    package: "picknik_ur_mobile_config"
+    path: "config/control/picknik_ur.ros2_control.yaml"
+  # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
+  # If not specified, it is up to the user to ensure the appropriate controllers are active and available
+  # for running the application.
+  # [Optional, default=[]]
+  controllers_active_at_startup:
+    - "force_torque_sensor_broadcaster"
+    - "robotiq_gripper_controller"
+    - "joint_state_broadcaster"
+    - "servo_controller"
+  # Load but do not start these controllers so they can be activated later if needed.
+  controllers_inactive_at_startup:
+    - "joint_trajectory_controller"
+  # Any controllers here will not be spawned by MoveIt Pro.
+  # [Optional, default=[]]
+  controllers_not_managed: []
+  # Optionally configure remapping rules to let multiple controllers receive commands on the same topic.
+  # [Optional, default=[]]
+  controller_shared_topics: []

src/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml

@@ -0,0 +1,9 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="00 Solution - Move to Initial Pose">
+  <!--//////////-->
+  <BehaviorTree ID="00 Solution - Move to Initial Pose" _description="Move to a pre-defined waypoint" _favorite="true" _hardcoded="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Move to Waypoint" waypoint_name="Look at Wall" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/01_solution_-_create_a_pose.xml

@@ -0,0 +1,9 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="01 Solution - Create a Pose">
+  <!--//////////-->
+  <BehaviorTree ID="01 Solution - Create a Pose" _description="Creates a pose relative to a frame" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/02_solution_-_visualize_a_pose.xml

@@ -0,0 +1,10 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="02 Solution - Visualize a Pose">
+  <!--//////////-->
+  <BehaviorTree ID="02 Solution - Visualize a Pose" _description="Creates a pose and visualizes it" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose" marker_size="0.200000"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/03_solution_-_pose_2m_above_the_robot.xml

@@ -0,0 +1,10 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="03 Solution - Pose 2m above the robot">
+  <!--//////////-->
+  <BehaviorTree ID="03 Solution - Pose 2m above the robot" _description="Creates a visualizes a pose 2m right above the robot 'base_link'" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;2" reference_frame="base_link"/>
+      <Action ID="VisualizePose" marker_size="0.200000"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/04_solution_-_move_to_a_pose.xml

@@ -0,0 +1,11 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="04 Solution - Move to a Pose">
+  <!--//////////-->
+  <BehaviorTree ID="04 Solution - Move to a Pose" _description="Creates a pose relative to the tool, and moves to it" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;1" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose" marker_size="0.200000"/>
+      <SubTree ID="Move to a StampedPose" _collapsed="true" target_pose="{stamped_pose}" controller_names="/joint_trajectory_controller"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/05_solution_-_define_a_path.xml

@@ -0,0 +1,12 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="05 Solution - Define a path">
+  <!--//////////-->
+  <BehaviorTree ID="05 Solution - Define a path" _description="Creates a simple path with two waypoints" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/06_solution_-_visualize_a_path.xml

@@ -0,0 +1,13 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="06 Solution - Visualize a path">
+  <!--//////////-->
+  <BehaviorTree ID="06 Solution - Visualize a path" _description="How to visualize a path" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/07_solution_-_plan_motion_for_a_path.xml

@@ -0,0 +1,12 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="07 Solution - Plan motion for a path">
+  <!--//////////-->
+  <BehaviorTree ID="07 Solution - Plan motion for a path" _description="Given a path, plans the motion for the robot joints" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/08_solution_-_execute_path_plan.xml

@@ -0,0 +1,13 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="08 Solution - Execute path plan">
+  <!--//////////-->
+  <BehaviorTree ID="08 Solution - Execute path plan" _description="Plans joint-space motion to track a path, and executes it" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0.2" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/09_solution_-_an_infeasible_path.xml

@@ -0,0 +1,12 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="09 Solution - An infeasible path">
+  <!--//////////-->
+  <BehaviorTree ID="09 Solution - An infeasible path" _description="An example of a path that is not feasible" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="1;0;-0.5" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/10_solution_-_visualize_path_error.xml

@@ -0,0 +1,15 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="10 Solution - Visualize path error">
+  <!--//////////-->
+  <BehaviorTree ID="10 Solution - Visualize path error" _description="How to visualize what's going on with an infeasible path" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="1;0;-0.5" reference_frame="world"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Control ID="Fallback">
+        <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      </Control>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/11_solution_-_draw_a_square.xml

@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="11 Solution - Draw a square">
+  <!--//////////-->
+  <BehaviorTree ID="11 Solution - Draw a square" _description="Hands-on exercise: draw a square" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+      <Action ID="PlanCartesianPath" ik_joint_space_density="0.01000" ik_cartesian_space_density="0.01000"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/12_solution_-_constrain_orientation.xml

@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="12 Solution - Constrain orientation">
+  <!--//////////-->
+  <BehaviorTree ID="12 Solution - Constrain orientation" _description="How to constrain end-effector orientation along paths" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0.4;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-0.4;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+      <Action ID="PlanCartesianPath" ik_joint_space_density="0.01000" ik_cartesian_space_density="0.01000" position_only="false"/>
+      <Action ID="ExecuteFollowJointTrajectory"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/13_solution_-_load_a_path_from_file.xml

@@ -0,0 +1,18 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="13 Solution - Load a path from file">
+  <!--//////////-->
+  <BehaviorTree ID="13 Solution - Load a path from file" _description="Loads a complex path from a file, and visualizes it" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="LoadPoseStampedVectorFromYaml" file_path="path.yaml"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;1;0" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose"/>
+      <Control ID="Parallel" success_count="1" failure_count="1">
+        <Action ID="PublishStaticFrame" child_frame_id="local"/>
+        <Control ID="Sequence">
+          <Action ID="WaitForDuration" delay_duration="0.5"/>
+          <Action ID="VisualizePath" path="{pose_stamped_msgs}"/>
+        </Control>
+      </Control>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/14_solution_-_load_from_file_execute.xml

@@ -0,0 +1,21 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="14 Solution - Load from file, execute">
+  <!--//////////-->
+  <BehaviorTree ID="14 Solution - Load from file, execute" _description="Loads a path from a file, plans motion and executes it" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree ID="Close Gripper" _collapsed="true"/>
+      <Action ID="LoadPoseStampedVectorFromYaml" file_path="path.yaml"/>
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;1;0" position_xyz="0;0;0" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose"/>
+      <Control ID="Parallel" success_count="1" failure_count="1">
+        <Action ID="PublishStaticFrame" child_frame_id="local"/>
+        <Control ID="Sequence">
+          <Action ID="WaitForDuration" delay_duration="0.5"/>
+          <Action ID="VisualizePath" path="{pose_stamped_msgs}"/>
+          <Action ID="PlanCartesianPath" path="{pose_stamped_msgs}" ik_joint_space_density="0.1000" ik_cartesian_space_density="0.01000" blending_radius="0.001" position_only="false" tip_offset="0;0;0.04;0;0;0"/>
+          <Action ID="ExecuteFollowJointTrajectory"/>
+        </Control>
+      </Control>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/15_solution_-_a_path_that_collides.xml

@@ -0,0 +1,13 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="15 Solution - A path that collides">
+  <!--//////////-->
+  <BehaviorTree ID="15 Solution - A path that collides" _description="An example of a path kinematically feasible, but colliding" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;5" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000" position_only="false"/>
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/16_solution_-_checking_for_collisions.xml

@@ -0,0 +1,17 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="16 Solution - Checking for collisions">
+  <!--//////////-->
+  <BehaviorTree ID="16 Solution - Checking for collisions" _description="How to check if a plan will collide" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;5" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000" position_only="false"/>
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
+      <Control ID="Fallback">
+        <Action ID="ValidateTrajectory" debug_solution="{debug_solution}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      </Control>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/17_solution_-_execute_the_feasible_path.xml

@@ -0,0 +1,18 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="17 Solution - Execute the feasible path">
+  <!--//////////-->
+  <BehaviorTree ID="17 Solution - Execute the feasible path" _description="" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;0;5" reference_frame="grasp_link"/>
+      <Action ID="AddPoseStampedToVector" input="{stamped_pose}"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="PlanCartesianPath" ik_cartesian_space_density="0.01000" ik_joint_space_density="0.01000" position_only="false"/>
+      <Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene_msg}"/>
+      <Control ID="Fallback">
+        <Action ID="ValidateTrajectory" debug_solution="{debug_solution}"/>
+        <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}"/>
+      </Control>
+      <Action ID="ExecuteFollowJointTrajectory" joint_trajectory_msg="{joint_trajectory_msg}"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/mobile_manipulation_config/objectives/18_solution_-_define_a_coverage_path.xml

@@ -0,0 +1,12 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="18 Solution - Define a coverage path">
+  <!--//////////-->
+  <BehaviorTree ID="18 Solution - Define a coverage path" _description="Automatically creates a path to cover a rectangular area" _favorite="true">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="CreateStampedPose" orientation_xyzw="0;0;0;1" position_xyz="0;-0.5;0.2" reference_frame="grasp_link"/>
+      <Action ID="VisualizePose"/>
+      <Action ID="GenerateCoveragePath" stride_distance="0.1" width="0.6" height="0.6" bottom_right_corner="{stamped_pose}" vector_of_poses="{pose_stamped_vector}"/>
+      <Action ID="VisualizePath" path="{pose_stamped_vector}"/>
+    </Control>
+  </BehaviorTree>
+</root>