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

Makes Interpolating and Motion Validating OMPL Paths Consistent #869

Merged
merged 10 commits into from May 1, 2018

Conversation

Projects
None yet
5 participants
@BryceStevenWilley
Copy link
Contributor

BryceStevenWilley commented Apr 27, 2018

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

BryceStevenWilley added some commits Apr 25, 2018

Removed max solution segement length as a parameter, as it should be …
…indirectly set by the valid segment fraction. Otherwise, it's possible for OMPL to find a path that is continually rejected by MoveIt.
@@ -512,7 +529,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion
if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
{
double ptime = getLastPlanTime();
if (simplify_solutions_ && ptime < request_.allowed_planning_time)
if (simplify_solutions_)

This comment has been minimized.

@zkingston

zkingston Apr 27, 2018

Contributor

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)

@@ -551,7 +568,7 @@ bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion
getSolutionPath(*res.trajectory_.back());

// simplify solution if time remains
if (simplify_solutions_ && ptime < request_.allowed_planning_time)
if (simplify_solutions_)

This comment has been minimized.

@rhaschke

rhaschke Apr 29, 2018

Contributor

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

This comment has been minimized.

@BryceStevenWilley

BryceStevenWilley Apr 29, 2018

Author Contributor

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.

This comment has been minimized.

@rhaschke

rhaschke Apr 29, 2018

Contributor

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.

@@ -329,8 +328,26 @@ void ompl_interface::ModelBasedPlanningContext::interpolateSolution()
if (ompl_simple_setup_->haveSolutionPath())
{
og::PathGeometric& pg = ompl_simple_setup_->getSolutionPath();
pg.interpolate(

This comment has been minimized.

@rhaschke

rhaschke Apr 29, 2018

Contributor

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.

This comment has been minimized.

@BryceStevenWilley

BryceStevenWilley Apr 29, 2018

Author Contributor

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.

This comment has been minimized.

@rhaschke

rhaschke Apr 29, 2018

Contributor

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.

This comment has been minimized.

@BryceStevenWilley

BryceStevenWilley Apr 29, 2018

Author Contributor

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
Copy link
Contributor

rhaschke left a comment

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

This comment has been minimized.

Copy link
Contributor Author

BryceStevenWilley commented Apr 29, 2018

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
Copy link
Contributor

rhaschke left a comment

@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

This comment has been minimized.

Copy link
Contributor

rhaschke commented Apr 29, 2018

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

This comment has been minimized.

Copy link
Contributor Author

BryceStevenWilley commented Apr 29, 2018

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

This comment has been minimized.

Copy link
Contributor

rhaschke commented Apr 29, 2018

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

This comment has been minimized.

Copy link
Member

v4hn commented Apr 30, 2018

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented Apr 30, 2018

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 added some commits Apr 30, 2018

Revert "Removed max solution segement length as a parameter, as it sh…
…ould be indirectly set by the valid segment fraction. Otherwise, it's possible for OMPL to find a path that is continually rejected by MoveIt."

This reverts commit 1706604.
@BryceStevenWilley

This comment has been minimized.

Copy link
Contributor Author

BryceStevenWilley commented Apr 30, 2018

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 rhaschke changed the base branch from melodic-devel to kinetic-devel Apr 30, 2018

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented Apr 30, 2018

@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

This comment has been minimized.

Copy link
Member

v4hn commented May 1, 2018

@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 😁 ?

@rhaschke rhaschke merged commit efb8faa into ros-planning:kinetic-devel May 1, 2018

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details
@davetcoleman

This comment has been minimized.

Copy link
Member

davetcoleman commented May 5, 2018

@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

This comment has been minimized.

Copy link
Contributor Author

BryceStevenWilley commented May 5, 2018

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

@BryceStevenWilley BryceStevenWilley deleted the KavrakiLab:interpolate-fix-melodic branch May 11, 2018

dg-shadow added a commit to shadow-robot/moveit that referenced this pull request Jul 30, 2018

Make trajectory interpolation in MoveIt consistent to OMPL (ros-plann…
…ing#869)

Adapt final path simplification to match latest OMPL changes:
As optimizing planners, like RRT*, consume all planning time, there should be at least one final simplification step. If more time is available OMPL will continue simplifying the path. No need for checking the time in MoveIt anymore.

Resolved redundancy of parameters `longest_valid_segment_fraction` and `max_waypoint_distance`, only passing the most conservative to OMPL.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.