Reachability-based Trajectory Optimization for Robotic Systems Given Sequences of Rigid Contacts

This paper proposes a method to generate feasible trajectories for robotic systems with predefined sequences of switched contacts. The proposed trajectory generation method relies on sampling-based methods, optimal control, and reachability analysis. In particular, the proposed method is able to quickly test whether a simplified model-based planner, such as the Time-to-Velocity-Reversal planner, provides a reachable contact location based on reachability analysis of the multi-body robot system. When the contact location is reachable, we generate a feasible trajectory to change the contact mode of the robotic system smoothly. To perform reachability analysis efficiently, we devise a method to compute forward and backward reachable sets based on element-wise optimization over a finite time horizon. Then, we compute robot trajectories by employing optimal control. The main contributions of this study are the following. Firstly, we guarantee whether planned contact locations via simplified models are feasible by the robot system. Secondly, we generate optimal trajectories subject to various constraints given a feasible contact sequence. Lastly, we improve the efficiency of computing reachable sets for a class of constrained nonlinear systems by incorporating bi-directional propagation (forward and backward). To validate our methods we perform numerical simulations applied to a humanoid robot walking.