get RRTPlanner working, without path shortening