WIP: Add planner chain tutorial

CMakeLists.txt

@@ -69,6 +69,7 @@ add_subdirectory(doc/how_to_guides/parallel_planning)
 add_subdirectory(doc/how_to_guides/chomp_planner)
 add_subdirectory(doc/how_to_guides/persistent_scenes_and_states)
 add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner)
+add_subdirectory(doc/how_to_guides/pipeline_testbench)
 add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
 add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor)
 add_subdirectory(doc/tutorials/quickstart_in_rviz)

doc/concepts/motion_planning.rst

@@ -47,36 +47,30 @@ Pre-processing is useful in several situations, e.g. when a start state for the
 Post-processing is needed for several other operations, e.g. to convert paths generated for a robot into time-parameterized trajectories.
 MoveIt provides a set of default motion planning adapters that each perform a very specific function.
 
-FixStartStateBounds
+CheckStartStateBounds
 ^^^^^^^^^^^^^^^^^^^
 
 The fix start state bounds adapter fixes the start state to be within the joint limits specified in the URDF.
 The need for this adapter arises in situations where the joint limits for the physical robot are not properly configured.
 The robot may then end up in a configuration where one or more of its joints is slightly outside its joint limits.
 In this case, the motion planner is unable to plan since it will think that the starting state is outside joint limits.
-The "FixStartStateBounds" planning request adapter will "fix" the start state by moving it to the joint limit.
+The "CheckStartStateBounds" planning request adapter will "fix" the start state by moving it to the joint limit.
 However, this is obviously not the right solution every time - e.g. where the joint is really outside its joint limits by a large amount.
 A parameter for the adapter specifies how much the joint can be outside its limits for it to be "fixable".
 
-FixWorkspaceBounds
+ValidateWorkspaceBounds
 ^^^^^^^^^^^^^^^^^^
 
 The fix workspace bounds adapter will specify a default workspace for planning: a cube of size 10 m x 10 m x 10 m.
 This workspace will only be specified if the planning request to the planner does not have these fields filled in.
 
-FixStartStateCollision
+CheckStartStateCollision
 ^^^^^^^^^^^^^^^^^^^^^^
 
 The fix start state collision adapter will attempt to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount.
 The amount that it will perturb the values by is specified by the **jiggle_fraction** parameter that controls the perturbation as a percentage of the total range of motion for the joint.
 The other parameter for this adapter specifies how many random perturbations the adapter will sample before giving up.
 
-FixStartStatePathConstraints
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-This adapter is applied when the start state for a motion plan does not obey the specified path constraints.
-It will attempt to plan a path between the current configuration of the robot to a new location where the path constraint is obeyed.
-The new location will serve as the start state for planning.
 
 AddTimeParameterization
 ^^^^^^^^^^^^^^^^^^^^^^^

doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp

@@ -94,13 +94,13 @@ int main(int argc, char** argv)
   // Note that we are using the ROS pluginlib library here.
   std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
   planning_interface::PlannerManagerPtr planner_instance;
-  std::string planner_plugin_name;
+  std::vector<std::string> planner_plugin_names;
 
   // We will get the name of planning plugin we want to load
   // from the ROS parameter server, and then load the planner
   // making sure to catch all exceptions.
-  if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugin", planner_plugin_name))
-    RCLCPP_FATAL(LOGGER, "Could not find planner plugin name");
+  if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugins", planner_plugin_names))
+    RCLCPP_FATAL(LOGGER, "Could not find planner plugin names");
   try
   {
     planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
@@ -110,9 +110,11 @@ int main(int argc, char** argv)
   {
     RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
   }
+
+  const auto& planner_name = planner_plugin_names.at(0);
   try
   {
-    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
+    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_name));
     if (!planner_instance->initialize(robot_model, motion_planning_api_tutorial_node,
                                       motion_planning_api_tutorial_node->get_namespace()))
       RCLCPP_FATAL(LOGGER, "Could not initialize planner instance");
@@ -124,7 +126,7 @@ int main(int argc, char** argv)
     std::stringstream ss;
     for (const auto& cls : classes)
       ss << cls << " ";
-    RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_plugin_name.c_str(),
+    RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_name.c_str(),
                  ex.what(), ss.str().c_str());
   }
 
@@ -166,14 +168,14 @@ int main(int argc, char** argv)
   geometry_msgs::msg::PoseStamped pose;
   pose.header.frame_id = "panda_link0";
   pose.pose.position.x = 0.3;
-  pose.pose.position.y = 0.4;
+  pose.pose.position.y = 0.0;
   pose.pose.position.z = 0.75;
-  pose.pose.orientation.w = 1.0;
+  pose.pose.orientation.z = 1.0;
 
-  // A tolerance of 0.01 m is specified in position
-  // and 0.01 radians in orientation
-  std::vector<double> tolerance_pose(3, 0.01);
-  std::vector<double> tolerance_angle(3, 0.01);
+  // A tolerance of 0.1 m is specified in position
+  // and 0.1 radians in orientation
+  std::vector<double> tolerance_pose(3, 0.1);
+  std::vector<double> tolerance_angle(3, 0.1);
 
   // We will create the request as a constraint using a helper function available
   // from the
@@ -185,16 +187,28 @@ int main(int argc, char** argv)
   req.group_name = PLANNING_GROUP;
   req.goal_constraints.push_back(pose_goal);
 
+  // Define workspace bounds
+  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
+      req.workspace_parameters.min_corner.z = -5.0;
+  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
+      req.workspace_parameters.max_corner.z = 5.0;
+
   // We now construct a planning context that encapsulate the scene,
   // the request and the response. We call the planner using this
   // planning context
   planning_interface::PlanningContextPtr context =
       planner_instance->getPlanningContext(planning_scene, req, res.error_code);
+
+  if (!context)
+  {
+    RCLCPP_ERROR(LOGGER, "Failed to create planning context");
+    return -1;
+  }
   context->solve(res);
   if (res.error_code.val != res.error_code.SUCCESS)
   {
     RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
-    return 0;
+    return -1;
   }
 
   // Visualize the result
@@ -246,7 +260,7 @@ int main(int argc, char** argv)
   if (res.error_code.val != res.error_code.SUCCESS)
   {
     RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
-    return 0;
+    return -1;
   }
   /* Visualize the trajectory */
   res.getMessage(response);

doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp

@@ -141,6 +141,8 @@ int main(int argc, char** argv)
   req.pipeline_id = "ompl";
   req.planner_id = "RRTConnectkConfigDefault";
   req.allowed_planning_time = 1.0;
+  req.max_velocity_scaling_factor = 1.0;
+  req.max_acceleration_scaling_factor = 1.0;
   planning_interface::MotionPlanResponse res;
   geometry_msgs::msg::PoseStamped pose;
   pose.header.frame_id = "panda_link0";

doc/examples/moveit_cpp/config/moveit_cpp.yaml

@@ -14,5 +14,6 @@ planning_pipelines:
 plan_request_params:
   planning_attempts: 1
   planning_pipeline: ompl
+  planner_id: "RRTConnectkConfigDefault"
   max_velocity_scaling_factor: 1.0
   max_acceleration_scaling_factor: 1.0

doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py

@@ -11,6 +11,7 @@ def generate_launch_description():
         MoveItConfigsBuilder("moveit_resources_panda")
         .robot_description(file_path="config/panda.urdf.xacro")
         .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_pipelines("ompl", ["ompl"])
         .moveit_cpp(
             file_path=get_package_share_directory("moveit2_tutorials")
             + "/config/moveit_cpp.yaml"

doc/examples/planning_adapters/planning_adapters_tutorial.rst

@@ -6,7 +6,7 @@
 Planning Adapter Tutorials
 ==========================
 
-Planning Request Adapters is a concept in MoveIt which can be used to modify the trajectory (pre-processing and/or post-processing) for a motion planner. Some examples of existing planning adapters in MoveIt include AddTimeParameterization, FixWorkspaceBounds, FixStartBounds, FixStartStateCollision, FixStartStatePathConstraints, CHOMPOptimizerAdapter, etc. ! Using the concepts of Planning Adapters, multiple motion planning algorithms can be used in a pipeline to produce robust motion plans. For example, a sample pipeline of motion plans might include an initial plan produced by OMPL which can then be optimized by CHOMP to produce a motion plan which would likely be better than a path produced by OMPL or CHOMP alone. Similarly, using the concept of Planning Adapters, other motion planners can be mixed and matched depending on the environment the robot is operating in. This section provides a step by step tutorial on using a mix and match of different motion planners and also provides insights on when to use which particular motion planners.
+Planning Request Adapters is a concept in MoveIt which can be used to modify the trajectory (pre-processing and/or post-processing) for a motion planner. Some examples of existing planning adapters in MoveIt include AddTimeParameterization, ValidateWorkspaceBounds, FixStartBounds, CheckStartStateCollision, CHOMPOptimizerAdapter, etc. ! Using the concepts of Planning Adapters, multiple motion planning algorithms can be used in a pipeline to produce robust motion plans. For example, a sample pipeline of motion plans might include an initial plan produced by OMPL which can then be optimized by CHOMP to produce a motion plan which would likely be better than a path produced by OMPL or CHOMP alone. Similarly, using the concept of Planning Adapters, other motion planners can be mixed and matched depending on the environment the robot is operating in. This section provides a step by step tutorial on using a mix and match of different motion planners and also provides insights on when to use which particular motion planners.
 
 Getting Started
 ---------------
@@ -46,11 +46,10 @@ To achieve this, follow the steps:
 #. Open the ``ompl_planning_pipeline.launch`` file in the ``<robot_moveit_config>/launch`` folder of your robot. For the Panda robot it is this `file <https://github.com/ros-planning/panda_moveit_config/blob/melodic-devel/launch/ompl_planning_pipeline.launch.xml>`_. Edit this launch file, find the lines where ``<arg name="planning_adapters">`` is mentioned and change it to: ::
 
     <arg name="planning_adapters"
-         value="default_planner_request_adapters/AddTimeParameterization
-                default_planner_request_adapters/FixWorkspaceBounds
-                default_planner_request_adapters/FixStartStateBounds
-                default_planner_request_adapters/FixStartStateCollision
-                default_planner_request_adapters/FixStartStatePathConstraints
+         value="default_planning_request_adapters/AddTimeParameterization
+                default_planning_request_adapters/ValidateWorkspaceBounds
+                default_planning_request_adapters/CheckStartStateBounds
+                default_planning_request_adapters/CheckStartStateCollision
                 chomp/OptimizerAdapter" />
 
 #. The values of the ``planning_adapters`` is the order in which the mentioned adapters are called / invoked. Order here matters. Inside the CHOMP adapter, a call to OMPL is made before invoking the CHOMP optimization solver, so CHOMP takes the initial path computed by OMPL as the starting point to further optimize it.
@@ -81,11 +80,10 @@ To achieve this, follow the steps:
 
 #. Open the ``stomp_planning_pipeline.launch`` file in the ``<robot_moveit_config>/launch`` folder of your robot. For the Panda robot it is `this <https://github.com/ros-planning/panda_moveit_config/blob/melodic-devel/launch/stomp_planning_pipeline.launch.xml>`_ file. Edit this launch file, find the lines where ``<arg name="planning_adapters">`` is mentioned and change it to: ::
 
-    <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
-                   default_planner_request_adapters/FixWorkspaceBounds
-                   default_planner_request_adapters/FixStartStateBounds
-                   default_planner_request_adapters/FixStartStateCollision
-                   default_planner_request_adapters/FixStartStatePathConstraints
+    <arg name="planning_adapters" value="default_planning_request_adapters/AddTimeParameterization
+                   default_planning_request_adapters/ValidateWorkspaceBounds
+                   default_planning_request_adapters/CheckStartStateBounds
+                   default_planning_request_adapters/CheckStartStateCollision
                    chomp/OptimizerAdapter" />
 
 #. The values of the ``planning_adapters`` is the order in which the mentioned adapters are called / invoked. Order here matters. Inside the CHOMP adapter, a call to STOMP is made before invoking the CHOMP optimization solver, so CHOMP takes the initial path computed by STOMP as the starting point to further optimize it.
@@ -118,11 +116,10 @@ To achieve this, follow the steps:
 
 #. Open the ``ompl_planning_pipeline.launch`` file in the ``<robot_moveit_config>/launch`` folder of your robot. For the Panda robot it is this `file <https://github.com/ros-planning/panda_moveit_config/blob/melodic-devel/launch/ompl_planning_pipeline.launch.xml>`_. Edit this launch file, find the lines where ``<arg name="planning_adapters">`` is mentioned and change it to: ::
 
-    <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
-                   default_planner_request_adapters/FixWorkspaceBounds
-                   default_planner_request_adapters/FixStartStateBounds
-                   default_planner_request_adapters/FixStartStateCollision
-                   default_planner_request_adapters/FixStartStatePathConstraints
+    <arg name="planning_adapters" value="default_planning_request_adapters/AddTimeParameterization
+                   default_planning_request_adapters/ValidateWorkspaceBounds
+                   default_planning_request_adapters/CheckStartStateBounds
+                   default_planning_request_adapters/CheckStartStateCollision
                    stomp_moveit/StompSmoothingAdapter" />
 
 #. The values of the ``planning_adapters`` is the order in which the mentioned adapters are called / invoked. Order here matters. Inside the STOMP adapter, a call to OMPL is made before invoking the STOMP smoothing solver, so STOMP takes the initial path computed by OMPL as the starting point to further optimize it.

doc/examples/subframes/subframes_tutorial.rst

@@ -69,5 +69,5 @@ For older moveit_config packages that you have not generated yourself recently,
 required for subframes might not be configured, and the subframe link might not be found. To fix this for your
 moveit_config package, open the ``ompl_planning_pipeline.launch`` file in the ``<robot_moveit_config>/launch``
 folder of your robot. For the Panda robot it is :panda_codedir:`this <launch/ompl_planning_pipeline.launch.xml>` file.
-Edit this launch file, find the lines where ``<arg name="planning_adapters">`` is mentioned and insert ``default_planner_request_adapters/ResolveConstraintFrames`` after
-the line ``default_planner_request_adapters/FixStartStatePathConstraints``.
+Edit this launch file, find the lines where ``<arg name="planning_adapters">`` is mentioned and insert ``default_planning_request_adapters/ResolveConstraintFrames`` after
+the line ``default_planning_request_adapters/FixStartStatePathConstraints``.

doc/examples/time_parameterization/time_parameterization_tutorial.rst

@@ -34,7 +34,7 @@ Finally, add the Ruckig smoothing algorithm to the list of planning request adap
 
 .. code-block:: yaml
 
-      request_adapters: >-
-        default_planner_request_adapters/AddRuckigTrajectorySmoothing
-        default_planner_request_adapters/AddTimeOptimalParameterization
+      response_adapters:
+        - default_planning_request_adapters/AddRuckigTrajectorySmoothing
+        - default_planning_request_adapters/AddTimeOptimalParameterization
         ...

doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py

@@ -46,15 +46,20 @@ def generate_launch_description():
     # Load additional OMPL pipeline
     ompl_planning_pipeline_config = {
         "ompl_rrtc": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": """\
-                default_planner_request_adapters/AddTimeOptimalParameterization \
-                default_planner_request_adapters/FixWorkspaceBounds \
-                default_planner_request_adapters/FixStartStateBounds \
-                default_planner_request_adapters/FixStartStateCollision \
-                default_planner_request_adapters/FixStartStatePathConstraints \
-              """,
-            "start_state_max_bounds_error": 0.1,
+            "planning_plugins": [
+                "ompl_interface/OMPLPlanner",
+            ],
+            "request_adapters": [
+                "default_planning_request_adapters/ResolveConstraintFrames",
+                "default_planning_request_adapters/ValidateWorkspaceBounds",
+                "default_planning_request_adapters/CheckStartStateBounds",
+                "default_planning_request_adapters/CheckStartStateCollision",
+            ],
+            "response_adapters": [
+                "default_planning_response_adapters/AddTimeOptimalParameterization",
+                "default_planning_response_adapters/ValidateSolution",
+                "default_planning_response_adapters/DisplayMotionPath",
+            ],
         }
     }
     ompl_planning_yaml = load_yaml(