Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable parallel planning with PipelinePlanner #450

Merged
merged 7 commits into from
Oct 4, 2023

Conversation

sjahr
Copy link
Contributor

@sjahr sjahr commented Mar 30, 2023

This PR refactors the PipelinePlanner to enable the use of MoveIt's parallel planning API. Usage would look for example like this

  // Configure a set of pipelines to be used
  std::unordered_map<std::string, std::string> pipeline_id_planner_id_map;
  pipeline_id_planner_id_map["ompl"] = "RRTConnectkConfigDefault";
  pipeline_id_planner_id_map["pilz_industrial_motion_planner"] = "LIN";
  pipeline_id_planner_id_map["chomp"] = "chomp";

  // Construct solver
  auto parallel_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_, pipeline_id_planner_id_map);
  
  // Optional: Configure custom solution selection and/or stopping criterion
  parallel_planner->setStoppingCriterionFunction(...);
  parallel_planner->setSolutionSelectionFunction(
      [](const std::vector<::planning_interface::MotionPlanResponse>& solutions) {...});

You can test the PR by using my fork of the moveit2_tutorials. Build the custom MTC and moveit2_tutorials branch and run in two terminal windows:

  1. ros2 launch moveit2_tutorials mtc_demo.launch.py
  2. ros2 launch moveit2_tutorials pick_place_demo.launch.py

@rhaschke
Copy link
Contributor

Have a look at #429, which provides parallel planning functionality not only including pipeline planners but also joint interpolation and Cartesian interpolation.

@sjahr sjahr changed the title WIP: Enable parallel planning with PipelinePlanner Enable parallel planning with PipelinePlanner Mar 31, 2023
@sjahr sjahr marked this pull request as ready for review March 31, 2023 16:13
@sjahr
Copy link
Contributor Author

sjahr commented Mar 31, 2023

Have a look at #429, which provides parallel planning functionality not only including pipeline planners but also joint interpolation and Cartesian interpolation.

Cool! I was not aware of this. As I understand it, it is still different from the proposed Parallel Planning solver as you can not customize stopping and solution selection criteria and the MultiPlanner is capable of using multiple MTC solvers at the same time. So I'd see both as alternatives.

@rhaschke
Copy link
Contributor

That's true. The MultiPlanner defines a sequence of planning alternatives that are tried one after the other, while parallel planning considers all alternatives in parallel. Would it be possible to apply the same approach as in MultiPlanner to realize your goal? This would allow to include joint-space and Cartesian-space interpolation planners as alternatives as well.

By the way, a stopping criterium might be useful for the existing sequential MultiPlanner as well - to continue planning if the criterium (e.g. smoothness) is not yet satisfied.

What about the new names FallbacksPlanner and AlternativesPlanner to emphasize the similarity to the corresponding parallel containers?

@sjahr
Copy link
Contributor Author

sjahr commented Apr 3, 2023

That's true. The MultiPlanner defines a sequence of planning alternatives that are tried one after the other, while parallel planning considers all alternatives in parallel. Would it be possible to apply the same approach as in MultiPlanner to realize your goal? This would allow to include joint-space and Cartesian-space interpolation planners as alternatives as well.

By the way, a stopping criterium might be useful for the existing sequential MultiPlanner as well - to continue planning if the criterium (e.g. smoothness) is not yet satisfied.

What about the new names FallbacksPlanner and AlternativesPlanner to emphasize the similarity to the corresponding parallel containers?

That's a good idea 👍 I'll do that and use this chance to port the MultiPlanner over to the ros2 branch and rename it

@codecov
Copy link

codecov bot commented Apr 5, 2023

Codecov Report

Attention: 16 lines in your changes are missing coverage. Please review.

Comparison is base (0ba9796) 41.67% compared to head (8d8006e) 42.71%.

Additional details and impacted files
@@            Coverage Diff             @@
##             ros2     #450      +/-   ##
==========================================
+ Coverage   41.67%   42.71%   +1.04%     
==========================================
  Files          79       80       +1     
  Lines        7939     7945       +6     
==========================================
+ Hits         3308     3393      +85     
+ Misses       4631     4552      -79     
Files Coverage Δ
...moveit/task_constructor/solvers/pipeline_planner.h 100.00% <100.00%> (ø)
core/src/solvers/pipeline_planner.cpp 78.51% <83.16%> (+78.51%) ⬆️

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

Copy link
Contributor

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looking good! I have a few minor comments.

I tried your tutorial fork and it works, but the placement pose doesn't seem correct. However, also seems to look like that on main, so I don't think it's a problem with this branch.

2023-04-13.07-47-18.mp4

core/src/solvers/pipeline_planner.cpp Outdated Show resolved Hide resolved
core/src/solvers/pipeline_planner.cpp Outdated Show resolved Hide resolved
core/src/solvers/pipeline_planner.cpp Outdated Show resolved Hide resolved
@rhaschke
Copy link
Contributor

As stated in #451 (comment), I don't see added value in this PR in addition to #451. Before you put more effort into this, we should agree on the general route...

@sjahr
Copy link
Contributor Author

sjahr commented Apr 14, 2023

As stated in #451 (comment), I don't see added value in this PR in addition to #451. Before you put more effort into this, we should agree on the general route...

Yes 👍 I answered in the other thread

@MarqRazz
Copy link

Is this actually planning in parallel? The console output looks suspiciously in series, but maybe that's just the ROS logging framework.

[mtc_tutorial-1] [INFO] [1681497796.624044521] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] [INFO] [1681497796.661303311] [moveit.::planning_interface.planning_pipeline_interfaces]: Stopping criterion met: Terminating planning pipelines that are still active
[mtc_tutorial-1] [INFO] [1681497796.684124377] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[mtc_tutorial-1] [INFO] [1681497796.684154697] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] [ERROR] [1681497796.684650693] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Joint acceleration limit of panda_joint6 violated. Set the acceleration scaling factor lower! Actual joint acceleration is 5.1937, while the limit is 5. 
[mtc_tutorial-1] [ERROR] [1681497796.684675490] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Inverse kinematics solution at 0.2s violates the joint velocity/acceleration/deceleration limits.
[mtc_tutorial-1] [ERROR] [1681497796.684719357] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Failed to generate valid joint trajectory from the Cartesian path
[mtc_tutorial-1] [INFO] [1681497796.831316182] [chomp_planner]: CHOMP trajectory initialized using method: quintic-spline
[mtc_tutorial-1] [INFO] [1681497796.831353964] [chomp_optimizer]: Active collision detector is: HYBRID
[mtc_tutorial-1] [INFO] [1681497796.885109117] [chomp_optimizer]: First coll check took 0.053745 sec
[mtc_tutorial-1] [INFO] [1681497796.897210374] [chomp_optimizer]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[mtc_tutorial-1] [INFO] [1681497796.897222659] [chomp_optimizer]: Chomp path is collision free
[mtc_tutorial-1] [INFO] [1681497796.897225003] [chomp_optimizer]: Terminated after 1 iterations, using path from iteration 0
[mtc_tutorial-1] [INFO] [1681497796.897226478] [chomp_optimizer]: Optimization core finished in 0.004804 sec
[mtc_tutorial-1] [INFO] [1681497796.897228978] [chomp_optimizer]: Time per iteration 0.004806 sec
[mtc_tutorial-1] [INFO] [1681497798.548378078] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] [INFO] [1681497798.589949033] [moveit.::planning_interface.planning_pipeline_interfaces]: Stopping criterion met: Terminating planning pipelines that are still active
[mtc_tutorial-1] [INFO] [1681497798.726495083] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[mtc_tutorial-1] [INFO] [1681497798.726616093] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] Length of solution for planner: 'LIN' is -1'
[mtc_tutorial-1] Length of solution for planner: 'RRTConnectkConfigDefault' is 6.14397'
[mtc_tutorial-1] Length of solution for planner: 'chomp' is 6.14404'

Why are we getting the message

Stopping criterion met: Terminating planning pipelines that are still active

when we have Omitted default stopping criterion and allow all pipelines to use the full budget?

@sjahr
Copy link
Contributor Author

sjahr commented Apr 17, 2023

Is this actually planning in parallel?

It's the ROS logging. Look at the first two lines, PILZ and OMPL logging are interwoven which wouldn't be the case if it is sequential:

[mtc_tutorial-1] [INFO] [1681497796.624044521] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_tutorial-1] [INFO] [1681497796.661303311] [moveit.::planning_interface.planning_pipeline_interfaces]: Stopping criterion met: Terminating planning pipelines that are still active
[mtc_tutorial-1] [INFO] [1681497796.684124377] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[mtc_tutorial-1] [INFO] [1681497796.684154697] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.

Why are we getting the message

I fixed that, the parallel_planning_solver worked fine but the sampling_solver uses the parallel planning interface with a single pipeline. The previously configured default stopping criteria was met by that planner (which doesn't have any effect since it is a single pipeline anyways). I updated the default, so this doesn't happen anymore

sjahr added a commit to sjahr/moveit_task_constructor that referenced this pull request May 3, 2023
commit 8e2c560
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Mon Apr 17 09:49:51 2023 +0200

    Use no default stopping criteria

commit d095f7f
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Fri Apr 14 12:20:44 2023 +0200

    Refactor to avoid calling .at(0) twice

commit 4238dc3
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Fri Apr 14 12:10:45 2023 +0200

    Format

commit 2c74526
Author: Sebastian Jahr <sebastian.jahr@tuta.io>
Date:   Fri Apr 14 10:39:26 2023 +0200

    Update core/src/solvers/pipeline_planner.cpp

commit f06a93c
Author: Sebastian Jahr <sebastian.jahr@tuta.io>
Date:   Fri Apr 14 10:34:02 2023 +0200

    Update core/src/solvers/pipeline_planner.cpp

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

commit 168cbd5
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Tue Apr 11 11:14:19 2023 +0200

    Small clang-tidy fix

commit c0ea5cd
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Apr 6 10:34:20 2023 +0200

    Pass pipeline map by reference

commit 9be2ab3
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Fri Mar 31 17:55:58 2023 +0200

    Add API to set parallel planning callbacks and deprecate functions

commit 132235f
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 20:18:50 2023 +0200

    Cleanup and documentation

commit 7dd2743
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 17:12:44 2023 +0200

    Add callbacks

commit f050adb
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 16:00:25 2023 +0200

    Enable configuring multiple pipelines

commit b96992c
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 10:29:47 2023 +0200

    Make usable with parallel planning

commit 2a7234d
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 09:47:38 2023 +0200

    Re-order plan functions

commit 6679c98
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Tue Mar 28 15:28:02 2023 +0200

    Make code readable
@sjahr sjahr requested review from v4hn and rhaschke May 9, 2023 14:15
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
const std::string& planner_id = "");

[[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it make sense to extend from PipelinePlanner instead of deprecating the old interface?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With the current implementation, this constructor would be underdefined because to my understanding it is not possible to infer the default planner name from the planning pipeline pointer because that information is hidden inside the planner plugin

* \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node,
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

usually, pipelines have default algorithms configured, right? Shouldn't the planner_id map specified per stage, if at all? The properties would be a great place to specify planner ids

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the planner_id map should be defined because rather than using unconsciously some default algorithm, I think it should be explicitly configured which one is used.
To have an additional property to override this pipeline_id_planner_id_map on the stage level is a good idea. With that it would be possible to use a subset of the defined planner-pipeline pairs. Would this require more work in the stage implementations? If yes, I think this should be follow-up work.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the last commit achieves what you requested:
f57e659

core/src/solvers/pipeline_planner.cpp Outdated Show resolved Hide resolved
Copy link
Contributor

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I noticed these warnings when building moveit_task_constructor_demos with the latest version of MoveIt 2.

#39 1131.7 --- stderr: moveit_task_constructor_demo
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp: In lambda function:
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:35:33: warning: ‘void moveit::task_constructor::solvers::PipelinePlanner::setPlannerId(const string&)’ is deprecated: Replaced with setPlannerId(pipeline_name, planner_id) [-Wdeprecated-declarations]
#39 1131.7    35 |                 pp->setPlannerId("PTP");
#39 1131.7       |                 ~~~~~~~~~~~~~~~~^~~~~~~
#39 1131.7 In file included from /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:9:
#39 1131.7 /opt/underlay_ws/install/moveit_task_constructor_core/include/moveit/task_constructor/solvers/pipeline_planner.h:91:14: note: declared here
#39 1131.7    91 |         void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
#39 1131.7       |              ^~~~~~~~~~~~
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp: In lambda function:
#39 1131.7 /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:41:33: warning: ‘void moveit::task_constructor::solvers::PipelinePlanner::setPlannerId(const string&)’ is deprecated: Replaced with setPlannerId(pipeline_name, planner_id) [-Wdeprecated-declarations]
#39 1131.7    41 |                 pp->setPlannerId("RRTConnect");
#39 1131.7       |                 ~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~
#39 1131.7 In file included from /opt/underlay_ws/src/moveit_task_constructor/demo/src/fallbacks_move_to.cpp:9:
#39 1131.7 /opt/underlay_ws/install/moveit_task_constructor_core/include/moveit/task_constructor/solvers/pipeline_planner.h:91:14: note: declared here
#39 1131.7    91 |         void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
#39 1131.7       |              ^~~~~~~~~~~~

So I think this PR should also update those accordingly.

Make code readable

Re-order plan functions

Make usable with parallel planning

Enable configuring multiple pipelines

Add callbacks

Cleanup and documentation

Add API to set parallel planning callbacks and deprecate functions

Pass pipeline map by reference

Small clang-tidy fix

Update core/src/solvers/pipeline_planner.cpp

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

Update core/src/solvers/pipeline_planner.cpp

Format

Refactor to avoid calling .at(0) twice

Use no default stopping criteria

Update fallbacks_move demo
@sjahr sjahr force-pushed the pr-add_parallel_planning_solver branch from d6c3ef2 to f57e659 Compare September 4, 2023 10:41
@sjahr
Copy link
Contributor Author

sjahr commented Sep 4, 2023

@rhaschke @henningkayser Do you mind giving this a review?

Copy link
Contributor

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

I still think we should find a way to ensure that PRs to the humble branch run humble CI, but we can handle that in a follow-on PR.

@sjahr
Copy link
Contributor Author

sjahr commented Oct 4, 2023

@rhaschke All that is blocking this PR from being merged, is that CI doesn't pass due to low code coverage. I just added a unit test for my changes and it barely increased coverage. Do you have any idea what is missing and/or if this is a problem caused by this PR?

@sjahr
Copy link
Contributor Author

sjahr commented Oct 4, 2023

The ros2 branch in codev is out-of-date https://app.codecov.io/gh/ros-planning/moveit_task_constructor/tree/ros2

@sjahr
Copy link
Contributor Author

sjahr commented Oct 4, 2023

The ros2 branch in codev is out-of-date https://app.codecov.io/gh/ros-planning/moveit_task_constructor/tree/ros2

Looks like re-triggering the CI job for the ros2 branch fixed it

@sjahr sjahr merged commit f4cd7d5 into moveit:ros2 Oct 4, 2023
5 checks passed
rhaschke pushed a commit to ubi-agni/moveit_task_constructor that referenced this pull request Mar 9, 2024
* Refactor pipeline planner

Make code readable

Re-order plan functions

Make usable with parallel planning

Enable configuring multiple pipelines

Add callbacks

Cleanup and documentation

Add API to set parallel planning callbacks and deprecate functions

Pass pipeline map by reference

Small clang-tidy fix

Update core/src/solvers/pipeline_planner.cpp

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

Update core/src/solvers/pipeline_planner.cpp

Format

Refactor to avoid calling .at(0) twice

Use no default stopping criteria

Update fallbacks_move demo

* Cleanup + address deprecation warnings

* Enabling optionally using a property defined pipeline planner map

* Address review

* Disable humble CI for ros2 branch

* Add pipeline planner unittests + some checks

* Add short comment
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Mar 10, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Mar 10, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Apr 14, 2024
rhaschke added a commit that referenced this pull request May 25, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request May 25, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Jul 12, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Jul 12, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Jul 12, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Jul 13, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Jul 13, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Jul 13, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Jul 13, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Sep 17, 2024
rhaschke added a commit that referenced this pull request Sep 27, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Oct 15, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Oct 15, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Oct 15, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Oct 15, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Oct 15, 2024
rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this pull request Oct 15, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants