From d141834135df68f63018305591fccce514c39820 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 12 Oct 2023 14:36:16 +0200 Subject: [PATCH] Update tutorials --- doc/concepts/motion_planning.rst | 6 ---- .../planning_adapters_tutorial.rst | 5 +-- .../time_parameterization_tutorial.rst | 6 ++-- .../launch/run_benchmarks.launch.py | 1 - .../chomp_planner/chomp_planner_tutorial.rst | 28 ----------------- .../parallel_planning_example.launch.py | 1 - .../stomp_planner/stomp_planner.rst | 31 +++++-------------- 7 files changed, 11 insertions(+), 67 deletions(-) diff --git a/doc/concepts/motion_planning.rst b/doc/concepts/motion_planning.rst index b10a48bd38..e0237bea5c 100644 --- a/doc/concepts/motion_planning.rst +++ b/doc/concepts/motion_planning.rst @@ -71,12 +71,6 @@ The fix start state collision adapter will attempt to sample a new collision-fre 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 ^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/examples/planning_adapters/planning_adapters_tutorial.rst b/doc/examples/planning_adapters/planning_adapters_tutorial.rst index b1635a5688..fe190a94b7 100644 --- a/doc/examples/planning_adapters/planning_adapters_tutorial.rst +++ b/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, FixWorkspaceBounds, FixStartBounds, FixStartStateCollision, 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 --------------- @@ -50,7 +50,6 @@ To achieve this, follow the steps: default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints 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 :moveit_codedir:`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. @@ -85,7 +84,6 @@ To achieve this, follow the steps: default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints 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. @@ -122,7 +120,6 @@ To achieve this, follow the steps: default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints 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. diff --git a/doc/examples/time_parameterization/time_parameterization_tutorial.rst b/doc/examples/time_parameterization/time_parameterization_tutorial.rst index 418c1e26c7..6493f56127 100644 --- a/doc/examples/time_parameterization/time_parameterization_tutorial.rst +++ b/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_planner_request_adapters/AddRuckigTrajectorySmoothing + - default_planner_request_adapters/AddTimeOptimalParameterization ... diff --git a/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py b/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py index ebf3d07ffe..0e22a949df 100644 --- a/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py +++ b/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py @@ -52,7 +52,6 @@ def generate_launch_description(): 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, } diff --git a/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst b/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst index 9c401b427a..4baa791e0d 100644 --- a/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst +++ b/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst @@ -98,31 +98,3 @@ OMPL is a open source library for sampling based / randomized motion planning al CHOMP: While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, CHOMP capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamic quantities such as joint velocities and accelerations. It rapidly converges to a smooth, collision-free trajectory that can be executed efficiently on the robot. A covariant update rule ensures that CHOMP quickly converges to a locally optimal trajectory. For scenes containing obstacles, CHOMP often generates paths which do not prefer smooth trajectories by addition of some noise (*ridge_factor*) in the cost function for the dynamic quantities of the robot (like acceleration, velocity). CHOMP is able to avoid obstacles in most cases, but it can fail if it gets stuck in local minima due to a bad initial guess for the trajectory. OMPL can be used to generate collision-free seed trajectories for CHOMP to mitigate this issue. - -Using CHOMP as a post-processor for OMPL ----------------------------------------- -Here, we will demonstrate that CHOMP can also be used as a post-processing optimization technique for plans obtained by other planning algorithms. The intuition behind this is that some randomized planning algorithm produces an initial guess for CHOMP. CHOMP then takes this initial guess and further optimizes the trajectory. -To achieve this, use the following steps: - -#. Edit ``ompl_planning.yaml`` in the ``/config`` folder of your robot. Add ``chomp/OptimizerAdapter`` to the bottom of the list of request_adapters: :: - - request_adapters: >- - ... - default_planner_request_adapters/FixStartStatePathConstraints - chomp/OptimizerAdapter - -#. Change the ``trajectory_initialization_method`` parameter in ``chomp_planning.yaml`` to ``fillTrajectory`` so that OMPL can provide the input for the CHOMP algorithm: :: - - trajectory_initialization_method: "fillTrajectory" - -#. Add the CHOMP config file to the launch file of your robot, ``/launch/chomp_demo.launch.py``, if it is not there already: :: - - .planning_pipelines(pipelines=["ompl", "chomp"]) - -#. Now you can launch the newly configured planning pipeline as follows: :: - - ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True - -This will launch RViz. Select OMPL in the Motion Planning panel under the Context tab. Set the desired start and goal states by moving the end-effector around in the same way as was done for CHOMP above. Finally click on the Plan button to start planning. The planner will now first run OMPL, then run CHOMP on OMPL's output to produce an optimized path. To make the planner's task more challenging, add obstacles to the scene using: :: - - ros2 run moveit2_tutorials collision_scene_example diff --git a/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py b/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py index b7362d3ee7..2c14a701fe 100644 --- a/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py +++ b/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py @@ -63,7 +63,6 @@ def launch_setup(context, *args, **kwargs): 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, } diff --git a/doc/how_to_guides/stomp_planner/stomp_planner.rst b/doc/how_to_guides/stomp_planner/stomp_planner.rst index f99c31cfc8..cf66e58a1b 100644 --- a/doc/how_to_guides/stomp_planner/stomp_planner.rst +++ b/doc/how_to_guides/stomp_planner/stomp_planner.rst @@ -32,12 +32,13 @@ Using STOMP with Your Robot #. Simply add the `stomp_planning.yaml `__ configuration file into the config directory of your MoveIt config package. It contains the plugin identifier, a planning pipeline adapter list, and the STOMP planning parameters. The config file should look like example below: :: planning_plugin: stomp_moveit/StompPlanner - 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 + request_adapters: + - default_planner_request_adapters/ResolveConstraintFrames + - default_planner_request_adapters/FixWorkspaceBounds + - default_planner_request_adapters/FixStartStateBounds + - default_planner_request_adapters/FixStartStateCollision + response_adapters: + - default_planner_response_adapters/AddTimeOptimalParameterization stomp_moveit: num_timesteps: 60 @@ -51,24 +52,6 @@ Using STOMP with Your Robot #. Configure MoveIt to load the STOMP planning pipeline by adding "stomp" to your MoveItConfiguration launch statement next to "ompl" and the other planners. You can find an example for this in the `demo.launch.py `_ of the Panda config. -Using STOMP's planner adapter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -STOMP can also be used for smoothing and optimizing trajectories from other planner plugins using the ``StompSmoothingAdapter`` plugin. -The only step needed is to add the plugin name ``stomp_moveit/StompSmoothingAdapter`` to the ``request_adapters`` parameter list configured for the planning pipeline: :: - - 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 - stomp_moveit/StompSmoothingAdapter - -In addition, STOMP parameters can be specified just like for the usual planning setup. -An important detail is that now the parameter ``num_iterations_after_valid`` is used for specifying the smoothing steps since the input trajectory is already valid. -It should therefore be larger than 0 to have an effect. - Running the Demo ---------------- If you have the ``panda_moveit_config`` from the `ros-planning/moveit_resources `_ repository you should be able to simply launch the demo setup and start planning with STOMP in RViZ ::