Makes Interpolating and Motion Validating OMPL Paths Consistent

[BryceStevenWilley]

Description

This change is related to #416 and OMPL's recent changes.

When @zkingston and I made the above changes we had the curious issue that OMPL would find a path that, according to OMPL, was collision free. However, after interpolating the path, MoveIt would complain and say that the path was in collision.

The gist of the issue is that a path can be interpolated with different waypoints than those that are checked by the discrete motion validator (responsible for collision checking along a path segment).

There are actually three parameters controlling the interpolation density:

• longest_valid_segment_fraction (an OMPL planner property, actually used during planning)
• max_solution_segment_length_ (a dynamic-reconfigure parameter in MoveIt's OMPL interface that was finally used for interpolation)
• minimum_waypoint_count_ (a dynamic-reconfigure parameter in MoveIt's OMPL interface)

The first two are redundant: max_solution_segment_length_ specifies an absolute value, while longest_valid_segment_fraction specifies a value relative to the state space extension.

If collision checking and later interpolation uses different waypoints (due to different parameters being used), this will result in the observed behaviour, even when collision checking is at a finer-grain than the final interpolation:

Collision Checked path, 8 points: (x = collision)
o---o--xo---o---o---o---o---o

Interpolated Path, 5 points:
o------x------o------o------o

While the whole issue can be avoided by setting longest_valid_segment_fraction very small (0.001 worked for us), the default behaviour should at least be consistent according to itself.

As a result of this, the variable max_solution_segment_length_ is no longer needed (this value is redundant to longest_valid_segment_fraction), which is why this is filed against Melodic (API change).

This should be cherry-picked to Kinetic, with a deprecation warning on the max_solution_segment_length_ functions instead of deleting them, but I'll wait for feedback on this first.

Checklist

• Required: Code is auto formatted using clang-format
• Extended the tutorials / documentation, if necessary reference
• Optional: Created tests, which fail without this PR reference
• Cherry-pick this into Kinetic with a deprecation warning on the to-be-removed parameter

[zkingston]

Another thing to note is that the usage of the path simplifier is slightly changed. The semantics are now "do at least one simplification pass, and then more if time allows" rather than "only if time allows". This is related to some recent changes in OMPL that make the expected simplifier behavior explicit.

This is also a nice change for the asymptotically optimal planners like RRT*, as no simplification pass would be run as the planner used all available time budget. Now, a quick pass is done and a better path is generated (in general)

[rhaschke]

According to @zkingston's above remark, this additional call to simplifySolution can be removed, can't it?

[BryceStevenWilley]

Not sure if I follow: the call here and where @zkingston commented are a part of different functions, so a single call to solve will only end up calling simplifySolution once.

zkingston's comment is clarifying that OMPL now will always attempt to simplify the solution at least once, and that this deleted ptime < request_.allowed_planning_time is redundant to what OMPL is now doing internally.

[rhaschke]

Sorry, you are right. I missed the fact that two different solve functions are involved - wasn't obvious from github's diff.
I suggest to change the comment in line 570 accordingly.

[rhaschke]

According to the documentation of og::PathGeometric::interpolate(unsigned int count), this change is not necessary:

Changes are performed only if a path has less than count states.

It would suffice to call pg.interpolate(minimum_waypoint_count_).
However, this re-interpolates as well and thus again might cause the issue addressed in this PR.

[BryceStevenWilley]

However, this re-interpolates as well and thus again might cause the issue addressed in this PR.

Yep, that's why this change was made. Originally, we just had

pg.interpolate();
if (pg.getStateCount() < minimum_waypoint_count_)
  pg.interpolate(minimum_waypoint_count_);

But we realized that trying to add just a few evenly interpolated states to an already evenly interpolated path without changing any of the original states would result in several parts of the paths having more states than the rest of the path, resulting in jerky trajectories.

[rhaschke]

I don't get why do you re-interpolate at all? According to your initial comment, this is not a good idea.
Probably I didn't understand the semantics of pg.interpolate() yet. Is it performing the same interpolation as during planning, because these sub waypoints are not actually stored in the path anymore? Then, I see your point.

[BryceStevenWilley]

Ah, sorry, my initial comment glossed over the specifics of this.

Is it performing the same interpolation as during planning, because these sub waypoints are not actually stored in the path anymore?

Yes, this is exactly it. During planning, we are checking the path at specific states, but those states aren't added to the final path. The returned path from OMPL has only a handful of states (ideally as few as were needed to solve the problem). Calling pg.interpolate() does add those states.

[rhaschke]

I agree to your point that the planned geometric path, shouldn't be re-interpolated in MoveIt after planning.
However, I cannot find the parameter longest_valid_segment_fraction_, you refer to, in MoveIt! Is this a parameter in OMPL?
I'm missing in the (previous) source, how max_solution_segment_length_ is passed to OMPL?
Maybe that's the real underlying reason of your issue.

In any case, I don't agree to removing the MoveIt parameter max_solution_segment_length_ as this essentially defines the planning resolution, doesn't it?
By the way, which units does this parameter have?

On the other hand, I think that the parameter minimum_waypoint_count_ doesn't make sense for planning.

[BryceStevenWilley]

I cannot find the parameter longest_valid_segment_fraction_, you refer to, in MoveIt! Is this a parameter in OMPL?

Yes, it is. The best discussion of it I found is here. (I also realized I had an extra _ at the end of the variable in the original comment, which suggested that it's a class variable in MoveIt, which it is not. I edited the above comment to fix that).

I'm missing in the (previous) source, how max_solution_segment_length_ is passed to OMPL?

So that is the redundancy: in OMPL, the max segment length in a solution is implicitly defined by longest_valid_segment_fraction (max segment length = the volume of the space * longest_valid_segment_fraction).

I don't agree to removing the MoveIt parameter max_solution_segment_length_ as this essentially defines the planning resolution

Yes, and no. max_solution_segment_length_ defines the interpolation resolution, but the resolution of collision checking is defined by longest_valid_segment_fraction.
Having two, possibly conflicting, ways of setting segment length in a solution seemed strange, and having a way to set your segment length to be drastically different from what is actually being collision checked in OMPL and MoveIt! seemed even more risky.

By the way, which units does this (max_solution_segment_length`_) parameter have?

OMPL sees it as state space distance, which for MoveIt would be joint space distance.

On the other hand, I think that the parameter minimum_waypoint_count_ doesn't make sense for planning.

I actually agree, but I left it in because I'd imagine some controllers require a specific number of waypoints in a path to be executed, and if we removed both minimum_waypoint_count_ and max_solution_segment_length_, then there would be no way for users to control the resolution of their final path.
With the current solution, we limit them to have at least the resolution of collision checking, but if they need more they can set minimum_waypoint_count_ to be higher.

[rhaschke]

@BryceStevenWilley Thanks for this clarification. This was very helpful and I updated your initial comment accordingly.
The only remaining thing is to remove the parameter maximum_waypoint_distance from the dynamic-reconfigure gui in cfg/OMPLDynamicReconfigure.cfg. Otherwise I approve.

[rhaschke]

Instead of removing maximum_waypoint_distance, what about setting longest_valid_segment_fraction from it in useConfig() like this:

longest_valid_segment_fraction = std::min(longest_valid_segment_fraction, maximum_waypoint_distance / state space size)

Of course, interpolation should still be changed according to your proposal.

[BryceStevenWilley]

Instead of removing maximum_waypoint_distance, what about setting longest_valid_segment_fraction from it

I like this idea, since these changes should eventually be cherry-picked to kinetic and this would keep both variables, but I'm mostly just not sure in the benefit of keeping two different variables that set the same thing. I can see that maximum_waypoint_distance might be a more intuitive way of setting this than 'a fraction of entire space volume', but I still think we should only keep one or the other.

[rhaschke]

I think both parameters can be kept, because they set the interpolation distance either in absolute or relative fashion. Finally, only the smaller value of both is finally passed to OMPL, which ensures uniqueness.

[v4hn]

I think both parameters can be kept,

👎 This just makes things more confusing than before. If you want support for both, provide the setter that converts as required. `longest_valid_segment_fraction` has a rather long history in all of MoveIt, so it would be a hassle to remove it. But it *is* a very unintuitive parameter and most people fail to set it to reasonable values (see e.g. [here](shadow-robot/sr_interface#328)). Another overall question is whether the current approach makes sense at all. - Currently we treat prismatic and revolute joints the same - From a geometric point of view joints that move links further should have smaller valid distances than those close to the eef. - As I discussed with @rhaschke some time ago, it would be so much more useful in MoveIt to define the validity-checking distance in terms of Cartesian distances of all links instead of joint space distances. But this discussion is much broader than the issue discussed here.

[rhaschke]

I think both parameters can be kept,

 This just makes things more confusing than before.
If you want support for both, provide the setter that converts as required.

Isn't that what I proposed indirectly? However, as both parameters are provided as parameters and not through some setter, what do you exactly mean?

[BryceStevenWilley]

I agree with @v4hn that the real solution should include an overhaul of longest_valid_segment_solution, but that is a much bigger issue than this particular PR.

As for there being 2 variables, I realized that max_solution_segment_length/max_waypoint_distance and longest_valid_segment_length are set at different levels:

• max_waypoint_distance is set at the node level, so all planners made in MoveIt! use that particular distance.
• longest_valid_segment_length is set per joint groups: you can have a different segment length for a prismatic torso vs an 7-dof arm.

For now, this should justify the existence of each variable until more work can be done fixing longest_valid_segment_fraction in general. PTAL at the changes just pushed, which should address the comments.

[rhaschke]

@BryceStevenWilley Thanks a lot for your effort.
I think, we don't have any API changes, so I re-targeted this PR directly for Kinetic.

[v4hn]

@rhaschke you're right.

I approve the changes here and it's good to have them in kinetic-devel already.

Format checking failed, could you please fix that @BryceStevenWilley ?
Also, it would be great if you could add a very short explanation of this to the tutorial.
Pretty please 😁 ?

[davetcoleman]

@BryceStevenWilley thanks for the summary of how OMPL does collision checking in the description of this PR. Do you mind updating the documentation on this subject in the tutorials?
https://github.com/PickNikRobotics/moveit_tutorials/blob/kinetic-devel/doc/ompl_interface/ompl_interface_tutorial.rst

(note: we're about to move that repo back to ros-planning)

[BryceStevenWilley]

I have a PR ready on the ros-planning fork (moveit/moveit_tutorials#165), I can make another easily though.