TAMOLS: Terrain-Aware Motion Optimization for Legged Systems

Terrain geometry is, in general, nonsmooth, nonlinear, nonconvex, and, if perceived through a robot-centric visual unit, appears partially occluded and noisy. This article presents the complete control pipeline capable of handling the aforementioned problems in real-time. We formulate a trajectory optimization problem that jointly optimizes over the base pose and footholds, subject to a height map. To avoid converging into undesirable local optima, we deploy a graduated optimization technique. We embed a compact, contact-force free stability criterion that is compatible with the nonflat ground formulation. Direct collocation is used as transcription method, resulting in a nonlinear optimization problem that can be solved online in less than ten milliseconds. To increase robustness in the presence of external disturbances, we close the tracking loop with a momentum observer. Our experiments demonstrate stair climbing, walking on stepping stones, and over gaps, utilizing various dynamic gaits.