BiRRT planner test