fillet based RRT

[MaximilianDio]

Hello,

I am currently exploring ways to obtain smooth paths without post-processing, and I came across the work of James Swedeen: Fillet-based RRT∗: A Rapid Convergence Implementation of RRT for Curvature Constrained Vehicles*.

I understand that specific state spaces can encapsulate curvature, but I would like to achieve this in a general R^n state space. While the idea of using quadratic fillets seems straightforward conceptually, the implementation in OMPL appears more involved. Especially since the motion validator needs to distinguish whether the system is currently on a fillet or on a straight segment.

Do you have suggestions for how this could be integrated into the existing OMPL framework without altering too much? My initial thought is to define a new state space with a custom motion validator (similar to DubinsStateSpace), and then implement a FilletRRT as a starting point.

Any guidance or recommendations that could help.

Regards,
Max

[saahu27]

Hi @MaximilianDio ,

I'd Suggest to implement Custom StateSpace with Specialized Motion Validator. This is similar to how DubinsStateSpace work.

• Create FilletStateSpace inheriting from the appropriate base space (e.g., SE2StateSpace or RealVectorStateSpace for general R^n)
  -Override methods like distance() and interpolate() (similar to DubinsStateSpace::interpolate()).
• Create FilletMotionValidator (inheriting from MotionValidator). It should Compute the fillet path once and cache it (like DubinsMotionValidator does).
• The planner doesn't need modification. setStateSpace() to provide your custom fillet state space.

Reference Implementation Pattern
Look at ompl/base/spaces/DubinsStateSpace.{h,cpp} as a template:

See how it caches the DubinsPath in the motion validator
Note the dubinsPathType_ enum for segment types
Study how interpolate() computes intermediate states along the path

Additional Considerations :

Triangle inequality: Note that fillet-based distance may not satisfy the triangle inequality (like Dubins). Document this and ensure nearest neighbor structures handle it (avoid NearestNeighborsGNAT)
Symmetry: Decide if your distance metric should be symmetric (affects RRT* correctness guarantees)
Optimization objective: You can use path length as the objective, which aligns naturally with fillet paths.

Feel free to reach out if you'd like to contribute this to OMPL - fillet-based planning would be a valuable addition! You could also look at creating a demo similar to demos/PlanarManipulator.cpp or demos/Dubins.cpp.

[MaximilianDio]

Awsome, the first attemt seems to work (quadratic fillets with no noticible computational overhead), I just tested RRT for now. It still needs some adjustments especially regarding your mentioned considerations and finding the correct fillet lenght, etc. - but the general idea seems to work... I would love to contribute when it is in a propper state ;)