diff --git a/CMakeLists.txt b/CMakeLists.txt index 31ef0b667e..d03584062f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ add_subdirectory(doc/how_to_guides/parallel_planning) add_subdirectory(doc/how_to_guides/chomp_planner) add_subdirectory(doc/how_to_guides/persistent_scenes_and_states) add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner) +add_subdirectory(doc/how_to_guides/pipeline_testbench) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor) add_subdirectory(doc/tutorials/quickstart_in_rviz) diff --git a/doc/concepts/motion_planning.rst b/doc/concepts/motion_planning.rst index b10a48bd38..f05d1cceb6 100644 --- a/doc/concepts/motion_planning.rst +++ b/doc/concepts/motion_planning.rst @@ -47,36 +47,30 @@ Pre-processing is useful in several situations, e.g. when a start state for the Post-processing is needed for several other operations, e.g. to convert paths generated for a robot into time-parameterized trajectories. MoveIt provides a set of default motion planning adapters that each perform a very specific function. -FixStartStateBounds +CheckStartStateBounds ^^^^^^^^^^^^^^^^^^^ The fix start state bounds adapter fixes the start state to be within the joint limits specified in the URDF. The need for this adapter arises in situations where the joint limits for the physical robot are not properly configured. The robot may then end up in a configuration where one or more of its joints is slightly outside its joint limits. In this case, the motion planner is unable to plan since it will think that the starting state is outside joint limits. -The "FixStartStateBounds" planning request adapter will "fix" the start state by moving it to the joint limit. +The "CheckStartStateBounds" planning request adapter will "fix" the start state by moving it to the joint limit. However, this is obviously not the right solution every time - e.g. where the joint is really outside its joint limits by a large amount. A parameter for the adapter specifies how much the joint can be outside its limits for it to be "fixable". -FixWorkspaceBounds +ValidateWorkspaceBounds ^^^^^^^^^^^^^^^^^^ The fix workspace bounds adapter will specify a default workspace for planning: a cube of size 10 m x 10 m x 10 m. This workspace will only be specified if the planning request to the planner does not have these fields filled in. -FixStartStateCollision +CheckStartStateCollision ^^^^^^^^^^^^^^^^^^^^^^ The fix start state collision adapter will attempt to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount. The amount that it will perturb the values by is specified by the **jiggle_fraction** parameter that controls the perturbation as a percentage of the total range of motion for the joint. The other parameter for this adapter specifies how many random perturbations the adapter will sample before giving up. -FixStartStatePathConstraints -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -This adapter is applied when the start state for a motion plan does not obey the specified path constraints. -It will attempt to plan a path between the current configuration of the robot to a new location where the path constraint is obeyed. -The new location will serve as the start state for planning. AddTimeParameterization ^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp b/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp index 3fd2c116aa..ad8fe38bd9 100644 --- a/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp +++ b/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp @@ -94,13 +94,13 @@ int main(int argc, char** argv) // Note that we are using the ROS pluginlib library here. std::unique_ptr> planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; - std::string planner_plugin_name; + std::vector planner_plugin_names; // We will get the name of planning plugin we want to load // from the ROS parameter server, and then load the planner // making sure to catch all exceptions. - if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugin", planner_plugin_name)) - RCLCPP_FATAL(LOGGER, "Could not find planner plugin name"); + if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugins", planner_plugin_names)) + RCLCPP_FATAL(LOGGER, "Could not find planner plugin names"); try { planner_plugin_loader.reset(new pluginlib::ClassLoader( @@ -110,9 +110,11 @@ int main(int argc, char** argv) { RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what()); } + + const auto& planner_name = planner_plugin_names.at(0); try { - planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); + planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_name)); if (!planner_instance->initialize(robot_model, motion_planning_api_tutorial_node, motion_planning_api_tutorial_node->get_namespace())) RCLCPP_FATAL(LOGGER, "Could not initialize planner instance"); @@ -124,7 +126,7 @@ int main(int argc, char** argv) std::stringstream ss; for (const auto& cls : classes) ss << cls << " "; - RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_plugin_name.c_str(), + RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_name.c_str(), ex.what(), ss.str().c_str()); } @@ -166,14 +168,14 @@ int main(int argc, char** argv) geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "panda_link0"; pose.pose.position.x = 0.3; - pose.pose.position.y = 0.4; + pose.pose.position.y = 0.0; pose.pose.position.z = 0.75; - pose.pose.orientation.w = 1.0; + pose.pose.orientation.z = 1.0; - // A tolerance of 0.01 m is specified in position - // and 0.01 radians in orientation - std::vector tolerance_pose(3, 0.01); - std::vector tolerance_angle(3, 0.01); + // A tolerance of 0.1 m is specified in position + // and 0.1 radians in orientation + std::vector tolerance_pose(3, 0.1); + std::vector tolerance_angle(3, 0.1); // We will create the request as a constraint using a helper function available // from the @@ -185,16 +187,28 @@ int main(int argc, char** argv) req.group_name = PLANNING_GROUP; req.goal_constraints.push_back(pose_goal); + // Define workspace bounds + req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = + req.workspace_parameters.min_corner.z = -5.0; + req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = + req.workspace_parameters.max_corner.z = 5.0; + // We now construct a planning context that encapsulate the scene, // the request and the response. We call the planner using this // planning context planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code); + + if (!context) + { + RCLCPP_ERROR(LOGGER, "Failed to create planning context"); + return -1; + } context->solve(res); if (res.error_code.val != res.error_code.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); - return 0; + return -1; } // Visualize the result @@ -246,7 +260,7 @@ int main(int argc, char** argv) if (res.error_code.val != res.error_code.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); - return 0; + return -1; } /* Visualize the trajectory */ res.getMessage(response); diff --git a/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp b/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp index e13600cb53..61259cff64 100644 --- a/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp +++ b/doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp @@ -141,6 +141,8 @@ int main(int argc, char** argv) req.pipeline_id = "ompl"; req.planner_id = "RRTConnectkConfigDefault"; req.allowed_planning_time = 1.0; + req.max_velocity_scaling_factor = 1.0; + req.max_acceleration_scaling_factor = 1.0; planning_interface::MotionPlanResponse res; geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "panda_link0"; diff --git a/doc/examples/moveit_cpp/config/moveit_cpp.yaml b/doc/examples/moveit_cpp/config/moveit_cpp.yaml index ffa333f8a4..0dbd6f88ee 100644 --- a/doc/examples/moveit_cpp/config/moveit_cpp.yaml +++ b/doc/examples/moveit_cpp/config/moveit_cpp.yaml @@ -14,5 +14,6 @@ planning_pipelines: plan_request_params: planning_attempts: 1 planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 diff --git a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py index b1ea4678f2..4e1a9143c2 100644 --- a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py +++ b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -11,6 +11,7 @@ def generate_launch_description(): MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines("ompl", ["ompl"]) .moveit_cpp( file_path=get_package_share_directory("moveit2_tutorials") + "/config/moveit_cpp.yaml" diff --git a/doc/examples/planning_adapters/planning_adapters_tutorial.rst b/doc/examples/planning_adapters/planning_adapters_tutorial.rst index ef8cde65e9..49db9f9ca1 100644 --- a/doc/examples/planning_adapters/planning_adapters_tutorial.rst +++ b/doc/examples/planning_adapters/planning_adapters_tutorial.rst @@ -6,7 +6,7 @@ Planning Adapter Tutorials ========================== -Planning Request Adapters is a concept in MoveIt which can be used to modify the trajectory (pre-processing and/or post-processing) for a motion planner. Some examples of existing planning adapters in MoveIt include AddTimeParameterization, FixWorkspaceBounds, FixStartBounds, FixStartStateCollision, FixStartStatePathConstraints, CHOMPOptimizerAdapter, etc. ! Using the concepts of Planning Adapters, multiple motion planning algorithms can be used in a pipeline to produce robust motion plans. For example, a sample pipeline of motion plans might include an initial plan produced by OMPL which can then be optimized by CHOMP to produce a motion plan which would likely be better than a path produced by OMPL or CHOMP alone. Similarly, using the concept of Planning Adapters, other motion planners can be mixed and matched depending on the environment the robot is operating in. This section provides a step by step tutorial on using a mix and match of different motion planners and also provides insights on when to use which particular motion planners. +Planning Request Adapters is a concept in MoveIt which can be used to modify the trajectory (pre-processing and/or post-processing) for a motion planner. Some examples of existing planning adapters in MoveIt include AddTimeParameterization, ValidateWorkspaceBounds, FixStartBounds, CheckStartStateCollision, CHOMPOptimizerAdapter, etc. ! Using the concepts of Planning Adapters, multiple motion planning algorithms can be used in a pipeline to produce robust motion plans. For example, a sample pipeline of motion plans might include an initial plan produced by OMPL which can then be optimized by CHOMP to produce a motion plan which would likely be better than a path produced by OMPL or CHOMP alone. Similarly, using the concept of Planning Adapters, other motion planners can be mixed and matched depending on the environment the robot is operating in. This section provides a step by step tutorial on using a mix and match of different motion planners and also provides insights on when to use which particular motion planners. Getting Started --------------- @@ -46,11 +46,10 @@ To achieve this, follow the steps: #. Open the ``ompl_planning_pipeline.launch`` file in the ``/launch`` folder of your robot. For the Panda robot it is this `file `_. Edit this launch file, find the lines where ```` is mentioned and change it to: :: #. The values of the ``planning_adapters`` is the order in which the mentioned adapters are called / invoked. Order here matters. Inside the CHOMP adapter, a call to OMPL is made before invoking the CHOMP optimization solver, so CHOMP takes the initial path computed by OMPL as the starting point to further optimize it. @@ -81,11 +80,10 @@ To achieve this, follow the steps: #. Open the ``stomp_planning_pipeline.launch`` file in the ``/launch`` folder of your robot. For the Panda robot it is `this `_ file. Edit this launch file, find the lines where ```` is mentioned and change it to: :: - #. The values of the ``planning_adapters`` is the order in which the mentioned adapters are called / invoked. Order here matters. Inside the CHOMP adapter, a call to STOMP is made before invoking the CHOMP optimization solver, so CHOMP takes the initial path computed by STOMP as the starting point to further optimize it. @@ -118,11 +116,10 @@ To achieve this, follow the steps: #. Open the ``ompl_planning_pipeline.launch`` file in the ``/launch`` folder of your robot. For the Panda robot it is this `file `_. Edit this launch file, find the lines where ```` is mentioned and change it to: :: - #. The values of the ``planning_adapters`` is the order in which the mentioned adapters are called / invoked. Order here matters. Inside the STOMP adapter, a call to OMPL is made before invoking the STOMP smoothing solver, so STOMP takes the initial path computed by OMPL as the starting point to further optimize it. diff --git a/doc/examples/subframes/subframes_tutorial.rst b/doc/examples/subframes/subframes_tutorial.rst index b62a5ee259..5afd044d8e 100644 --- a/doc/examples/subframes/subframes_tutorial.rst +++ b/doc/examples/subframes/subframes_tutorial.rst @@ -69,5 +69,5 @@ For older moveit_config packages that you have not generated yourself recently, required for subframes might not be configured, and the subframe link might not be found. To fix this for your moveit_config package, open the ``ompl_planning_pipeline.launch`` file in the ``/launch`` folder of your robot. For the Panda robot it is :panda_codedir:`this ` file. -Edit this launch file, find the lines where ```` is mentioned and insert ``default_planner_request_adapters/ResolveConstraintFrames`` after -the line ``default_planner_request_adapters/FixStartStatePathConstraints``. +Edit this launch file, find the lines where ```` is mentioned and insert ``default_planning_request_adapters/ResolveConstraintFrames`` after +the line ``default_planning_request_adapters/FixStartStatePathConstraints``. diff --git a/doc/examples/time_parameterization/time_parameterization_tutorial.rst b/doc/examples/time_parameterization/time_parameterization_tutorial.rst index 418c1e26c7..cab3d7a5b2 100644 --- a/doc/examples/time_parameterization/time_parameterization_tutorial.rst +++ b/doc/examples/time_parameterization/time_parameterization_tutorial.rst @@ -34,7 +34,7 @@ Finally, add the Ruckig smoothing algorithm to the list of planning request adap .. code-block:: yaml - request_adapters: >- - default_planner_request_adapters/AddRuckigTrajectorySmoothing - default_planner_request_adapters/AddTimeOptimalParameterization + response_adapters: + - default_planning_request_adapters/AddRuckigTrajectorySmoothing + - default_planning_request_adapters/AddTimeOptimalParameterization ... diff --git a/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py b/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py index ebf3d07ffe..739b51d1d9 100644 --- a/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py +++ b/doc/how_to_guides/benchmarking/launch/run_benchmarks.launch.py @@ -46,15 +46,20 @@ def generate_launch_description(): # Load additional OMPL pipeline ompl_planning_pipeline_config = { "ompl_rrtc": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """\ - default_planner_request_adapters/AddTimeOptimalParameterization \ - default_planner_request_adapters/FixWorkspaceBounds \ - default_planner_request_adapters/FixStartStateBounds \ - default_planner_request_adapters/FixStartStateCollision \ - default_planner_request_adapters/FixStartStatePathConstraints \ - """, - "start_state_max_bounds_error": 0.1, + "planning_plugins": [ + "ompl_interface/OMPLPlanner", + ], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], } } ompl_planning_yaml = load_yaml( diff --git a/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst b/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst index 9c401b427a..4baa791e0d 100644 --- a/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst +++ b/doc/how_to_guides/chomp_planner/chomp_planner_tutorial.rst @@ -98,31 +98,3 @@ OMPL is a open source library for sampling based / randomized motion planning al CHOMP: While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, CHOMP capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamic quantities such as joint velocities and accelerations. It rapidly converges to a smooth, collision-free trajectory that can be executed efficiently on the robot. A covariant update rule ensures that CHOMP quickly converges to a locally optimal trajectory. For scenes containing obstacles, CHOMP often generates paths which do not prefer smooth trajectories by addition of some noise (*ridge_factor*) in the cost function for the dynamic quantities of the robot (like acceleration, velocity). CHOMP is able to avoid obstacles in most cases, but it can fail if it gets stuck in local minima due to a bad initial guess for the trajectory. OMPL can be used to generate collision-free seed trajectories for CHOMP to mitigate this issue. - -Using CHOMP as a post-processor for OMPL ----------------------------------------- -Here, we will demonstrate that CHOMP can also be used as a post-processing optimization technique for plans obtained by other planning algorithms. The intuition behind this is that some randomized planning algorithm produces an initial guess for CHOMP. CHOMP then takes this initial guess and further optimizes the trajectory. -To achieve this, use the following steps: - -#. Edit ``ompl_planning.yaml`` in the ``/config`` folder of your robot. Add ``chomp/OptimizerAdapter`` to the bottom of the list of request_adapters: :: - - request_adapters: >- - ... - default_planner_request_adapters/FixStartStatePathConstraints - chomp/OptimizerAdapter - -#. Change the ``trajectory_initialization_method`` parameter in ``chomp_planning.yaml`` to ``fillTrajectory`` so that OMPL can provide the input for the CHOMP algorithm: :: - - trajectory_initialization_method: "fillTrajectory" - -#. Add the CHOMP config file to the launch file of your robot, ``/launch/chomp_demo.launch.py``, if it is not there already: :: - - .planning_pipelines(pipelines=["ompl", "chomp"]) - -#. Now you can launch the newly configured planning pipeline as follows: :: - - ros2 launch moveit2_tutorials chomp_demo.launch.py rviz_tutorial:=True - -This will launch RViz. Select OMPL in the Motion Planning panel under the Context tab. Set the desired start and goal states by moving the end-effector around in the same way as was done for CHOMP above. Finally click on the Plan button to start planning. The planner will now first run OMPL, then run CHOMP on OMPL's output to produce an optimized path. To make the planner's task more challenging, add obstacles to the scene using: :: - - ros2 run moveit2_tutorials collision_scene_example diff --git a/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py b/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py index b7362d3ee7..7e84fa8f73 100644 --- a/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py +++ b/doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py @@ -57,15 +57,20 @@ def launch_setup(context, *args, **kwargs): # Load additional OMPL pipeline ompl_planning_pipeline_config = { "ompl_2": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """\ - default_planner_request_adapters/AddTimeOptimalParameterization \ - default_planner_request_adapters/FixWorkspaceBounds \ - default_planner_request_adapters/FixStartStateBounds \ - default_planner_request_adapters/FixStartStateCollision \ - default_planner_request_adapters/FixStartStatePathConstraints \ - """, - "start_state_max_bounds_error": 0.1, + "planning_plugins": [ + "ompl_interface/OMPLPlanner", + ], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], } } ompl_planning_yaml = load_yaml( diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 3a20e20a73..5535a2af3b 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -299,7 +299,7 @@ Using the planner The *pilz_industrial_motion_planner::CommandPlanner* is provided as a MoveIt Motion Planning Pipeline and, therefore, can be used with all other manipulators using MoveIt. Loading the plugin requires the param -``/move_group//planning_plugin`` to be set to ``pilz_industrial_motion_planner/CommandPlanner`` +``/move_group//planning_plugins`` to be set to ``[pilz_industrial_motion_planner/CommandPlanner]`` before the ``move_group`` node is started. For example, the `panda_moveit_config package `_ @@ -308,7 +308,7 @@ has a ``pilz_industrial_motion_planner`` pipeline set up as follows: :: - ros2 param get /move_group pilz_industrial_motion_planner.planning_plugin + ros2 param get /move_group pilz_industrial_motion_planner.planning_plugins String value is: pilz_industrial_motion_planner/CommandPlanner diff --git a/doc/how_to_guides/pipeline_testbench/CMakeLists.txt b/doc/how_to_guides/pipeline_testbench/CMakeLists.txt new file mode 100644 index 0000000000..42ae2881aa --- /dev/null +++ b/doc/how_to_guides/pipeline_testbench/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(pipeline_testbench_example src/pipeline_testbench_main.cpp) +target_include_directories(pipeline_testbench_example PRIVATE include) +ament_target_dependencies(pipeline_testbench_example ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + +install(TARGETS pipeline_testbench_example + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/how_to_guides/pipeline_testbench/config/testbench_config.rviz b/doc/how_to_guides/pipeline_testbench/config/testbench_config.rviz new file mode 100644 index 0000000000..3a01ab27cd --- /dev/null +++ b/doc/how_to_guides/pipeline_testbench/config/testbench_config.rviz @@ -0,0 +1,473 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 671 + - 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: false + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /moveit_cpp/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 + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: false + - 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: + Path: true + Sphere: true + Text: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep All + Reliability Policy: Reliable + Value: /stomp_visualization + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Path: true + Sphere: true + Text: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pipeline_testbench + 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: 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.8018128871917725 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.4451259672641754 + Y: -0.10787936300039291 + Z: 0.5332368612289429 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.16039825975894928 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.0435791015625 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1058 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000164000003ccfc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000023000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000002da000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000003b000003cc0000003f00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000293000001740000016900ffffff000000010000010f000003ccfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003cc000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000616000003cc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 22 diff --git a/doc/how_to_guides/pipeline_testbench/config/testbench_moveit_cpp.yaml b/doc/how_to_guides/pipeline_testbench/config/testbench_moveit_cpp.yaml new file mode 100644 index 0000000000..07e1739470 --- /dev/null +++ b/doc/how_to_guides/pipeline_testbench/config/testbench_moveit_cpp.yaml @@ -0,0 +1,21 @@ +# Default configurations. More information can be found in the moveit_cpp and planning scene monitor tutorials +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +# Define which planning pipelines are loaded by moveit_cpp. These loaded pipelines will be available for simple and parallel planning +planning_pipelines: + pipeline_names: ["stomp", "ompl", "ompl_stomp"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: stomp + planner_id: "stomp" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 4.0 diff --git a/doc/how_to_guides/pipeline_testbench/launch/pipeline_testbench.launch.py b/doc/how_to_guides/pipeline_testbench/launch/pipeline_testbench.launch.py new file mode 100644 index 0000000000..5960f1c2cf --- /dev/null +++ b/doc/how_to_guides/pipeline_testbench/launch/pipeline_testbench.launch.py @@ -0,0 +1,200 @@ +import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "rviz_config", + default_value="panda_moveit_config_demo.rviz", + description="RViz configuration file", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) + + +def launch_setup(context, *args, **kwargs): + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines(pipelines=["ompl", "stomp"]) + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .moveit_cpp( + os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "testbench_moveit_cpp.yaml", + ) + ) + .to_moveit_configs() + ) + + # Warehouse config + sqlite_database = os.path.join( + get_package_share_directory("moveit_benchmark_resources"), + "databases", + "panda_kitchen_test_db.sqlite", + ) + + warehouse_ros_config = { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "warehouse": { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "host": sqlite_database, + "port": 33828, + "scene_name": "kitchen_panda_scene_sensed1", + }, + } + + # Load additional OMPL-STOMP chain pipeline + ompl_stomp_planning_pipeline_config = { + "ompl_stomp": { + "planning_plugins": [ + "ompl_interface/OMPLPlanner", + "stomp_moveit/StompPlanner", + ], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], + "planner_configs": { + "RRTConnectkConfigDefault": { + "type": "geometric::RRTConnect", + "range": 0.0, # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()} + } + }, + } + } + + # MoveItCpp demo executable + moveit_cpp_node = Node( + name="pipeline_testbench_example", + package="moveit2_tutorials", + executable="pipeline_testbench_example", + output="screen", + parameters=[ + moveit_config.to_dict(), + ompl_stomp_planning_pipeline_config, + warehouse_ros_config, + ], + ) + + # RViz + rviz_config_file = os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "testbench_config.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, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # 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="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + nodes_to_start = [ + static_tf, + robot_state_publisher, + rviz_node, + moveit_cpp_node, + ros2_control_node, + joint_state_broadcaster_spawner, + arm_controller_spawner, + hand_controller_spawner, + ] + + return nodes_to_start diff --git a/doc/how_to_guides/pipeline_testbench/pipeline_testbench.rst b/doc/how_to_guides/pipeline_testbench/pipeline_testbench.rst new file mode 100644 index 0000000000..b34ef7ede8 --- /dev/null +++ b/doc/how_to_guides/pipeline_testbench/pipeline_testbench.rst @@ -0,0 +1,3 @@ +Experimenting with the MoveIt Planning Pipeline +=============================================== +TODO diff --git a/doc/how_to_guides/pipeline_testbench/src/pipeline_testbench_main.cpp b/doc/how_to_guides/pipeline_testbench/src/pipeline_testbench_main.cpp new file mode 100644 index 0000000000..2278037ac1 --- /dev/null +++ b/doc/how_to_guides/pipeline_testbench/src/pipeline_testbench_main.cpp @@ -0,0 +1,333 @@ +#include +#include +// MoveitCpp +#include +#include + +#include + +#include + +// Warehouse +#include +#include +#include +#include +#include +#include + +namespace rvt = rviz_visual_tools; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("pipeline_testbench"); +const std::string PLANNING_GROUP = "panda_arm"; +static const std::vector CONTROLLERS(1, "panda_arm_controller"); +} // namespace +namespace pipeline_testbench +{ + +// Color to string +[[nodiscard]] std::string colorToString(rviz_visual_tools::Colors rviz_color) +{ + std::string color_str = "unknown"; + switch (rviz_color) + { + case rviz_visual_tools::Colors::BLACK: + color_str = "black"; + break; + case rviz_visual_tools::Colors::BROWN: + color_str = "brown"; + break; + case rviz_visual_tools::Colors::BLUE: + color_str = "blue"; + break; + case rviz_visual_tools::Colors::CYAN: + color_str = "cyan"; + break; + case rviz_visual_tools::Colors::GREY: + color_str = "grey"; + break; + case rviz_visual_tools::Colors::DARK_GREY: + color_str = "dark grey"; + break; + case rviz_visual_tools::Colors::GREEN: + color_str = "green"; + break; + case rviz_visual_tools::Colors::LIME_GREEN: + color_str = "lime green"; + break; + case rviz_visual_tools::Colors::MAGENTA: + color_str = "magenta"; + break; + case rviz_visual_tools::Colors::ORANGE: + color_str = "orange"; + break; + case rviz_visual_tools::Colors::PURPLE: + color_str = "purple"; + break; + case rviz_visual_tools::Colors::RED: + color_str = "red"; + break; + case rviz_visual_tools::Colors::PINK: + color_str = "pink"; + break; + case rviz_visual_tools::Colors::WHITE: + color_str = "white"; + break; + case rviz_visual_tools::Colors::YELLOW: + color_str = "yellow"; + break; + default: + break; + } + return color_str; +} + +/// \brief Utility class to create and interact with the parallel planning demo +class Demo +{ +public: + Demo(rclcpp::Node::SharedPtr node) + : node_{ node } + , moveit_cpp_{ std::make_shared(node) } + , planning_component_{ std::make_shared(PLANNING_GROUP, moveit_cpp_) } + , visual_tools_(node, "panda_link0", "pipeline_testbench", moveit_cpp_->getPlanningSceneMonitorNonConst()) + { + moveit_cpp_->getPlanningSceneMonitorNonConst()->providePlanningSceneService(); + + visual_tools_.deleteAllMarkers(); + visual_tools_.loadRemoteControl(); + + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.75; + visual_tools_.publishText(text_pose, "Pipeline Testbench", rvt::WHITE, rvt::XLARGE); + visual_tools_.trigger(); + } + + bool loadPlanningSceneAndQuery() + { + std::string hostname = ""; + int port = 0.0; + std::string scene_name = ""; + + node_->get_parameter_or(std::string("warehouse.host"), hostname, std::string("127.0.0.1")); + node_->get_parameter_or(std::string("warehouse.port"), port, 33829); + + if (!node_->get_parameter("warehouse.scene_name", scene_name)) + { + RCLCPP_WARN(LOGGER, "Warehouse scene_name NOT specified"); + } + + moveit_warehouse::PlanningSceneStorage* planning_scene_storage = nullptr; + + // Initialize database connection + try + { + warehouse_ros::DatabaseLoader db_loader(node_); + warehouse_ros::DatabaseConnection::Ptr warehouse_connection = db_loader.loadDatabase(); + warehouse_connection->setParams(hostname, port, 20); + if (warehouse_connection->connect()) + { + planning_scene_storage = new moveit_warehouse::PlanningSceneStorage(warehouse_connection); + RCLCPP_INFO(LOGGER, "Connected to database: '%s'", hostname.c_str()); + } + else + { + RCLCPP_ERROR(LOGGER, "Failed to connect to database"); + return false; + } + } + catch (std::exception& e) + { + RCLCPP_ERROR(LOGGER, "Failed to initialize planning scene storage: '%s'", e.what()); + return false; + } + + // Load planning scene + moveit_msgs::msg::PlanningScene scene_msg; + try + { + if (!planning_scene_storage) + { + RCLCPP_ERROR(LOGGER, "No planning scene storage"); + return false; + } + + if (planning_scene_storage->hasPlanningScene(scene_name)) // Just the world (no robot) + { + moveit_msgs::msg::PlanningSceneWorld world_meta_data; + if (!planning_scene_storage->getPlanningSceneWorld(world_meta_data, scene_name)) + { + RCLCPP_ERROR(LOGGER, "Failed to load planning scene world '%s'", scene_name.c_str()); + return false; + } + scene_msg.world = world_meta_data; + scene_msg.robot_model_name = "No robot information. Using only world geometry."; + } + else + { + RCLCPP_ERROR(LOGGER, "Failed to find planning scene '%s'", scene_name.c_str()); + return false; + } + } + catch (std::exception& ex) + { + RCLCPP_ERROR(LOGGER, "Error loading planning scene: %s", ex.what()); + return false; + } + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_->getPlanningSceneMonitorNonConst()); + scene->processPlanningSceneWorldMsg(scene_msg.world); + } // Unlock PlanningScene + + RCLCPP_INFO(LOGGER, "Loaded planning scene successfully"); + + // Get planning scene query + for (int index = 0; index < 10; index++) + { + moveit_warehouse::MotionPlanRequestWithMetadata planning_query; + std::string query_name = "kitchen_panda_scene_sensed" + std::to_string(index) + "_query"; + try + { + planning_scene_storage->getPlanningQuery(planning_query, scene_name, query_name); + } + catch (std::exception& ex) + { + RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what()); + } + + motion_plan_requests.push_back(static_cast(*planning_query)); + } + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + visual_tools_.trigger(); + return true; + } + + struct PipelineConfig + { + std::string planning_pipeline; + std::string planner_id; + }; + + /// \brief Request a motion plan based on the assumption that a goal is set and print debug information. + void planAndVisualize(std::vector pipeline_configs, + moveit_msgs::msg::MotionPlanRequest const& motion_plan_request) + { + visual_tools_.deleteAllMarkers(); + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // Set goal state + planning_component_->setGoal(motion_plan_request.goal_constraints); + // Set start state as current state + planning_component_->setStartStateToCurrentState(); + + // Get start state + auto robot_start_state = planning_component_->getStartState(); + + // Get planning scene + auto planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->updateFrameTransforms(); + auto planning_scene = [planning_scene_monitor] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + return planning_scene::PlanningScene::clone(ls); + }(); + + auto group_name = motion_plan_request.group_name; + + moveit_cpp::PlanningComponent::PlanRequestParameters plan_request_parameters; + plan_request_parameters.load(node_); + RCLCPP_INFO_STREAM( + LOGGER, "Default plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ',' + << " planner_id: " << plan_request_parameters.planner_id << ',' + << " planning_time: " << plan_request_parameters.planning_time << ',' + << " planning_attempts: " << plan_request_parameters.planning_attempts << ',' + << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ',' + << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); + + std::vector solutions; + solutions.reserve(pipeline_configs.size()); + for (auto const& pipeline_config : pipeline_configs) + { + plan_request_parameters.planning_pipeline = pipeline_config.planning_pipeline; + plan_request_parameters.planner_id = pipeline_config.planner_id; + auto solution = planning_component_->plan(plan_request_parameters, planning_scene); + solution.planner_id = pipeline_config.planning_pipeline; // TODO(sjahr): Fix this in MoveIt planning pipeline + solutions.push_back(solution); + } + + int color_index = 1; + auto robot_model_ptr = moveit_cpp_->getRobotModel(); + auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + // Check if PlanningComponents succeeded in finding the plan + for (auto const& plan_solution : solutions) + { + if (plan_solution.trajectory) + { + RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str() + << ": " << colorToString(rviz_visual_tools::Colors(color_index)) + << ", Path length: " << robot_trajectory::pathLength(*plan_solution.trajectory)); + // Visualize the trajectory in Rviz + visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr, + rviz_visual_tools::Colors(color_index)); + color_index++; + } + } + visual_tools_.trigger(); + } + + moveit_visual_tools::MoveItVisualTools& getVisualTools() + { + return visual_tools_; + } + + std::vector getMotionPlanRequests() + { + return motion_plan_requests; + } + +private: + std::shared_ptr node_; + std::shared_ptr moveit_cpp_; + std::shared_ptr planning_component_; + moveit_visual_tools::MoveItVisualTools visual_tools_; + std::vector motion_plan_requests; +}; +} // namespace pipeline_testbench + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + RCLCPP_INFO(LOGGER, "Initialize node"); + + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("pipeline_testbench", "", node_options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + + pipeline_testbench::Demo demo(node); + + if (!demo.loadPlanningSceneAndQuery()) + { + rclcpp::shutdown(); + return 0; + } + + RCLCPP_INFO(LOGGER, "Starting Pipeline Testbench example ..."); + for (auto const& motion_plan_req : demo.getMotionPlanRequests()) + { + demo.planAndVisualize( + { { "ompl", "RRTConnectkConfigDefault" }, { "stomp", "stomp" }, { "ompl_stomp", "RRTConnectkConfigDefault" } }, + motion_plan_req); + } + + demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); + rclcpp::shutdown(); + return 0; +} diff --git a/doc/how_to_guides/stomp_planner/stomp_planner.rst b/doc/how_to_guides/stomp_planner/stomp_planner.rst index f99c31cfc8..e97299b692 100644 --- a/doc/how_to_guides/stomp_planner/stomp_planner.rst +++ b/doc/how_to_guides/stomp_planner/stomp_planner.rst @@ -31,13 +31,17 @@ Using STOMP with Your Robot #. Simply add the `stomp_planning.yaml `__ configuration file into the config directory of your MoveIt config package. It contains the plugin identifier, a planning pipeline adapter list, and the STOMP planning parameters. The config file should look like example below: :: - planning_plugin: stomp_moveit/StompPlanner - request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints + planning_plugins: + - stomp_moveit/StompPlanner + request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision + response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath stomp_moveit: num_timesteps: 60 @@ -51,24 +55,6 @@ Using STOMP with Your Robot #. Configure MoveIt to load the STOMP planning pipeline by adding "stomp" to your MoveItConfiguration launch statement next to "ompl" and the other planners. You can find an example for this in the `demo.launch.py `_ of the Panda config. -Using STOMP's planner adapter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -STOMP can also be used for smoothing and optimizing trajectories from other planner plugins using the ``StompSmoothingAdapter`` plugin. -The only step needed is to add the plugin name ``stomp_moveit/StompSmoothingAdapter`` to the ``request_adapters`` parameter list configured for the planning pipeline: :: - - request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/FixWorkspaceBounds - default_planner_request_adapters/FixStartStateBounds - default_planner_request_adapters/FixStartStateCollision - default_planner_request_adapters/FixStartStatePathConstraints - stomp_moveit/StompSmoothingAdapter - -In addition, STOMP parameters can be specified just like for the usual planning setup. -An important detail is that now the parameter ``num_iterations_after_valid`` is used for specifying the smoothing steps since the input trajectory is already valid. -It should therefore be larger than 0 to have an effect. - Running the Demo ---------------- If you have the ``panda_moveit_config`` from the `ros-planning/moveit_resources `_ repository you should be able to simply launch the demo setup and start planning with STOMP in RViZ ::