ZMP support areas for multi-contact mobility
under frictional constraints

Let $m$ and $G$ represent the total mass and center-of-mass (COM) of the robot, respectively. We write ${\bm{p}}_{A}$ the vector of absolute coordinates of a point $A$ and denote by $O$ the origin of the absolute frame (so that ${\bm{p}}_{O}=\bm{0}$). For a link $k$, define:

• •

  $m_{k}$ the total mass of the link;

• •

  ${\bm{p}}_{G_{k}}$ the vector of absolute coordinates of its COM $G_{k}$;

• •

  $\mathbf{R}_{k}$ its orientation matrix in the absolute frame;

• •

  $\bm{\omega}_{k}$ its angular velocity in the link frame;

• •

  $\mathbf{I}_{k}$ its inertia matrix in the link frame.

The linear momentum $\mathbf{P}$ and angular momentum $\mathbf{L}_{G}$ of the robot, taken at the COM $G$, are defined by:

The fundamental principle of dynamics states that the rate of change of the momentum is equal to the total wrench of forces acting on the system, that is:

where ${\bm{f}}^{g}$ denotes the gravity force, $C_{i}$ the $i^{\textrm{th}}$ contact point and ${\bm{f}}_{i}$ the contact force exerted by the environment on the robot at $C_{i}$. Equation (3) is called the Newton-Euler equations of the system, sometimes also referred to as “dynamic balance” or the “dynamic equilibrium”. It can be equivalently derived from Gauss’s principle of least constraint, and corresponds to the six unactuated components in the equations of motion of the system $\{$robot $+$ environment$\}$ [32].

Define the gravito-inertial wrench, taken this time at $O$:

Define similarly the contact wrench:

The Newton-Euler equation (3) can be written in terms of these two wrenches as:

This formulation separates spatial accelerations (${\bm{w}}^{gi}$), that are usually measured by inertial measurement units, from interaction forces with the environment (${\bm{w}}^{c}$), usually measured by force sensors. Since the two wrenches are simply opposites, we will only use the contact wrench $({\bm{f}},\bm{\tau}_{O})$ in the following calculations, dropping the superscript $c$ to alleviate notations.

We assume that all contacts between the environment and the robot are surface contacts, i.e. contacts between a flat surface of the robot and a flat surface of the environment. Wrenches exerted on each contacting link are then fully described by applying contact forces at the vertices of the contact polygon [33], which warrants the formulation by contact points that we have followed so far. Define the contact normal ${\bm{n}}_{i}$ at $C_{i}$ as the normal to the contact surface pointing from the environment towards the contacting link. Under Coulomb’s model of dry friction, contact forces ${\bm{f}}_{i}$ lie inside a friction cone directed by ${\bm{n}}_{i}$:

Frictional constraints restrict the range of contact wrenches ${\bm{w}}^{c}$ that the robot can generate without breaking any contact: when each contact force lies in a cone ${\cal C}_{i}$, the contact wrench lies in the Contact Wrench Cone (CWC) ${\cal C}^{c}$ projected from all ${\cal C}_{i}$’s via the mapping (5). Because of the connection (6), the gravito-inertial wrench belongs to Gravito-inertial Wrench Cone (GIWC) ${\cal C}^{gi}=-{\cal C}^{c}$. This link between feasible motions and keeping contacts is embodied by the notion of contact stability:

Weak contact stability is the underlying stability criterion used in recent multi-contact developments [14, 15, 16, 17]. Static stability [36] corresponds to contact stability when the whole-body momentum is zero. Note that the term “stability” is used here in the sense defined by Pang and Trinkle [34]. It should not be confused with the (a-priori unrelated) notion of Lyapunov stability.

We linearize the quadratic constraint (7) by approximating friction cones by friction pyramids . Denoting by ${\bm{f}}_{ij}$ the ray vectors of the latter, the constraint becomes:

The set of ray vectors $\{{\bm{f}}_{ij}\}$ is known as the span representation of the pyramidal cone. It can be computed directly from the contact frame and friction coefficient $\mu_{i}$. For example, the expression of a four-sided pyramid is $\left\{{\bm{n}}_{i}\pm\frac{\mu_{i}}{\sqrt{2}}\,{\bm{t}}_{i}\pm\frac{\mu_{i}}{\sqrt{2}}\,{\bm{b}}_{i}\right\}$, with $({\bm{t}}_{i},{\bm{b}}_{i},{\bm{n}}_{i})$ an orthonormal contact frame, where the normalization by $\sqrt{2}$ is added to make the friction pyramid an inner approximation of the friction cone. Injecting (8) into Equation (5) yields a span representation for the contact wrench cone:

Let us define $\bm{\tau}_{O,ij}={\bm{p}}_{C_{i}}\times{\bm{u}}_{ij}$. After re-indexing the couples $i,j$ into a single index $i$ (counting the same contact point $C_{i}$ multiple times accordingly), we get:

We have thus obtained the span representation of the CWC. A motion is weak-contact stable if and only if its contact wrench can be written as (10) for a certain set of coefficients $\lambda_{i}\geq 0$.

Equation (13) presents the ZMP as a two-dimensional projection of the contact wrench $({\bm{f}},\bm{\tau}_{O})$. Since contact stability is characterized by the CWC, we define the support area of the ZMP as the image of the CWC by this projection:

The key idea to calculate this area is to use the span representation (10) of the CWC, which enables rewriting Equation (13) as

Next, define the points $Z_{i}$ by:

and denote by $p_{i}\stackrel{{\scriptstyle\mathrm{def}}}{{=}}({\bm{n}}\cdot{\bm{f}}_{i})$ the virtual pressure of the contact force generator ${\bm{f}}_{i}$ through the virtual plane. Then,

On horizontal floors, ${\bm{n}}$ and all contact forces ${\bm{f}}_{i}$ point upwards, so that all pressures $p_{i}$ are positive. This makes the ZMP $Z$ a convex combination of the $Z_{i}$’s from Equation (17). Furthermore, Equation (16) simplifies to $Z_{i}=C_{i}$, i.e. , the vertices of the support area ${\cal S}$ coincide with contact points. Our definition of the full support area therefore coincides with the conventional support area on horizontal floors.

In general, however, virtual pressures $p_{i}$ can be either positive or negative.²²2We assume ${\bm{n}}$ is chosen so that none of them is zero, which is easy to do since there is only a finite set of generators $\{{\bm{f}}_{i}\}$. Let us then partition the set of generator indices $I$ into $I^{+}\stackrel{{\scriptstyle\mathrm{def}}}{{=}}\{i\,|\,p_{i}>0\}$ and $I^{-}\stackrel{{\scriptstyle\mathrm{def}}}{{=}}\{i\,|\,p_{i}<0\}$. For any $K\subset I$, denote by $\sigma(K)\ \stackrel{{\scriptstyle\mathrm{def}}}{{=}}\ \sum_{i\in K}\lambda_{i}|p_{i}|$ and define

Equation (15) becomes

Define the positive-pressure polygon as the convex hull of $Z_{i}$’s for $i\in I^{+}$: ${\cal P}^{+}\stackrel{{\scriptstyle\mathrm{def}}}{{=}}\{\sum_{i\in I^{+}}{\alpha}_{i}{\bm{p}}_{Z_{i}},\alpha_{i}\geq 0,\sum_{i}{\alpha}_{i}=1\}$, and define the negative-pressure polygon ${\cal P}^{-}$ similarly. Equation (20) can be rewritten as

where $\alpha\geq 0$, $\beta\geq 0$, $Z^{+}\in{\cal P}^{+}$ and $Z^{-}\in{\cal P}^{-}$. Next, let

denote the Minkowski difference of ${\cal P}^{+}$ and ${\cal P}^{-}$. It is a polygon as difference of two polygons, so that it admits a span representation ${\cal P}^{+}-{\cal P}^{-}=\textsc{conv}(\{{\bm{r}}_{1},\ldots,{\bm{r}}_{k}\})$ as convex hull (conv) of a set of vertices. We can now characterize the support area as follows:

The geometric construction given by Proposition 1 provides fast computations of the full support area. In particular, it is faster to use this approach than to project the CWC computed by polyhedral duality methods [17]. (A comparison of computation times for both approaches is reported in Table I.)

Proposition 1 gives the span representation of the full support area. By construction, this area depends only on contact locations $\{{\bm{p}}_{C_{i}}\}$ and on the choice of the virtual plane $\Pi(O,{\bm{n}})$. The latter involves the choice of a fixed point $O$, but we will now see that the location of this point only matters as far as it defines the virtual plane $\Pi(O,{\bm{n}})$ of the ZMP.

By contrast, the support area does change for displacements of $O$ along the plane normal ${\bm{n}}$. Let us analyze the impact of this remaining coordinate by relaxing the assumption that $({\bm{n}}\cdot{\bm{p}}_{Z})=0$. We now denote by $d_{Z}$ the coordinate of the virtual plane, so that $d_{Z}\stackrel{{\scriptstyle\mathrm{def}}}{{=}}({\bm{n}}\cdot{\bm{p}}_{Z})$ and $d_{G}\stackrel{{\scriptstyle\mathrm{def}}}{{=}}({\bm{n}}\cdot{\bm{p}}_{G})$. On a horizontal floor, $d_{Z}$ and $d_{G}$ correspond to the altitude of the ZMP and COM, respectively. Given Proposition 2, we can write the virtual plane $\Pi(d_{Z},{\bm{n}})$. The definition ${\bm{n}}\times\bm{\tau}_{Z}=\bm{0}$ of the ZMP yields:

And repeating the step from Equation (16),

We see from this equation that the vertices $Z_{i}$ are located at the intersection between the plane $\Pi(d_{Z},{\bm{n}})$ and the ray $(C_{i},{\bm{f}}_{i})$ of the linearized friction cone, which establishes that:

Combining Propositions 1 and 3, we get:

This result is coherent with the horizontal-floor setting where the virtual plane intersects friction cones at their apexes (i.e. at contact points) and virtual pressures are all positive from contact unilaterality. A condition similar to the positivity of virtual pressures was also observed by Saida et al. (Proposition 4.1 in [13]) for some plane components of the contact wrench.

Figure 2 illustrates the geometric construction in the polygonal case. The support area (in green) is the convex hull of black points projected from friction rays (red lines). Alternatively, each individual contact surface projects its own support polygon (blue polygon in Figure 2), and the ZMP support area is the convex hull of these individual polygons. This second construction can be useful for contact planning, where the quality of contact candidates can be assessed by the expansion that they bring to the support area.

Figure 3 shows a configuration where the support area is the union of two polygonal cones. In this setting, the robot has its two feet on a horizontal floor, and pushes its hand on a wall in front of it. The positive cone ${\cal C}^{+}$ illustrates how the wall contact expands the conventional support area between the two feet, while the negative cone ${\cal C}^{-}$ reveals that a second support area appears behind the two floor contacts. This new area results from the ability for the robot to generate downward forces at the wall contact.

Finally, let us determine in which cases a polygonal ZMP support area can be found, which boils down to finding a suitable vector ${\bm{n}}$.

Figure 4 provides an example where contacts are in force closure. As they can generate arbitrary contact wrenches, the support area spans the whole virtual plane. Note that being able to generate arbitrary forces is a weaker condition than force closure, which usually assumes that contact forces can generate arbitrary resultant forces and momenta.

Excluding such configurations, from Proposition 5 one can choose ${\bm{n}}$ such that ${\bm{n}}\cdot{\bm{f}}>0$ for all resultant forces ${\bm{f}}$ generated by valid contacts. This is desirable insofar as it eliminates potential singularities ${\bm{n}}\cdot{\bm{f}}=0$ where the ZMP would be undefined (13). In the classical horizontal-floor setting, taking ${\bm{n}}$ as the floor normal is an example of such a solution.

The full support area describes the ZMPs sustainable by valid contact forces. As such, it does not take into account kinematic and dynamic limitations of the robot itself, in particular the limited changes in whole-body momentum $(\dot{\mathbf{P}},\dot{\mathbf{L}}_{G})$ that the robot can generate around its COM by moving its limbs. Consider the simple example depicted in Figure 5, with a single point contact. According to Proposition 4, the full support area is given by the intersection of the friction cone with the virtual plane $\Pi(O,n)$. To generate a ZMP $Z$, the robot needs to change its angular momentum around the COM by

Thus, ZMPs close to the boundary of the full support area ($|x_{Z}|\to d_{Z}\sin\alpha$) tend to generate higher angular momenta, a phenomenon which is amplified by the altitude $d_{Z}$ of the virtual plane. In practice, legged robots cannot generate a arbitrary angular momenta, as torque limits and their bounded range of motion yield additional constraints, which in turn restrict the ZMPs that the robot can generate. One way to take these limitations into account is to put additional constraints on the whole-body momentum, as we will now see with the linear pendulum mode.

The Linear Pendulum Mode (LPM) is a particular mode of Equation (41), obtained by regulating the COM altitude and the angular momentum to constant values:

When $d_{Z}<d_{G}$, the COM is above the virtual plane with respect to the plane normal. In such situations, Equation (41) yields the well-known linear inverted pendulum mode (LIPM) [4, 10]:

The LIPM was a necessity in previous works as the ZMP was assumed to lie on the floor below the COM. But now that the virtual plane coordinate $d_{Z}$ can be chosen freely, we may also consider the case where $d_{Z}>d_{G}$, i.e. taking the virtual plane above the COM. In this case, Equation (41) yields a linear non-inverted pendulum:

The behavior of the ZMP with respect to COM acceleration differs between the two modes. In the LIPM, the ZMP is a repulsor of the COM: when it is maintained at a fixed position, the COM is “pushed away” from it and diverges to infinity. When selecting a virtual plane below the center of mass $(d_{Z}<d_{G})$, ZMP trajectories will therefore follow the COM when it accelerates, and precede it when it decelerates.

In the LNPM, the ZMP is a marginal attractor of the COM: when it is maintained at a fixed position, the COM will “orbit” around it (yet without asymptotic convergence, as there is no damping term in Equation (48)). In a virtual plane above the center of mass ($d_{Z}>d_{G}$), ZMP trajectories will therefore precede the COM when it accelerates, and follow it otherwise.

These two complementary behaviors reflect the fact that, in the LPM, the resultant contact force ${\bm{f}}$ always points from the COM to the ZMP. In what follows, we choose to take the virtual plane above the center of mass and use the LNPM.

Previous methods [9, 10, 18, 7, 11, 12] applied the LIPM using the convex hull of contact points (CHCP) as ZMP support area. By Proposition 4, when all contacts are coplanar the CHCP is the full support area. However, it turns out that the constraints (45)-(46) of the linear-pendulum mode shrink the ZMP support area.

This can be seen as follows. Suppose that the ZMP is located at a vertex $C_{k}$ of a given contact polygon. The resultant contact force ${\bm{f}}$ must then be realized as one contact force ${\bm{f}}_{k}$ applied at $C_{k}$, while all other contact forces ${\bm{f}}_{j}=\bm{0}$ for $j\neq k$. (From Equation (17), ${\bm{p}}_{Z}={\bm{p}}_{Z_{k}}={\bm{p}}_{C_{k}}$ implies that only the $\lambda_{i}$’s corresponding to $C_{k}$ can be strictly positive.) However, in the LPM the contact force ${\bm{f}}=\ddot{{\bm{p}}}_{G}-{\bm{g}}$ is also directed from $Z=C_{k}$ to $G$. Hence, the ZMP support area cannot be the CHCP as soon as the COM lies outside of the friction cone of at least one ground contact point. An example of such situation is when a biped robot stretches its legs, as depicted in Figure 6.

Methods that take the Convex Hull of Contact Points (CHCP) as ZMP support areas rely by construction on the assumption of “infinite friction” to rule out such cases, at the cost of being problematic for locomotion on low-friction floors (apart from sliding, foot yaw rotations due to insufficient friction have also been observed and studied [37, 33]). This assumption explains why the shrinking of the ZMP support area in the LPM was not highlighted in previous works.

We call pendular support area the shrunk support area that takes into consideration the Equations (45)-(46) of the LPM.

In this section, we assume that ${\bm{n}}={\bm{e}}_{Z}$, the upward vertical (opposite to gravity) unit vector. We now propose an algorithm to compute the pendular support area based on the double-description method [38]. It turns out that computations of this set are very similar to that of the COM static-equilibrium polygon. We explain both calculations here.

We now design a trajectory generator for ZMP-COM trajectories based on the model-preview control formalism introduced in previous works [18, 30]. We use the ZMP as a command and COM as the output variable. First, we define the support of the ZMP trajectory ${\bm{p}}_{Z}(\gamma(t))$ as a line segment:

where ${\bm{p}}_{0}$ (resp. ${\bm{p}}_{1}$) denotes the initial (resp. final) ZMP position. Assuming that its initial velocity is zero, the COM follows a parallel line segment ${\bm{p}}_{G}(\eta(t))$:

where $\ddot{\eta}(t)=\omega^{2}(\gamma(t)-\eta(t))$. Next, define the state of the control problem by ${\bm{x}}(t)~=~[\eta(t)\ \dot{\eta}(t)]^{\top}$ and its command by the linear position $\gamma(t)$ of the ZMP. Discretizing the time interval into $K$ steps of duration $\delta t$, the system’s dynamics become

Let $\mathbf{X}=[{\bm{x}}_{0}^{\top}\cdots{\bm{x}}_{K}^{\top}]^{\top}$ and $\bm{\gamma}=[\gamma_{0}\cdots\gamma_{K-1}]^{\top}$. Applying (76) repeatedly, we build the matrices $\bm{\Phi}$ and $\bm{\Psi}$ such that $\mathbf{X}=\bm{\Phi}{\bm{x}}_{0}+\bm{\Psi}\bm{\gamma}$. We assume that the system starts with zero COM velocity, so that ${\bm{x}}_{0}=\bm{0}$ and $\mathbf{X}=\bm{\Psi}\bm{\gamma}$.

We formulate the trajectory generation problem as a Quadratic Program (QP) as follows:

The objective is the weighted sum of two terms:

The first one minimizes COM accelerations while the second regularizes the ZMP trajectory. The weights of the cost function are set to $w_{1}=1$ and $w_{2}=100$. Meanwhile, the constraints ensure respectively that:

• •

  the ZMP belongs to the line segment $(\gamma(t)\in[0,1])$

• •

  the COM ends at the destination point $(\eta_{K}=1)$ with zero velocity $(\dot{\eta}_{K}=0)$,

• •

  the ZMP also ends at the destination point $(\gamma_{K-1}=1)$.

The solution to this QP provides a ZMP trajectory $p_{Z}(t)$, which is then integrated into a COM trajectory $p_{G}(t)$ by applying (48). We also added a damping term at this stage to smooth out undesired COM oscillations. On a side note, set aside the two regularization objectives, this optimization problem falls under the framework of Time-Optimal Path Parameterization (TOPP) [39, 40]. One could then trade smoothness of COM accelerations for Admissible Velocity Propagation (AVP), allowing for the integration into a kinodynamic planner of COM trajectories [41].

With this method, contact stability is mostly enforced by checking that the segment $[{\bm{p}}_{0},{\bm{p}}_{1}]$ lies inside the pendular support area computed for the extremities ${\bm{p}}_{0}$ and ${\bm{p}}_{1}$ of the segment. However, the COM moves as the robot performs the motion, which affects both the position and shape of the pendular support area. This phenomenon can have two undesired outcomes:

• •

  the area becomes empty: we observed empirically that this would only happen when the COM is far away (e.g. more than one meter) from contacts, and was not a threat in practice.

• •

  the area constraints the COM motion toward divergence: past a certain COM-to-contacts distance, the support area slants away from contacts, in a way such that it contains no ZMP that could bring the pendulum back above contacts.

Figure 8 illustrates this phenomenon. In practice, ensuring that the ZMP is well within the support area computed at the beginning ${\bm{p}}_{0}$ and end ${\bm{p}}_{1}$ of the segment was enough to rule out both undesirable outcomes.

We implemented the whole pipeline described so far to generate multi-contact motions for a model of the HRP-4 humanoid robot. The scenario is depicted in Figure 9. The robot has to step on inclined platforms in order to reach its goal configuration on the right. Because there is no platform for its left foot in the middle of the course, the only way for it to complete the task is to put its left hand on the elevated “wall” platform while pushing with its right foot on the opposite tilted surface. Relying on these two simultaneous contacts, the humanoid can perform a long stride which would have been impossible to achieve in single-support.

As input given to solve this scenario, we assume that a contact planner provides a sequence of stances, where a stance $\sigma_{i}(\{{\bm{p}}_{C_{i}}\},{\bm{p}}_{i})$ provides both a reference position of the COM ${\bm{p}}_{i}$ and a set of contact points $\{{\bm{p}}_{C_{i}}\}$. The first stage of our solution computes stance-to-stance COM trajectories. To move from $\sigma_{i}$ to stance $\sigma_{i+1}$, the controller considers the line segment $[{\bm{p}}_{i},{\bm{p}}_{i+1}]$. The trajectory generator is called if this segment is included in the pendular support area ${\cal S}({\bm{p}}_{i},d_{Z})$ for the initial COM position. Otherwise, $d_{Z}$ is increased until the segment is included in ${\cal S}({\bm{p}}_{i},d_{Z})$. This condition is easy to fulfill in practice, as we observed that the region ${\cal S}({\bm{p}}_{G},d_{Z})$ grows like the section by the plane $\Pi(d_{Z},{\bm{n}})$ of a cone passing through $G$ (see Figure 7). In the motion depicted in Figure 9, this process lead us to take the virtual plane one meter above the center of mass of the humanoid (HRP-4 is 1.5-meter tall). Although contact planning was done by hand in this experiment, we also used the pendular support area to select COM positions ${\bm{p}}_{i}$ from contact locations $\{{\bm{p}}_{C_{i}}\}$, by enforcing that the ZMP at the vertical of ${\bm{p}}_{i}$ lies well inside ${\cal S}({\bm{p}}_{i},d_{Z})$.

Once a reference COM trajectory ${\bm{p}}_{G}(t)$ has been computed, we generate whole-body joint-angles by differential inverse kinematics (IK) under the following constraints, by decreasing task weight:

1. 1.

   tracking of contacting end-effector poses,

2. 2.

   tracking of COM trajectory,

3. 3.

   tracking of free end-effector poses,

4. 4.

   minimum variations in angular momentum

5. 5.

   preferred values for some joint-angles.

We applied our own IK solver for the task, which is released in the pymanoid library.⁴⁴4https://github.com/stephane-caron/pymanoid Similarly to [26], this solver relies on a single-layer QP problem, using the above cost function and a set of inequality constraints

to limit joint velocities. Gains and weights used in the simulations are reported in Table II, while other simulation parameters are given in Table III. Implementation details can be checked in the library and source code released with the paper (see the motion_editor distributed in [42]).

ZMP tracking is not an explicit task in the list above, and is realized as a side effect of the COM tracking task. The latter comes second in the hierarchy of the differential IK solver and is therefore not fulfilled perfectly, as illustrated in Figure 10. The same holds for the task $\dot{\mathbf{L}}_{G}=0$, which has an even lower rank in the hierarchy. Yet, ZMP deviations in the generated trajectory are small enough and one can check that the ZMP always stays well within the pendular support area (see the accompanying video). We cross-validated the contact-stability of the final motion by computing, at each time step, a set of valid contact forces ${\bm{f}}_{all}$. Force vectors are depicted in Figure 9 and displayed in the accompanying video [42].