Better path quality when using MoveItCpp

[mamoll]

Is your feature request related to a problem? Please describe.
As a non-motion planning expert, I want predictable performance on easy to moderately-hard motion planning problems, where performance is broadly defined to include: fast planning times, "intuitive" motions, high repeatability. The default planner in the default planning pipeline (RRTConnect from the OMPL pipeline) is typically fast, requires minimal to no parameter tuning, but can produce wacky solutions on seemingly easy problems.

Describe the solution you'd like
I am looking at how moveitcpp::PlanningComponent::plan plans a path. I see it looks up the planning pipeline by some parameter, calls generatePlan for the selected pipeline and returns the solution if it finds one. I would like to propose to augment the semantics of the parameters.planning_pipeline parameter: instead of selecting one pipeline, you can select multiple, e.g., by setting its value to “ompl|stomp|pilz”. The only change would then be in moveitcpp::PlanningComponent::plan: it spawns interruptible threads for each planning pipeline, picks the first found solution, terminates the other threads. All existing code will behave the same way. This seems simple enough to try and might result in faster, more deterministic behavior in many cases. With some extra code, we can keep some stats around inside moveitcpp::PlanningComponent that tracks how often each planner “wins” and how much time it needs. These stats could be printed/logged upon destruction. (You could also further augment the parameter syntax to allow pipelines to run sequentially until one succeeds, e.g., using > as a separator: “pilz>stomp>ompl”.)

Describe alternatives you've considered

• Alternative planners in the OMPL planner pipeline have more parameters that non-experts find hard to tune or are simply not as performant in general.
• There are several alternative planning pipelines that each have their own weaknesses:
  • The Pilz planner pipeline is a fast, deterministic planner, but is incomplete (i.e., it won't always find a solution if one exists.
  • STOMP and CHOMP are typically fast and usually produce nice-looking paths, but are also incomplete.
• Currently, the ompl_interface in MoveIt measures distance as the unweighted L1 norm over joint angles. Weighting the distance in each joint differently might help a little bit, but picking “good” weights automatically may be hard. Alternatively, using swept volume as a distance measure to get more intuitive motions sounds nice, but is computationally too expensive. Another distance measure that is trying to capture some aspect of swept volume is to measure the distance between two robot configurations is to measure the sum of distances between the Cartesian positions of each joint in the two configurations (i.e. use FK to compute 3D positions of each joint, measure the sum of distances between corresponding 3D joint positions). Even that might be somewhat expensive. None of the alternative distance measures would eliminate the occasional wacky path. It would only make it more likely that the shortest path corresponds to what the user actually wants.

[v4hn]

Hey Mark.

As a non-motion planning expert

? That was you writing it, right? :)

To be honest what you describe is exactly one of the use-cases I want to have solved in MTC, including different parallel containers and scheduling methods. Additionally wrappers can be used to define pipelines such as RRTConnect+CHOMP as Bryce looked into. I'm not saying the demos we have for these things are perfect or the underlying implementation exactly what you want yet, but I would definitely not want to go for yet another "some way to define planner approaches with multiple planners involved".

Of course we could have some text declaration to describe the structure, but this quickly becomes more than "just use | or <", so maybe full yaml or some real well-defined grammar makes more sense.

[gavanderhoorn]

This seems like a reasonable wish, but the text does not explicitly explain how

instead of selecting one pipeline, you can select multiple, e.g., by setting its value to “ompl|stomp|pilz”. [..] it spawns interruptible threads for each planning pipeline, picks the first found solution, terminates the other threads.

results in:

[..] predictable performance on easy to moderately-hard motion planning problems, where performance is broadly defined to include: fast planning times, "intuitive" motions, high repeatability.

I believe.

How would the results of the planners ran in parallel be evaluated for their "wacky-ness"?

[v4hn]

Adding to Gijs statement, in my experience adding parallel methods to solve something will actually make the whole system *less* predictable. But it depends on the exact scenarios the solvers are used in. Imagine you setup Pilz PTP as a first solver and fall back to RRT: *Usually* it will generate short intuitive motions. But if some noise renders the short motions infeasible you will suddenly get a wacky RRT path without expecting it. On the other hand if the only source of rendering PTP infeasible can be a well-defined collision state of, e.g., another robot arm/human/... obviously getting in the way, the resulting behavior will be exactly what you expect: the obvious way is blocked, so the result will be less obvious. But this is talking about fallbacks only. If you run planners in parallel and just pick the first one finished you have introduced another (discrete) source of randomness to your system that totally changes the nature of your path (e.g., fetch the sandwich from the fridge or go buy one at the closest kiosk).

[AndyZe]

I think it would be very nice to have a mode where you can run multiple planners in parallel, and the output will tell you which one solved fastest. Even better if you could preview the output of each planner to inspect visually -- but then this change becomes a lot bigger.

What if you request a constrained motion? I don't think Pilz regards motion constraints.

As a non-motion planning expert

^He's trying a technique to put himself in the shoes of the user ;) Could be more clearly stated, I think

... in MoveIt measures distance as the unweighted L1 norm over joint angles

I think the L2 norm would be better because it weights large joint angle changes more heavily automatically.

[AndyZe]

Maybe this idea should be limited to running OMPL planning threads with different settings in parallel.

[mamoll]

My solution should be considered a hypothesis that is easily testable without setting up a whole bunch of other infrastructure. As @gavanderhoorn points out, there is no guarantee that wackiness is minimized. A more sophisticated scheme would allow you to express that some pipeline solutions might be preferable over others even if they take longer to return a solution. However, that requires a lot more engineering, whereas my proposed solution seems like a good first step to inform next steps.

@v4hn, adding MTC would add a lot more complexity that's not really needed at this point. The RRTConnect+CHOMP example you give is already possible at this point without MTC using a Planning Request Adapter. I don't understand your comment why the proposed solution would make the whole system less predictable than the status quo.

@AndyZe, previewing alternative solutions would increase scope and could perhaps be done at a later stage. Re. constraints, if a planning pipeline / planner cannot handle constraints, it should simply not be invoked on that planning request. Re. switching to L2 norm, that's a different question.

[mamoll]

@AndyZe limiting this idea to OMPL is already possible without any code change by using the AnytimePathShortening planner which allows you specify a portfolio of planners (and their parameters). This can be done in the ompl yaml config file that is part of a MoveIt config package. By choosing the right optimization objective (path length, smoothness, "wackiness") and termination condition (first feasible, convergence, time-out), you can experiment a lot. I like the idea of having something that is planning pipeline agnostic, though.

[v4hn]

adding MTC would add a lot more complexity that's not really needed at this point

I understand the thought, but I can't say I agree (you don't have to care of course 🐈).

• You already proposed multiple logical mechanisms on parallelization strategies (, vs. <). More meta-structures will come up as well once you have the mechanism and experiment with it.
• Andy mentioned previewing different pipeline solutions and meta-data (solving time) already.
• You want a pipeline agnostic solution.
• You mentioned AnytimePathShortening, but if you want that mechanism pipeline agnostic, it's also not there yet and needs additional meta-structure.
  ...

These things either exist or are supported in the MTC structure. I can only reiterate my previous comment:

I would definitely not want to go for yet another "some way to define planner approaches with multiple planners involved".

---

I don't understand your comment why the proposed solution would make the whole system less predictable than the status quo.

If you pool, e.g., PTP and RRTConnect as first-solution-wins you add another discrete random variable to your probabilistic model, thus changing the sample distribution. So where you previously sampled from p(τ; rrtconnect), now you sample from p(τ; rrtconnect)*μ + p(τ; ptp)*(1-μ).
The worst-cases are the same unless μ = 0 and they are usually the relevant cases in industry. I absolutely agree that the average case would become more predictable.
The intuition issue I observed in past setups arises from this mismatch: If a naive user is used to see RRT solutions in a sufficiently safe/restricted workspace, they will accept them. But if they are used to PTP solutions, because μ > 0.99, they will feel a strong urge to press the e-stop when suddenly a RRT solution executes.
That is, unless there is a reason for the PTP trajectory to be invalid that is obvious to the naive user. I.e., in situations where the user would look at the state of the setup and say "ok, now it will crash/not execute because of collision", it is a great feature to say "obviously the usual will not work, so MoveIt knows better and does something slightly(?) less intuitive".
But the response drastically depends on the user you are talking about of course.

[sjahr]

Done with #1420