Parallel planning example by sjahr · Pull Request #551 · moveit/moveit2_tutorials

sjahr · 2022-11-28T15:52:31Z

Description

This PR adds an example of how to use the recently merged parallel planning interface for moveit2. To test it, run

ros2 launch moveit2_tutorials parallel_planning_example.launch.py

Checklist

Required by CI: Code is auto formatted using clang-format
While waiting for someone to review your request, please consider reviewing another open pull request to support the maintainers

doc/examples/parallel_planning/CMakeLists.txt

mamoll

Looks good. I'd add another ompl planner to the example config. and show how you can use multiple planners from the same planner pipeline.

doc/examples/parallel_planning/parallel_planning_tutorial.rst

doc/examples/parallel_planning/src/parallel_planning_main.cpp

CMakeLists.txt

doc/examples/parallel_planning/parallel_planning_tutorial.rst

stephanie-eng · 2022-11-28T17:31:10Z

Your title is a bit long, which makes the formatting of the header a bit crowded:

This is really not a real issue.

doc/examples/parallel_planning/parallel_planning_tutorial.rst

sea-bass

This looks great and worked for me no problem.

It would be nice if the tutorial printed not just how long a solution tool, but which planner ID was being used. I know the info is there if you read all the node names, but it would be a nice convenience to the user to just show things in one line.

Also, this doesn't have to be for this PR, but having a MoveGroup Interface example would be good as well.

doc/examples/parallel_planning/config/parallel_planning_moveit_cpp.yaml

doc/examples/parallel_planning/parallel_planning_tutorial.rst

doc/examples/parallel_planning/src/parallel_planning_main.cpp

sea-bass

Did another pass, but it looks good for me after these small tweaks.

doc/how_to_guides/parallel_planning/config/parallel_planning_moveit_cpp.yaml

doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

ChrisThrasher

No more comments

sea-bass

I'm good with this PR, but have more comments to add if you want!

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

sea-bass · 2022-12-07T19:10:10Z

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

+      node_, { "ompl_rrtc", "pilz_lin", "chomp" }
+    };
+
+The constructor of this function will initialize :code:`PlanningRequestParameter` configurations based on the config that is provided in the node's


Should this also be MultiPipelinePlanRequestParameters, or am I wrong here?

No, I rephrased the sentence to make it clearer what I am talking about:

The constructor of this class will initialize multiple :code:`PlanningRequestParameter` class members

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp

sea-bass · 2022-12-07T19:15:45Z

doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp

+  void setJointGoal(double const panda_joint1, double const panda_joint2, double const panda_joint3,
+                    double const panda_joint4, double const panda_joint5, double const panda_joint6,
+                    double const panda_joint7)
+  {
+    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);
+
+    // Set goal state
+    planning_component_->setGoal(*robot_goal_state);
+  }


... part of me wants to recommend that this accept a std::vector or something, but it's fine if you don't want to do that extra work.

If you went that way you'd have to loop through like this (UNTESTED)

void setJointGoal(std::vector<double> const& panda_joint_angles) { size_t idx = 0; auto robot_goal_state = planning_component_->getStartState(); for (const auto& joint_angle : panda_joint_angles) { robot_goal_state->setJointPositions("panda_joint" + std::to_string(idx++), joint_angle); } planning_component_->setGoal(*robot_goal_state); }

I see you point. Maybe this would be worth adding to the PlanningComponent class

sea-bass

Few small things, I'm good after this

sea-bass · 2022-12-09T18:39:44Z

doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py

+            "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""",


Indentation is weird here

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

sea-bass · 2022-12-09T18:41:09Z

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

+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 have been terminated. Please keep in mind, that no solution is also a possible result, the :code:`solution_selection_callback` is called to determine which


I commented on this earlier but it's gone away.

This "Once all pipelines have been terminated." sentence looks like an incomplete sentence that was left behind. I'm not sure what this means as it is.

Sorry, I put Please keep in mind, that no solution is also a possible result. in the middle of a sentence

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

sea-bass · 2022-12-09T18:42:21Z

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

+    # PlanRequestParameters for the first parallel pipeline
+    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
+
+    # PlanRequestParameters for the second parallel pipeline
+    pilz_lin:
+      plan_request_params:
+        planning_attempts: 1
+        planning_pipeline: pilz_industrial_motion_planner
+        planner_id: "LIN"
+        max_velocity_scaling_factor: 1.0
+        max_acceleration_scaling_factor: 1.0
+        planning_time: 0.8
+
+    # PlanRequestParameters for the third parallel pipeline
+    chomp:
+      plan_request_params:
+        planning_attempts: 1
+        planning_pipeline: chomp
+        max_velocity_scaling_factor: 1.0
+        max_acceleration_scaling_factor: 1.0
+        planning_time: 1.5


For these comments, maybe add more than just "first, second, and third" and describe them as you did in the fourth one?

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

Co-authored-by: Chris Thrasher <chrisjthrasher@gmail.com> Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com>

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

sea-bass

SHIP IT

sjahr force-pushed the pr_parallel_planning_example branch from 2eb9bc2 to ea2457b Compare November 28, 2022 15:56

sjahr mentioned this pull request Nov 28, 2022

WIP: Parallel planning with moveit_cpp / path quality improvement guide #493

Closed

2 tasks

sjahr force-pushed the pr_parallel_planning_example branch from 0fae6e4 to db1fe1d Compare November 28, 2022 16:38

sjahr requested a review from sea-bass November 28, 2022 16:39

ChrisThrasher reviewed Nov 28, 2022

View reviewed changes

doc/examples/parallel_planning/CMakeLists.txt Outdated Show resolved Hide resolved

mamoll approved these changes Nov 28, 2022

View reviewed changes

stephanie-eng reviewed Nov 28, 2022

View reviewed changes

doc/examples/parallel_planning/parallel_planning_tutorial.rst Outdated Show resolved Hide resolved

sea-bass reviewed Nov 29, 2022

View reviewed changes

sjahr force-pushed the pr_parallel_planning_example branch from c67dbc8 to edaf149 Compare November 30, 2022 10:23

sjahr requested review from ChrisThrasher, mamoll, sea-bass and stephanie-eng November 30, 2022 10:51

sea-bass reviewed Nov 30, 2022

View reviewed changes

sjahr requested review from ChrisThrasher and sea-bass and removed request for ChrisThrasher December 7, 2022 18:33

ChrisThrasher reviewed Dec 7, 2022

View reviewed changes

sea-bass approved these changes Dec 7, 2022

View reviewed changes

sjahr requested a review from sea-bass December 9, 2022 17:41

sea-bass reviewed Dec 9, 2022

View reviewed changes

sjahr force-pushed the pr_parallel_planning_example branch from e39578b to d7b0438 Compare December 9, 2022 21:06

sjahr added 6 commits December 9, 2022 14:07

First demo and tests

746cc15

Add path length evaluation function

8abb0a6

Add difficult collision object

ea5dd88

Better rviz config

9dc252a

Reorder experiments

b296c3d

Fix cart. orientation and add visiual text before each experiment

dd7f472

sjahr and others added 28 commits December 9, 2022 14:07

Cleanup config

80648ee

Cleanup + RVIZ config

f1080c8

Change upstream dependency so branch builds

be5af7b

Adjust to moveit_cpp changes

43e38cf

Switch back to moveit2 main

ca86a00

WIP

5a1c18c

Cleanup and fix launch file

09ea5d1

Small demo adjustments

85f94ab

Write tutorial text

e0d4b37

Bug fixing and cleanups

0d01fe1

Cleanup & use .rst file

2fa8776

Better visualzation

cd5ea17

Load planning problem from db

5f4c377

Undo temp fixes and changes

294c9c0

Update fasted to shortest

e27ef65

Language and spelling 1

b1f894d

Apply suggestions from code review

e8dbb4c

Co-authored-by: Chris Thrasher <chrisjthrasher@gmail.com> Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com>

Move parallel planning into how_to_guides

36a046c

Address review and cleanup

50cbd31

Add second OMPL pipeline

678821a

Apply suggestions from code review

d1d9dac

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Address review

b327668

Format

2256f3b

Apply suggestions from code review

5690562

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Address review

3459766

Fix bug

0744ce4

Apply suggestions from code review

39b4955

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Address review 3

d7b0438

sea-bass approved these changes Dec 9, 2022

View reviewed changes

sjahr merged commit ee4f9b2 into moveit:main Dec 9, 2022

		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 have been terminated. Please keep in mind, that no solution is also a possible result, the :code:`solution_selection_callback` is called to determine which

Conversation

sjahr commented Nov 28, 2022

Description

Checklist

Uh oh!

Uh oh!

mamoll left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

stephanie-eng commented Nov 28, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

sea-bass left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

sea-bass left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

ChrisThrasher left a comment

Choose a reason for hiding this comment

Uh oh!

sea-bass left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

sea-bass left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

stephanie-eng commented Nov 28, 2022 •

edited

Loading

sea-bass left a comment •

edited

Loading