WIP: Alternatives and fallback planners #451

sjahr · 2023-04-03T09:03:05Z

Depends on moveit/moveit2#2043 (Merged but not released 04/05/23)

Implement planners proposed by @rhaschke here

FallbackPlanner is a renamed version of the already existing MultiPlanner ported to ROS2 with the extensions, that it is now possible to pass a stopping criterion to the planner.

AlternativesPlanner is an implementation of the parallel planning interface in Moveit but on an MTC solver level so it is possible to run multiple solvers in parallel and select a solution. I did not find an easy way to add a stopping criteria here, so each parallel solver uses it's complete timeout for the moment.

This PR does not invalidate #450 because that is a refactoring of solely the PipelinePlanner to support MoveIt2's parallel planning feature.

Test with tutorial

MultiPlanner

codecov · 2023-04-05T07:47:01Z

Codecov Report

Patch coverage: 3.61% and project coverage change: -0.54 ⚠️

Comparison is base (a0befc5) 41.80% compared to head (439e368) 41.25%.

Additional details and impacted files

@@            Coverage Diff             @@
##             ros2     #451      +/-   ##
==========================================
- Coverage   41.80%   41.25%   -0.54%     
==========================================
  Files          78       80       +2     
  Lines        7846     7952     +106     
==========================================
+ Hits         3279     3280       +1     
- Misses       4567     4672     +105

Impacted Files	Coverage Δ
...oveit/task_constructor/solvers/planner_interface.h	`100.00% <ø> (ø)`
core/src/solvers/alternatives_planner.cpp	`0.00% <0.00%> (ø)`
core/src/solvers/cartesian_path.cpp	`83.73% <0.00%> (ø)`
core/src/solvers/fallback_planner.cpp	`0.00% <0.00%> (ø)`
core/src/solvers/pipeline_planner.cpp	`0.00% <0.00%> (ø)`
core/src/solvers/joint_interpolation.cpp	`88.24% <100.00%> (ø)`
core/src/solvers/planner_interface.cpp	`100.00% <100.00%> (ø)`

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

rhaschke

Do you plan to have these planners in ROS1 as well? If so, I suggest implementing them there first.
(Our standard development branch is master / ROS1 in this repo and we forward-port changes to ROS2 via merge commits.)

I don't yet like the code redundancy of the two plan() functions for the AlternativesPlanner. I think this calls for a template.

Which problems did you face when defining a stopping criterion for AlternativesPlanner?
I guess it's difficult to stop other planning threads? How did you solve that with ParallelPlanning?

rhaschke · 2023-04-12T13:15:13Z

core/include/moveit/task_constructor/solvers/fallback_planner.h

+	StoppingCriterionFunction stopping_criterion_function_ =
+	    [](const robot_trajectory::RobotTrajectoryPtr& result,
+	       const moveit_msgs::msg::Constraints& /*path_constraints*/) {


I suggest implementing this default as a reusable standalone function.

rhaschke · 2023-04-12T13:22:40Z

core/src/solvers/fallback_planner.cpp

+			remaining_time -= std::chrono::duration<double>(now - start_time).count();
+			start_time = now;
+		}
+		return false;


This return false; needs to be outside the for loop.

rhaschke · 2023-04-12T13:23:50Z

core/src/solvers/fallback_planner.cpp

+			if (stopping_criterion_function_(result, path_constraints)) {
+				return true;
+			}


In this repo, we usually omit parentheses for one-line statements like here.

Suggested change

if (stopping_criterion_function_(result, path_constraints)) {

return true;

}

if (stopping_criterion_function_(result, path_constraints))

return true;

rhaschke · 2023-04-12T13:32:57Z

core/src/solvers/alternatives_planner.cpp

+                               const moveit::core::JointModelGroup* jmg, double timeout,
+                               robot_trajectory::RobotTrajectoryPtr& result,
+                               const moveit_msgs::msg::Constraints& path_constraints) {
+	moveit::planning_pipeline_interfaces::PlanResponsesContainer plan_responses_container{ this->size() };


Why do you want to use a PlanResponsesContainer instead of a simple vector of solutions?

rhaschke · 2023-04-12T13:34:42Z

core/src/solvers/alternatives_planner.cpp

+
+	std::vector<::planning_interface::MotionPlanResponse> solutions;
+	solutions.reserve(1);
+	solutions.push_back(solution_selection_function_(plan_responses_container.getSolutions()));


MTC has a solution-selection inbuilt already: solutions have costs and the best solution (least costs) is selected.
There is no need to reinvent the wheel.

rhaschke · 2023-04-12T13:35:28Z

core/src/solvers/alternatives_planner.cpp

+	solutions.reserve(1);
+	solutions.push_back(solution_selection_function_(plan_responses_container.getSolutions()));
+
+	if (solutions.empty()) {


Solutions cannot be empty in your implementation. I don't see the need for a solutions vector...

rhaschke · 2023-04-12T14:47:43Z

This PR does not invalidate #450 because that is a refactoring of solely the PipelinePlanner to support MoveIt2's parallel planning feature.

I don't agree. Why would it be beneficial to have parallel planning implemented in PipelinePlanner too, when we already have AlternativesPlanner? I think we should build on modularity (combine simple planners via Fallback|Alternatives) instead of providing this functionality within PipelinePlanner again.

sjahr · 2023-04-14T09:12:06Z

@rhaschke Sorry for my late response.

Do you plan to have these planners in ROS1 as well? If so, I suggest implementing them there first.
(Our standard development branch is master / ROS1 in this repo and we forward-port changes to ROS2 via merge commits.)

I don't have the capacity at the moment to implement this in ROS 1, since I am working most of the time with ROS 2.

Which problems did you face when defining a stopping criterion for AlternativesPlanner?
I guess it's difficult to stop other planning threads? How did you solve that with ParallelPlanning?

For parallel planning within MoveIt 2 I used the member function terminate of the PlanningPipeline to terminate the pipelines (see here). For the PipelinePlanner it would be easy to expose it in the MTC solver itself, but implementing a terminate function for the Cartesian/ Joint Interpolator would have been a little more difficult, so my plan was to split it from this PR in a separate effort.

Additionally, I am running into a segfault when I am using the AlternativesPlanner planner. So I am assuming the solvers are not thread safe at the moment. I haven't taken the time to dig more into this

sjahr · 2023-04-14T09:36:01Z

I don't agree. Why would it be beneficial to have parallel planning implemented in PipelinePlanner too, when we already have AlternativesPlanner? I think we should build on modularity (combine simple planners via Fallback|Alternatives) instead of providing this functionality within PipelinePlanner again.

While both interfaces enable parallel planning, they are doing it on different hierachy levels. The Alternative Solver enables using multiple MTC solvers in parallel and the pipeline planner (as a single MTC solver) uses MoveIt's planning interface to use one or multiple planning pipelines. Having a parallel planning interface in MoveIt is useful for using the feature independent of MTC e.g. with moveit_cpp or hybrid planning.
Having parallel planning implemented in the PipelinePlanner won't reduce modularity since it does not prevent anybody from using it as a simple planner. It rather offers more liberty on which interface you'd like to use (maybe they'll diverge in the future in terms of available features e.g. at this point terminating MTC solvers is not possible).

rhaschke · 2023-04-14T09:50:24Z

I exactly want to avoid a redundant implementation at two different levels of hierarchy, which are then prone to diverge in implementation and features. Modularity stems from simple building blocks, which can be nicely combined with each other.

I'm not sure how parallel planning pipelines are configured in MoveItCpp: Do you dynamically pick several pipeline configs or do you need to define those statically in advance? In the former case, I don't see any limitation in using the AlternativesPlanner.
The tutorial lists an excerpt of a moveit_cpp.yaml config, which (due to the indentation of the "ompl_rrtc", "pilz_lin", and "chomp" configs) suggest the latter, more restrictive approach.

sjahr · 2023-04-14T10:04:05Z

Do you dynamically pick several pipeline configs or do you need to define those statically in advance?

The pipelines are created and initialized in advance & when you run plan() you decide which subset you'd like to use.

I exactly want to avoid a redundant implementation at two different levels of hierarchy,

I don't see how this can be avoided for example because MTC solver != MoveIt Planner. Parallel planning is a useful feature on both levels.

sjahr · 2023-04-14T10:07:33Z

Maybe this would be a good discussion to have in the Maintainer meeting 🤔

sjahr · 2023-05-08T08:14:36Z

@rhaschke I'd still like to see #450 merged. I think enabling the pipeline-level parallel planning feature is useful for MTC users.
I haven't found the time to continue this PR and the alternative planner currently doesn't work due to a segfault. But I see your point about modularity & avoiding a redundant implementation. Maybe merging #450 could be a temporary solution. At the previous maintainer meeting, @henningkayser came up with a potential solution to this:

How about moving MTC’s solver interface into MoveIt only? We are lacking a consistent interface anyway (example: Cartesian Interpolator) and the solver is not really specific to MTC at all. The only concern is supporting MTC’s planner properties which should be solvable.

Pushing the solver interface down to MoveIt would resolve the issues with redundant implementations while keeping the feature available in both, MoveIt and MTC. Do you have time this week for a video call to discuss how we can move forward?

v4hn · 2023-05-08T09:45:57Z

Maybe merging #450 could be a temporary solution

There are no temporary solutions in MoveIt (those are called feature branches).
Things in the main branches tend to persist and you know that. :-)
At the same time endless debate never gets anything merged 😇

In contrast to Robert I would encourage #450 over #451.
The point of #450 is to forward an existing MoveIt feature into MTC, which I still think is the wrong way of handling parallelism in MTC, but it's basically "plugin what is there".
Implementing MTC planners as vectors of planners as proposed here pretty much re-implements the whole concept of MTC's parallel containers inside a single stage. But the feature-set is even worse because inspecting only solutions produced by a specific planner and setting different solution properties depending on the planner has to be reimplemented in a different way than with the concept of having one planner per stage.

Pushing the solver interface down to MoveIt would resolve the issues with redundant implementations

The MTC solver interface is very limited in that you cannot pass arbitrary goal constraints, not even multiple poses for different links. It suffices for what we modeled in MTC so far, but I believe it is definitely too limited to describe general MoveIt interface.

rhaschke · 2023-05-08T09:53:02Z

The MTC solver interface is very limited in that you cannot pass arbitrary goal constraints, not even multiple poses for different links. It suffices for what we modeled in MTC so far, but I believe it is definitely too limited to describe general MoveIt interface.

I agree. The MTC solver interface is very much tailored toward MTC's needs. I will think about #450 again, now having Michael's opinion on it.

sjahr · 2023-05-08T11:27:22Z

There are no temporary solutions in MoveIt (those are called feature branches).
Things in the main branches tend to persist and you know that. :-)
At the same time endless debate never gets anything merged innocent

😅

The MTC solver interface is very limited in that you cannot pass arbitrary goal constraints, not even multiple poses for different links. It suffices for what we modeled in MTC so far, but I believe it is definitely too limited to describe general MoveIt interface.

Ok, that makes sense. I am imagining this interface to provide the same functionality as the solver interface does: The capability to use a solver (Joint or Cartesian Interpolation) or a planner (more specifically: A planning pipeline) for a given MotionPlanRequest but more generalized as the MTC solver interface. Within MTC this new MoveIt interface replaces the solver interface. @henningkayser Is this understanding of your idea correct and can you elaborate a bit more on the idea and maybe how it should look like?

henningkayser · 2023-05-16T22:04:01Z

. @henningkayser Is this understanding of your idea correct and can you elaborate a bit more on the idea and maybe how it should look like?

Yes, my idea was more along the lines of reducing the number of planning interfaces that we have across MoveIt and MTC while also filling the gap in combining the different solvers that MoveIt has (as in, MoveIt doesn't provide a common interface for Cartesian interpolation, planners, joint interpolation). MTC's interface already provides this in a limited way, so I would very much be in favor of a single solver interface that is part of MoveIt, but is also used by MTC. (I don't think that's feasible without a significant amount of extra work, though).

About parallel planning: I think Cartesian interpolation is not thread-safe by default because of the IK solver (I think kdl and bio_ik are both not thread-safe, pick_ik should be). Until there is a way to enforce thread-safety, we shouldn't enable parallel planning for this type of solver. To me, MTC's stages and containers are sufficient for now.

I would advocate for merging #450 since the MoveIt-internal version is a feature that already exists and that is under active development. I agree that there is a certain conceptual overlap between MTC's container types and this feature, however MTC's approach does not suffice configuring and optimizing specific planner setups. And that's the whole point of this feature, allowing to experiment with and optimizing groups of planners and configurations that can be used as a default in MoveIt and in MTC, without having to implement new boilerplate code around that. It is not problem-specific, it is a tool for switching to say OMPL+STOMP+PILZ for all planning requests in an application instead of only solving with RRTConnect. That's where I see the advantage of enabling MoveIt's version for MTC.

rhaschke and others added 7 commits April 3, 2023 08:32

Merge PR moveit#429: MultiPlanner

b79ec83

MultiPlanner

Port to ROS 2

bf58108

Rename multi-planner to fallback planner

dd7aa11

Add stopping criterion function

b75338f

Add alternatives planner

e53a063

Minor variable renaming

06129ea

Format

2fdce55

sjahr changed the title ~~Alternatives and fallback planners~~ WIP: Alternatives and fallback planners Apr 3, 2023

Check for nullptr and add missing headers

439e368

rhaschke reviewed Apr 12, 2023

View reviewed changes

rhaschke mentioned this pull request Apr 14, 2023

Enable parallel planning with PipelinePlanner #450

Merged

sjahr marked this pull request as draft September 4, 2023 09:18

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

WIP: Alternatives and fallback planners #451

WIP: Alternatives and fallback planners #451

sjahr commented Apr 3, 2023 •

edited

Loading

codecov bot commented Apr 5, 2023

rhaschke left a comment

rhaschke Apr 12, 2023

rhaschke Apr 12, 2023

rhaschke Apr 12, 2023

rhaschke Apr 12, 2023

rhaschke Apr 12, 2023

rhaschke Apr 12, 2023

rhaschke commented Apr 12, 2023

sjahr commented Apr 14, 2023 •

edited

Loading

sjahr commented Apr 14, 2023 •

edited

Loading

rhaschke commented Apr 14, 2023

sjahr commented Apr 14, 2023 •

edited

Loading

sjahr commented Apr 14, 2023

sjahr commented May 8, 2023

v4hn commented May 8, 2023

rhaschke commented May 8, 2023

sjahr commented May 8, 2023

henningkayser commented May 16, 2023

WIP: Alternatives and fallback planners #451

Are you sure you want to change the base?

WIP: Alternatives and fallback planners #451

Conversation

sjahr commented Apr 3, 2023 • edited Loading

codecov bot commented Apr 5, 2023

Codecov Report

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Apr 12, 2023

Choose a reason for hiding this comment

rhaschke Apr 12, 2023

Choose a reason for hiding this comment

rhaschke Apr 12, 2023

Choose a reason for hiding this comment

rhaschke Apr 12, 2023

Choose a reason for hiding this comment

rhaschke Apr 12, 2023

Choose a reason for hiding this comment

rhaschke Apr 12, 2023

Choose a reason for hiding this comment

rhaschke commented Apr 12, 2023

sjahr commented Apr 14, 2023 • edited Loading

sjahr commented Apr 14, 2023 • edited Loading

rhaschke commented Apr 14, 2023

sjahr commented Apr 14, 2023 • edited Loading

sjahr commented Apr 14, 2023

sjahr commented May 8, 2023

v4hn commented May 8, 2023

rhaschke commented May 8, 2023

sjahr commented May 8, 2023

henningkayser commented May 16, 2023

sjahr commented Apr 3, 2023 •

edited

Loading

sjahr commented Apr 14, 2023 •

edited

Loading

sjahr commented Apr 14, 2023 •

edited

Loading

sjahr commented Apr 14, 2023 •

edited

Loading