IEEE Robotics & Automation Magazine - June 2020 - 97

closure at more than 500 Hz. Furthermore, in practice,
whole-body QP controllers exhibit robustness to model
uncertainties due to fast, frequent loop closures and work
reliably on the real Valkyrie platform.
After the MJMPC generates a reference CoM trajectory
y ref , a whole-body controller minimizes the tracking error
J task = (1/2) < y - y ref < while guaranteeing physically feasible
torques that realize the push recovery motion. The reformulation of the tracking tasks of J task = (1/2) < y - y ref < 2 =
..
(1/2) < AX - b < 2 with state X = [ q, x, m] T consisting of
torque commands x, joint accelerations qp , and contact wrench
T
T
m yields the matrices H = A A, f = - A b. The QP problem
is formulated as
min X T HX + f T X,
X
s.t. A eq X + B eq = 0,
A ineq X + B ineq $ 0.

(6)

Further tasks, e.g., tracking feet trajectories from the nonlinear step-location optimizer, regularization terms, or other
tasks benefiting the stability of the controller, can be stacked in
A = [w 0 A 0, w 1 A 1, ..., w n A n] T , b = [w 0 b 0, w 1 b 1, ..., w n b n] T ,
and task priorities are represented by the weights w i, where
i = 0, ..., n is the ith task (for more details, compare [14]).
The equations of motion-guaranteeing physical coherence
between the states form the equality constraints of the QP
problem (6):

Ankle Real
Toe Fitted

Ankle Fitted
Step Real

..

6M (q) - S - J (q)@ >xH + h (q, qo ) = 0,
m

Toe Real
Step Fitted

Ankle Real
Toe Fitted
Position (m)

Position (m)

0.1
0
0.5

1

1.5

2

Toe Real
Step Fitted

0.96
0.94
0.92

2.5

0.4
Velocity (m/s)

Velocity (m/s)

Ankle Fitted
Step Real

0.98

0.2

0

0.2
0
0

0.5

1

1.5

2

Ankle

1.34

Toe

Torque (Nm)

1.36

Step

1.32
0

0.5

1

1.5
Time (s)

2

2.5

0
.10-2

0.5

1

1.5

2

2.5

0

0.5

1

1.5

2

2.5

0
-5

2.5

1.38
Force (kN)

(7)

with inertia matrix M(q), selection matrix S, Jacobian matrices J T (q) of the contact links, and nonlinear effects h (q, qo ) .
In addition to the physical coherence of the solution, locomotion-specific constraints are formulated, ensuring that the
robot will not fall over. This is achieved by imposing inequality constraints in the QP problem on the contact wrench
m = [fx, f y, fz, x x, x y, x z] between its feet and the ground.
Slippage in the x, y direction is prevented by constraining the
force in the respective direction | fx | # nfz, | f y | # nfz for a
given friction coefficient n. Unilateral forces (no suction of
the feet to the ground) are guaranteed by fz 2 0. A stability
constraint is achieved by constraining the CoP within the
support polygon | x x | # Yfz, | x y | # Xfz with dimensions
X, Y of the support polygon. Lastly, yaw slippage is prevented by x min # x z # x max with x min = - n (X + Y) fz +
| Yfx - nx x | + | Xf y - nx y |, x max = + n (X + Y) fz - | Yfx +
nx x | -| Xf y + nx y | .
The physical feasibility of the motions on the actuators
during push recovery motions can be further shown using an
IP model [16] as in Figure 10(b), which simplifies the dynamics of a robot into an IP whose length can be extended via a
"kick force" and a torque applied to the pivot point corresponding to the ground reaction force and torque of the real
robot. Using the torque limits of the actuators, the robot can

0.3

-0.2

q

T

0
-200

Ankle

Toe

Step

-400
0

0.5

1

(a)

1.5
Time (s)

2

2.5

(b)

Figure 9. The fit between human data and the reverse-engineered controller. (a) The x position, velocity, and required force. (b) The z
position, velocity, and required torque.

JUNE 2020

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

97
IEEE Robotics & Automation Magazine - June 2020

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2020
