Multi-contact Stability Support Areas and Volumes for Humanoid Locomotion under Frictional Contacts

Stéphane Caron
LIRMM, CNRS-UM2
Talk at Max Planck Institute
September 19, 2016
https://scaron.info/slides/mpi-2016/

Contact stability

Contact stability of a motion

A motion $q (t), τ (t), f (t), C (t)$ is a solution to the equations of motion:

M (q) ¨ q + h (q, ˙ q) = S^{⊤} τ + \sum i \in C J_{i}^{⊤} f_{i}

A motion is contact stable if it contains no transition between contact modes, that is, if $C (t)$ is stationary.

Contact stability of a trajectory

A trajectory $q (t)$ in contact state $C$ is (weak) contact stable if it is possible to perform it with no contact switch:

\exists {f_{i} \in {FC}_{i}}, M (q) ¨ q + h (q, ˙ q) = S^{⊤} τ + \sum i \in C J_{i}^{⊤} f_{i}

Example

Contact stable

Not contact stable

Zoom on previous example

Contact stable

Not contact stable

Contact modes

Complementarity

Let $v_{i}$ and $f_{i}$ denote the velocity and force on a given DOF $i$ of a rigid body.

When one is equality-constrained, the other is always inequality-constrained:

$v_{i} =$	$v_{i} \leq$
$f_{i} \leq$	$f_{i} =$

Contact mode

A contact mode specifies which of $v_{i}, f_{i}$ is equality-constrained for all contact DOFs $i$ .

Example

In the fixed contact mode, all contact velocities are zero $J_{i} ˙ q = 0$ and contact forces lie in their friction cones $F_{i} f_{i} \leq 0$ .

Contact mode graph

What we currently use

Single contact stability

in the fixed mode

Contact stability

The Contact Wrench Cone characterizes contact stability, that is:

\begin{matrix} \exists {f_{i} \in {FC}_{i}}, M (q) ¨ q + h (q, ˙ q) & = & S^{⊤} τ + 4 \sum i = 1 J_{C_{i}}^{⊤} f_{i} \Leftrightarrow \exists w_{O} \in {CWC}_{O}, M (q) ¨ q + h (q, ˙ q) & = & S^{⊤} τ + J_{O}^{⊤} w_{O} \end{matrix}

Structure of the CWC

Friction cone on the net contact force
Center of pressure condition
Yaw moment condition

Yaw moment condition

“Torsional friction”

Model found in previous works: $| τ_{O}^{z} | \leq μ_{τ} f^{z}$ ... but physically incorrect.

Condition found in the CWC

Taken at the center of pressure $C$ , the correct condition is:

| τ_{C}^{z} - τ_{safe} | \leq μ d_{edge} f^{z}

$μ$ : usual static friction coefficient
$d_{edge}$ : distance to the edge of the contact polygon
$τ_{safe}$ : function of $f$ and $p_{C}$ (details in (Caron, 2016), p.82)

ZMP support areas

for multi-contact locomotion

Zero-tilting Moment Point

Definition

The ZMP is traditionally defined as the point on the floor where the moment of the contact wrench is parallel to the surface normal (Sardain & Bessonnet, 2004).

Intuition as Center of Pressure

The point at which the robot “applies its weight”.

Contact stability

The ZMP must lie inside the support polygon (convex hull of ground contact points).

Limitations

This definition requires a single “floor” surface (no multi-contact)
The support polygon does not account for frictional limits (slippage, yaw rotations).

ZMP of a Wrench

The ZMP is mathematically defined from a wrench (Sardain & Bessonnet, 2004). The ZMP in the plane $Π (O, n)$ of normal $n$ containing $O$ is the point such that $n \times τ_{Z} = 0$ , that is:

p_{Z} = \frac{n \times τ_{O}}{n \cdot f} + p_{O} .

We define the full support area $S$ as the image of the CWC by this equation.

Good news

This area can be computed geometrically (Caron et al., 2016):

It coincidates with the usual support area on horizontal floor.

Bad news

The area is not always a polygon:

This depends on the choice of the virtual plane normal

n

Hands on!

Decomposition

The equations of motion consist of two components:

Newton-Euler equations: “external” interactions between environment and free-floating base:

[\begin{matrix} m {¨ p}_{G} {˙ L}_{G} \end{matrix}] = [\begin{matrix} m g 0 \end{matrix}] + \sum c o n t a c t i [\begin{matrix} f_{i} {- - \to G C}_{i} \times f_{i} \end{matrix}]

Actuated-joint equations: “internal” interactions between motors and joints:

M^{'} (q) ¨ q + h^{'} (q, ˙ q) = τ + \sum i \in C {J^{'}}_{i}^{⊤} f_{i}

Newton-Euler equations

[\begin{matrix} m {¨ p}_{G} {˙ L}_{G} \end{matrix}] = [\begin{matrix} m g 0 \end{matrix}] + \sum c o n t a c t i [\begin{matrix} f_{i} {- - \to G C}_{i} \times f_{i} \end{matrix}]

$m$ and $g$ : total mass and gravity vector
${¨ p}_{G}$ : acceleration of the center of mass (COM)
${˙ L}_{G}$ : rate of change of the angular momentum
$f_{i}$ : contact force received at contact point $C_{i}$

Hierarchical contact stability

First, solve contact stability for the COM and angular momentum trajectory.

Next, joint angles will follow...

Linear Pendulum Mode

The Newton equation can be written equivalently:

{¨ x}_{G} = \frac{g + {¨ z}_{G}}{z_{G} - z_{Z}} (x_{G} - x_{Z}) - \frac{{˙ L}_{G x}}{m (z_{G} - z_{Z})}

and similarly for ${¨ y}_{G}$ . The Linear Inverted Pendulum Mode (Kajita et al., 2001) is obtained by constraining:

\begin{matrix} z_{G} - z_{Z} & = & h {˙ L}_{G} & = & 0 \end{matrix}

The system dynamics become ${¨ x}_{G} = \frac{g}{h} (x_{Z} - x_{G})$ .

Observation

The support area in the LIPM is smaller than the convex hull of contact points:

We added 4 equality constraints to the resultant wrench, so the CWC is smaller.

Pendular Support Area

The double-description method can be used (Caron et al., 2016) to compute the support area corresponding to the system:

\begin{matrix} w & \in & CWC z_{G} - z_{Z} & = & h {˙ L}_{G} & = & 0 \end{matrix}

We call it the pendular support area.

It is smaller than the full support area and characterizes contact stability in the pendular mode.

Linear Pendulum Mode

Let us consider the ZMP above the COM:

{¨ x}_{G} = \frac{g}{h} (x_{G} - x_{Z})

This is the dynamic equation of a spring.

Attractivity

LIPM: the ZMP is a repellor of the COM
LPM: the ZMP is a marginal attractor of the COM

Control law where the robot is driven from above.

Demo 2

Beyond Good and ZMP

a multi-contact walking pattern generator

Cone of 3D COM Accelerations

Beyond ZMP

Removes the planar COM constraint.

Calculation

Algorithm to compute it as convex hull of 2D dual points, fast and numerically robust (Caron & Kheddar, submitted).

Predictive control

Consider the linear control problem:

x (k + 1) = A x (k) + B u (k)

$x (k)$ : COM state (position and velocity) at preview time $k Δ T$
$u (k)$ : COM acceleration at preview time $k Δ T$

Optimization problem

Predictive control achieves a task over a preview horizon:

\begin{matrix} min {u (k)} & ∥ x (N) - x_{goal} ∥^{2} + ϵ \sum k ∥ u (k) ∥^{2} s.t. & \forall k, u (k) \in C (x (k)) \end{matrix}

where $C (x (k))$ is the COM acceleration cone at time step $k$ . This constraint is linear-quadratic due to its dependency on $x (k)$ .

Linearizing the MPC problem

Idea

Bound the preview COM trajectory in a tube $T$ , then compute the intersection:

I_{T} = ⋂ p_{G} \in T C (p_{G})

Conservative contact-stability criterion.

Inequality constraints

Decouples $u (k) \in C (x (k))$ into:

\begin{matrix} C_{T} u (k) & \leq & 0 P_{T} x (k) & \leq & 0 \end{matrix}

Demo 3

Sweets for later

When to make a step?

Problem: fixed step timings do not work any more in multi-contact (now depends on terrain topology).

Time-Optimal Path Parameterization

Given a path $q (s)$ and dynamic constraints, TOPP finds a retiming $q (t)$ satisfying all constraints and with minimum duration.

Integration with the MPC

Continuous interpolation/retiming loop running at 20-30 Hz thanks to a new TOPP-Polygon algorithm.

Demo 4

Wrapping remarks

Single contact stability

Analytical formula of the CWC: friction cone, COP area, yaw moment.

ZMP support areas

Applicable to general multi-contact locomotion, but COM planar constraint.

3D COM acceleration cones

Generalizes the latter. Computed fast enough for the control loop.

Multi-contact Walking Pattern Generator

Use a QCQP solver, or... linearize inequality constraints.

Thank you for your attention.