Add mobile manipulation config (#339)

* Create mobile_manipulation_config package

* Add config file

* Add launch files

* Add waypoints

* Add objectives

---------

Co-authored-by: Matt Hansen <matt.hansen@picknik.ai>

src/mobile_manipulation_config/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.22)
+project(mobile_manipulation_config)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    launch
+    objectives
+    waypoints
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/mobile_manipulation_config/LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

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/launch/agent_bridge.launch.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
+    <include file="$(find-pkg-share moveit_studio_rest_api)/launch/rest_api.launch.xml"/>
+</launch>

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>