Uncommented, very ad-hoc (not very reusable) code to boost OMPL development.