# Multi-contact Walking Pattern Generation based on Model Preview Control of 3D COM Accelerations

Stéphane Caron, Abderrahmane Kheddar

► To cite this version:

Stéphane Caron, Abderrahmane Kheddar. Multi-contact Walking Pattern Generation based on Model Preview Control of 3D COM Accelerations. Humanoids 2016 - 16th IEEE-RAS International Conference on Humanoid Robots, Nov 2016, Cancún, Mexico. pp.550-557, 10.1109/HUMANOIDS.2016.7803329. [hal-01349880v5](https://hal.science/hal-01349880v5)

HAL Id: hal-01349880

https://hal.science/hal-01349880v5

Submitted on 20 Oct 2016

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>
# Multi-contact Walking Pattern Generation based on Model Preview Control of 3D COM Accelerations

Stéphane Caron<fnref n="1" /> and Abderrahmane Kheddar<fnref n="1" />,<fnref n="2" />

**Abstract**— We present a multi-contact walking pattern generator based on preview-control of the 3D acceleration of the center of mass (COM). A key point in the design of our algorithm is the calculation of contact-stability constraints. Thanks to a mathematical observation on the algebraic nature of the frictional wrench cone, we show that the 3D volume of feasible COM accelerations is always an upward-pointing cone. We reduce its computation to a convex hull of (dual) 2D points, for which optimal $O(n \log n)$ algorithms are readily available. This reformulation brings a significant speedup compared to previous methods, which allows us to compute time-varying contact-stability criteria fast enough for the control loop. Next, we propose a conservative *trajectory-wide* contact-stability criterion, which can be derived from COM-acceleration volumes at marginal cost and directly applied in a model-predictive controller. We finally implement this pipeline and exemplify it with the HRP-4 humanoid model in multi-contact dynamically walking scenarios.

## I. INTRODUCTION

Years ago, humanoid robots were considered as research platforms with vague perspectives in terms of concrete applications. Without much conviction, they were envisioned for entertainment, as receptionists, or as a high-tech showcase for other businesses. Some projects are challenging humanoids to be a daily companion or an assistant for frail persons<fnref n="1" />. The DARPA robotics challenge boosted the idea that humanoid robots can operate in disaster interventions. The challenge exhibited interesting developments while highlighting the road ahead. Nowadays, Airbus Group seriously envisions humanoids as manufacturing robots to act in large-scale airliner assembly lines. What makes humanoid robots a plausible solution in these applications is their physical ability to move in confined spaces, on non-flat floors, using stairs, etc. In such environments, there are large parts where the robot has to walk robustly.

Walking robustly on uneven floors is still an open problem in humanoid research. Recently, Boston Dynamics released an impressive video showing robust humanoid walks on various terrains<fnref n="2" />. This demonstration proves that the goal can be achieved. One key difficulty in locomotion is that the viability (the ability to avoid falling) of the states traversed while walking depends on future contacts. This problem can be addressed geometrically, as done in [2] using the generalized 3D capture point, or using dynamic programming as

<figure><img src="image_2.png" /><figcaption>Fig. 1. HRP-4 walking on a circular staircase with tilted stepping stones using a preview controller based on 3D COM accelerations. By bounding future COM positions (preview trajectory in yellow) into a polytope $T$ (red box), we derive a *trajectory-wide* contact-stability condition, the COM acceleration cone $I_T$ (in green), that is efficiently computed using a 2D convex hull algorithm. (Scale and position of this acceleration cone were chosen arbitrarily for depiction purposes.) See the accompanying video [1] for demonstrations of the controller in various multi-contact scenarios.</figcaption></figure>

done in [3] walking in the phase-space of the center of mass (COM), the latter being constrained on predefined surfaces that conform to terrain shape.

Model Predictive Control (MPC) is another widely applied framework that gives controllers the required hindsight to tackle this question. Following the design introduced by Hirukawa et al. [4], one active line of research [5], [6], [7] uses contact forces as control variables, which produces optimization problems with simple inequality constraints but a high number of control variables. Another line of research reduces the problem to the center of mass motion [8], [9], [10], [11]. Optimization problems are then much smaller, but their inequality constraints become quadratic (and non-positive-semidefinite [10]). So far, this problem has only been addressed for walking on parallel horizontal surfaces: in [8], by bounding vertical COM accelerations to keep the formulation linear, and in [9], [10], where Sequential Quadratic Programming was used to cope with quadratic inequalities.

In this paper, we introduce a method that decouples the quadratic inequalities of the general multi-contact problem into pairs of linear constraints, thus opening the way for resolution with classical MPC solvers.

<footnote>*This work is supported in part by H2020 EU project COMANOID http://www.comanoid.eu/, RIA No 645097.</footnote>

<footnote n="1">¹CNRS-UM2 LIRMM, IDH group, UMR5506, Montpellier, France.</footnote>

<footnote n="2">²CNRS-AIST Joint Robotics Laboratory (JRL), UMI3218/RL.</footnote>

<footnote>Corresponding author: stephane.caron@normalesup.org</footnote>

<footnote n="1">¹http://projetromeo.com/</footnote>

<footnote n="2">²https://www.youtube.com/watch?v=rVlhMGQgDkY</footnote>
## II. BACKGROUND

### A. Screw algebra

Humanoid robots are commonly modeled as a set of rigid bodies and joints whose motion can be described by screws [12]. A screw $s_O = (r, m_O)$ is a vector field generated by two vectors: its resultant $r$ and its moment $m_O$ at a given point $O$. From $m_O$ and $r$, the moment at any other point $P$ results from the Varignon formula:

$$m_P = m_O + \overrightarrow{PO} \times r. \quad (1)$$

The generalized velocity of a rigid body, called twist, is the screw $t_O = (v_O, \omega)$ with resultant $\omega$, the angular velocity, and moment $v_O$, its velocity at given point $O$. A generalized force acting on the body, called wrench, is a screw $w_O = (f, \tau_O)$ with resultant net force $f$, and net moment $\tau_O$, at a reference point $O$. Although the coordinate vector $s_P$ of a screw depends on the point $P$ where it is taken, the screw itself does not depend on the choice of $P$ as a consequence of the Varignon formula (1). We denote screws with hats ($\hat{t}$, $\hat{w}$) and their coordinates with point subscripts ($t_O, w_O$).

Twists and wrenches live in two dual spaces [12]: the motion space $M^6$ and the force space $F^6$. The scalar product between a twist $\hat{t} \in M^6$ and a wrench $\hat{w} \in F^6$ is given by:

$$\hat{t} \cdot \hat{w} \stackrel{\text{def}}{=} t_O \cdot w_O = v_O \cdot f + \omega \cdot \tau_O. \quad (2)$$

From (1), this number does not depend on the point $O$ where it is computed. When $\hat{t}$ and $\hat{w}$ are acting on a single rigid body, their product is the instantaneous power of the motion.

### B. Newton-Euler equations

Let $m$ denote the total mass of the robot and $G$ its center of mass (COM). We write $p_A$ the coordinate vector of a point $A$ in the inertial frame and denote by $O$ the origin of this frame (so that $p_O = 0$). Suppose that contacts between the robot and its environment are described by $K$ contact points. (This formulation includes surface contacts; see e.g. [13].) The Newton-Euler equations of motion of the whole robot are then given by:

$$\begin{bmatrix} m \ddot{p}_G \\ \dot{\mathbf{L}}_G \end{bmatrix} = \begin{bmatrix} mg \\ \mathbf{0} \end{bmatrix} + \sum_{i=1}^{K} \left[ \overrightarrow{G \hat{C}_i} \times \mathbf{f}_i \right] \quad (3)$$

where $\mathbf{L}_G$ is the angular momentum of the robot around $G$, $g = [0 \ 0 \ -g]^\top$ is the gravity vector defined from the gravity constant $g \approx 9.81 \text{ m s}^{-1}$ and $\mathbf{f}_i$ is the force exerted onto the robot at the $i$th contact point $C_i$. We say that a contact force $\mathbf{f}_i$ is feasible when it lies in the friction cone $C_i$ directed by the contact normal $\mathbf{n}_i$, i.e.,

$$\| \mathbf{f}_i - (\mathbf{f}_i \cdot \mathbf{n}_i) \mathbf{n}_i \|_2 \le \mu_i (\mathbf{f}_i \cdot \mathbf{n}_i) \quad (4)$$

where $\mu_i$ is the static friction coefficient. The problem of contact stability (also called force balance) is to find whole-body motions for which (3) admits solutions with feasible contact forces $\{\mathbf{f}_i\}$.

The Newton-Euler equations describe the components of motion that are independent from the actuation power of

the robot, and play a critical role in locomotion. Most of today's trajectory generators [14], [4], [15], [8], [6], [2], [10] focus on solving these equations and rely on whole-body controllers to take actuation limits into account at a later stage of the motion generation process.

### C. Wrench cones

Equations (3)-(4) include a large number of force variables. Although some walking pattern generators chose to work directly on this representation [4], another line of research [16], [17] found that these force variables can be eliminated by propagating their inequality constraints (4) into inequalities on the target rate of change $(m\ddot{p}_G, \dot{\mathbf{L}}_G)$ of the whole-body momentum.

Define the net contact wrench by:

$$w_O = \begin{bmatrix} f \\ \tau_O \end{bmatrix} \stackrel{\text{def}}{=} \sum_{i=1}^{K} \left[ \overrightarrow{O C_i} \times \mathbf{f}_i \right] \quad (5)$$

In matrix form, $w_O = \mathbf{G}_O f_{\text{all}}$ where $f_{\text{all}}$ is the stacked vector of contact forces and $\mathbf{G}_O$ is the grasp matrix. This wrench can be directly computed from whole-body motions, as it only differs from the whole-body momentum by a constant $\hat{w}^g$ due to gravity.

Next, one can linearize regular friction cones $C_i$ into polyhedral convex cones $\tilde{C}_i$, so that (4) becomes in matrix form (see e.g. [17] for details):

$$\mathbf{f}_i \in \tilde{C}_i \Leftrightarrow \mathbf{F}_i \mathbf{f}_i \le \mathbf{0} \quad (6)$$

This form is known as the *halfspace representation* of a polyhedral cone. From the Weyl-Minkowski theorem, any polyhedron thus described can be equivalently written as:

$$\tilde{C}_i = \text{conv}(\{v_i\}) + \text{rays}(\{r_j\}), \quad (7)$$

where $\text{conv}(\{v_i\}) = \{\sum_i \alpha_i v_i, \forall i \alpha_i \ge 0 \sum_i \alpha_i = 1\}$ is the convex hull of a set of vertices, and similarly $\text{rays}(\{r_j\}) = \{\sum_i \lambda_i r_i, \forall i \lambda_i \ge 0\}$ denotes positive combinations of a set of rays. This form is known as the *vertex representation* of a polyhedron.

Using suitable conversions between these two representations [16], [17], one can finally compute the Contact Wrench Cone (CWC) described in *halfspace representation* by:

$$\mathbf{A}_O w_O \le \mathbf{0} \quad (8)$$

By construction, a net contact wrench $w_O$ belongs to the CWC if and only if there exists a set of contact forces $\{\mathbf{f}_i\}$ satisfying both (3)-(5) and (6). Hence, the CWC provides a necessary and sufficient condition for the contact stability of whole-body motions.

## III. FRICTION CONES ARE DUAL TWISTS

Let us consider a row $\mathbf{a} = [\mathbf{a}_1^\top \mathbf{a}_2^\top]^\top$ of the CWC matrix $\mathbf{A}_O$. It defines an inequality constraint of the form

$$\mathbf{a}_1 \cdot \mathbf{f} + \mathbf{a}_2 \cdot \tau_O \le 0 \quad (9)$$

Applying the Varignon formula (1), the cone for the same wrench $w_G$ taken at a different point $G$ is subject to:

$$\mathbf{a}_1 \cdot \mathbf{f} + \mathbf{a}_2 \cdot (\tau_G + \overrightarrow{OG} \times \mathbf{f}) \le 0 \quad (10)$$
Using the invariance of the mixed product under circular shift, we can rewrite the left-hand side as:

$$ (a_1 + \overrightarrow{GO} \times a_2) \cdot f + a_2 \cdot \tau_G \le 0 \quad (11) $$

Let us now defined the dual twist $\hat{a} \in M^6$ by:

$$ \left[ \begin{matrix} a_O \\ a \end{matrix} \right] \stackrel{\text{def}}{=} \left[ \begin{matrix} a_1 \\ a_2 \end{matrix} \right] \quad (12) $$

Equations (9) and (11) rewrite to:

$$ a_O \cdot f + a \cdot \tau_O \le 0 \quad (13) $$

$$ a_G \cdot f + a \cdot \tau_G \le 0 \quad (14) $$

where $a_G = a_1 + \overrightarrow{GO} \times a_2$. In concise form:

$$ \hat{a} \cdot \dot{w} \le 0 \quad (15) $$

This inequality is independent from O where $A_O$ is computed. Therefore, the CWC can be interpreted as a set of dual twists, the coordinates $A_O$ of which one can compute at a fixed reference point using known techniques [16], [17].

This shift in the way of considering the cone has an important implication: using the Varignon formula, we can now calculate analytically the cone $A_G$ at a mobile point G using a fixed solution $A_O$ and the vector coordinates $p_G$. Our following contributions build upon this property.

# IV. CONTACT STABILITY AREAS AND VOLUMES

## A. Static-equilibrium COM polygon

Bretl and Lall [18] showed how static equilibrium can be sustained by feasible contact forces if and only if the (horizontal projection of the) center of mass lies inside a specific polygon, henceforth called the *static-equilibrium polygon*.

In fact, the static-equilibrium polygon is embedded in the CWC. Suppose that its matrix $A_O$ was computed at a given point O, and let $\hat{a}$ denote a twist of the CWC corresponding to the inequality (13). In static equilibrium, the whole-body momentum is zero, so that the net contact wrench $w_G$ at the center of mass G is simply opposed to gravity:

$$ w_G = \begin{bmatrix} f \\ \tau_G \end{bmatrix} = \begin{bmatrix} -mg \\ 0 \end{bmatrix} \quad (16) $$

Then, expressing (13) at G, (14) yields:

$$ a_G \cdot (-mg) + a \cdot 0 \le 0 \quad (17) $$

which also writes, since $m > 0$:

$$ -(a_O + a \times p_G) \cdot g \le 0 \quad (18) $$

Expanding this scalar product yields:

$$ a_{Oz} - a_y x_G + a_x y_G \le 0 \quad (19) $$

where $a_O = [a_{Ox} \ a_{Oy} \ a_{Oz}]^\top$ and $a = [a_x \ a_y \ a_z]^\top$. The set of inequalities (19) over all twists $\hat{a}$ of the CWC provides the half-plane representation of the static-equilibrium polygon. Note that the static-equilibrium polygon does not depend on the mass, which was not observed in previous works [18], [19], [11], [20].

In what follows, we will use the following equivalent formulation. Let us define the slackness of (19) by:

$$ \sigma_{\hat{a}}(x_G, y_G) \stackrel{\text{def}}{=} -a_{Oz} + a_y x_G - a_x y_G \quad (20) $$

it is the signed distance between $(x_G, y_G)$ and the supporting line $-a_y x + a_x y + a_{Oz} = 0$ of the corresponding static-equilibrium polygon's edge. A point $(x_G, y_G)$ is then inside the polygon if and only if $\sigma_{\hat{a}}(x_G, y_G) \ge 0$ for all the CWC twists $\hat{a}$.

## B. Vertex enumeration for polygons

The half-plane representation (19) is best-suited for COM feasibility tests. Meanwhile, the vertex representation is best-suited for planning. Converting from halfspace to vertex representation is known as the *vertex enumeration problem*, for which the *double description method* [21] has been applied in previous works [22], [16], [23], [17].

For general $d$-dimensional polyhedra, vertex enumeration has polynomial, yet super-linear time complexity. For example, the Avis-Fukuda algorithm [24] runs in $O(dhv)$, with $h$ and $v$ the numbers of hyperplanes and vertices, while the original double-description method by Motzkin has a worst-case time complexity of $O(h^2v^3)$ [21]. Yet, for $d = 2$, the problem boils into computing the *convex hull* of a set of points, for which optimal algorithms (for instance [25]) are known that match the theoretical lower-bound of $\Omega(h \log v)$.

Our formulation (19) allows us to enumerate vertices in 2D. Let us assume for now that the origin $(x_G, y_G) = (0, 0)$ lies in the interior of the polygon, and divide each inequality (19) by $a_{Oz}$ to put the overall inequality system in polar form:

$$ \mathbf{B} \begin{bmatrix} x_G \\ y_G \end{bmatrix} \le \mathbf{1} \quad (21) $$

We run a convex hull algorithm on the rows of the matrix $\mathbf{B}$. By duality, the cyclic order of extreme points thus computed corresponds to a cyclic order of adjacent edges for the primal problem. Intersecting pairs of adjacent lines in this order yields the vertices of the initial polygon. The conversion of inequalities (19) to (21) being $O(n)$, computing the output polygon is done overall in $O(h \log v)$. See the Appendix for a comparison with existing approaches.

To construct (21), we assumed that the origin lies inside the polygon. When this is not the case, one can simply compute the Chebyshev center $(x_C, y_C)$ by solving a single Linear Program (LP) as detailed e.g. in [26] p. 148. From there, a translation $(x'_G, y'_G) = (x_G - x_C, y_G - y_C)$ brings the origin inside the polygon.

## C. Pendular ZMP support areas

Let us revisit the derivation of the pendular ZMP support area [11] using our new approach. To achieve linear-pendulum mode of the Newton-Euler equations of the system, the following four equality constraints are applied to the contact wrench:

$$ n \cdot f = m(n \cdot g) \quad (22) $$

$$ \tau_G = \mathbf{0} \quad (23) $$
where $n$ denotes the unit vector normal to the plane in which the ZMP is taken. In what follows, we suppose that $n$ is opposite to gravity, so that $n \cdot g = -g$. Equation (22) is used to linearize the pendulum dynamics, the ZMP being defined in general by the non-linear formula $\mathbf{p}_Z \stackrel{\text{def}}{=} \frac{n \times \tau_O}{n \cdot f} + \mathbf{p}_O$. Under Equations (22)-(23), the resultant force can be computed from COM and ZMP positions by [11]:

$$ \mathbf{f} = \frac{mg}{h} \begin{bmatrix} x_Z - x_G \\ y_Z - y_G \\ 1 \end{bmatrix} \quad (24) $$

where $h = z_Z - z_G$ is the constant difference between ZMP and COM altitudes. This value can be positive or negative: for the sake of exposition, we will take $h > 0$. Injecting Equations (22)-(23) into an inequality constraint (14) of the CWC yields:

$$ (\mathbf{a}_O + \mathbf{a} \times \mathbf{p}_G) \cdot \mathbf{f} + \mathbf{a} \cdot \mathbf{0} \le 0 \quad (25) $$

Substituting (24) into (25) yields:

$$ a_i(x_Z - x_G) + b_i(y_Z - y_G) \le h\sigma_{\hat{a}}(\mathbf{p}_G) \quad (26) $$

where

$$ \begin{bmatrix} a_i \\ b_i \end{bmatrix} = \begin{bmatrix} a_{Ox} \\ a_{Oy} \end{bmatrix} + \begin{bmatrix} a_y z_G - a_z y_G \\ -a_x z_G + a_z x_G \end{bmatrix} \quad (27) $$

Assuming that the COM lies inside the static-equilibrium polygon,<fnref n="3" /> the right-hand side of this expression is positive from (19). The inequality is then expressed in polar form as

$$ \mathbf{B}_{\text{ZMP}}(\mathbf{p}_G) \begin{bmatrix} x_Z - x_G \\ y_Z - y_G \end{bmatrix} \le \mathbf{1} \quad (28) $$

where the origin $(x_Z, y_Z) = (x_G, y_G)$ lies inside the polygon by construction. As in (21), a convex hull algorithm can finally be applied to compute the vertices of the pendular ZMP support area.

### D. 3D Volume of COM accelerations

Equation (22) is a limitation of the linear-pendulum mode in that the COM trajectory needs to lie in a plane pre-defined by the vector $n$. This limitation is all the less grounded that, from Equation (24), controlling the ZMP in this mode is equivalent to controlling the resultant contact force $\mathbf{f}$. We therefore propose to directly control this force, or equivalently, to directly control the three-dimensional COM acceleration $\ddot{\mathbf{p}}_G = \frac{1}{m}\mathbf{f} + \mathbf{g}$.

Substituting this acceleration into (25), one gets:

$$ (\mathbf{a}_O + \mathbf{a} \times \mathbf{p}_G) \cdot \ddot{\mathbf{p}}_G \le (\mathbf{a}_O + \mathbf{a} \times \mathbf{p}_G) \cdot \mathbf{g} \quad (29) $$

Expanding scalar products, this inequality rewrites to:

$$ a_i \ddot{x}_G + b_i \ddot{y}_G - \sigma_{\hat{a}} \ddot{z}_G \le g \sigma_{\hat{a}} \quad (30) $$

(We dropped the argument $\mathbf{p}_G$ of $\sigma_{\hat{a}}$ to alleviate notations.) Assuming that the COM lies in the interior of the polygon<fnref n="3" /> ($\sigma_{\hat{a}} > 0$) and that $\ddot{z}_G > -g$,

$$ \left( \frac{a_i}{\sigma_{\hat{a}}} \right) \cdot \frac{\ddot{x}_G}{g + \ddot{z}_G} + \left( \frac{b_i}{\sigma_{\hat{a}}} \right) \cdot \frac{\ddot{y}_G}{g + \ddot{z}_G} \le 1 \quad (31) $$

<figure><img src="image_3.png" /><figcaption>Fig. 2. Static-equilibrium polygon (in green at the altitude of the center of mass) and cone of 3D COM accelerations (in red, with the zero acceleration centered on the COM) in a double-support configuration. For the latter, the scaling from accelerations to positions is 0.08 s², and the cone was cut at $\ddot{z}_G = g$ to show its cross-section.</figcaption></figure>

This expression is in polar form $\mathbf{B}_{3D}(\mathbf{p}_G)[\tilde{x} \ \tilde{y}]^\top \le \mathbf{1}$ for the new coordinates:

$$ \begin{bmatrix} \tilde{x} \\ \tilde{y} \end{bmatrix} = \frac{1}{g + \ddot{z}_G} \begin{bmatrix} \ddot{x}_G \\ \ddot{y}_G \end{bmatrix} \quad (32) $$

We can enumerate the vertices $\{(\tilde{x}_i, \tilde{y}_i)\}$ of the corresponding polygon using a convex hull again. For a given vertical acceleration $\ddot{z}_{G,i} > -g$, the COM acceleration coordinates $(\ddot{x}_{G,i}, \ddot{y}_{G,i})$ corresponding to a vertex $(\tilde{x}_i, \tilde{y}_i)$ are:

$$ \begin{bmatrix} \ddot{x}_{G,i} \\ \ddot{y}_{G,i} \end{bmatrix} = (g + \ddot{z}_{G,i}) \begin{bmatrix} \tilde{x}_i \\ \tilde{y}_i \end{bmatrix} \quad (33) $$

We recognize the equation of a 3D polyhedral convex cone, pointing upward, with apex located at $(\ddot{x}_G, \ddot{y}_G, \ddot{z}_G) = (0, 0, -g)$ and rays defined by $\mathbf{r}_i = [\tilde{x}_i \ \tilde{y}_i \ 1]^\top$. Figure 2 shows the cone in a sample contact configuration.

Overall, we have thus both (1) a geometric characterization and (2) an algorithm to compute the cone of feasible COM accelerations when the angular momentum is regulated to zero.<fnref n="4" /> This construction generalizes the pendular support area defined in [11]. Furthermore, as a consequence of Equations (26)-(30), we have the following property:

**Proposition 1:** The COM is in the interior of the static-equilibrium polygon if and only if the set of feasible COM accelerations (equivalently, whole-body ZMPs) under zero angular momentum contains a neighborhood of the origin.

In other words, the static-equilibrium polygon is not only related to static equilibrium: it is also the set of positions from which the robot can accelerate its center of mass in any direction (with zero angular momentum). When the COM reaches the edge of this polygon, the zero acceleration touches a facet of the acceleration cone. Denoting by $\mathbf{d}$ the facet normal, all feasible accelerations $\ddot{\mathbf{p}}_G$ are then such that $\mathbf{d} \cdot \ddot{\mathbf{p}}_G \ge 0$, i.e., $\mathbf{d}$ is an “irresistible” direction of motion.

<footnote n="3">³Otherwise, center the polygon on its Chebyshev center as previously.</footnote>

<footnote n="4">⁴The same derivation can be applied with non-zero angular-momentum references; however, the question of finding such references is still open.</footnote>
V. PREVIEW CONTROL OF COM ACCELERATIONS

Let the control variable $\mathbf{u}$ be the COM acceleration $\mathbf{u} := \ddot{\mathbf{p}}_G$. The discretized COM dynamics with sampling $\Delta T$ are:

$$ \mathbf{x}(k+1) = \mathbf{A}\mathbf{x}(k) + \mathbf{B}\mathbf{u}(k) \quad (34) $$

where $\mathbf{x}(k) = [\mathbf{p}_G(k \Delta T)^\top \ \dot{\mathbf{p}}_G(k \Delta T)^\top]^\top$ and, denoting by $\mathbf{E}_3$ the $3 \times 3$ identity matrix,

$$ \mathbf{A} = \begin{bmatrix} \mathbf{E}_3 & \Delta T \mathbf{E}_3 \\ \mathbf{0}_{3 \times 3} & \mathbf{E}_3 \end{bmatrix} \quad \mathbf{B} = \begin{bmatrix} \frac{1}{2} \Delta T^2 \mathbf{E}_3 \\ \Delta T \mathbf{E}_3 \end{bmatrix} \quad (35) $$

At each control step, a preview controller receives the current state $\mathbf{x}_0 = (\mathbf{p}_0, \dot{\mathbf{p}}_0)$ and computes a sequence of controls $\mathbf{u}(0), \dots, \mathbf{u}(N)$ driving the system from $\mathbf{x}_0$ to $\mathbf{x}(N)$ at the end of the time horizon $T = N \Delta T$ of the preview window. By recursively applying (34), $\mathbf{x}(k)$ can be written as a function of $\mathbf{x}_0$ and of $\mathbf{u}(0), \dots, \mathbf{u}(k-1)$ (see e.g., [5]):

$$ \mathbf{x}(k) = \mathbf{\Phi}_k \mathbf{x}_0 + \mathbf{\Psi}_k \mathbf{U}(k-1) \quad (36) $$

$$ \mathbf{U}(k) = [\mathbf{u}(0)^\top \dots \mathbf{u}(k)^\top]^\top \quad (37) $$

A necessary condition for contact-stability throughout the trajectory is that all accelerations $\mathbf{u}(k)$ lie in the COM-acceleration cone $C(\mathbf{p}_G(k \Delta T))$. Expanding its inequalities (29) for an arbitrary twist $\hat{\mathbf{a}} \in \text{CWC}$ yields:

$$ \mathbf{p}_G(k \Delta T)^\top [-\mathbf{a}] (\mathbf{u}(k) - \mathbf{g}) + \mathbf{a}_O^\top (\mathbf{u}(k) - \mathbf{g}) \le 0 \quad (38) $$

Let $L$ denote the number of twists in the CWC. By stacking up the $N$ inequalities (38), we get in more concise form:

$$ \mathbf{p}_G(k \Delta T)^\top \mathbf{A}_\times (\mathbf{u}(k) - \mathbf{g}) + \mathbf{A}_O'(\mathbf{u}(k) - \mathbf{g}) \le \mathbf{0} \quad (39) $$

where $\mathbf{A}_\times$ is a $3 \times L \times 3$ tensor, and $\mathbf{A}_O'$ consists of the first three columns of $\mathbf{A}_O$. Combining (36) and (39), this condition yields a set of quadratic inequality constraints of the form:

$$ \forall k < N, \quad \mathbf{U}(k)^\top \mathbb{P}_k \mathbf{U}(k) + \mathbf{Q}_k \mathbf{U}(k) + \mathbf{l}_k \le \mathbf{0} \quad (40) $$

where $\mathbb{P}_k$ is a $3(k+1) \times L \times 3(k+1)$ tensor, $\mathbf{Q}_k$ is a $L \times 3(k+1)$ matrix $\mathbf{l}_k$ is an $L$-dimensional vector. One can thus formulate the preview control problem as a Quadratically Constrained Quadratic Program (QCQP). Although a QCQP formulation was successfully applied for walking on even terrains with variable COM height [10], we chose not to do so for the following reasons:

* QCQP is a harder class of problems than Quadratic Programming (QP), especially when inequality constraints are not positive-semidefinite [10] so that the problem non-convex. Real-timeness implies that only a small number of SQP iterations can be run in the control loop (two in [10]), thus with no convergence guarantee.

* Constraints (40) are given without any redundancy elimination. In practice, eliminating redundancy (in our case, by applying a convex hull algorithm) significantly reduces the number of inequality constraints (Table I).

Instead, we propose a trajectory-wide contact-stability criterion that yields linear inequality constraints, and for which we can apply the convex-hull reduction from Section IV.

A. Robust trajectory-wide contact-stability criterion

We want to drive the COM from $\mathbf{x}_0 = (\mathbf{p}_0, \dot{\mathbf{p}}_0)$, its current state, to a goal position $\mathbf{x}_T$ through a trajectory $t \in [0, T] \mapsto \mathbf{p}_G(t)$. Due to the initial velocity $\dot{\mathbf{p}}_G(0)$ and real-world uncertainties, the trajectory will not be exactly a line segment $[\mathbf{p}_0, \mathbf{p}_T]$. Yet, we assume that it lies within a polyhedral “tube” $\mathcal{T} = \text{conv}(\{\boldsymbol{\nu}_1, \dots, \boldsymbol{\nu}_q\})$ containing $[\mathbf{p}_0, \mathbf{p}_T]$. A point $\mathbf{p}_G \in \mathcal{T}$ can be written as a convex combination $\mathbf{p}_G = \sum_{i=1}^q \alpha_i \boldsymbol{\nu}_i$, where the $\alpha_i$'s are positive and sum-up to one, so that the inequality (39) becomes:

$$ \sum_{i=1}^{q} \alpha_i (\boldsymbol{\nu}_i^\top \mathbf{A}_\times + \mathbf{A}_O') (\ddot{\mathbf{p}}_G - \mathbf{g}) \le \mathbf{0} \quad (41) $$

A particular way to enforce the negativity of a convex combination is to ensure that all of its terms are negative. Thus, a sufficient condition for the satisfaction of (41) is

$$ \mathbf{C}_T(\ddot{\mathbf{p}}_G - \mathbf{g}) \le \mathbf{0}, \quad \mathbf{C}_T := \begin{bmatrix} \boldsymbol{\nu}_1^\top \mathbf{A}_\times + \mathbf{A}_O' \\ \vdots \\ \boldsymbol{\nu}_q^\top \mathbf{A}_\times + \mathbf{A}_O' \end{bmatrix} \quad (42) $$

which is the halfspace representation of the intersection

$$ I_T := C(\boldsymbol{\nu}_1) \cap \dots \cap C(\boldsymbol{\nu}_q) \subseteq C(\mathbf{p}_G) \quad (43) $$

**Proposition 2:** $I_T$ is the set of COM accelerations that are feasible everywhere in $\mathcal{T}$, i.e.,

$$ I_T = \bigcap_{\mathbf{p} \in \mathcal{T}} C(\mathbf{p}) \quad (44) $$

*Proof:* We showed that $\ddot{\mathbf{p}}_G \in I_T$ is feasible at any position $\mathbf{p}_G \in \mathcal{T}$ (by construction, (42) ⇒ (41) ⇒ (38)). Conversely, suppose that $\ddot{\mathbf{p}}_G$ is feasible for all $\mathbf{p}_G \in \mathcal{T} = \text{conv}(\{\boldsymbol{\nu}_1, \dots, \boldsymbol{\nu}_q\})$. In particular, (41) holds when $\mathbf{p}_G$ is equal to any vertex $\boldsymbol{\nu}_j$. Thus, $\ddot{\mathbf{p}}_G \in C(\boldsymbol{\nu}_j)$, and since the index $j$ can be chosen arbitrarily, $\ddot{\mathbf{p}}_G \in \cap_j C(\boldsymbol{\nu}_j) = I_T$. ■

A trajectory $\mathbf{p}_G(t)$ is contact-stable within $\mathcal{T}$ if it satisfies:

$$ \forall t \in [0, T], \quad \mathbf{p}_G(t) \in \mathcal{T} \quad \text{and} \quad \ddot{\mathbf{p}}_G(t) \in I_T \quad (45) $$

This condition can be compared with the previous trajectory-wide stability criterion from [17], where trajectories were computed such that

$$ \forall t \in [0, T], \quad \ddot{\mathbf{p}}_G(t) \in C(\mathbf{p}_G(t)) \quad (46) $$

The implicit assumption behind such a strategy is that either trajectory-tracking is perfect, or small deviations $\delta \mathbf{p}_G$ from the reference $\mathbf{p}_G(t)$ can be coped with if $\ddot{\mathbf{p}}_G$ lies “inside enough” of its cone. By taking $\mathcal{T}$ around the trajectory, we explicitly model how far $\ddot{\mathbf{p}}_G$ needs to be inside the cone to cope with a set of deviations $\delta \mathbf{p}_G$. In this sense, our criterion is a robust condition for trajectory-wide contact stability, with explicit modelling of the robustness margin.

Although the tube-wise intersection formalized by Equation (44) seems like a severely restrictive condition, we noticed (to our surprise) that the acceleration cones left after intersection are still sufficient for locomotion in challenging scenarios (see e.g., Section VI).
<caption>TABLE I</caption>

EFFECT OF THE DUAL CONVEX-HULL REDUCTION ON THE SIZE OF THE INEQUALITY-CONSTRAINT MATRIX $C_T$. AVERAGES ARE GIVEN ON 26 MATRICES (ONE FOR EACH STEP OF THE CIRCULAR STAIRCASE).

<table><thead><tr><th></th><th>$|C_T|$</th><th>$|C'_T|$</th></tr></thead><tbody><tr><td>Single support</td><td>376 ± 226</td><td>6 ± 2</td></tr><tr><td>Double support</td><td>484 ± 221</td><td>6 ± 2</td></tr></tbody></table>

### B. Integration with preview control

At each new control step, the current state $x_0 = (p_0, \dot{p}_0)$ of the center of mass feeds the preview controller. We define the target state at the end of the time horizon by $x_T = (p_T, v_T)$, where $p_T$ is determined from the current stance in the step sequence, $v_T$ is a reference velocity of 0.4 m.s$^{-1}$ oriented in the direction of motion, and $T$ is calculated from the time remaining in the current gait phase. From there, our method goes as follows:

1) Define $\mathcal{T}$ containing $[p_0, p_T]$ and compute its half-space representation $P_T$ using the double-description. In practice, we defined $\mathcal{T}$ by a polyhedral cylinder centered on $[p_0, p_T]$ with square cross-sections and a robustness radius of 5 cm.

2) Compute the halfspace representation $C_T$ of $I_T$ from the contact wrench cone $A_O$ (which is computed only once per support phase).

3) Reduce $C_T$ to polar form $B_T[x, y]^T \le 1$ and compute its vertex representation $g + \text{rays}(\{r_i\})$ using a convex hull algorithm<fnref n="5" /> (Section IV-D).

4) Compute its non-redundant halfspace-representation $C'_T$ using again the double-description.

Next, we formulate the preview control problem as a quadratic program:

$$ \min_{U} \quad \|x(N) - x_T\|^2 + \epsilon \|U\|^2 \quad (47) $$

$$ \text{s.t.} \quad \forall k, C'_{T}u(k) \le C'_{T}g \quad (48) $$

$$ \forall k, P_T x(k) \le 1 \quad (49) $$

where $U \stackrel{\text{def}}{=} U(N-1)$ is the stacked vector of controls from which all $x(k)$ and $u(k)$ derive by Equation (36). The inequality constraints (48) and (49) are a linear decoupling of (40) via polyhedral bounds $\mathcal{T}$.

As a matter of fact, using polyhedral bounds can be thought of as a general linearization technique. In [8], a linear decoupling was also obtained by bounding vertical COM accelerations. Similarly, polytopes of robust COM positions $p_G$ were obtained in [17], [19] by defining polyhedral bounds on disturbances $\epsilon$, thus eliminating the bilinear coupling between $p_G$ and $\epsilon$.

Eliminating redundancy in the pipeline above is a significant computational step. From one of our experiments (Table I), the number of lines $|C'_T|$ in $C'_T$ is two orders of magnitude smaller than that of $C_T$. Given that the number of inequalities (48) in the above QP is $N|C'_T|$, this makes

the difference in practice between solving a problem of size 100 versus 10,000 (we use $N = 10$ steps).

<caption>TABLE II</caption>

BREAKDOWN OF COMPUTATION TIMES INSIDE THE PREDICTIVE CONTROLLER, AVERAGED OVER 2000 CALLS IN EXPERIMENT VI.B.

<table><thead><tr><th>Function</th><th>Output</th><th>Time (ms)</th></tr></thead><tbody><tr><td>Double description of T (SS and DS)</td><td>P<sub>T</sub></td><td>0.3 ± 0.1</td></tr><tr><td>H-representation of I<sub>T</sub> (SS and DS)</td><td>C<sub>T</sub></td><td>0.2 ± 0.1</td></tr><tr><td>Convex hull of I<sub>T</sub> (SS and DS)</td><td>C'<sub>T</sub></td><td>2.4 ± 1.2</td></tr><tr><td>Other matrix operations</td><td>-</td><td>0.4 ± 0.4</td></tr><tr><td>Solving final QP (N = 10)</td><td>U</td><td>0.2 ± 0.1</td></tr></tbody><tfoot><tr><td>Total</td><td></td><td>3.5 ± 2.1</td></tr></tfoot></table>

### C. Locomotion state machine

Our preview controller is applied to the HRP-4 humanoid in various environments. The inputs to the controller are the current COM position $p_0$ and velocity $\dot{p}_0$, the preview time horizon $T$, a target COM position $p_T$, $\mathcal{T}$ and its acceleration cone $I_T$. These computation of these inputs is supervised by a Finite State Machine (FSM) that cycles between four phases $\varphi \in \{\text{SS-L, DS-R, SS-R, DS-L}\}$, where SS (resp. DS) stands for single-support (resp. double-support), while L and R indicate that the phase ends on the left and right foot, respectively. Each phase is thus associated with a unique foot contact. The target COM position $p_G^*(\varphi)$ of a phase is then taken 0.8 m above this contact.

Phase durations are set to $T_{SS} = 1$ s for single-support and $T_{DS} = 0.5$ s for double-support. At each iteration of the control loop, the input to the preview controller is decided based on the time $T_{rem}$ remaining until the next phase transition. Let us denote by $\varphi$ the current phase in the FSM and $\varphi'$ the phase after $\varphi$. We define preview targets by:

1) if $\varphi$ is single-support and $T_{rem} < \frac{1}{2}T_{SS}$:

• $T \leftarrow T_{rem} + T_{DS} + \frac{1}{2}T_{SS}$

• $p_G \leftarrow p_G^*(\varphi'')$

2) otherwise, if $\varphi$ is double-support:

• $T \leftarrow T_{rem} + \frac{1}{2}T_{SS}$

• $p_G \leftarrow p_G^*(\varphi)$

3) otherwise ($\varphi$ is single-support and $T_{rem} > \frac{1}{2}T_{SS}$):

• $T \leftarrow T_{rem}$

• $p_G \leftarrow p_G^*(\varphi)$

Case 1) switches the target of the preview controller to the next staircase step in the middle of single-support phases<fnref n="6" />, which forces the robot to start its next step while allowing it to re-use the kinetic momentum in the direction of motion.

Note that cases 1) and 2) imply contact switches in the middle of preview trajectories, which different cones $C'_T$ depending on the step $k$ in the preview problem. To take this into account, we compute the switching step $k_{rem} = T_{rem}/\Delta T$ along with two tubes $\mathcal{T}_{SS} \subset \mathcal{T}_{DS}$ and their dual cones (computations between these two overlapping tubes can be factored; see [1] for details). The corresponding

<footnote n="6">⁶The same behavior is present in [14]: if the control from Figure 5 of this paper was followed to the end, the COM velocity would go to zero between each step, which is not the behavior observed in Figures 7 and 8.</footnote>

<footnote n="5">⁵We used Qhull [27], available from http://www.qhull.org/</footnote>
<figure><img src="image_4.png" /><figcaption>Fig. 3. Overview of our control pipeline. The COM trajectory polyhedron $\mathbf{P}_{\mathcal{T}}$ and its corresponding contact-stability cone $\mathbf{C}_{\mathcal{T}}$ are computed using the method described in Section V.</figcaption></figure>

matrices ($\mathbf{C}'_{\mathcal{T}_{SS}}$, $\mathbf{P}_{\mathcal{T}_{SS}}$) and ($\mathbf{C}'_{\mathcal{T}_{DS}}$, $\mathbf{P}_{\mathcal{T}_{DS}}$) are then respectively used in Equations (48)-(49) for $k \le k_{\text{rem}}$ and $k > k_{\text{rem}}$.

A breakdown of computation times inside the overall predictive controller is reported in Table II. All computations were run on an average laptop computer (Intel(R) Core(TM) i7-6500U CPU @ 2.50 Ghz).

### D. Whole-body controller

The last step of the pipeline is to convert task objectives, such as COM or foot positions, into joint commands sent to motor controllers. For this, we used our own solver implemented in the *pymanoid* library.<fnref n="7" /> It solves a single quadratic program on five weighted tasks (see [11] for details), by decreasing priority: support foot, swing foot, COM tracking, constant angular-momentum, and posture tracking for regularization. The corresponding task weights were respectively set to ($10^4, 10^2, 10, 1, 10^{-1}$). Each QP solution provides joint-angle velocities $\dot{q}_{\text{ref}}$, which is then sent to the robot. See Figure 3 for a summary of our pipeline.

## VI. EXPERIMENTS

The ground truth for contact stability is the existence of feasible contact forces $f_{\text{all}}$ summing up to the net wrench $\hat{w}$ of the motion. In all experiments, we validate our trajectories by checking the existence of such $f_{\text{all}}$ at each time instant. In both experiments, friction coefficients were set to $\mu = 0.7$.

### A. Regular staircase and walking into an aircraft

Our first scenario, provided by Airbus Group, takes place in a 3D realistic model of scale 1:1 for a section of the A350 airplane. This mock-up will be used for experiments with the real robot when the research matures to an integrated software, and gets approval from Airbus Group. In order to access a predefined spot in the A350, the robot needs to climb stairs (size of those available in the factory), walk on a platform (flat ground) to finally reach the mockup floor composed of removable tiles that can be uneven and disposed in various locations, see Figure 4. In this simulation, the footprints were given together with the timing for the DS and SS phases (respectively 0.5 s and 1 s).

### B. Slanted circular staircase with tilted steps

The slanted circular staircase depicted in Figure 1 has 26 steps randomly rolled, pitched and yawed by angles $(\theta_r, \theta_p, \theta_y) \in [-0.5, +0.5]^3$ rad. The average radius of the

<figure><img src="image_5.png" /><figcaption>Fig. 4. HRP-4 accessing the floor-shop of an A350 through stairs and reach the working areas by walking on tiles. Foot trajectories (dashed lines) step over two staircase steps at a time, as done in natural stair climbing. The COM trajectory (blue line) illustrates the progression of the robot until its target configuration inside the aircraft. Contact stability of the whole motion was cross-validated by checking the existence of groundtruth contact forces $f_{\text{all}}$ at each timestep, as shown in the accompanying video [1].</figcaption></figure>

staircase is 1.4 m, and the altitude difference between the highest and lower steps is also 1.4 m.

For this scenario, the reference durations of single and double support phases were set to $T_{\text{SS}} = 1$ s and $T_{\text{DS}} = 0.5$ s, respectively. We concur with [5] that the question of finding proper timings becomes crucial in multi-contact. Having constant durations overlooks the fact that some steps are harder to take than others (due to their respective tilting, altitude difference, etc.).<fnref n="8" /> Being unable to find a single pair of constants ($T_{\text{SS}}$, $T_{\text{DS}}$) suited to the whole staircase, we opted for the following workaround condition:

w) At the end of a double-support phase, wait for the COM to be above the static-equilibrium polygon of the next single-support before activating the phase transition.

This choice is motivated by the link (30) between the 3D cone of COM accelerations and the position of the COM in the static-equilibrium polygon. In practice, it allows the use of “optimistic” values of ($T_{\text{SS}}$, $T_{\text{DS}}$) while only extending $T_{\text{DS}}$ when necessary.

## VII. CONCLUSION

We presented a multi-contact walking pattern generator based on preview-control of the 3D acceleration of the center of mass. Our development builds upon algebraic manipulations of friction cones as dual twists, thanks to which we can recompute 3D cones of feasible COM accelerations in real-time. We then showed how to intersect these cones over the preview window of a model-preview controller to construct a conservative trajectory-wide contact-stability criterion. We implemented this pipeline and illustrated it with the HRP-4 humanoid model in multi-contact dynamically walking scenarios. All our source code is released at [1].

<footnote n="8">⁸This question is less critical for walking on horizontal or well structured floors, where all steps are similar.</footnote>

<footnote n="7">⁷https://github.com/stephane-caron/pymanoid</footnote>
## ACKNOWLEDGMENT

The authors would like to warmly thank Komei Fukuda for his helpful feedback and Patrick Wensing for pointing out a calculation mistake in a preliminary version of the paper. This paper has also benefitted from rich discussions with Hervé Audren and Adrien Escande.

## REFERENCES

[1] [Online]. Available: https://github.com/stephane-caron/3d-mpc

[2] J. Englberger, C. Ott, and A. Albu-Schaffer, "Three-dimensional bipedal walking control based on divergent component of motion," *IEEE Transactions on Robotics*, vol. 31, no. 2, pp. 355-368, 2015.

[3] Y. Zhao, B. Fernandez, and L. Sentis, "Robust phase-space planning for agile legged locomotion over various terrain topologies," in *Robotics: Science and System*, 2016.

[4] H. Hirukawa, S. Hattori, S. Kajita, K. Harada, K. Kaneko, F. Kanehiro, M. Morisawa, and S. Nakaoka, "A pattern generator of humanoid robots walking on a rough terrain," in *IEEE International Conference on Robotics and Automation*. IEEE, 2007, pp. 2181-2187.

[5] H. Audren, J. Vaillant, A. Kheddar, A. Escande, K. Kaneko, and E. Yoshida, "Model preview control in multi-contact motion-application to a humanoid robot," in *IEEE/RSJ International Conference on Intelligent Robots and Systems*. IEEE, 2014, pp. 4030-4035.

[6] A. Herzog, N. Rotella, S. Schaal, and L. Righetti, "Trajectory generation for multi-contact momentum control," in *Humanoid Robots (Humanoids), 2015 IEEE-RAS 15th International Conference on*. IEEE, 2015, pp. 874-880.

[7] J. Carpentier, S. Tonneau, M. Naveau, O. Stasse, and N. Mansard, "A versatile and efficient pattern generator for generalized legged locomotion," in *2016 IEEE International Conference on Robotics and Automation (ICRA)*, May 2016, pp. 3555-3561.

[8] C. Brasseur, A. Sherikov, C. Collette, D. Dimitrov, and P.-B. Wieber, "A robust linear mpc approach to online generation of 3d biped walking motion," in *Humanoid Robots (Humanoids), 2015 IEEE-RAS 15th International Conference on*. IEEE, 2015, pp. 595-601.

[9] M. Naveau, M. Kudruss, O. Stasse, C. Kirches, K. Mombaur, and P. Souères, "A reactive walking pattern generator based on nonlinear model predictive control," *IEEE Robotics and Automation Letters*, vol. 2, no. 1, pp. 10-17, 2017.

[10] K. Van Heerden, "Real-time variable center of mass height trajectory planning for humanoids robots," *IEEE Robotics and Automation Letters*, vol. 2, no. 1, pp. 135-142, 2017.

[11] S. Caron, Q.-C. Pham, and Y. Nakamura, "ZMP support areas for multi-contact mobility under frictional constraints," *IEEE Transactions on Robotics*, to appear.

[12] R. Featherstone, *Rigid body dynamics algorithms*. Springer, 2014.

[13] S. Caron, Q.-C. Pham, and Y. Nakamura, "Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas," in *IEEE International Conference on Robotics and Automation*. IEEE, 2015.

[14] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, "Biped walking pattern generation by using preview control of zero-moment point," in *IEEE International Conference on Robotics and Automation*, vol. 2. IEEE, 2003, pp. 1620-1626.

[15] I. Mordatch, M. De Lasa, and A. Hertzmann, "Robust physics-based locomotion using low-dimensional planning," *ACM Transactions on Graphics (SIGGRAPH)*, vol. 29, no. 4, p. 71, July 2010.

[16] Z. Qiu, A. Escande, A. Micaelli, and T. Robert, "Human motions analysis and simulation based on a general criterion of stability," in *International Symposium on Digital Human Modeling*, 2011.

[17] S. Caron, Q.-C. Pham, and Y. Nakamura, "Leveraging cone double description for multi-contact stability of humanoids with applications to statics and dynamics," in *Robotics: Science and System*, 2015.

[18] T. Bretl and S. Lall, "Testing static equilibrium for legged robots," *IEEE Transactions on Robotics*, vol. 24, no. 4, pp. 794-807, 2008.

[19] A. Del Prete, S. Tonneau, and N. Mansard, "Fast algorithms to test robust static equilibrium for legged robots," in *IEEE International Conference on Robotics and Automation*, Stockholm, Sweden, May 2016.

[20] T. Zhang, S. Caron, and Y. Nakamura, "Humanoid stair climbing based on dedicated plane segment estimation and multi-contact motion generation," to appear in: *International Journal of Humanoid Robotics*, 2016.

[21] K. Fukuda and A. Prodon, "Double description method revisited," in *Combinatorics and computer science*. Springer, 1996, pp. 91-111.

[22] K. Bouyarmane, A. Escande, F. Lamiraux, and A. Kheddar, "Potential field guide for humanoid multicontacts acyclic motion planning," in *IEEE International Conference on Robotics and Automation*. IEEE, 2009, pp. 1165-1170.

[23] A. Escande, A. Kheddar, and S. Miossec, "Planning contact points for humanoid robots," *Robotics and Autonomous Systems*, vol. 61, no. 5, pp. 428-442, 2013.

[24] D. Avis and K. Fukuda, "A pivoting algorithm for convex hulls and vertex enumeration of arrangements and polyhedra," *Discrete & Computational Geometry*, vol. 8, no. 3, pp. 295-313, 1992.

[25] D. G. Kirkpatrick and R. Seidel, "The ultimate planar convex hull algorithm?" *SIAM Journal on Computing*, vol. 15, no. 1, pp. 287-299, 1986.

[26] S. Boyd and L. Vandenberghe, *Convex Optimization*, 2004.

[27] C. B. Barber, D. P. Dobkin, and H. Huhdanpaa, "The quickhull algorithm for convex hulls," *ACM Transactions on Mathematical Software*, vol. 22, no. 4, pp. 469-483, 1996.

[28] Q.-C. Pham and O. Stasse, "Time-optimal path parameterization for redundantly-actuated robots: A numerical integration approach," *IEEE/ASME Transactions on Mechatronics*, 2015.

## APPENDIX

We compare the convex hull reduction to the original calculation [18] of the static-equilibrium polygon. Computation times for randomly sampled contact configurations are reported in Table III for four algorithms:

* cdd + hull: the method described in Section IV-B, where cdd [21] is used to compute the CWC while convex hulls are computed with *Qhull* [27].

* Parma + hull: same approach, using the Parma Polyhedra Library<fnref n="9" /> rather than cdd to compute the CWC.

* cdd only: as described in [20], cdd can also be used to compute the static-equilibrium polygon directly.

* Bretl & Lall: the algorithm from [18], in the implementation from [28] but using GLPK as LP solver.<fnref n="10" />

The Parma + hull solution is the slowest but most numerically stable, while cdd only is only competitive in single-support. Neck to neck are Bretl & Lall and cdd + hull, with the latter faster in single- and double-support. But the real benefit of our approach comes with the computation of time-varying criteria: in double- and triple-support, we see that executing hull only is more than ten times faster than applying any other algorithm from scratch. We also highlight it as the fastest solution for single-support, as in this case the CWC is known analytically [13] and there is no need for the cdd step.

<table><caption>TABLE III
TIME (IN MS) TO COMPUTE THE STATIC-EQUILIBRIUM POLYGON, AVERAGED OVER 100 RANDOM CONTACT CONFIGURATIONS.</caption><thead><tr><th>Algorithm</th><th>Single support</th><th>Double support</th><th>Triple support</th></tr></thead><tbody><tr><td>Parma + hull</td><td>6.02 ± 0.20</td><td>21.0 ± 4.2</td><td>42 ± 11</td></tr><tr><td>cdd only</td><td>0.38 ± 0.01</td><td>7.0 ± 2.7</td><td>> 500</td></tr><tr><td>Brel & Lall [18]</td><td>1.00 ± 0.02</td><td>3.1 ± 0.8</td><td><b>5.9 ± 1.6</b></td></tr><tr><td>cdd + hull</td><td>0.60 ± 0.01</td><td><b>2.7 ± 0.6</b></td><td>7.1 ± 1.9</td></tr><tr><td>hull only</td><td><b>0.17 ± 0.003</b></td><td>0.28 ± 0.04</td><td>0.38 ± 0.09</td></tr></tbody></table>

<footnote n="9">⁹https://github.com/haudren/pyparma</footnote>

<footnote n="10">¹⁰Using an efficient LP solver is crucial here: in a preliminary version of this paper, we used the more general CVXOPT, which resulted in computations around 10× slower than those we now report using GLPK.</footnote>