Tutorial on OMPL's Constrained State Space for position or orientation constraints

doc/ompl_constrained_planning/CMakeLists.txt

@@ -0,0 +1,4 @@
+install(PROGRAMS
+  scripts/ompl_constrained_planning_tutorial.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

doc/ompl_constrained_planning/ompl_constrained_planning.rst

@@ -0,0 +1,85 @@
+OMPL Constrained Planning
+=========================
+.. image:: ompl_constrained_planning_header.png
+   :width: 600px
+
+This tutorial shows you how to use OMPL's `Constrained planning capabilities`_ from MoveIt. To illustrate the capabilities of this planner, three planning problems are solved with different types of path constraints. It will be easier to follow if you have read through the tutorial on the `move group Python interface`_.
+
+For a short walkthrough of running the tutorial, you can watch `this video`_.
+
+When can I use this planner?
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+The interface currently only supports position constraints on any link of the robot, where the constrained regions is represented using a box_. The planning approach provides an alternative for the `enforce_joint_model_state_space`_ option. It is expected to be most valuable for constraints regions that have a small (or zero) volume in Cartesian space, where the rejection sampling does not always works. For example, keeping the end-effector on a plane or along a line.
+
+Configure OMPL
+^^^^^^^^^^^^^^^^
+OMPL reads configurations parameters from a file called :code:`ompl_planning.yaml`. This tutorial uses the Panda robot, for which this file can be found in :code:`panda_moveit_config/config/ompl_planning.yaml`. We will add a parameter to tell OMPL to plan in a constrained state space by setting :code:`enforce_constrained_state_space`. In addition, if the parameter `projection_evaluator`_ was not yet specified we also need to add it.
+
+.. code-block:: yaml
+
+   panda_arm:
+      enforce_constrained_state_space: true
+      projection_evaluator: joints(panda_joint1,panda_joint2)
+
+Note that, as we are changing configuration files, you should also clone the repository `panda_moveit_config`_ in you're workspace. (Instead of using the version installed with :code:`sudo apt install`.
+
+.. code-block:: bash
+
+  cd catkin_ws/src
+  git clone https://github.com/ros-planning/panda_moveit_config.git
+  cd ../..
+  catkin build
+
+Run the tutorial
+^^^^^^^^^^^^^^^^
+
+After you change the configuration as explained in the previous section, launch the move group node for the panda robot: ::
+
+   roslaunch panda_moveit_config demo.launch
+
+Then add a Marker display to Rviz:
+
+.. image:: rviz_add_marker_topic.png
+   :width: 200px
+
+Open a new terminal window to run the tutorial node: ::
+
+   rosrun moveit_tutorials ompl_constrained_planning_tutorial.py
+
+A red and green sphere should appear in Rviz to show the start and goal states respectively. In addition, a grey box should appear that represents the position constraint on the link :code:`panda_link8`. If planning succeeds, you should see a preview of the trajectory that was planned.
+
+.. image:: case_1.gif
+   :width: 300px
+
+The following message appears in the terminal: ::
+
+   ============ Press enter to continue with the second planning problem.
+
+After pressing enter, the next planning problem is solved.
+
+.. image:: case_2.gif
+   :width: 300px
+
+Again, if planning succeeds, the trajectory is animated in rviz. And finally the last planning problem is solved after pressing enter again.
+
+.. image:: case_3.gif
+   :width: 300px
+
+
+To see the output from the planner, look in the terminal window where you launched the Panda's move group node. To replay the planned trajectory, you can add a "Trajectory Slider" panel in Rviz.
+
+.. image:: trajectory_slider.png
+   :width: 200px
+
+The code explained
+^^^^^^^^^^^^^^^^^^
+
+.. tutorial-formatter:: ./scripts/ompl_constrained_planning_tutorial.py
+
+.. _this video: https://youtu.be/RkPydgtIq-M
+.. _panda_moveit_config: https://github.com/ros-planning/panda_moveit_config
+.. _Constrained planning capabilities: http://ompl.kavrakilab.org/constrainedPlanning.html
+.. _move group Python interface: ../move_group_python_interface/move_group_python_interface_tutorial.html
+.. _box: http://docs.ros.org/latest/api/shape_msgs/html/msg/SolidPrimitive.html
+.. _enforce_joint_model_state_space: ../ompl_interface/ompl_interface_tutorial.html#enforce-planning-in-joint-space
+.. _projection_evaluator: ../ompl_interface/ompl_interface_tutorial.html#projection-evaluator

doc/ompl_constrained_planning/scripts/extra_examples.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2020, KU Leuven
+# All rights reserved.
+#
+# 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 KU Leuven 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 OWNER 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.
+#
+# Author: Jeroen De Maeyer
+
+from __future__ import print_function
+from six.moves import input  # Python 3 compatible alternative for raw_input
+
+from ompl_constrained_planning_tutorial import ConstrainedPlanningTutorial
+
+
+def solve(move_group, start_state, pose_goal, path_constraints):
+    """ Convenience function not used by the main tutorial """
+    move_group.set_start_state(start_state)
+    move_group.set_pose_target(pose_goal)
+
+    # Don't forget the path constraints! That's the whole point of this tutorial.
+    move_group.set_path_constraints(path_constraints)
+
+    # And let the planner find a solution.
+    # The move_group node should automatically visualize the solution in Rviz if a path is found.
+    move_group.plan()
+
+    # Clear the path constraints for our next experiment
+    move_group.clear_path_constraints()
+
+
+def run_vertical_plane_example():
+    """ Run an example where we want to keep the end-effector on a vertical plane. """
+    tutorial = ConstrainedPlanningTutorial()
+    tutorial.remove_all_markers()
+
+    tutorial.add_obstacle()
+
+    start_state = tutorial.create_start_state()
+    pose_goal = tutorial.create_pose_goal_under_obstacle()
+    pcm = tutorial.create_vertical_plane_constraints()
+
+    # We need two wrap the constraints in a generic `Constraints` message.
+    path_constraints = moveit_msgs.msg.Constraints()
+    path_constraints.position_constraints.append(pcm)
+    path_constraints.name = "use_equality_constraints"
+
+    tutorial.solve(start_state, pose_goal, path_constraints)
+
+    print("============ Press enter to continue with the second planning problem.")
+    input()
+    tutorial.remove_all_markers()
+    tutorial.remove_obstacle()
+    print("Done!")
+
+
+def main():
+    """ Catch interupt when the user presses `ctrl-c`. """
+    try:
+        run_vertical_plane_example()
+    except KeyboardInterrupt:
+        return
+
+
+if __name__ == "__main__":
+    main()