Complementarity-based dynamic simulation for kinodynamic motion planning

In this paper, we present the use of complementarity-based dynamic simulation algorithms for kinodynamic motion planning. Dynamic simulation algorithms are used as local planning methods in sampling-based motion planning algorithms to find inputs that ensure the resulting trajectory satisfies the dynamics constraints. However, the inputs are not guaranteed to give collision-free path segments. The inputs, chosen either by random sampling or from a discretization of the available inputs, are rejected if the path segment is not collision free. In cluttered environments, finding a feasible input is difficult and sensitive to the duration Δt of application of the input, and to the discretization resolution of the input set. When the collision constraints (or any inequality constraints on the state of the robot) are modeled as a set of complementarity constraints, the dynamic simulation algorithm gives a path segment that touches the obstacles and a set of contact forces whenever the robot makes contact with the obstacles. The sum of the chosen input forces and the contact forces transformed to the input space gives a control input that guarantees a collision-free path segment (provided it is within the actuator bounds). Thus in cluttered environments, using a complementarity-based dynamic simulation algorithm, we can find a feasible input that is relatively insensitive to the choice of Δt and the discretization resolution of the input set. We present simple simulation examples showing the advantages of our algorithm in cluttered environments.