fixed RRTConnect planner (reverse tree wasn't growing...)