More support has been added for state spaces whose distance functions aren't metrics. Virtual methods were implemented in StateSpace in order for clients (such as planners) to query whether they're working in a StateSpace with a) a symmetric distance function, b) a distance metric, or c) a symmetric interpolate() function. RRT* has been updated to perform correctly in these non-metric spaces (for instance, doing the near-neighbors routines in both directions).
The notion of a cost associated with an OptimizationObjective has been generalized from the previous assumption of all costs being floating-point values. Cost is now a class associated with an OptimizationObjective in a very similar way to how State is associated with StateSpace (care was taken to have analogous functionality between the two ideas). TRRT has its own specific OptimizationObjective that's based on this now). The idea for this is to allow for more sophisticated optimization measures such as maximizing minimum clearance over a path, or even combining multiple objectives. The RRTstar planner has been updated to handle this new implementation of Cost with nearly no hits to performance. Also, some serious flaws in the RRTstar implementation currently in OMPL were fixed.
OptimizationObjective hierarchy has been moved around a bit to reflect some discussion on the prior version of this pull req. OptimizationObjective is now assumed to be accumulative, and has a getStateCost() method which reflects evaluating a cost map defined on the state space. It has a child PathIntegralOptimizationObjective, whose default functionality is to optimize path length. Mechanical work optimization is also included. (mini-update: added helper method to PathIntegralOptimizationObjective and removed redundant check for StateValidityChecker in BallTreeRRTstar)
As always, I'm very happy to discuss any possible changes to this API to allow easier use and more expressivity.
This is awesome Luis! I think we can merge this as soon as 1 & 3 get resolved. Thank you for your contribution!
If you have any test code or data to evaluate performance loss with respect to the previous implementation, that would be useful. I really appreciate the consistency in style with the rest of OMPL.
This is fantastic work!
I ran an extremely unscientific test to compare the performance of the previous RRT* implementation and the new one with the generalized cost. I timed how long each version of the planner took to run 50,000 planning iterations. Averaged over 10 trials, the previous implementation took (4.83 +/- 0.088) seconds, and the new one took (5.42 +/- 0.12) seconds, which makes a performance hit of 12%. I'll note that the planners were run on a 2D point robot, so in more realistic problems the collision detection will likely overshadow the performance hit. That said, I'll still look into any other ways I can optimize cost computation.
However, the generalized cost framework opens up some neat possibilities! Let me show you some of the neat cost metrics RRT* can now optimize without any change to the planner itself. This first picture shows a path found which optimizes only path length:
Then, here's a path which tries to maximize the minimum clearance of the path (note that, besides the point of minimum clearance, the rest of the path is unconstrained):
And finally, here's a path which prioritizes maximizing minimum clearance of the path, while otherwise optimizing path length. This was very easy to do with the generalized cost framework:
This looks great! I think the performance penalty is acceptable. Mark, what do you think?
As far as I am concerned, the only major thing we need to address is the changes to the MotionValidator. I'd like the code that used to be there (in TRRT, for this pull req) for computing costs for a path to be accessible to other planners. Perhaps in SpaceInformation? And we would pass the objective as a const& argument? (It does not really make sense to make SpaceInformation keep an OptimizationObjectivePtr). Also, functions like the average cost of a state could be made part of the OptimizationObjective base class.
If remove cost() from StateValidityChecker, it makes sense to remove cost computation from the MotionValidator as well, but I think we still need to have it somewhere in base/;
I think the additional flexibility of the generalized cost optimization is worth a 12% performance hit. I see you have removed quite a few things from code in ompl::base. Are both of you sure we don't lose any functionality with these changes? Also, do we still need the isSymmetricDistance member function now that I have added the method isMetricSpace to StateSpace?
The pictures look very cool. I am very eager to get this code merged into the OMPL repo. We should pull into a separate branch first, do some more testing, and then merge with default.
Thanks for the feedback, Mark! My reasoning for leaving in isSymmetricDistance was for cases like DubinsStateSpace: if the user elects to enable symmetric distance in the DubinsStateSpace, it's still not a metric because the triangle inequality does not hold. Distinguishing symmetric spaces and metric spaces could be useful in cases where planners like RRT* can still make use of the symmetry of the space, but we need to ensure that GNAT doesn't get used for nearest neighbors because the space doesn't define a metric distance.
I do admit it's a bit of a stretch, though; do you think this use-case is worth having the extra symmetry methods in StateSpace?
Alright, things look good to me. I would still like to check out the code and run some tests (but the earliest I will have time is maybe at the weekend), but if Luis and Mark/Ryan also give a +1, we can merge. I recommend merging into a branch of the ompl repo first, test things thoroughly, make sure omplapp still works (propagate updates if needed), and then merge to default.