From 2b6e3b9b097ba38a17070af043c71df1fa975cca Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Thu, 10 Aug 2023 00:17:15 -0700 Subject: [PATCH 1/5] Migrate CHOMP and collision tutorials from ROS1 (#492) * Migrated tutorial from ROS1 * Fix merging error in CMakeLists.txt --------- Co-authored-by: Henning Kayser Co-authored-by: Sebastian Jahr (cherry picked from commit 725c7d80438d4eaae95aa8f9ad3ab780c31caf74) # Conflicts: # CMakeLists.txt # doc/examples/chomp_planner/chomp_planner_tutorial.rst # doc/examples/planning_adapters/planning_adapters_tutorial.rst # doc/how_to_guides/how_to_guides.rst --- CMakeLists.txt | 19 + .../collision_environments/CMakeLists.txt | 15 +- .../scripts/collision_scene_example.py | 90 ---- .../src/collision_scene_example.cpp | 97 ++++ doc/examples/examples.rst | 1 + .../planning_adapters_tutorial.rst | 5 + .../chomp_planner/CMakeLists.txt | 6 + .../chomp_planner/chomp.png | Bin .../chomp_planner/chomp_planner_tutorial.rst | 128 +++++ .../chomp_planner/config/moveit_chomp.rviz | 508 ++++++++++++++++++ .../chomp_planner/launch/chomp_demo.launch.py | 158 ++++++ doc/how_to_guides/how_to_guides.rst | 26 + 12 files changed, 959 insertions(+), 94 deletions(-) delete mode 100755 doc/examples/collision_environments/scripts/collision_scene_example.py create mode 100644 doc/examples/collision_environments/src/collision_scene_example.cpp create mode 100644 doc/how_to_guides/chomp_planner/CMakeLists.txt rename doc/{examples => how_to_guides}/chomp_planner/chomp.png (100%) create mode 100644 doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst create mode 100644 doc/how_to_guides/chomp_planner/config/moveit_chomp.rviz create mode 100644 doc/how_to_guides/chomp_planner/launch/chomp_demo.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 01550f43a7..56f04ca241 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,12 +75,31 @@ add_subdirectory(doc/examples/robot_model_and_robot_state) # add_subdirectory(doc/examples/subframes) # add_subdirectory(doc/examples/tests) # add_subdirectory(doc/examples/trajopt_planner) +<<<<<<< HEAD # add_subdirectory(doc/examples/creating_moveit_plugins/lerp_motion_planner) +======= +# add_subdirectory(doc/examples/visualizing_collisions) +add_subdirectory(doc/examples/jupyter_notebook_prototyping) +add_subdirectory(doc/examples/motion_planning_api) +add_subdirectory(doc/examples/motion_planning_pipeline) +add_subdirectory(doc/examples/motion_planning_python_api) +add_subdirectory(doc/examples/move_group_interface) +>>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) add_subdirectory(doc/examples/moveit_cpp) # add_subdirectory(doc/examples/collision_environments) # add_subdirectory(doc/examples/visualizing_collisions) # add_subdirectory(doc/examples/bullet_collision_checker) add_subdirectory(doc/examples/realtime_servo) +<<<<<<< HEAD +======= +add_subdirectory(doc/examples/robot_model_and_robot_state) +add_subdirectory(doc/how_to_guides/isaac_panda) +add_subdirectory(doc/how_to_guides/kinematics_cost_function) +add_subdirectory(doc/how_to_guides/parallel_planning) +add_subdirectory(doc/how_to_guides/chomp_planner) +add_subdirectory(doc/how_to_guides/persistent_scenes_and_states) +add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner) +>>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) ament_export_dependencies( diff --git a/doc/examples/collision_environments/CMakeLists.txt b/doc/examples/collision_environments/CMakeLists.txt index 3148de8905..b92c24adce 100644 --- a/doc/examples/collision_environments/CMakeLists.txt +++ b/doc/examples/collision_environments/CMakeLists.txt @@ -1,4 +1,11 @@ -catkin_install_python(PROGRAMS - scripts/collision_scene_example.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +add_executable(collision_scene_example + src/collision_scene_example.cpp) +target_include_directories(collision_scene_example + PUBLIC include) +ament_target_dependencies(collision_scene_example + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + +install(TARGETS collision_scene_example + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) diff --git a/doc/examples/collision_environments/scripts/collision_scene_example.py b/doc/examples/collision_environments/scripts/collision_scene_example.py deleted file mode 100755 index 5572a0fa04..0000000000 --- a/doc/examples/collision_environments/scripts/collision_scene_example.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python - -# Python 2/3 compatibility import -from __future__ import print_function - -import rospy -from moveit_commander import RobotCommander, PlanningSceneInterface -import geometry_msgs.msg -import time -import sys - - -class CollisionSceneExample(object): - def __init__(self): - self._scene = PlanningSceneInterface() - - # clear the scene - self._scene.remove_world_object() - - self.robot = RobotCommander() - - # pause to wait for rviz to load - print("============ Waiting while RVIZ displays the scene with obstacles...") - - # TODO: need to replace this sleep by explicitly waiting for the scene to be updated. - rospy.sleep(2) - - def add_one_box(self): - box1_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] - box1_dimensions = [0.25, 0.25, 0.75] - - self.add_box_object("box1", box1_dimensions, box1_pose) - - print("============ Added one obstacle to RViz!!") - - def add_four_boxes(self): - box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1] - box1_dimensions = [0.2, 0.2, 0.5] - - box2_pose = [-0.55, -0.55, 0, 0, 0, 0, 1] - box2_dimensions = [0.25, 0.25, 1.75] - - box3_pose = [0.5, -0.55, 0.14, 0, 0, 0, 1] - box3_dimensions = [0.28, 0.28, 0.22] - - box4_pose = [-0.4, 0.4, 0.5, 0, 0, 0, 1] - box4_dimensions = [0.25, 0.25, 1.1] - - self.add_box_object("box1", box1_dimensions, box1_pose) - self.add_box_object("box2", box2_dimensions, box2_pose) - self.add_box_object("box3", box3_dimensions, box3_pose) - self.add_box_object("box4", box4_dimensions, box4_pose) - - print("========== Added 4 obstacles to the scene!!") - - def add_box_object(self, name, dimensions, pose): - p = geometry_msgs.msg.PoseStamped() - p.header.frame_id = self.robot.get_planning_frame() - p.header.stamp = rospy.Time.now() - p.pose.position.x = pose[0] - p.pose.position.y = pose[1] - p.pose.position.z = pose[2] - p.pose.orientation.x = pose[3] - p.pose.orientation.y = pose[4] - p.pose.orientation.z = pose[5] - p.pose.orientation.w = pose[6] - - self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - - -if __name__ == "__main__": - rospy.init_node("collision_scene_example_cluttered") - while ( - not rospy.search_param("robot_description_semantic") and not rospy.is_shutdown() - ): - time.sleep(0.5) - load_scene = CollisionSceneExample() - - if len(sys.argv) != 2: - print( - 'Correct usage:: \n"rosrun moveit_tutorials collision_scene_example.py cluttered" OR \n"rosrun moveit_tutorials collision_scene_example.py sparse"' - ) - sys.exit() - if sys.argv[1] == "cluttered": - load_scene.add_four_boxes() - elif sys.argv[1] == "sparse": - load_scene.add_one_box() - else: - print("Please specify correct type of scene as cluttered or sparse") - sys.exit() diff --git a/doc/examples/collision_environments/src/collision_scene_example.cpp b/doc/examples/collision_environments/src/collision_scene_example.cpp new file mode 100644 index 0000000000..9081506c8e --- /dev/null +++ b/doc/examples/collision_environments/src/collision_scene_example.cpp @@ -0,0 +1,97 @@ +#include + +// MoveIt +#include +#include +#include +#include +#include +#include +// TF2 +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_scene_example"); + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto collision_scene_example_node = rclcpp::Node::make_shared("planning_scene_tutorial", node_options); + + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; + // Create vector to hold 3 collision objects. + std::vector collision_objects(3); + + // Add the first table where the cube will originally be kept. + collision_objects.at(0).id = "table1"; + collision_objects.at(0).header.frame_id = "panda_link0"; + + // Create identity rotation quaternion + tf2::Quaternion zero_orientation; + zero_orientation.setRPY(0, 0, 0); + const geometry_msgs::msg::Quaternion zero_orientation_msg = tf2::toMsg(zero_orientation); + + // Define the primitive and its dimensions. + collision_objects[0].primitives.resize(1); + collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX; + collision_objects[0].primitives[0].dimensions.resize(3); + collision_objects[0].primitives[0].dimensions[0] = 0.2; + collision_objects[0].primitives[0].dimensions[1] = 0.4; + collision_objects[0].primitives[0].dimensions[2] = 0.4; + + // Define the pose of the table. + collision_objects[0].primitive_poses.resize(1); + collision_objects[0].primitive_poses[0].position.x = 0.5; + collision_objects[0].primitive_poses[0].position.y = 0; + collision_objects[0].primitive_poses[0].position.z = 0.2; + collision_objects[0].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[0].operation = collision_objects[0].ADD; + + // Add the second table where we will be placing the cube. + collision_objects[1].id = "table2"; + collision_objects[1].header.frame_id = "panda_link0"; + + // Define the primitive and its dimensions. + collision_objects[1].primitives.resize(1); + collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX; + collision_objects[1].primitives[0].dimensions.resize(3); + collision_objects[1].primitives[0].dimensions[0] = 0.4; + collision_objects[1].primitives[0].dimensions[1] = 0.2; + collision_objects[1].primitives[0].dimensions[2] = 0.4; + + // Define the pose of the table. + collision_objects[1].primitive_poses.resize(1); + collision_objects[1].primitive_poses[0].position.x = 0; + collision_objects[1].primitive_poses[0].position.y = 0.5; + collision_objects[1].primitive_poses[0].position.z = 0.2; + collision_objects[1].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[1].operation = collision_objects[1].ADD; + + // Define the object that we will be manipulating. + collision_objects[2].header.frame_id = "panda_link0"; + collision_objects[2].id = "object"; + + // Define the primitive and its dimensions. + collision_objects[2].primitives.resize(1); + collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX; + collision_objects[2].primitives[0].dimensions.resize(3); + collision_objects[2].primitives[0].dimensions[0] = 0.02; + collision_objects[2].primitives[0].dimensions[1] = 0.02; + collision_objects[2].primitives[0].dimensions[2] = 0.2; + + // Define the pose of the object. + collision_objects[2].primitive_poses.resize(1); + collision_objects[2].primitive_poses[0].position.x = 0.5; + collision_objects[2].primitive_poses[0].position.y = 0; + collision_objects[2].primitive_poses[0].position.z = 0.5; + collision_objects[2].primitive_poses[0].orientation = zero_orientation_msg; + + collision_objects[2].operation = collision_objects[2].ADD; + planning_scene_interface_.applyCollisionObjects(collision_objects); + + rclcpp::shutdown(); + return 0; +} diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 8205baf4d0..6a7bc08b23 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -63,6 +63,7 @@ Configuration kinematics_configuration/kinematics_configuration_tutorial custom_constraint_samplers/custom_constraint_samplers_tutorial ompl_interface/ompl_interface_tutorial + stomp_planner/stomp_planner_tutorial chomp_planner/chomp_planner_tutorial stomp_planner/stomp_planner_tutorial trajopt_planner/trajopt_planner_tutorial diff --git a/doc/examples/planning_adapters/planning_adapters_tutorial.rst b/doc/examples/planning_adapters/planning_adapters_tutorial.rst index 1c8b911c5b..70dfc9d257 100644 --- a/doc/examples/planning_adapters/planning_adapters_tutorial.rst +++ b/doc/examples/planning_adapters/planning_adapters_tutorial.rst @@ -200,8 +200,13 @@ This section has insights as to when to use which planner and how using certain For more information on each of these motion planners, refer to their individual tutorial pages :doc:`OMPL `, +<<<<<<< HEAD :doc:`CHOMP ` and :doc:`STOMP `. +======= +:doc:`CHOMP `, and +:doc:`STOMP `. +>>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) - **OMPL as a pre-processor for CHOMP**: OMPL can used as a base planner to produce an initial motion plan which can act as a initial guess for CHOMP. CHOMP can then produce optimized paths. In most cases, the quality of such a path produced should be better than that produced by OMPL alone or CHOMP alone. diff --git a/doc/how_to_guides/chomp_planner/CMakeLists.txt b/doc/how_to_guides/chomp_planner/CMakeLists.txt new file mode 100644 index 0000000000..9e0360b0e3 --- /dev/null +++ b/doc/how_to_guides/chomp_planner/CMakeLists.txt @@ -0,0 +1,6 @@ +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/examples/chomp_planner/chomp.png b/doc/how_to_guides/chomp_planner/chomp.png similarity index 100% rename from doc/examples/chomp_planner/chomp.png rename to doc/how_to_guides/chomp_planner/chomp.png diff --git a/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst b/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst new file mode 100644 index 0000000000..9c401b427a --- /dev/null +++ b/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst @@ -0,0 +1,128 @@ +Using CHOMP Planner +=================== + +.. image:: chomp.png + :width: 700px + +Covariant Hamiltonian Optimization for Motion Planning (CHOMP) is a gradient-based trajectory optimization procedure that makes many everyday motion planning problems both simple and trainable (Ratliff et al., 2009c). While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, this algorithm 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. `More info `_ + +Getting Started +--------------- +If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. + +You should also have gone through the steps in :doc:`Visualization with MoveIt RViz Plugin ` + +Prerequisites +-------------- +To use CHOMP with your robot you must have a MoveIt configuration package for your robot. For example, if you have a Panda robot, it's called ``panda_moveit_config`` and can be found :moveit_resources_codedir:`here `. These are typically configured using the :doc:`MoveIt Setup Assistant `. + +Using CHOMP with Your Robot +--------------------------- +**Note:** If you plan to use the ``panda_moveit_config`` package from the `ros-planning/moveit_resources `_ repository, these steps are already done for you and you can skip this section. Otherwise, to add the configuration for your robot you must: + +#. Create a :codedir:`chomp_demo.launch.py` file in the launch directory for your MoveIt config package. +#. Modify all references to Panda in ``chomp_demo.launch.py`` to point to your custom configuration instead +#. Ensure you have included a :moveit_resources_codedir:`chomp_planning.yaml ` file in the config directory of your MoveIt config package. +#. Open ``chomp_planning.yaml`` in your favorite editor and change ``animate_endeffector_segment: "panda_rightfinger"`` to the appropriate link for your robot. Feel free to modify any parameters you think may better suit your needs. + +Running the Demo +---------------- +If you have the ``panda_moveit_config`` from the `ros-planning/moveit_resources `_ repository along with ``moveit2_tutorials`` you can run the demo using: :: + + ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True + +Note: For convenience we have provided an RViz configuration you may use, but setting ``rviz_tutorial`` to ``False`` or simply omitting it will allow you to set up RViz according to your personal preferences. + +Adding Obstacles to the Scene ++++++++++++++++++++++++++++++ +To add obstacles to the scene, we can use :codedir:`this node` to create a scene with obstacles. + +To run the CHOMP planner with obstacles, open a second shell. In the first shell (if you closed the one from from the previous step) start RViz and wait for everything to finish loading: :: + + ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True + +In the second shell, run the command: :: + + ros2 run moveit2_tutorials collision_scene_example + +Next, in RViz, select CHOMP in the MotionPlanning panel under the Context tab. Set the desired start and goal states by moving the end-effector around with the marker and then click on the Plan button under the Planning tab in the MotionPlanning panel to start planning. The planner will now attempt to find a feasible solution between the given start and end position. + +Modifying the parameters for CHOMP +----------------------------------------- +CHOMP has some optimization parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the :moveit_resources_codedir:`chomp_planning.yaml ` file in the config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. The following may provide some insight on what the parameters in ``chomp_planning.yaml`` are used for: + +- *planning_time_limit*: Maximum amount of time the optimizer can take to find a solution before terminating. + +- *max_iterations*: Maximum number of iterations that the planner can take to find a good solution during optimization. + +- *max_iterations_after_collision_free*: Maximum number of iterations to be performed after a collision-free path is found. + +- *smoothness_cost_weight*: Weight of smoothness in the final cost function for CHOMP to optimize. + +- *obstacle_cost_weight*: Weight given to obstacles for the final cost CHOMP optimizes over. e.g., 0.0 would have obstacles to be ignored, 1.0 would be a hard constraint. + +- *learning_rate*: The rate used by the optimizer to find the local / global minima while reducing the total cost. + +- *smoothness_cost_velocity, smoothness_cost_acceleration, smoothness_cost_jerk*: Variables associated with the cost in velocity, acceleration and jerk. + +- *ridge_factor*: Noise added to the diagonal of the total :moveit_codedir:`quadratic cost matrix` in the objective function. Addition of small noise (e.g., 0.001) allows CHOMP to avoid obstacles at the cost of smoothness in trajectory. + +- *use_pseudo_inverse*: Enables pseudo inverse calculations when ``true``. + +- *pseudo_inverse_ridge_factor*: Set the ridge factor if pseudo inverse is enabled. + +- *joint_update_limit*: Update limit for the robot joints. + +- *collision_clearance*: Minimum distance from obstacles needed to avoid collision. + +- *collision_threshold*: The cost threshold that that must be maintained to avoid collisions. + +- *use_stochastic_descent*: Use stochastic descent while optimizing the cost when set to ``true``. In stochastic descent, a random point from the trajectory is used, rather than all the trajectory points. This is faster and guaranteed to converge, but it may take more iterations in the worst case. + +- *enable_failure_recovery*: When ``true``, CHOMP will tweak certain parameters in an attempt to find a solution when one does not exist with the default parameters specified in the ``chomp_planning.yaml`` file. + +- *max_recovery_attempts*: Maximum times that CHOMP is run with a varied set of parameters after the first attempt with the default parameters fails. + +- *trajectory_initializaiton_method*: The type of trajectory initialization given to CHOMP, which can be ``quintic-spline``, ``linear``, ``cubic`` or ``fillTrajectory``. The first three options refer to the interpolation methods used for trajectory initialization between start and goal states. ``fillTrajectory`` provides an option of initializing the trajectory with a path computed from an existing motion planner like OMPL. + +Choosing parameters for CHOMP requires some intuition that is informed by the planning environment. For instance, the default parameters for CHOMP work well in environments without obstacles; however, in environments with many obstacles the default parameters will likely cause CHOMP to get stuck in local minima. By tweaking parameters, we can improve the quality of plans generated by CHOMP. + +Some of the unused/commented parameters are *hmc_stochasticity*, *hmc_annealing_factor*, *hmc_discretization*, *use_hamiltonian_montecarlo*, *animate_endeffector*, *animate_endeffector_segment*, *animate_path*, *random_jump_amount*, *add_randomness*. + +Difference between plans obtained by CHOMP and OMPL +--------------------------------------------------- +Optimizing planners optimize a cost function that may sometimes lead to surprising results: moving through a thin obstacle might be lower cost than a long, winding trajectory that avoids all collisions. In this section we make a distinction between paths obtained from CHOMP and contrast it to those obtained from OMPL. + +OMPL is a open source library for sampling based / randomized motion planning algorithms. Sampling based algorithms are probabilistically complete: a solution would be eventually found if one exists, however, non-existence of a solution cannot be reported. These algorithms are efficient and usually find a solution quickly. OMPL does not contain any code related to collision checking or visualization, as the designers of OMPL did not want to tie it to a particular collision checker or visualization front end. The library is designed so it can be easily integrated into systems that provide the additional components. MoveIt integrates directly with OMPL and uses the motion planners from OMPL as its default set of planners. The planners in OMPL are abstract; i.e. OMPL has no concept of a robot. Instead, MoveIt configures OMPL and provides the back-end for OMPL to work with problems in robotics. + +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/chomp_planner/config/moveit_chomp.rviz b/doc/how_to_guides/chomp_planner/config/moveit_chomp.rviz new file mode 100644 index 0000000000..3af5147466 --- /dev/null +++ b/doc/how_to_guides/chomp_planner/config/moveit_chomp.rviz @@ -0,0 +1,508 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 341 + - 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 +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: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + 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: 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.009999999776482582 + 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 + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + 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 + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_arm_hand + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + 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.009999999776482582 + 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 + Velocity_Scaling_Factor: 0.1 + 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: 3.119211196899414 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.02386285550892353 + Y: 0.15478567779064178 + Z: 0.039489321410655975 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.665398120880127 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.8935770988464355 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: true + Width: 1846 + X: 74 + Y: 1080 diff --git a/doc/how_to_guides/chomp_planner/launch/chomp_demo.launch.py b/doc/how_to_guides/chomp_planner/launch/chomp_demo.launch.py new file mode 100644 index 0000000000..edd19b08a8 --- /dev/null +++ b/doc/how_to_guides/chomp_planner/launch/chomp_demo.launch.py @@ -0,0 +1,158 @@ +import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +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(): + + # Command-line arguments + tutorial_arg = DeclareLaunchArgument( + "rviz_tutorial", default_value="False", description="Tutorial flag" + ) + + db_arg = DeclareLaunchArgument( + "db", default_value="False", description="Database flag" + ) + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .robot_description_semantic(file_path="config/panda.srdf") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines(pipelines=["ompl", "chomp"]) + .to_moveit_configs() + ) + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict()], + arguments=["--ros-args", "--log-level", "info"], + ) + + # RViz + tutorial_mode = LaunchConfiguration("rviz_tutorial") + rviz_base = os.path.join(get_package_share_directory("moveit2_tutorials"), "config") + rviz_full_config = os.path.join(rviz_base, "moveit_chomp.rviz") + rviz_empty_config = os.path.join(rviz_base, "moveit_chomp.rviz") + rviz_node_tutorial = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_empty_config], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + ], + condition=IfCondition(tutorial_mode), + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_full_config], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + ], + condition=UnlessCondition(tutorial_mode), + ) + + # Static TF + static_tf_node = 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="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + panda_hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + # Warehouse mongodb server + db_config = LaunchConfiguration("db") + mongodb_server_node = Node( + package="warehouse_ros_mongo", + executable="mongo_wrapper_ros.py", + parameters=[ + {"warehouse_port": 33829}, + {"warehouse_host": "localhost"}, + {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"}, + ], + output="screen", + condition=IfCondition(db_config), + ) + + return LaunchDescription( + [ + tutorial_arg, + db_arg, + rviz_node, + rviz_node_tutorial, + static_tf_node, + robot_state_publisher, + move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + panda_hand_controller_spawner, + mongodb_server_node, + ] + ) diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index d561a1301f..8596700950 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -3,6 +3,32 @@ How-To Guides These how-to guides will help you quickly solve specific problems using MoveIt. +<<<<<<< HEAD +======= +For more information, refer to :doc:`/doc/how_to_guides/how_to_guide`. + +Configuring and Using MoveIt +---------------------------- + +.. toctree:: + :maxdepth: 1 + + kinematics_cost_function/kinematics_cost_function_tutorial + moveit_configuration/moveit_configuration_tutorial + pilz_industrial_motion_planner/pilz_industrial_motion_planner + stomp_planner/stomp_planner + chomp_planner/chomp_planner_tutorial + using_ompl_constrained_planning/ompl_constrained_planning + parallel_planning/parallel_planning_tutorial + controller_teleoperation/controller_teleoperation + persistent_scenes_and_states/persistent_scenes_and_states + isaac_panda/isaac_panda_tutorial + pick_ik/pick_ik_tutorial + +Developing and Documenting MoveIt +--------------------------------- + +>>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) .. toctree:: :maxdepth: 1 From b57b77de0f9c2654e5c1d64317547aade4a04013 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 10 Aug 2023 09:50:58 +0200 Subject: [PATCH 2/5] Address rebasing conflicts --- CMakeLists.txt | 13 -- .../chomp_planner/chomp_planner_tutorial.rst | 146 ------------------ .../planning_adapters_tutorial.rst | 9 +- 3 files changed, 2 insertions(+), 166 deletions(-) delete mode 100644 doc/examples/chomp_planner/chomp_planner_tutorial.rst diff --git a/CMakeLists.txt b/CMakeLists.txt index 56f04ca241..c059040380 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,31 +75,18 @@ add_subdirectory(doc/examples/robot_model_and_robot_state) # add_subdirectory(doc/examples/subframes) # add_subdirectory(doc/examples/tests) # add_subdirectory(doc/examples/trajopt_planner) -<<<<<<< HEAD -# add_subdirectory(doc/examples/creating_moveit_plugins/lerp_motion_planner) -======= # add_subdirectory(doc/examples/visualizing_collisions) add_subdirectory(doc/examples/jupyter_notebook_prototyping) add_subdirectory(doc/examples/motion_planning_api) add_subdirectory(doc/examples/motion_planning_pipeline) add_subdirectory(doc/examples/motion_planning_python_api) add_subdirectory(doc/examples/move_group_interface) ->>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) add_subdirectory(doc/examples/moveit_cpp) # add_subdirectory(doc/examples/collision_environments) # add_subdirectory(doc/examples/visualizing_collisions) # add_subdirectory(doc/examples/bullet_collision_checker) add_subdirectory(doc/examples/realtime_servo) -<<<<<<< HEAD -======= -add_subdirectory(doc/examples/robot_model_and_robot_state) -add_subdirectory(doc/how_to_guides/isaac_panda) -add_subdirectory(doc/how_to_guides/kinematics_cost_function) -add_subdirectory(doc/how_to_guides/parallel_planning) add_subdirectory(doc/how_to_guides/chomp_planner) -add_subdirectory(doc/how_to_guides/persistent_scenes_and_states) -add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner) ->>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) ament_export_dependencies( diff --git a/doc/examples/chomp_planner/chomp_planner_tutorial.rst b/doc/examples/chomp_planner/chomp_planner_tutorial.rst deleted file mode 100644 index 262d3db0ea..0000000000 --- a/doc/examples/chomp_planner/chomp_planner_tutorial.rst +++ /dev/null @@ -1,146 +0,0 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - -CHOMP Planner -=============== - -.. image:: chomp.png - :width: 700px - -Covariant Hamiltonian optimization for motion planning (CHOMP) is a gradient-based trajectory optimization procedure that makes many everyday motion planning problems both simple and trainable (Ratliff et al., 2009c). While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, this algorithm 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 dynamical quantities such as joint velocities and accelerations. It rapidly converges to a smooth collision-free trajectory that can be executed efficiently on the robot. Integration into latest version of MoveIt is `work in progress `_. `More info `_ - -Getting Started ---------------- -If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. - -You should also have gone through the steps in :doc:`Visualization with MoveIt RViz Plugin ` - -Prerequisites --------------- -#. On ROS Melodic you do not need to build MoveIt from source, but for older versions of MoveIt you do (see previous tutorial versions). -#. To use CHOMP with your robot you must already have a MoveIt configuration package for your robot already. For example, if you have a Panda robot, it's called ``panda_moveit_config``. This is typically configured using the :doc:`MoveIt Setup Assistant `. - -Using CHOMP with Your Robot ---------------------------- -**Note:** if you are following this demo using the ``panda_moveit_config`` from the `ros-planning/panda_moveit_config `_ repository, these steps are already done for you and you can skip this section. - -#. Simply download :panda_codedir:`chomp_planning_pipeline.launch.xml` file into the launch directory of your MoveIt config package. In our case, we will save this file in the ``panda_moveit_config/launch`` directory. -#. Adjust the line ```` to ```` replacing ```` with the name of your MoveIt configuration package. -#. Download :panda_codedir:`chomp_planning.yaml ` file into the config directory of your MoveIt config package. In our case, we will save this file in the ``panda_moveit_config/config`` directory. -#. Open ``chomp_planning.yaml`` in your favorite editor and change ``animate_endeffector_segment: "panda_rightfinger"`` to the appropriate link for your robot. - -Running the Demo ----------------- -If you have the ``panda_moveit_config`` from the `ros-planning/panda_moveit_config `_ repository you should be able to simply run the demo: :: - - roslaunch panda_moveit_config demo.launch pipeline:=chomp - -Running CHOMP with Obstacles in the Scene -+++++++++++++++++++++++++++++++++++++++++ -To run CHOMP in an environment with obstacles, you can run the sample python script: - - :codedir:`collision_scene_example.py`. - -This script creates a cluttered scene with four obstacles or a simple scene with one obstacle depending on the argument given to the script. One can also change the position/size of the obstacles to change the scene. - -To run the CHOMP planner with obstacles, open two shells. In the first shell start RViz and wait for everything to finish loading: :: - - roslaunch panda_moveit_config demo.launch pipeline:=chomp - -In the second shell, run either of the two commands: :: - - rosrun moveit_tutorials collision_scene_example.py cluttered - -or: :: - - rosrun moveit_tutorials collision_scene_example.py sparse - -Next, in RViz, select CHOMP in the MotionPlanning panel under the Context tab. Set the desired start and goal states by moving the end-effector around with the imarker and then click on the Plan button under the Planning tab in the MotionPlanning panel to start planning. The planner will now attempt to find a feasible solution between the given start and end position. - -Tweaking some of the parameters for CHOMP ------------------------------------------ -CHOMP has some optimization parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the :panda_codedir:`chomp_planning.yaml ` file in config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. The following are some of the insights to set up these parameter values for some of them: - -- *planning_time_limit*: the maximum time the optimizer can take to find a solution before terminating - -- *max_iterations*: this is the maximum number of iterations that the planner can take to find a good solution while optimization - -- *max_iterations_after_collision_free*: maximum iterations to be performed after a collision-free path is found. - -- *smoothness_cost_weight*: the smoothness_cost_weight parameters controls its weight in the final cost that CHOMP is actually optimizing over - -- *obstacle_cost_weight*: this controls the weight to be given to obstacles towards the final cost CHOMP optimizes over. e.g., 0.0 would have obstacles to be ignored, 1.0 would be a hard constraint - -- *learning_rate*: this is the learning rate used by the optimizer to find the local / global minima while reducing the total cost. - -- *smoothness_cost_velocity, smoothness_cost_acceleration, smoothness_cost_jerk*: variables associated with the cost in velocity, acceleration and jerk. - -- *ridge_factor*: the noise added to the diagonal of the total :moveit_codedir:`quadratic cost matrix` in the objective function. Addition of small noise (e.g., 0.001) allows CHOMP to avoid obstacles at the cost of smoothness in trajectory. - -- *use_pseudo_inverse*: enable pseudo inverse calculations or not. - -- *pseudo_inverse_ridge_factor*: set the ridge factor if pseudo inverse is enabled. - -- *joint_update_limit*: set the update limit for the robot joints - -- *collision_clearance*: the minimum distance that needs to be maintained to avoid obstacles. - -- *collision_threshold*: the collision threshold cost that needs to be maintained to avoid collisions - -- *use_stochastic_descent*: set this to true/false if you want to use stochastic descent while optimizing the cost. In stochastic descent, a random point from the trajectory is used, rather than all the trajectory points. This is faster and guaranteed to converge, but it may take more iterations in the worst case. - -- *enable failure recovery*: if this is set to true, CHOMP tweaks certain parameters in the hope to find a solution when one does not exist with the default parameters specified in the ``chomp_planning.yaml`` file. - -- *max_recovery_attempts*: this is the maximum times that CHOMP is run with a varied set of parameters after the first attempt with the default parameters. - -- *trajectory_initializaiton_method*: the type of initialization of the trajectory can be supplied here for CHOMP, this can be ``quintic-spline``, ``linear``, ``cubic`` or ``fillTrajectory``. The first three options refer to the interpolation methods used for trajectory initialization between start and goal states. ``fillTrajectory`` provides an option of initializing the trajectory from path computed from an existing motion planner like OMPL. - -Choosing parameters for CHOMP requires some intuition that is informed by the planning environment. For instance, the default parameters for CHOMP work well in environments without obstacles; however, in environments with many obstacles the default parameters will likely cause CHOMP to get stuck in local minima. By tweaking parameters, we can improve the quality of plans generated by CHOMP. - -Some of the unused/commented parameters are *hmc_stochasticity*, *hmc_annealing_factor*, *hmc_discretization*, *use_hamiltonian_montecarlo*, *animate_endeffector*, *animate_endeffector_segment*, *animate_path*, *random_jump_amount*, *add_randomness*. - -Difference between plans obtained by CHOMP and OMPL ---------------------------------------------------- -Optimizing planners optimize a cost function that may sometimes lead to surprising results: moving through a thin obstacle might be lower cost than a long, winding trajectory that avoids all collisions. In this section we make a distinction between paths obtained from CHOMP and contrast it to those obtained from OMPL. - -OMPL is a open source library for sampling based / randomized motion planning algorithms. Sampling based algorithms are probabilistically complete: a solution would be eventually found if one exists, however non-existence of a solution cannot be reported. These algorithms are efficient and usually find a solution quickly. OMPL does not contain any code related to collision checking or visualization as the designers of OMPL did not want to tie it to a any particular collision checker or visualization front end. The library is designed so it can be easily integrated into systems that provide the additional components. MoveIt integrates directly with OMPL and uses the motion planners from OMPL as its default set of planners. The planners in OMPL are abstract; i.e. OMPL has no concept of a robot. Instead, MoveIt configures OMPL and provides the back-end for OMPL to work with problems in Robotics. - -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 dynamical 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 dynamical 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 the 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, it is demonstrated 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, follow the steps: - -#. Create a new launch file ``ompl-chomp_planning_pipeline.launch`` in the ``/launch`` folder of your robot with the following contents: :: - - - - - - - - - - - - -#. This launch file defines the new planning pipeline ``ompl-chomp``, deriving from the ``ompl`` pipeline, - but adding the CHOMP post-processor as a planning adapter. Also, the ``trajectory_initialization_method`` is overridden to use the OMPL-generated trajectory. - -#. Now you can launch the newly configure planning pipeline as follows: :: - - roslaunch panda_moveit_config demo.launch pipeline:=ompl-chomp - -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. diff --git a/doc/examples/planning_adapters/planning_adapters_tutorial.rst b/doc/examples/planning_adapters/planning_adapters_tutorial.rst index 70dfc9d257..7e8e29eb17 100644 --- a/doc/examples/planning_adapters/planning_adapters_tutorial.rst +++ b/doc/examples/planning_adapters/planning_adapters_tutorial.rst @@ -200,13 +200,8 @@ This section has insights as to when to use which planner and how using certain For more information on each of these motion planners, refer to their individual tutorial pages :doc:`OMPL `, -<<<<<<< HEAD -:doc:`CHOMP ` and -:doc:`STOMP `. -======= -:doc:`CHOMP `, and -:doc:`STOMP `. ->>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) +:doc:`STOMP `, and +:doc:`CHOMP `. - **OMPL as a pre-processor for CHOMP**: OMPL can used as a base planner to produce an initial motion plan which can act as a initial guess for CHOMP. CHOMP can then produce optimized paths. In most cases, the quality of such a path produced should be better than that produced by OMPL alone or CHOMP alone. From a8b413fc1d8e284338efa191fefede7b5e401add Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 10 Aug 2023 09:23:12 +0200 Subject: [PATCH 3/5] Remove references to non-existing files and cleanup --- doc/examples/examples.rst | 1 - doc/how_to_guides/how_to_guides.rst | 4 ---- 2 files changed, 5 deletions(-) diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 6a7bc08b23..3745a127c9 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -64,7 +64,6 @@ Configuration custom_constraint_samplers/custom_constraint_samplers_tutorial ompl_interface/ompl_interface_tutorial stomp_planner/stomp_planner_tutorial - chomp_planner/chomp_planner_tutorial stomp_planner/stomp_planner_tutorial trajopt_planner/trajopt_planner_tutorial pilz_industrial_motion_planner/pilz_industrial_motion_planner diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index 8596700950..ef6388bb79 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -3,8 +3,6 @@ How-To Guides These how-to guides will help you quickly solve specific problems using MoveIt. -<<<<<<< HEAD -======= For more information, refer to :doc:`/doc/how_to_guides/how_to_guide`. Configuring and Using MoveIt @@ -16,7 +14,6 @@ Configuring and Using MoveIt kinematics_cost_function/kinematics_cost_function_tutorial moveit_configuration/moveit_configuration_tutorial pilz_industrial_motion_planner/pilz_industrial_motion_planner - stomp_planner/stomp_planner chomp_planner/chomp_planner_tutorial using_ompl_constrained_planning/ompl_constrained_planning parallel_planning/parallel_planning_tutorial @@ -28,7 +25,6 @@ Configuring and Using MoveIt Developing and Documenting MoveIt --------------------------------- ->>>>>>> 725c7d8 (Migrate CHOMP and collision tutorials from ROS1 (#492)) .. toctree:: :maxdepth: 1 From b8554044104a122f2e69a06fbfcae0fb37b60bb6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 10 Aug 2023 10:17:26 +0200 Subject: [PATCH 4/5] Cleanups --- CMakeLists.txt | 6 +----- doc/how_to_guides/how_to_guides.rst | 7 ------- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c059040380..30af510b34 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,12 +75,8 @@ add_subdirectory(doc/examples/robot_model_and_robot_state) # add_subdirectory(doc/examples/subframes) # add_subdirectory(doc/examples/tests) # add_subdirectory(doc/examples/trajopt_planner) +# add_subdirectory(doc/examples/creating_moveit_plugins/lerp_motion_planner) # add_subdirectory(doc/examples/visualizing_collisions) -add_subdirectory(doc/examples/jupyter_notebook_prototyping) -add_subdirectory(doc/examples/motion_planning_api) -add_subdirectory(doc/examples/motion_planning_pipeline) -add_subdirectory(doc/examples/motion_planning_python_api) -add_subdirectory(doc/examples/move_group_interface) add_subdirectory(doc/examples/moveit_cpp) # add_subdirectory(doc/examples/collision_environments) # add_subdirectory(doc/examples/visualizing_collisions) diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index ef6388bb79..e8de0e244d 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -11,16 +11,9 @@ Configuring and Using MoveIt .. toctree:: :maxdepth: 1 - kinematics_cost_function/kinematics_cost_function_tutorial - moveit_configuration/moveit_configuration_tutorial - pilz_industrial_motion_planner/pilz_industrial_motion_planner chomp_planner/chomp_planner_tutorial using_ompl_constrained_planning/ompl_constrained_planning - parallel_planning/parallel_planning_tutorial - controller_teleoperation/controller_teleoperation - persistent_scenes_and_states/persistent_scenes_and_states isaac_panda/isaac_panda_tutorial - pick_ik/pick_ik_tutorial Developing and Documenting MoveIt --------------------------------- From 3fe8f61dbc1c6894187ba2119bbff8f44703de62 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 10 Aug 2023 11:02:21 +0200 Subject: [PATCH 5/5] Update doc/examples/examples.rst --- doc/examples/examples.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 3745a127c9..a54d306982 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -64,7 +64,6 @@ Configuration custom_constraint_samplers/custom_constraint_samplers_tutorial ompl_interface/ompl_interface_tutorial stomp_planner/stomp_planner_tutorial - stomp_planner/stomp_planner_tutorial trajopt_planner/trajopt_planner_tutorial pilz_industrial_motion_planner/pilz_industrial_motion_planner planning_adapters/planning_adapters_tutorial.rst