Safe Whole-Body Task Space Control for Humanoid Robots

Complex robotic systems require whole-body controllers to handle contact interactions, handle closed kinematic chains, and track task-space control objectives. However, for many applications, safety-critical controllers are essential to steer away from undesired robot configurations and prevent unsafe behaviors. A prime example is legged robotics, where we can have tasks such as balance control, regulation of torso orientation, and, most importantly, walking. As the coordination of multi-body systems is non-trivial, following a combination of those tasks might lead to configurations that are deemed dangerous, such as stepping on its support foot during walking, leaning the torso excessively, or producing excessive centroidal momentum, resulting in non-human-like walking. To address these challenges, we propose a formulation of an inverse dynamics control enhanced with control barrier functions that allow general higher-order relative degree safe sets for robotic systems with numerous degrees of freedom. Our approach utilizes a quadratic program that respects closed kinematic chains, minimizes the control objectives, and imposes desired constraints on the Zero Moment Point, friction cone, and torque. More importantly, it also ensures the forward invariance of a general user-defined high Relative-Degree safety set. We demonstrate the effectiveness of our method by applying it to the 3D biped robot Digit, both in simulation and with hardware experiments.