An efficient and direct method for trajectory optimization of robots constrained by contact kinematics and forces

In this work, we propose a trajectory generation method for robotic systems with contact kinematics and force constraints based on optimal control and reachability analysis tools. Normally, the dynamics and constraints of a contact-constrained robot are nonlinear and coupled to each other. Instead of linearizing the model and constraints, we solve the optimal control problem directly to obtain feasible state trajectories and their corresponding control inputs. A tractable optimal control problem is formulated and subsequently addressed by dual approaches, which rely on sampling-based dynamic programming and rigorous reachability analysis tools. In particular, a sampling-based method together with a Partially Observable Markov Decision Process solution approach are used to break down the end-to-end trajectory generation problem by generating a sequence of subregions that the system’s trajectory will have to pass through to reach its final destination. The distinctive characteristic of the proposed trajectory optimization algorithm is its ability to handle the intricate contact constraints, coupled with the system dynamics, in a computationally efficient way. We validate our method using extensive numerical simulations with two legged robots.