POSQ properly Working in OMPL RRT, in a hacky way, waiting OMPL mods from the master