A control system for controlling a movement of a vehicle using a motion planner, comprising: at least one processor; and a memory having instructions stored thereon that cause the at least one processor of the control system to: construct a graph having multiple nodes defining states of the vehicle, the multiple nodes of the graph comprising an initial node defining an initial state of the vehicle and a target node defining a target state of the vehicle, wherein each pair of nodes in the graph is connected with an edge defined by one or a combination of collision-free motion primitives moving the vehicle between the states of the connected nodes, each node comprising a number of motion cusps, and wherein the multiple nodes connected through the corresponding edges form a first path through the graph connecting the initial node with the target node by a sequence of motion primitives for moving the vehicle from the initial state to the target state, wherein the first path is constructed without a constraint associated with the number of motion cusps; determine a first number of motion cusps in the sequence of motion primitives of the first path, wherein a motion cusp from the first number of motion cusps indicates a switching between a forward motion and a backward motion in the sequence of motion primitives; on determining that the first number of motion cusps is above a threshold, expand the graph to add new nodes until a termination condition is met, wherein the expansion of the graph is subjected to a hard constraint associated with a total number of motion cusps, the graph being expanded to form a second path connecting the initial node with the target node, wherein the second path has a second number of motion cusps that is less than the first number of motion cusps; and control the movement of the vehicle based on the second path. at least one processor; and a memory having instructions stored thereon that cause the at least one processor of the control system to: construct a graph having multiple nodes defining states of the vehicle, the multiple nodes of the graph comprising an initial node defining an initial state of the vehicle and a target node defining a target state of the vehicle, wherein each pair of nodes in the graph is connected with an edge defined by one or a combination of collision-free motion primitives moving the vehicle between the states of the connected nodes, each node comprising a number of motion cusps, and wherein the multiple nodes connected through the corresponding edges form a first path through the graph connecting the initial node with the target node by a sequence of motion primitives for moving the vehicle from the initial state to the target state, wherein the first path is constructed without a constraint associated with the number of motion cusps; determine a first number of motion cusps in the sequence of motion primitives of the first path, wherein a motion cusp from the first number of motion cusps indicates a switching between a forward motion and a backward motion in the sequence of motion primitives; on determining that the first number of motion cusps is above a threshold, expand the graph to add new nodes until a termination condition is met, wherein the expansion of the graph is subjected to a hard constraint associated with a total number of motion cusps, the graph being expanded to form a second path connecting the initial node with the target node, wherein the second path has a second number of motion cusps that is less than the first number of motion cusps; and control the movement of the vehicle based on the second path. construct a graph having multiple nodes defining states of the vehicle, the multiple nodes of the graph comprising an initial node defining an initial state of the vehicle and a target node defining a target state of the vehicle, wherein each pair of nodes in the graph is connected with an edge defined by one or a combination of collision-free motion primitives moving the vehicle between the states of the connected nodes, each node comprising a number of motion cusps, and wherein the multiple nodes connected through the corresponding edges form a first path through the graph connecting the initial node with the target node by a sequence of motion primitives for moving the vehicle from the initial state to the target state, wherein the first path is constructed without a constraint associated with the number of motion cusps; determine a first number of motion cusps in the sequence of motion primitives of the first path, wherein a motion cusp from the first number of motion cusps indicates a switching between a forward motion and a backward motion in the sequence of motion primitives; on determining that the first number of motion cusps is above a threshold, expand the graph to add new nodes until a termination condition is met, wherein the expansion of the graph is subjected to a hard constraint associated with a total number of motion cusps, the graph being expanded to form a second path connecting the initial node with the target node, wherein the second path has a second number of motion cusps that is less than the first number of motion cusps; and control the movement of the vehicle based on the second path.