Convex feasible set algorithm for constrained trajectory smoothing
RRT-connect: An efficient approach to single-query path planning