  Terrain geometry is, in general, non-smooth, non-linear, non-convex, and, if
perceived through a robot-centric visual unit, appears partially occluded and
noisy. This work 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
heightmap. 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 non-flat ground formulation.
Direct collocation is used as transcription method, resulting in a non-linear
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.
