From c67dbc882967fcfb7951fb04ddce4c9450d277e5 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 30 Nov 2022 10:54:56 +0100 Subject: [PATCH] Address review and cleanup --- .../config/parallel_planning_config.rviz | 18 ++++++++------- .../config/parallel_planning_moveit_cpp.yaml | 23 +++++++++++-------- .../parallel_planning_tutorial.rst | 20 ++++++++-------- .../src/parallel_planning_main.cpp | 17 ++++++-------- 4 files changed, 42 insertions(+), 36 deletions(-) diff --git a/doc/how_to_guides/parallel_planning/config/parallel_planning_config.rviz b/doc/how_to_guides/parallel_planning/config/parallel_planning_config.rviz index 583301a7bb..04d7bb3483 100644 --- a/doc/how_to_guides/parallel_planning/config/parallel_planning_config.rviz +++ b/doc/how_to_guides/parallel_planning/config/parallel_planning_config.rviz @@ -9,7 +9,7 @@ Panels: - /PlanningScene1 - /Trajectory1 Splitter Ratio: 0.5 - Tree Height: 679 + Tree Height: 671 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -157,6 +157,8 @@ Visualization Manager: Enabled: true Name: MarkerArray Namespaces: + Path: true + Sphere: true Text: true Topic: Depth: 5 @@ -411,25 +413,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 5.1277079582214355 + 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.2651444971561432 - Y: 0.5764509439468384 - Z: 0.707146406173706 + 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.27039825916290283 + Pitch: 0.16039825975894928 Target Frame: Value: Orbit (rviz) - Yaw: 6.033580303192139 + Yaw: 4.0435791015625 Saved: ~ Window Geometry: Displays: @@ -441,7 +443,7 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RvizVisualToolsGui: collapsed: false Selection: diff --git a/doc/how_to_guides/parallel_planning/config/parallel_planning_moveit_cpp.yaml b/doc/how_to_guides/parallel_planning/config/parallel_planning_moveit_cpp.yaml index 15ea0cf423..c6f965ef08 100644 --- a/doc/how_to_guides/parallel_planning/config/parallel_planning_moveit_cpp.yaml +++ b/doc/how_to_guides/parallel_planning/config/parallel_planning_moveit_cpp.yaml @@ -1,3 +1,4 @@ +# 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" @@ -7,18 +8,22 @@ planning_scene_monitor_options: 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: - #namespace: "moveit_cpp" # optional, default is ~ pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"] -ompl_rrtc: - plan_request_params: - planning_attempts: 1 - planning_pipeline: ompl - planner_id: "RRTConnectkConfigDefault" - max_velocity_scaling_factor: 1.0 - max_acceleration_scaling_factor: 1.0 - planning_time: 0.5 +# Configure parameters of a multi-pipeline plan request. The actual request is created in the cpp code when calling for example: +# MultiPipelinePlanRequestParameters multi_pipeline_plan_request{ +# node, { "ompl_rrtc", "pilz_lin" } +# }; +ompl_rrtc: # Namespace for individual plan request + plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp + planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem + planning_pipeline: ompl # Name of the pipeline that is being used + planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline + max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + planning_time: 0.5 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned pilz_lin: plan_request_params: diff --git a/doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst b/doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst index 765d73c50e..c198b1f07f 100644 --- a/doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst +++ b/doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst @@ -1,5 +1,5 @@ -How to run multiple planning pipelines in parallel with MoveItCpp? -================================================================== +Running Multiple Planning Pipelines in Parallel with MoveItCpp +============================================================== MoveItCpp offers an API that allows: @@ -9,7 +9,7 @@ MoveItCpp offers an API that allows: Using multiple pipelines can be beneficial for several reasons, including: -- It is unsure which planner will produce the best solution for a given planning problem +- The planner that will produce the best solution is not known a priori - There is a chance that the preferred planner might fail and a backup solution should be available A general introduction to moveit_cpp can be found in the :doc:`/doc/examples/moveit_cpp/moveitcpp_tutorial`. @@ -27,25 +27,27 @@ of the planning component's :code:`plan(...)` function is used: SolutionCallbackFunction solution_selection_callback, StoppingCriterionFunction stopping_criterion_callback) -This function tries to plan a trajectory from the a start state, to a goal state that satisfy a set of constraints. Based on the configuration +This function tries to plan a trajectory from a start state to a goal state that satisfies a set of constraints. Based on the configuration provided by the :code:`parameters`, multiple threads are launched and each tries to solve the planning problem with a different planning pipeline. Once -all pipelines found a solution (Reminder: No solution is also a possible result), the :code:`solution_selection_callback` is called to determine which +all pipelines have been terminated (Reminder: No solution is also a possible result), the :code:`solution_selection_callback` is called to determine which solution is returned as :code:`MotionPlanResponse`. By default, all pipelines use their time budget defined by the :code:`planning_time` field of the :code:`PlanRequestParameters`, but it is possible to terminate the parallel planning earlier by using the :code:`stopping_criterion_callback`. This function is iteratively called during the parallel planning process and, if the stopping criterion is met, terminates pipelines that have not found a solution yet. -Demo ----- +Example +------- The following demo serves as an example of how you can configure and use MoveItCpp's parallel planning interface. First of all, let's run the demo: :: ros2 launch moveit2_tutorials parallel_planning_example.launch.py -A complex kitchen scene is loaded and two planning problems solved. The first one is a small motion of the EE towards the ground. This problem is likely to be solved by all three +A complex kitchen scene is loaded and two planning problems are solved. The first one is a small motion of the end effector towards the ground. This problem is likely to be solved by all three planners, but with significant differences in the planning time. The second problem is a much harder and most likely only the :code:`RRTConnect` planner will succeed. This demo suggests that a well-configured parallel planning setup is versatile, and can be used in a broad variety of motion planning problems. What code is necessary to use parallel planning? +------------------------------------------------ + First, you need to initialize :code:`moveit_cpp` and a planning component that will solve your planning problems. Next, you need to set start state and goal constraints: .. code-block:: c++ @@ -159,4 +161,4 @@ Once :code:`MultiPipelinePlanRequestParameters` and optionally :code:`SolutionCa Tips ---- -- When you want to use multiple pipelines with the same planner parallel it is recommended to initialize multiple planning pipelines in moveit_cpp rather than using the same one in multiple parallel planning requests +- When you want to use the same pipeline with the different planners (e.g. PILZ planner with PTP and LIN) in parallel, it is recommended to initialize multiple planning pipelines in moveit_cpp rather than using the same one in multiple parallel planning requests diff --git a/doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp b/doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp index ee87819c3c..9bc7f80eb4 100644 --- a/doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp +++ b/doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp @@ -86,8 +86,6 @@ class Demo text_pose.translation().z() = 1.75; visual_tools_.publishText(text_pose, "Parallel Planning Tutorial", rvt::WHITE, rvt::XLARGE); visual_tools_.trigger(); - - visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); } bool loadPlanningSceneAndQuery() @@ -184,6 +182,8 @@ class Demo } planning_query_request_ = static_cast(*planning_query); + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + visual_tools_.trigger(); return true; } @@ -196,11 +196,11 @@ class Demo auto robot_goal_state = planning_component_->getStartState(); robot_goal_state->setJointPositions("panda_joint1", &panda_joint1); robot_goal_state->setJointPositions("panda_joint2", &panda_joint2); + robot_goal_state->setJointPositions("panda_joint3", &panda_joint3); robot_goal_state->setJointPositions("panda_joint4", &panda_joint4); robot_goal_state->setJointPositions("panda_joint5", &panda_joint5); robot_goal_state->setJointPositions("panda_joint6", &panda_joint6); robot_goal_state->setJointPositions("panda_joint7", &panda_joint7); - robot_goal_state->setJointPositions("panda_joint3", &panda_joint3); // Set goal state planning_component_->setGoal(*robot_goal_state); @@ -224,6 +224,8 @@ class Demo // Set start state as current state planning_component_->setStartStateToCurrentState(); + // The MultiPipelinePlanRequestParameters choose a set of planning pipelines to be used for parallel planning. Here, + // we use all available pipelines but it is also possible to use a subset of pipelines. moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request{ node_, { "ompl_rrtc", "pilz_lin", "chomp" } }; @@ -274,9 +276,6 @@ int main(int argc, char** argv) parallel_planning_example::Demo demo(node); - /* Otherwise robot with zeros joint_states */ - rclcpp::sleep_for(std::chrono::seconds(1)); - if (!demo.loadPlanningSceneAndQuery()) { rclcpp::shutdown(); @@ -285,15 +284,13 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials..."); - // // Experiment 1 - Short free-space motion - // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // Experiment 1 - Short free-space motion, PILZ is expected to create the fastest and shortest solution RCLCPP_INFO(LOGGER, "Experiment 1 - Short free-space motion"); demo.setJointGoal(0.0, -0.8144019900299497, 0.0, -2.6488387075338133, 0.0, 1.8344367175038623, 0.7849999829891612); demo.planAndPrint(); - // // Experiment 2 - Long motion with collisions - // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // Experiment 2 - Long motion with collisions, CHOMP and PILZ are likely to fail here due to the difficulty of the planning problem RCLCPP_INFO(LOGGER, "Experiment 2 - Long motion with collisions"); demo.setQueryGoal(); demo.planAndPrint();