Using motion primitives to enforce vehicle motion constraints in sampling-based optimal planners