 edited description
RRTstar planner assumes symmetric distance/cost functions
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 runtime 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 (7)


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.

 edited description
 changed title to RRTstar planner assumes symmetric distance/cost functions
Removed references to "asymmetric metric" since an asymmetric distance function cannot technically define a metric

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, (1t), 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?

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.

 changed status to resolved
fixed since Luis' branch has been merged

I'm trying to use RRTStar with asymmetric cost and interpolation. It looks like this is handled in the code, but there is that test:
if (!si_>getStateSpace()>hasSymmetricDistance()  !si_>getStateSpace()>hasSymmetricInterpolate()) { OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str()); }
in the setup method (line 109 of RRTStar.cpp). Is that test remaining from before asymmetric cases were handled? Or are asymmetric cases still not properly handled by the algorithm?
 Log in to comment
updated readability