Basic RRTPlanner working