A Benchmarking of DCM Based Architectures for Position, Velocity and Torque Controlled Humanoid

Giulio Romualdi1, Stefano Dafarra1, Yue Hu2, Daniele Pucci1

Abstract— This work contributes towards the development p˙B, the angular ωB base velocity and the joint velocities s˙. of a three-layer controller architecture for bipedal locomotion. M(q) is the mass matrix and h(q, ν) indicates the Coriolis, From the top to the bottom, the layers consist on a trajectory centrifugal and gravitational terms. B is a selection matrix. optimization, a simplified model control and a whole-body QP control. In contrast to the first two layers, the third one Jc(q) is the contact wrenches Jacobian and f are the contact relies on the full dynamic model to produce either wrenches. τ are the control inputs. position, velocity or torque commands, and it is the main For cases in which, the walks maintaining responsible for the compliant behaviour of the robot. All these a constant height between the CoM and the stance foot, then approaches have been successfully implemented and tested on the motion of the robot can be approximated with the Linear the iCub humanoid robot. Noticeably, one of the proposed implementations allows the robot to reach a forward walking inverted pendulum model: velocity of 0.41 meters per second, which is the highest walking 1 x¨ = (x − rzmp), (2) velocity achieved for the iCub humanoid robot. b2 I.INTRODUCTION where x is the projection of the CoM on the walking surface zmp In the last decades, a common approach for facing the and b > 0 and r the zero moment point (ZMP). humanoid robot locomotion problem was developing a hier- The Divergent Component of Motion (DCM) is archical architecture composed of several layers. Each layer 1 ξ = x + bx˙ ξ˙ = (ξ − rzmp). (3) generates references for the layer below by processing inputs b from the robot, the environment, and the outputs of the layer It is interesting to notice that the DCM can be seen as the before. From top to bottom, these layers are called in this unstable dynamics of the linear inverted pendulum model. work: trajectory optimization, simplified model control, and whole-body quadratic programming (QP) control. III.ARCHITECTURE In this summary, the trajectory optimization layer is kept A. Trajectory optimization layer fixed with a unicycle based planner that generates desired The main purpose of the trajectory optimization layer is to DCM and feet trajectories. The simplified model control evaluated the desired footsteps, DCM and feet trajectories. layer, instead, implements two controllers for the tracking of The footsteps positions are planned by representing the the DCM: an instantaneous and an MPC controller. Finally, humanoid robot motion as an unicycle, where the feet are the whole-body QP control implements two controllers for represented by the unicycle wheels. Once the footsteps are the tracking of the Cartesian trajectory: a kinematics-based planned, the feet trajectory is evaluated by cubic spline and dynamics-based whole body controllers. The former interpolation. The footsteps positions are also used to plan uses the kinematics of the robot for generating desired the desired DCM trajectory. For the single support phase, the joint positions/velocities; while the latter is based on the DCM is chosen so as to satisfy (3). In order to guarantee a entire robot dynamics and its output is the desired joint continuous ZMP trajectory, during the double support phase, torques. The presented architecture was validated on the iCub the DCM is planned by mean of third order polynomial. humanoid robot (Fig. 2) and it was successfully exploited on a teleoperation framework 1. B. Simplified model control layer II.HUMANOIDROBOTSMODELS The simplified model control layer is in charge to im- A humanoid robot is a floating base multi-body system plement a control law based on the simplified model of the described by the following differential equation: humanoid robot to stabilize the unstable DCM dynamics (3). > M(q)ν ˙ + h(q, ν) = Bτ + Jc (q)f, (1) Feet Pose and Torso Orientation where q contains the pose of the base link with respect the inertial frame and the joint angles s. ν contains the linear Desired Desired CoM Joint Positions, Trajectory DCM Simplified Model Velocity or ZMP Whole-Body QP Velocities or Torques Robot Optimization Control Control 1 Fondazione Istituto Italiano di Tecnologia, Genova, Italy (e-mail: [email protected]). 2 JSPS Postdoctoral Fellowship for Research in Japan (Standard), Contact Wrenches CNRS-AIST JRL, UMI3218/RL, Intelligent Systems Research Institute, Na- Joint and CoM Position/Velocity tional Institute of AIST, Tsukuba, Japan (e-mail: [email protected]). Fig. 1: The control architecture is composed of three layers: the trajectory 1https://www.youtube.com/watch?v=jemGKRxdAM8 optimization, the simplified model control, and the whole-body QP control. The stabilization problem is faced by developing two con- the dynamics of the system. The control objective is achieved trollers, an instantaneous and a predictive control laws. by designing a constrained quadratic optimization problem h i The instantaneous control law is given by: whose conditional variable is u> = ν˙ > τ > f > . The Z zmp ξ control problem can be summarized in the following opti- r = ξ −bξ˙ + Kξ(ξ−ξ ) + K (ξ−ξ ) dt. ref ref ref p ref i ref mization problem: (4) ∗ ∗ 2 ∗ 2 2 Even if the proposed control law is very simple to imple- u = arg min kω˙ T − ω˙ T kΛ +ks¨ − s¨ kΛ +kτkΛ (6a) u T s t ment it may generate an infeasible desired ZMP. This draw- ref d p d back is handled by developing a model predictive control s.t. s¨ = −Ks s˙ − Ks (s − s ) (6b) (MPC). The DCM dynamics (3) is used as the prediction A◦sf = 0 ◦ = {s, d} (6c) model and it is discretized supposing piecewise constant Cf ≤ 0 (6d) ZMP trajectory. On the other hand, the tracking of the desired ref JC ν˙ =v ˙ − J˙C ν (6e) DCM trajectory as well as a smooth control signal is handled z Cz z ˙ ∗ by the cost function. Finally the feasibility of the ZMP is J◦F ν˙ = v◦F − J◦F ν ◦ = {L, R} (6f) h > i guaranteed by an inequality constraint. −M(q) BJc (q) u = h(q, ν) (6g) Independently from the chosen DCM controller, one ob- τ − ≤ τ ≤ τ +. (6h) tains a desired ZMP position. In case of kinematics-based whole-body QP control layer, another control loop is needed The high priority tasks are the tracking of the desired feet for ensuring the tracking of the desired ZMP trajectory. This pose (6f), of the ZMP desired trajectory (6c), and of the task is achieved by implementing a ZMP-CoM controller. CoM height (6e). The torso orientation and the postural task C. Whole-body QP controller are still considered as a low priority (6a). The term (6d) guarantees feasible contact wrenches. By rearranging (1), the The main objective of the whole-body QP control layer is robot dynamics can be treated as an equality constraint (6g). to ensure the tracking of the desired trajectories by using the entire robot model. The control problem can be tackled by IV. COMPARISON using either the kinematics model of the robot, or taking into Tab. I summarizes the maximum velocities achieved using account the entire robot dynamics. Independently from the the different implementations of the control architecture. In chosen architecture, the control objective can be achieved by particular, the labels instantaneous and predictive mean that design the controller as a constrained optimization problem the associated layer generates its outputs considering inputs where the low priority tasks are embedded in the cost func- and references either at the single time t or for a time win- tion and the high priority tasks are considered as constraints. dow, respectively. The labels, velocity, position and torque The kinematics-based whole-body controller can be sum- control, instead, mean that the layer outputs are either desired marized in the following optimization problem: joint velocities, position or torque, respectively. Finally, the ν∗ = arg min kω∗ − ω k2 +ks˙ − s˙∗k2 (5a) labels iCub and simulation are used to indicate platform used T T ΛT Λs ν for the experiments, namely the iCub humanoid robot, or a ∗ d s.t. s˙ = −Ks(s − s ) (5b) Gazebo simulation. Noticeably, the instantaneous controller ∗ JCν = vC (5c) combined with a position controlled robot, allow the robot to ∗ ∗ achieve forward walking velocity of 0.41 m s−1, which is the JLF ν = vLF JRF ν = vRF (5d) highest velocity ever achieved for the iCub humanoid robot. s˙− ≤ s˙ ≤ s˙+. (5e) When the robot is torque controlled, we noticed that the Here the tracking of the desired torso orientation and of noise affecting the measured DCM does not allow us to use the joint regularization term are embedded in the cost func- the simplified model controllers. So we decide to stabilize a tion (5a). On the other hand, the tracking of the desired feet desired CoM instead of DCM. In order to maintain consis- trajectory (5d) and the CoM (5c) are hard constraints. tency with the previous architecture, we decide to generate The decision variable ν contains the desired joint veloci- the desired CoM trajectory from the LIPM dynamics (2) ties. This desired quantity can be used directly as a reference starting from a desired DCM trajectory. to a joint velocity controller, if available. Otherwise, they can be integrated and fed to a low-level joints position controller. In case a low-level torque controller is available, the TABLE I: Maximum forward straight walking velocities achieved using presented architecture can be extended by considering also different implementations of the control architecture. Simplified Model Whole-Body Max Straight Platform Control QP Control Velocity (m/s) iCub Predictive Velocity 0.19 iCub Predictive Position 0.20 iCub Instantaneous Velocity 0.22 iCub Instantaneous Position 0.41 iCub - Torque 0.02 Simulation Predictive Torque 0.08 Simulation Instantaneous Torque 0.12 Fig. 2: iCub walks with the presented controller architecture.