For part of my research I need to use an optimal kinodynamic planner. Mark Moll’s OMPL is fabulous but he only has optimal geometric planners (RRT*) and suboptimal kinodynamic planners (RRT, SST). Digging through his issue tracker on this, it does not look like there are good alternatives.
Does anyone have experience modifying OMPL to have a kinodynamic RRT*, have any other libraries or am I stuck doing this from scratch? There are a lot of githubs splattered around with implementations, but I’m looking for something well maintained and in 3D (and preferable allows you to add optimization constraints, different ODE solvers, KD trees if I want them, etc.).