RRTstar planner assumes symmetric distance/cost functions

Luis Torres avatarLuis Torres created an issue

Currently, OMPL's RRTstar planner is implemented in a way which is invalid for state spaces with asymmetric distance/cost functions, e.g. DubinsStateSpace. To be helpful, I've attached my own OMPL implementation of RRTstar which correctly handles asymmetric distance and cost functions.

The current implementation of RRTstar makes the following assumptions which do not hold in a space like DubinsStateSpace: 1) checkMotion(s1, s2) is the same as checkMotion(s2,s1); 2) distance(s1,s2) is the same as distance(s2,s1); 3) cost(s1,s2) is the same as cost(s2,s1); 4) the nearby neighbors for the connection step are the same as the nearby neighbors for the rewiring step.

Therefore, an RRTstar implementation which works for DubinsStateSpace would do TWO nearest neighborhood searches (one FROM the new motion, and one TO the new motion), and it would perform distinct sets of distance(), cost() and checkMotion() calls for each of the two different neighborhoods. This is of course more computationally intensive, which further motivates having a way of knowing at run-time whether the StateSpace has symmetric distance/cost functions (this way we can save computation if we know the StateSpace's distance/cost functions are symmetric).

Another tangential issue in the current implementation of RRTstar is the assumption of a PathLengthOptimizationObjective. This is unnecessarily strict, since the cost metric only requires certain boundedness, monotonicity, and smoothness properties. In my implementation, I've used the getIncrementalCost() and combineObjectiveCosts() methods of OptimizationObjective in order to generalize the objective optimized by the planner. I've chosen to enforce the use of a BoundedAdditiveOptimizationObjective for semantic reasons, because RRT* assumes that path cost is "accumulated" from one configuration to another (although I think ideally we should enforce the use of a class called something like BoundedAccumulativeOptimizationObjective).

Comments (6)

  1. Ioan Sucan

    This looks good. We can add a virtual method to StateSpace to tell whether motions are symmetric. We can use that instead of some of the sanity checks flags too.

  2. Luis Torres

    In my fork of OMPL, I've added a virtual method hasSymmetricDistance() to StateSpace.

    However, it seems that there's more than one notion of symmetry in a given space; there's also the question of whether the state interpolation is symmetric; i.e. whether

    interpolate(from, to, t, s) == interpolate(to, from, (1-t), s)

    Ideally symmetry would imply that both the distance function and the state interpolation are symmetric, but this may not necessarily be the case. For instance, one may think that the current (asymmetric) DubinsStateSpace::distance() function is too expensive, and therefore opt to use the (symmetric) SE2StateSpace::distance() function instead as a heuristic, but still prefer to use the current (asymmetric) DubinsStateSpace::interpolate() function. In such a case, we'd have a symmetric distance function and an asymmetric interpolation.

    Maintaining this distinction in symmetry of interpolation is again important for RRTstar: if interpolation is symmetric, we can assume that if checkMotion(s1, s2) is true, then checkMotion(s2, s1) is true and save some computation time.

    Does adding yet another virtual method, StateSpace::hasSymmetricInterpolation(), make sense?

  3. Mark Moll

    Is the member function StateSpace::isMetricSpace() sufficient for your needs, or do you still need StateSpace::hasSymmetricInterpolation()? Given that you are working on this in your own fork, we should just wait for your pull request and close this ticket after you code has been pulled (provided it all looks look to the OMPL developers)? BTW, I think your generalization of RRT* is a great idea.

  4. Log in to comment
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.