diff --git a/CMakeLists.txt b/CMakeLists.txt index b79af3aa35..8be1b127bd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,6 +82,7 @@ add_subdirectory(doc/examples/moveit_cpp) # add_subdirectory(doc/examples/bullet_collision_checker) add_subdirectory(doc/examples/realtime_servo) add_subdirectory(doc/how_to_guides/isaac_panda) +add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/_static/videos/ompl_constraints_box.webm b/_static/videos/ompl_constraints_box.webm new file mode 100644 index 0000000000..b44be0723b Binary files /dev/null and b/_static/videos/ompl_constraints_box.webm differ diff --git a/_static/videos/ompl_constraints_line.webm b/_static/videos/ompl_constraints_line.webm new file mode 100644 index 0000000000..35428cc707 Binary files /dev/null and b/_static/videos/ompl_constraints_line.webm differ diff --git a/_static/videos/ompl_constraints_orientation.webm b/_static/videos/ompl_constraints_orientation.webm new file mode 100644 index 0000000000..6a0eff034b Binary files /dev/null and b/_static/videos/ompl_constraints_orientation.webm differ diff --git a/_static/videos/ompl_constraints_plane.webm b/_static/videos/ompl_constraints_plane.webm new file mode 100644 index 0000000000..49853fec1a Binary files /dev/null and b/_static/videos/ompl_constraints_plane.webm differ diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 09ee873d7e..8205baf4d0 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -63,7 +63,6 @@ Configuration kinematics_configuration/kinematics_configuration_tutorial custom_constraint_samplers/custom_constraint_samplers_tutorial ompl_interface/ompl_interface_tutorial - ompl_constrained_planning/ompl_constrained_planning chomp_planner/chomp_planner_tutorial stomp_planner/stomp_planner_tutorial trajopt_planner/trajopt_planner_tutorial diff --git a/doc/examples/ompl_constrained_planning/CMakeLists.txt b/doc/examples/ompl_constrained_planning/CMakeLists.txt deleted file mode 100644 index 2f32734794..0000000000 --- a/doc/examples/ompl_constrained_planning/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -install(PROGRAMS - scripts/ompl_constrained_planning_tutorial.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/doc/examples/ompl_constrained_planning/ompl_constrained_planning.rst b/doc/examples/ompl_constrained_planning/ompl_constrained_planning.rst deleted file mode 100644 index 43c0aad421..0000000000 --- a/doc/examples/ompl_constrained_planning/ompl_constrained_planning.rst +++ /dev/null @@ -1,89 +0,0 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - -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 -.. _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 diff --git a/doc/examples/ompl_constrained_planning/rviz_add_marker_topic.png b/doc/examples/ompl_constrained_planning/rviz_add_marker_topic.png deleted file mode 100644 index ee896d6594..0000000000 Binary files a/doc/examples/ompl_constrained_planning/rviz_add_marker_topic.png and /dev/null differ diff --git a/doc/examples/ompl_constrained_planning/scripts/extra_examples.py b/doc/examples/ompl_constrained_planning/scripts/extra_examples.py deleted file mode 100644 index 31e13d0e6a..0000000000 --- a/doc/examples/ompl_constrained_planning/scripts/extra_examples.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/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 interrupt when the user presses `ctrl-c`.""" - try: - run_vertical_plane_example() - except KeyboardInterrupt: - return - - -if __name__ == "__main__": - main() diff --git a/doc/examples/ompl_constrained_planning/scripts/ompl_constrained_planning_tutorial.py b/doc/examples/ompl_constrained_planning/scripts/ompl_constrained_planning_tutorial.py deleted file mode 100755 index 07b47a592a..0000000000 --- a/doc/examples/ompl_constrained_planning/scripts/ompl_constrained_planning_tutorial.py +++ /dev/null @@ -1,492 +0,0 @@ -#!/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 - -import sys -import rospy -import moveit_commander -import moveit_msgs.msg -import sensor_msgs.msg -import geometry_msgs.msg -import shape_msgs.msg -import visualization_msgs.msg -import std_msgs.msg - -from math import pi -from tf.transformations import quaternion_from_euler, quaternion_multiply -from six.moves import input # Python 3 compatible alternative for raw_input - -# define some colours for convenience -COLOR_RED = std_msgs.msg.ColorRGBA(1.0, 0.0, 0.0, 1.0) -COLOR_GREEN = std_msgs.msg.ColorRGBA(0.0, 1.0, 0.0, 1.0) -COLOR_TRANSLUCENT = std_msgs.msg.ColorRGBA(0.0, 0.0, 0.0, 0.5) - - -## BEGIN_SUB_TUTORIAL setup -## -## Setup a RobotCommander and a MoveGroupCommander. -## Everything is wrappen in a class to make it easily reusable. -## -class ConstrainedPlanningTutorial(object): - """Wrapper class for the tutorial.""" - - def __init__(self, group_name="panda_arm"): - """Start the ROS node and create object to handle the robot and the planning scene.""" - - moveit_commander.roscpp_initialize(sys.argv) - rospy.init_node("ompl_constrained_planning_example", anonymous=True) - - self.robot = moveit_commander.RobotCommander() - self.move_group = moveit_commander.MoveGroupCommander(group_name) - self.scene = moveit_commander.PlanningSceneInterface() - - # Create a publisher to visualize the position constraints in Rviz - self.marker_publisher = rospy.Publisher( - "/visualization_marker", - visualization_msgs.msg.Marker, - queue_size=20, - ) - rospy.sleep(0.5) # publisher needs some time to connect Rviz - self.remove_all_markers() - self.marker_id_counter = 0 # give each marker a unique idea - - # Save some commenly used variables in the setup class - self.ref_link = self.move_group.get_pose_reference_frame() - self.ee_link = self.move_group.get_end_effector_link() - self.obstacle_name = "obstacle" - - ## END_SUB_TUTORIAL - - ## BEGIN_SUB_TUTORIAL start_state - ## Instead of using the current robot state as start state, we use a fixed state for the panda robot defined in the srdf config file. - ## The :code:`get_named_target_values` returns a dictionary with joint names and values for the "ready" position. - def create_start_state(self, named_target="ready"): - ready = self.move_group.get_named_target_values(named_target) - - # Now create a robot state from these joint positions - joint_state = sensor_msgs.msg.JointState() - joint_state.header.stamp = rospy.Time.now() - joint_state.header.frame_id = self.move_group.get_pose_reference_frame() - joint_state.name = [key for key in ready.keys()] - joint_state.position = [val for val in ready.values()] - - state = moveit_msgs.msg.RobotState() - state.joint_state = joint_state - - return state - - ## END_SUB_TUTORIAL - - ## BEGIN_SUB_TUTORIAL pose_goal - ## - ## To keep things simple, we use the current end-effector pose to quickly create a reasonable goal. - ## We also visualize the start and goal position of the end-effector in Rviz with a simple sphere. - ## We assume that when the demo.launch file for the panda robot is launched, the robot is in the "ready" position. - def create_pose_goal(self): - self.move_group.clear_pose_targets() - pose = self.move_group.get_current_pose() - - self.display_sphere(pose.pose, color=COLOR_RED) - - pose.pose.position.y += 0.3 - pose.pose.position.z -= 0.3 - - self.display_sphere(pose.pose) - - return pose - - ## For the second planning problem with the tilted plane, we need to create a pose goal that lies in this plane. - ## The plane is tilted by 45 degrees, so moving an equal amount in the y and z direction should be ok. - def create_pose_goal_in_plane(self): - self.move_group.clear_pose_targets() - pose = self.move_group.get_current_pose() - - self.display_sphere(pose.pose, color=COLOR_RED) - - pose.pose.position.x += 0.2 - pose.pose.position.y += 0.3 - pose.pose.position.z -= 0.3 - - self.display_sphere(pose.pose) - - return pose - - ## END_SUB_TUTORIAL - - ## BEGIN_SUB_TUTORIAL pos_con - ## First we create simple box constraints on the current end-effector link (:code:`self.ee_link = "panda_link8"`). - def create_simple_box_constraints(self): - pcm = moveit_msgs.msg.PositionConstraint() - pcm.header.frame_id = self.ref_link - pcm.link_name = self.ee_link - - cbox = shape_msgs.msg.SolidPrimitive() - cbox.type = shape_msgs.msg.SolidPrimitive.BOX - cbox.dimensions = [0.1, 0.4, 0.4] - pcm.constraint_region.primitives.append(cbox) - - current_pose = self.move_group.get_current_pose() - - cbox_pose = geometry_msgs.msg.Pose() - cbox_pose.position.x = current_pose.pose.position.x - cbox_pose.position.y = 0.15 - cbox_pose.position.z = 0.45 - cbox_pose.orientation.w = 1.0 - pcm.constraint_region.primitive_poses.append(cbox_pose) - - # display the constraints in rviz - self.display_box(cbox_pose, cbox.dimensions) - - return pcm - - ## If you make a box really thin along one dimension, you get something plane like. - ## We create a plane perpendicular to the y-axis and tilt it by 45 degrees in the function below. - ## When solving the problem, you can tell the planner to model this really thin box as an equality constraint. - ## The magic number :code:`0.0005` is explained later. - def create_plane_constraints(self): - pcm = moveit_msgs.msg.PositionConstraint() - pcm.header.frame_id = self.ref_link - pcm.link_name = self.ee_link - - cbox = shape_msgs.msg.SolidPrimitive() - cbox.type = shape_msgs.msg.SolidPrimitive.BOX - cbox.dimensions = [1.0, 0.0005, 1.0] - pcm.constraint_region.primitives.append(cbox) - - current_pose = self.move_group.get_current_pose() - - cbox_pose = geometry_msgs.msg.Pose() - cbox_pose.position.x = current_pose.pose.position.x - cbox_pose.position.y = current_pose.pose.position.y - cbox_pose.position.z = current_pose.pose.position.z - - # turn the constraint region 45 degrees around the x-axis. - quat = quaternion_from_euler(pi / 4, 0, 0) - cbox_pose.orientation.x = quat[0] - cbox_pose.orientation.y = quat[1] - cbox_pose.orientation.z = quat[2] - cbox_pose.orientation.w = quat[3] - pcm.constraint_region.primitive_poses.append(cbox_pose) - - # display the constraints in rviz - self.display_box(cbox_pose, cbox.dimensions) - - return pcm - - ## Building on the previous constraint, we can make it a line, by also reducing the dimension of the box in the x-direction. - def create_line_constraints(self): - pcm = moveit_msgs.msg.PositionConstraint() - pcm.header.frame_id = self.ref_link - pcm.link_name = self.ee_link - - cbox = shape_msgs.msg.SolidPrimitive() - cbox.type = shape_msgs.msg.SolidPrimitive.BOX - cbox.dimensions = [0.0005, 0.0005, 1.0] - pcm.constraint_region.primitives.append(cbox) - - current_pose = self.move_group.get_current_pose() - - cbox_pose = geometry_msgs.msg.Pose() - cbox_pose.position.x = current_pose.pose.position.x - cbox_pose.position.y = current_pose.pose.position.y - cbox_pose.position.z = current_pose.pose.position.z - quat = quaternion_from_euler(pi / 4, 0, 0) - cbox_pose.orientation.x = quat[0] - cbox_pose.orientation.y = quat[1] - cbox_pose.orientation.z = quat[2] - cbox_pose.orientation.w = quat[3] - pcm.constraint_region.primitive_poses.append(cbox_pose) - - # display the constraints in rviz - self.display_box(cbox_pose, cbox.dimensions) - - return pcm - - ## END_SUB_TUTORIAL - - def display_box(self, pose, dimensions): - """Utility function to visualize position constraints.""" - assert len(dimensions) == 3 - - # setup cube / box marker type - marker = visualization_msgs.msg.Marker() - marker.header.stamp = rospy.Time.now() - marker.ns = "/" - marker.id = self.marker_id_counter - marker.type = visualization_msgs.msg.Marker.CUBE - marker.action = visualization_msgs.msg.Marker.ADD - marker.color = COLOR_TRANSLUCENT - marker.header.frame_id = self.ref_link - - # fill in user input - marker.pose = pose - marker.scale.x = dimensions[0] - marker.scale.y = dimensions[1] - marker.scale.z = dimensions[2] - - # publish it! - self.marker_publisher.publish(marker) - self.marker_id_counter += 1 - - def display_sphere(self, pose, radius=0.05, color=COLOR_GREEN): - """Utility function to visualize the goal pose""" - - # setup sphere marker type - marker = visualization_msgs.msg.Marker() - marker.header.stamp = rospy.Time.now() - marker.ns = "/" - marker.id = self.marker_id_counter - marker.type = visualization_msgs.msg.Marker.SPHERE - marker.action = visualization_msgs.msg.Marker.ADD - marker.header.frame_id = self.ref_link - - # fill in user input - marker.color = color - marker.pose = pose - marker.scale.x = radius - marker.scale.y = radius - marker.scale.z = radius - - # publish it! - self.marker_publisher.publish(marker) - self.marker_id_counter += 1 - - def remove_all_markers(self): - """Utility function to remove all Markers that we potentially published in a previous run of this script.""" - # setup cube / box marker type - marker = visualization_msgs.msg.Marker() - marker.header.stamp = rospy.Time.now() - marker.ns = "/" - # marker.id = 0 - # marker.type = visualization_msgs.msg.Marker.CUBE - marker.action = visualization_msgs.msg.Marker.DELETEALL - self.marker_publisher.publish(marker) - - def add_obstacle(self): - box_pose = geometry_msgs.msg.PoseStamped() - box_pose.header.frame_id = self.ref_link - box_pose.pose.orientation.w = 1.0 - box_pose.pose.position.x = 0.5 - box_pose.pose.position.y = 0.0 - box_pose.pose.position.z = 0.5 - self.obstacle_name = "obstacle" - self.scene.add_box(self.obstacle_name, box_pose, size=(0.2, 0.4, 0.1)) - - # Give the planning scene some time to update - rospy.sleep(0.5) - - def remove_obstacle(self): - self.scene.remove_world_object(self.obstacle_name) - # Give the planning scene some time to update - rospy.sleep(0.5) - - def create_pose_goal_under_obstacle(self): - self.move_group.clear_pose_targets() - pose = self.move_group.get_current_pose() - - self.display_sphere(pose.pose, color=COLOR_RED) - - pose.pose.position.x += 0.2 - pose.pose.position.y += 0.0 - pose.pose.position.z -= 0.3 - - # rotate the pose around the x axis? - quat_ee = [ - pose.pose.orientation.x, - pose.pose.orientation.y, - pose.pose.orientation.z, - pose.pose.orientation.w, - ] - quat_rotate = quaternion_from_euler(-pi / 2, 0, 0) - quat_new = quaternion_multiply(quat_rotate, quat_ee) - - pose.pose.orientation.x = quat_new[0] - pose.pose.orientation.y = quat_new[1] - pose.pose.orientation.z = quat_new[2] - pose.pose.orientation.w = quat_new[3] - - self.display_sphere(pose.pose) - - return pose - - def create_vertical_plane_constraints(self): - pcm = moveit_msgs.msg.PositionConstraint() - pcm.header.frame_id = self.ref_link - pcm.link_name = self.ee_link - - cbox = shape_msgs.msg.SolidPrimitive() - cbox.type = shape_msgs.msg.SolidPrimitive.BOX - cbox.dimensions = [1.0, 0.0005, 1.0] - pcm.constraint_region.primitives.append(cbox) - - current_pose = self.move_group.get_current_pose() - - cbox_pose = geometry_msgs.msg.Pose() - cbox_pose.position.x = current_pose.pose.position.x - cbox_pose.position.y = current_pose.pose.position.y - cbox_pose.position.z = current_pose.pose.position.z - - cbox_pose.orientation.w = 1.0 - pcm.constraint_region.primitive_poses.append(cbox_pose) - - # display the constraints in rviz - self.display_box(cbox_pose, cbox.dimensions) - - return pcm - - -def run_tutorial(): - ## BEGIN_SUB_TUTORIAL main - ## - ## Now we can use the different pieces we just defined to solve some planning problems. - # First create an instance of the Tutorial class - tutorial = ConstrainedPlanningTutorial() - - # Copy move group variable for readability - move_group = tutorial.move_group - - ## Create the first planning problem - start_state = tutorial.create_start_state() - pose_goal = tutorial.create_pose_goal() - - # Let's try the simple box constraints first! - pcm = tutorial.create_simple_box_constraints() - - # We need two wrap the constraints in a generic `Constraints` message. - path_constraints = moveit_msgs.msg.Constraints() - path_constraints.position_constraints.append(pcm) - - # Now we have everything we need to configure and solve a planning problem - 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() - - # Now wait for the user (you) to press enter before doing trying the position constraints. - print("============ Press enter to continue with the second planning problem.") - input() - # remove all markers in Rviz before starting the next tutorial - tutorial.remove_all_markers() - - ## In the second problem we plan with the end-effector constrained to a plane. - ## Remember we created a special pose goal that lies in the constraint plane. - ## We did that because any goal or start state should also satisfy the path constraints. - ## - ## Solving the problem using equality constraints is a bit more complicated. (Or should I say, hacky?) - ## We need to tell the planner explicitly that we want to use equality constraints for the small dimensions. - ## This is achieved by setting the name of the constraint to :code:`"use_equality_constraints"`. - ## In addition, any dimension of the box below a threshold of :code:`0.001` will be considered an equality constraint. - ## However, if we make it too small, the box will be thinner that the tolerance used by OMPL to evaluate constraints (:code:`1e-4` by default). - ## MoveIt will use the stricter tolerance (the box width) to check the constraints, and many states will appear invalid. - ## That's where the magic number :code:`0.0005` comes from, it is between :code:`0.00001` and :code:`0.001`. - pose_goal = tutorial.create_pose_goal_in_plane() - pcm = tutorial.create_plane_constraints() # this function uses the 'magic' number - - path_constraints = moveit_msgs.msg.Constraints() - path_constraints.position_constraints.append(pcm) - - path_constraints.name = "use_equality_constraints" - - # And again, configure and solve the planning problem. - move_group.set_start_state(start_state) - move_group.set_pose_target(pose_goal) - move_group.set_path_constraints(path_constraints) - move_group.plan() - move_group.clear_path_constraints() - - print("============ Press enter to continue with the second planning problem.") - input() - tutorial.remove_all_markers() - ## Finally we can also plan along the line. - pose_goal = tutorial.create_pose_goal() - - pcm = tutorial.create_line_constraints() - - path_constraints = moveit_msgs.msg.Constraints() - path_constraints.position_constraints.append(pcm) - - path_constraints.name = "use_equality_constraints" - - move_group.set_start_state(start_state) - move_group.set_pose_target(pose_goal) - move_group.set_path_constraints(path_constraints) - move_group.plan() - move_group.clear_path_constraints() - - print("Done!") - ## END_SUB_TUTORIAL - - -def main(): - """Catch interrupt when the user presses `ctrl-c`.""" - try: - run_tutorial() - except KeyboardInterrupt: - return - - -if __name__ == "__main__": - main() - -## BEGIN_TUTORIAL -## -## Setup -## ***** -## CALL_SUB_TUTORIAL setup -## -## Create a planning problem -## *************************** -## CALL_SUB_TUTORIAL start_state -## CALL_SUB_TUTORIAL pose_goal -## -## Create position constraints -## *************************** -## CALL_SUB_TUTORIAL pos_con -## -## Finally, solve a planning problem! -## ********************************** -## CALL_SUB_TUTORIAL main -## -## END_TUTORIAL diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index c02c8bc4b0..d561a1301f 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -11,3 +11,4 @@ These how-to guides will help you quickly solve specific problems using MoveIt. how_to_setup_docker_containers_in_ubuntu how_to_write_doxygen isaac_panda/isaac_panda_tutorial + using_ompl_constrained_planning/ompl_constrained_planning diff --git a/doc/how_to_guides/using_ompl_constrained_planning/CMakeLists.txt b/doc/how_to_guides/using_ompl_constrained_planning/CMakeLists.txt new file mode 100644 index 0000000000..080fec44ff --- /dev/null +++ b/doc/how_to_guides/using_ompl_constrained_planning/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(ompl_constrained_planning + src/ompl_constrained_planning_tutorial.cpp) +target_include_directories(ompl_constrained_planning + PUBLIC include) +ament_target_dependencies(ompl_constrained_planning + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + +install(TARGETS ompl_constrained_planning + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/how_to_guides/using_ompl_constrained_planning/RVizVisualTools.png b/doc/how_to_guides/using_ompl_constrained_planning/RVizVisualTools.png new file mode 100644 index 0000000000..ee542543d2 Binary files /dev/null and b/doc/how_to_guides/using_ompl_constrained_planning/RVizVisualTools.png differ diff --git a/doc/examples/ompl_constrained_planning/case_1.gif b/doc/how_to_guides/using_ompl_constrained_planning/case_1.gif similarity index 100% rename from doc/examples/ompl_constrained_planning/case_1.gif rename to doc/how_to_guides/using_ompl_constrained_planning/case_1.gif diff --git a/doc/examples/ompl_constrained_planning/case_2.gif b/doc/how_to_guides/using_ompl_constrained_planning/case_2.gif similarity index 100% rename from doc/examples/ompl_constrained_planning/case_2.gif rename to doc/how_to_guides/using_ompl_constrained_planning/case_2.gif diff --git a/doc/examples/ompl_constrained_planning/case_3.gif b/doc/how_to_guides/using_ompl_constrained_planning/case_3.gif similarity index 100% rename from doc/examples/ompl_constrained_planning/case_3.gif rename to doc/how_to_guides/using_ompl_constrained_planning/case_3.gif diff --git a/doc/how_to_guides/using_ompl_constrained_planning/config/ompl_planning.yaml b/doc/how_to_guides/using_ompl_constrained_planning/config/ompl_planning.yaml new file mode 100644 index 0000000000..65f1ff7e03 --- /dev/null +++ b/doc/how_to_guides/using_ompl_constrained_planning/config/ompl_planning.yaml @@ -0,0 +1,216 @@ +planning_plugin: ompl_interface/OMPLPlanner +request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + 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 +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt + +panda_arm: + enforce_constrained_state_space: true + projection_evaluator: joints(panda_joint1,panda_joint2) + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +panda_arm_hand: + enforce_constrained_state_space: true + projection_evaluator: joints(panda_joint1,panda_joint2) + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +hand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault diff --git a/doc/how_to_guides/using_ompl_constrained_planning/launch/ompl_constrained_planning.launch.py b/doc/how_to_guides/using_ompl_constrained_planning/launch/ompl_constrained_planning.launch.py new file mode 100644 index 0000000000..209406c22c --- /dev/null +++ b/doc/how_to_guides/using_ompl_constrained_planning/launch/ompl_constrained_planning.launch.py @@ -0,0 +1,125 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines(pipelines=["ompl"]) + .to_moveit_configs() + ) + + # Set planning pipeline parameters + moveit_config.planning_pipelines["ompl"]["panda_arm"][ + "enforce_constrained_state_space" + ] = True + moveit_config.planning_pipelines["ompl"]["panda_arm"][ + "projection_evaluator" + ] = "joints(panda_joint1,panda_joint2)" + moveit_config.planning_pipelines["ompl"]["panda_arm_hand"][ + "enforce_constrained_state_space" + ] = True + moveit_config.planning_pipelines["ompl"]["panda_arm_hand"][ + "projection_evaluator" + ] = "joints(panda_joint1,panda_joint2)" + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict()], + ) + + # Demo OMPL constrained planning node + demo_node = Node( + package="moveit2_tutorials", + executable="ompl_constrained_planning", + output="both", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + + "/launch/ompl_constrained_planning.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="both", + ) + + # Load controllers + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format(controller)], + shell=True, + output="screen", + ) + ] + + return LaunchDescription( + [ + static_tf, + robot_state_publisher, + rviz_node, + run_move_group_node, + demo_node, + ros2_control_node, + ] + + load_controllers + ) diff --git a/doc/how_to_guides/using_ompl_constrained_planning/launch/ompl_constrained_planning.rviz b/doc/how_to_guides/using_ompl_constrained_planning/launch/ompl_constrained_planning.rviz new file mode 100644 index 0000000000..5c6f3819f8 --- /dev/null +++ b/doc/how_to_guides/using_ompl_constrained_planning/launch/ompl_constrained_planning.rviz @@ -0,0 +1,314 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + - /MarkerArray1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Cuboid: true + Sphere: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.306405544281006 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6553979516029358 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7403981685638428 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1024 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000257000003aafc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002f5000000ab0000003f00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001de000002070000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006901000003a60000003f0000003f00ffffff000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003aa000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003d2000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1860 + X: 2620 + Y: 27 diff --git a/doc/how_to_guides/using_ompl_constrained_planning/ompl_constrained_planning.rst b/doc/how_to_guides/using_ompl_constrained_planning/ompl_constrained_planning.rst new file mode 100644 index 0000000000..7ba4a25f69 --- /dev/null +++ b/doc/how_to_guides/using_ompl_constrained_planning/ompl_constrained_planning.rst @@ -0,0 +1,291 @@ +Using 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, four planning problems are solved with different types of path constraints. It will be easier to follow if you have read through the example on how to use the :doc:`Move Group interface `. + +When to use this Planner +^^^^^^^^^^^^^^^^^^^^^^^^ + +The interface currently only supports a single position or orientation constraint on any link of the robot. The planning approach provides an alternative for the `enforce_joint_model_state_space <../../examples/ompl_interface/ompl_interface_tutorial.html#enforce-planning-in-joint-space>`_ option. It is expected to be most valuable for constraints regions that have a small (or zero) volume in Cartesian space, where rejection sampling does not always work. For example, this planner can be used to constrain the robot end-effector on a plane or along a line. + +How to Configure OMPL to use Constrained Planning +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +OMPL reads configurations parameters from a file called :code:`ompl_planning.yaml`. This guide adds the required parameters directly in the launch file, but they can also be set in the yaml file. This guide uses the Panda robot, for which this file can be found :moveit_resources_codedir:`here `. Add a parameter to your robot's ``ompl_planning.yaml`` to tell OMPL to plan in a constrained state space by setting :code:`enforce_constrained_state_space` to ``true``. In addition, if the parameter `projection_evaluator <../../examples/ompl_interface/ompl_interface_tutorial.html#projection-evaluator>`_ was not yet specified, we also need to add it. The projection evaluator is used to help with discretizing the state space with high dimensionality by using projections from the state space to a low dimensional Euclidean space. You can read more about it `here `_. In general, using the first couple joints works pretty well. + +.. code-block:: yaml + + panda_arm: + enforce_constrained_state_space: true + projection_evaluator: joints(panda_joint1,panda_joint2) + +You can see an example :codedir:`here `. + +This tutorial adds the parameters in the :codedir:`launch file `. The launch file uses ``moveit_configs_utils`` to simplify the launch file. The ``moveit_config`` is configured with the moveit_resources Panda MoveIt config. We add our OMPL parameters to the ``planning_pipelines`` with the following lines: + +.. code-block:: python + + moveit_config.planning_pipelines["ompl"]["panda_arm"]["enforce_constrained_state_space"] = True + moveit_config.planning_pipelines["ompl"]["panda_arm"]["projection_evaluator"] = "joints(panda_joint1,panda_joint2)" + moveit_config.planning_pipelines["ompl"]["panda_arm_hand"]["enforce_constrained_state_space"] = True + moveit_config.planning_pipelines["ompl"]["panda_arm_hand"]["projection_evaluator"] = "joints(panda_joint1,panda_joint2)" + + + +Running the Example +^^^^^^^^^^^^^^^^^^^ + +Run the following command to launch the example: :: + + ros2 launch moveit_tutorials ompl_constrained_planning.launch.py + +The Panda robot should appear, with the RViz Visual Tools and the Trajectory Slider panel in the bottom-left. You should also see this text in your terminal: :: + + Waiting to continue: Press 'Next' in the RvizVisualToolsGui window to start with the box constraint example + +To start the first example, click the Next button. + +.. image:: RVizVisualTools.png + :width: 700px + +The first example shows a plan with box constraints. 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. You can use the Trajectory Slider panel to inspect the trajectory. + +.. raw:: html + + + +The following message appears in the terminal: :: + + Waiting to continue: Press 'Next' in the RvizVisualToolsGui window to continue to the planar constraint example + +After pressing Next, the next planning problem is solved. This example uses equality constraints to plan a trajectory with the end effector constrained in a plane. + +.. raw:: html + + + +Again, if planning succeeds, the trajectory is animated in Rviz. Press Next again to move on to the third planning problem, using equality constraints again to plan along a line. + +.. raw:: html + + + +You can see the trajectory animated if planning succeeds. Finally, press Next to move to the orientation constraint example. + +.. raw:: html + + + +This example may take longer to plan. Press Next to remove all markers and end the example. If planning fails, you can start at the beginning of the section to try again. + + +How to Set Constraints +^^^^^^^^^^^^^^^^^^^^^^ + +This section will cover how to use position, equality, and orientation constraints using the Move Group interface. This section walks through the :codedir:`example code `. Some lines have been omitted for brevity. + +Be sure to add ``enforce_constrained_state_space`` and ``projection_evaluator`` to your ``ompl_planning.yaml``! + +Initial Setup +------------- + +First, we start off by setting up the ``MoveGroupInterface`` similarly to the :doc:`Move Group interface example page `. This assumes you've already set up your node. + +.. code-block:: c++ + + moveit::planning_interface::MoveGroupInterface move_group_interface(node, "panda_arm"); + + +Next, we create a lambda to help us create a pose at a given relative position from the current pose. + +.. code-block:: c++ + + auto current_pose = move_group_interface.getCurrentPose(); + + // Creates a pose at a given positional offset from the current pose + auto get_relative_pose = [current_pose, &moveit_visual_tools](double x, double y, double z) { + auto target_pose = current_pose; + target_pose.pose.position.x += x; + target_pose.pose.position.y += y; + target_pose.pose.position.z += z; + moveit_visual_tools.publishSphere(current_pose.pose, rviz_visual_tools::RED, 0.05); + moveit_visual_tools.publishSphere(target_pose.pose, rviz_visual_tools::GREEN, 0.05); + moveit_visual_tools.trigger(); + return target_pose; + }; + +Now, we're ready to set up some constraints, starting with a box constraint. + +Box Constraints +--------------- + +We start by using the lambda to create a target pose offset from the current pose. This pose should be within the box that we make. In this example, we make a box of size ``(0.1, 0.4, 0.4)``, so the target pose should be within our constraint region. + +.. code-block:: c++ + + auto target_pose = get_relative_pose(0.0, 0.3, -0.3); + +Now, we set up the constraints. A box is a ``PositionConstraint`` - see the full message definition ::moveit_msgs_codedir:`here `. We set the ``frame_id`` in the header, as well as the ``link_name`` of the link to be constrained (in this case, the end effector). We then create a box using ``shape_msgs`` and set its dimensiions. We then place that box into ``box_constraint``. + +.. code-block:: c++ + + // Let's try the simple box constraints first! + moveit_msgs::msg::PositionConstraint box_constraint; + box_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + box_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; + box.dimensions = { 0.1, 0.4, 0.4 }; + box_constraint.constraint_region.primitives.emplace_back(box); + +Next, we set the pose of our box constraint. This is done by using ``geometry_msgs``. We set the position and orientation of the box, and add the pose to ``box_constraint``. + +.. code-block:: c++ + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = current_pose.pose.position.x; + box_pose.position.y = 0.15; + box_pose.position.z = 0.45; + box_pose.orientation.w = 1.0; + box_constraint.constraint_region.primitive_poses.emplace_back(box_pose); + box_constraint.weight = 1.0; + +Finally, we create a generic ``Constraints`` message and add our ``box_constraint`` to the ``position_constraints``. + +.. code-block:: c++ + + moveit_msgs::msg::Constraints box_constraints; + box_constraints.position_constraints.push_bemplace_backack(box_constraint); + +Now that we've created our constraint, set the path constraints via the Move Group interface and plan. It's helpful to increase the default planning time, as planning with constraints can be slower. + +.. code-block:: c++ + + moveit::planning_interface::MoveGroupInterface::Plan plan; + move_group_interface.setPathConstraints(box_constraints); + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(10.0); + move_group_interface.plan(plan); + +Equality Constraints +-------------------- + +We can plan with the end-effector constrained to a plane or a line using equality constraints. First, we'll cover the plane case. + +We need to create a pose goal that lies in this plane. The plane is tilted by 45 degrees, so moving an equal amount in the y and z direction will be on the plane. Be sure that both the goal and the start state satisfy the path constraints, or planning will always fail. + +.. code-block:: c++ + + target_pose = get_relative_pose(0.0, 0.3, -0.3); + + +We create a plane perpendicular to the y-axis and tilt it by 45 degrees. We create a plane by making a box and setting one dimension ``0.0005``. This is an important numvber that we will cover shortly. + +.. code-block:: c++ + + moveit_msgs::msg::PositionConstraint plane_constraint; + plane_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + plane_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive plane; + plane.type = shape_msgs::msg::SolidPrimitive::BOX; + plane.dimensions = { 1.0, 0.0005, 1.0 }; + plane_constraint.constraint_region.primitives.emplace_back(plane); + + geometry_msgs::msg::Pose plane_pose; + plane_pose.position.x = current_pose.pose.position.x; + plane_pose.position.y = current_pose.pose.position.y; + plane_pose.position.z = current_pose.pose.position.z; + plane_pose.orientation.x = sin(M_PI_4 / 2); + plane_pose.orientation.y = 0.0; + plane_pose.orientation.z = 0.0; + plane_pose.orientation.w = cos(M_PI_4 / 2); + plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose); + plane_constraint.weight = 1.0; + + +Solving the problem using equality constraints is a bit more complicated. We need to tell the planner explicitly that we want to use equality constraints for the small dimensions. This is achieved by setting the name of the constraint to :code:`"use_equality_constraints"`. In addition, any dimension of the box below a threshold of :code:`0.001` will be considered an equality constraint. However, if we make it too small, the box will be thinner that the tolerance used by OMPL to evaluate constraints (:code:`1e-4` by default). MoveIt will use the stricter tolerance (the box width) to check the constraints, and many states will appear invalid. That's where the magic number :code:`0.0005` comes from, it is between :code:`0.00001` and :code:`0.001`. + +.. code-block:: c++ + + moveit_msgs::msg::Constraints plane_constraints; + plane_constraints.position_constraints.emplace_back(plane_constraint); + plane_constraints.name = "use_equality_constraints"; + move_group_interface.setPathConstraints(plane_constraints); + +As before, set the target and plan. + +.. code-block:: c++ + + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(10.0); + move_group_interface.plan(plan); + +Building on the previous constraint, we can make it a line by also reducing the dimension of the box in the x-direction. + +.. code-block:: c++ + + moveit_msgs::msg::PositionConstraint line_constraint; + line_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + line_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive line; + line.type = shape_msgs::msg::SolidPrimitive::BOX; + line.dimensions = { 0.0005, 0.0005, 1.0 }; + line_constraint.constraint_region.primitives.emplace_back(line); + +Orientation Constraints +----------------------- + +Finally, we can place constraints on orientation. We set the target pose to be the other side of the robot for a more drastic move as we are no longer constrained by position. + +.. code-block:: c++ + + target_pose = get_relative_pose(-0.6, 0.1, 0); + +We create an ``OrientationConstraint`` instead of a ``Position Constraint``, but similarly to before, we set the ``header.frame_id`` and the ``link_name``. + +.. code-block:: c++ + + moveit_msgs::msg::OrientationConstraint orientation_constraint; + orientation_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + orientation_constraint.link_name = move_group_interface.getEndEffectorLink(); + +We then can use a ``geometry_msgs::msg::Quaternion`` to set the orientation constraint, or in this case, we just constrain the orientation to not change from ``current_pose``. + +.. code-block:: c++ + + orientation_constraint.orientation = current_pose.pose.orientation; + orientation_constraint.absolute_x_axis_tolerance = 0.4; + orientation_constraint.absolute_y_axis_tolerance = 0.4; + orientation_constraint.absolute_z_axis_tolerance = 0.4; + orientation_constraint.weight = 1.0; + +Similarly to the position constraints, we need to use a generic ``Constraints`` message, but this time we add it to the ``orientation_constraints``. + +.. code-block:: c++ + + moveit_msgs::msg::Constraints orientation_constraints; + orientation_constraints.orientation_constraints.emplace_back(orientation_constraint); + +Set up the planning problem as before and plan. + +.. code-block:: c++ + + move_group_interface.setPathConstraints(orientation_constraints); + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(10.0); + move_group_interface.plan(plan); diff --git a/doc/examples/ompl_constrained_planning/ompl_constrained_planning_header.png b/doc/how_to_guides/using_ompl_constrained_planning/ompl_constrained_planning_header.png similarity index 100% rename from doc/examples/ompl_constrained_planning/ompl_constrained_planning_header.png rename to doc/how_to_guides/using_ompl_constrained_planning/ompl_constrained_planning_header.png diff --git a/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp b/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp new file mode 100644 index 0000000000..8fb7dc80cb --- /dev/null +++ b/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp @@ -0,0 +1,238 @@ +#include + +#include +#include +#include +#include +#include + +static const auto LOGGER = rclcpp::get_logger("ompl_constrained_planning_demo"); +int main(int argc, char** argv) +{ + using namespace std::chrono_literals; + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("ompl_constrained_planning_demo_node", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto spinner = std::thread([&executor]() { executor.spin(); }); + + moveit::planning_interface::MoveGroupInterface move_group_interface(node, "panda_arm"); + auto moveit_visual_tools = + moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, + move_group_interface.getRobotModel() }; + + rclcpp::sleep_for(1s); + + // Create some helpful lambdas + auto current_pose = move_group_interface.getCurrentPose(); + + // Creates a pose at a given positional offset from the current pose + auto get_relative_pose = [current_pose, &moveit_visual_tools](double x, double y, double z) { + auto target_pose = current_pose; + target_pose.pose.position.x += x; + target_pose.pose.position.y += y; + target_pose.pose.position.z += z; + moveit_visual_tools.publishSphere(current_pose.pose, rviz_visual_tools::RED, 0.05); + moveit_visual_tools.publishSphere(target_pose.pose, rviz_visual_tools::GREEN, 0.05); + moveit_visual_tools.trigger(); + return target_pose; + }; + + // Resets the demo by cleaning up any constraints and markers + auto reset_demo = [&move_group_interface, &moveit_visual_tools]() { + move_group_interface.clearPathConstraints(); + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.trigger(); + }; + + reset_demo(); + moveit_visual_tools.loadRemoteControl(); + moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to start with the box constraint example"); + + // Create the first planning problem + auto target_pose = get_relative_pose(0.0, 0.3, -0.3); + + // Let's try the simple box constraints first! + moveit_msgs::msg::PositionConstraint box_constraint; + box_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + box_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; + box.dimensions = { 0.1, 0.4, 0.4 }; + box_constraint.constraint_region.primitives.emplace_back(box); + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = current_pose.pose.position.x; + box_pose.position.y = 0.15; + box_pose.position.z = 0.45; + box_pose.orientation.w = 1.0; + box_constraint.constraint_region.primitive_poses.emplace_back(box_pose); + box_constraint.weight = 1.0; + + // Visualize the box constraint + Eigen::Vector3d box_point_1(box_pose.position.x - box.dimensions[0] / 2, box_pose.position.y - box.dimensions[1] / 2, + box_pose.position.z - box.dimensions[2] / 2); + Eigen::Vector3d box_point_2(box_pose.position.x + box.dimensions[0] / 2, box_pose.position.y + box.dimensions[1] / 2, + box_pose.position.z + box.dimensions[2] / 2); + moveit_visual_tools.publishCuboid(box_point_1, box_point_2, rviz_visual_tools::TRANSLUCENT_DARK); + moveit_visual_tools.trigger(); + + // We need to wrap the constraints in a generic `Constraints` message. + moveit_msgs::msg::Constraints box_constraints; + box_constraints.position_constraints.emplace_back(box_constraint); + + // Don't forget the path constraints! That's the whole point of this tutorial. + move_group_interface.setPathConstraints(box_constraints); + + // Now we have everything we need to configure and solve a planning problem - plan to the target pose + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(10.0); + + // And let the planner find a solution. + // The move_group node should automatically visualize the solution in Rviz if a path is found. + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with box constraint %s", success ? "" : "FAILED"); + + // Now wait for the user to press Next before trying the planar constraints. + moveit_visual_tools.prompt( + "Press 'Next' in the RvizVisualToolsGui window to continue to the planar constraint example"); + + // Clear the path constraints and markers for the next example + reset_demo(); + + // In the second problem we plan with the end-effector constrained to a plane. + // We need to create a pose goal that lies in this plane. + // The plane is tilted by 45 degrees, so moving an equal amount in the y and z direction should be ok. + // Any goal or start state should also satisfy the path constraints. + target_pose = get_relative_pose(0.0, 0.3, -0.3); + + // We create a plane perpendicular to the y-axis and tilt it by 45 degrees + + // Solving the problem using equality constraints is a bit more complicated. (Or should I say, hacky?) + // We need to tell the planner explicitly that we want to use equality constraints for the small dimensions. + // This is achieved by setting the name of the constraint to :code:`"use_equality_constraints"`. + // In addition, any dimension of the box below a threshold of :code:`0.001` will be considered an equality constraint. + // However, if we make it too small, the box will be thinner that the tolerance used by OMPL to evaluate constraints + // (:code:`1e-4` by default). MoveIt will use the stricter tolerance (the box width) to check the constraints, and + // many states will appear invalid. That's where the magic number :code:`0.0005` comes from, it is between + // :code:`0.00001` and :code:`0.001`. + moveit_msgs::msg::PositionConstraint plane_constraint; + plane_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + plane_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive plane; + plane.type = shape_msgs::msg::SolidPrimitive::BOX; + plane.dimensions = { 1.0, 0.0005, 1.0 }; + plane_constraint.constraint_region.primitives.emplace_back(plane); + + geometry_msgs::msg::Pose plane_pose; + plane_pose.position.x = current_pose.pose.position.x; + plane_pose.position.y = current_pose.pose.position.y; + plane_pose.position.z = current_pose.pose.position.z; + plane_pose.orientation.x = sin(M_PI_4 / 2); + plane_pose.orientation.y = 0.0; + plane_pose.orientation.z = 0.0; + plane_pose.orientation.w = cos(M_PI_4 / 2); + plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose); + plane_constraint.weight = 1.0; + + // Visualize the constraint + auto d = sqrt(pow(target_pose.pose.position.y, 2) + pow(target_pose.pose.position.z, 2)); + + Eigen::Vector3d normal(0, 1, 1); + moveit_visual_tools.publishNormalAndDistancePlane(normal, d, rviz_visual_tools::TRANSLUCENT_DARK); + moveit_visual_tools.trigger(); + + moveit_msgs::msg::Constraints plane_constraints; + plane_constraints.position_constraints.emplace_back(plane_constraint); + plane_constraints.name = "use_equality_constraints"; + move_group_interface.setPathConstraints(plane_constraints); + + // And again, configure and solve the planning problem + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(10.0); + success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with plane constraint %s", success ? "" : "FAILED"); + + moveit_visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui window to continue to the linear constraint example"); + + reset_demo(); + + // We can also plan along a line. We can use the same pose as last time. + target_pose = get_relative_pose(0.0, 0.3, -0.3); + + // Building on the previous constraint, we can make it a line, by also reducing the dimension of the box in the x-direction. + moveit_msgs::msg::PositionConstraint line_constraint; + line_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + line_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive line; + line.type = shape_msgs::msg::SolidPrimitive::BOX; + line.dimensions = { 0.0005, 0.0005, 1.0 }; + line_constraint.constraint_region.primitives.emplace_back(line); + + geometry_msgs::msg::Pose line_pose; + line_pose.position.x = current_pose.pose.position.x; + line_pose.position.y = current_pose.pose.position.y; + line_pose.position.z = current_pose.pose.position.z; + line_pose.orientation.x = sin(M_PI_4 / 2); + line_pose.orientation.y = 0.0; + line_pose.orientation.z = 0.0; + line_pose.orientation.w = cos(M_PI_4 / 2); + line_constraint.constraint_region.primitive_poses.emplace_back(line_pose); + line_constraint.weight = 1.0; + + // Visualize the constraint + moveit_visual_tools.publishLine(current_pose.pose.position, target_pose.pose.position, + rviz_visual_tools::TRANSLUCENT_DARK); + moveit_visual_tools.trigger(); + + moveit_msgs::msg::Constraints line_constraints; + line_constraints.position_constraints.emplace_back(line_constraint); + line_constraints.name = "use_equality_constraints"; + move_group_interface.setPathConstraints(line_constraints); + move_group_interface.setPoseTarget(target_pose); + + success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with line constraint %s", success ? "" : "FAILED"); + + moveit_visual_tools.prompt( + "Press 'Next' in the RvizVisualToolsGui window to continue to the orientation constraint example"); + reset_demo(); + + // Finally, we can place constraints on orientation. + // Set the target pose to be the other side of the robot + target_pose = get_relative_pose(-0.6, 0.1, 0); + + // Create an orientation constraint + moveit_msgs::msg::OrientationConstraint orientation_constraint; + orientation_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + orientation_constraint.link_name = move_group_interface.getEndEffectorLink(); + + orientation_constraint.orientation = current_pose.pose.orientation; + orientation_constraint.absolute_x_axis_tolerance = 0.4; + orientation_constraint.absolute_y_axis_tolerance = 0.4; + orientation_constraint.absolute_z_axis_tolerance = 0.4; + orientation_constraint.weight = 1.0; + + moveit_msgs::msg::Constraints orientation_constraints; + orientation_constraints.orientation_constraints.emplace_back(orientation_constraint); + move_group_interface.setPathConstraints(orientation_constraints); + move_group_interface.setPoseTarget(target_pose); + + success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with orientation constraint %s", success ? "" : "FAILED"); + + // Done! + moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to clear the markers"); + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.trigger(); + move_group_interface.clearPathConstraints(); + + rclcpp::shutdown(); + spinner.join(); + return 0; +} diff --git a/doc/examples/ompl_constrained_planning/trajectory_slider.png b/doc/how_to_guides/using_ompl_constrained_planning/trajectory_slider.png similarity index 100% rename from doc/examples/ompl_constrained_planning/trajectory_slider.png rename to doc/how_to_guides/using_ompl_constrained_planning/trajectory_slider.png