# Balance control using both ZMP and COM height variations: A convex boundedness approach

Stéphane Caron, Bastien Mallein

► To cite this version:

Stéphane Caron, Bastien Mallein. Balance control using both ZMP and COM height variations: A convex boundedness approach. ICRA 2018 - 35th IEEE International Conference on Robotics and Automation, May 2018, Brisbane, Australia. pp.1779-1784, 10.1109/ICRA.2018.8460942. hal-01590509v4

HAL Id: hal-01590509

https://hal.science/hal-01590509v4

Submitted on 26 Feb 2018

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>
# Balance control using both ZMP and COM height variations: A convex boundedness approach

Stéphane Caron and Bastien Mallein

**Abstract**— Developments for 3D control of the center of mass (CoM) of biped robots are currently located in two local minima: on the one hand, methods that allow CoM height variations but only work in the 2D sagittal plane; on the other hand, nonconvex direct transcriptions of centroidal dynamics that are delicate to handle. This paper presents an alternative that controls the CoM in 3D via an indirect transcription that is both low-dimensional and solvable fast enough for real-time control. The key to this development is the notion of boundedness condition, which quantifies the capturability of 3D CoM trajectories.

## I. INTRODUCTION

Three-dimensional control of the center of mass follows in the wake of major achievements obtained in 2D locomotion with the linear inverted pendulum mode (LIPM) [1]. The core idea of the LIPM was to keep the CoM in a plane, which made the model tractable and paved the way for key discoveries, including the capture point [2] and capture-point based feedback control [3], subsequently applied in successful walking controllers [4], [5].

For a while, the ability to leverage vertical CoM motions seemed lost on the way, but recent developments showed a regain of interest for this capability [6], [7], [8]. All of them share a design choice that can be traced back to the seminal work of Pratt and Drakunov [9]: they interpolate CoM trajectories in a 2D sagittal plane for the inverted pendulum model (IPM) with fixed center of pressure. The key result of [9] is the conservation of the “orbital energy” of a CoM path, a variational principle that was later translated into a predictive controller in an equally inspirational study by Koolen et al. [7]. Ramos and Hauser [6] also noticed that the capture point, interpreted as point where to step, was a function of the CoM path, which they computed via a single shooting method. Interestingly, Hopkins et al. pointed out that vertical CoM motions is equivalent to turning the constant $\omega$ of the LIPM into a time-varying function $\omega(t)$ [10]. They brought to light a differential equation that this function must satisfy, and used it to compute back $\omega(t)$ from, once again, an a priori CoM-height trajectory.

Different as they may seem, the variational and point-where-to-step approaches are two instances of the same underlying concept: convergence of the system towards a

steady state requires that its divergent component of motion stays bounded. Lanari et al. [11], [12] showed how the condition for this to happen involves an integral over the future trajectory of the system (one can see a similar integral in the orbital energy), which they named the boundedness condition.

The solution we explore in this study differs from existing approaches in that it considers $\omega(t)$ per se rather than as a result of CoM trajectories. This change of perspective is made possible by the derivation of the boundedness condition for the 3D IPM (Section II), which is then cast into an optimization problem for 2D and 3D control of the CoM (Sections III and IV). The resulting predictive controller is implemented and tested in Section V.

## II. BOUNDEDNESS CONDITION OF THE IPM

The equation of motion of the inverted pendulum model is:

$$ \ddot{c}(t) = \lambda(t)(\mathbf{c}(t) - \mathbf{r}(t)) + \mathbf{g} \quad (1) $$

where $\mathbf{c}$ is the center of mass (CoM) of the robot, $\mathbf{r}$ is the center of pressure (CoP) under its contacting foot, and $\mathbf{g} = -g\mathbf{e}_z$ is the gravity vector. The quantity $\lambda$ has the unit of a stiffness. It must be positive $\lambda \ge 0$ by unilaterality of contact, while the CoP $\mathbf{r}$ always belongs to the contact area.

### A. Divergent component of motion

To alleviate calculations, let us formulate the divergent component of motion [13] of this model as a velocity rather than a point:<fnref n="1" />

$$ \xi(t) = \dot{c}(t) - \dot{r}(t) + \omega(t)(\mathbf{c}(t) - \mathbf{r}(t)) \quad (2) $$

where $\omega(t)$ is a solution to the differential equation [10]:

$$ \dot{\omega} = \omega^2 - \lambda \quad (3) $$

The quantity $\omega$ has the unit of a damping. The interest of its differential equation appears when differentiating the divergent component of motion:

$$ \dot{\xi} = (\lambda + \dot{\omega})(\mathbf{c} - \mathbf{r}) + \omega(\dot{c} - \dot{r}) + \mathbf{g} - \ddot{\mathbf{r}} = \omega\xi + \mathbf{g} - \ddot{\mathbf{r}} \quad (4) $$

The solution to this first-order differential equation is:

$$ \xi(t) = \left( \xi(0) + \int_{0}^{t} e^{-\Omega(\tau)} (\mathbf{g} - \ddot{\mathbf{r}}(\tau)) d\tau \right) e^{\Omega(t)} \quad (5) $$

where $\Omega$ is the antiderivative of $\omega$ such that $\Omega(0) = 0$. In the LIPM where the damping $\omega$ is a constant, $\Omega(t) = \omega t$ and

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

S. Caron is with the Montpellier Laboratory of Informatics, Robotics and Microelectronics, CNRS–University of Montpellier, Montpellier, France.

B. Mallein is with the Laboratoire Analyse, Géométrie et Applications, CNRS–Paris 13 University, Villetaneuse, France.

Corresponding author: stephane.caron@lirmm.fr

<footnote n="1">¹Consider the derivative of a product uv compared to that of a ratio u/v.</footnote>
this equation is equivalent to the well-known capture point dynamics [13].

To be viable [14], the trajectory of the system must be bounded, which implies that the above expression does not diverge despite its exponential factor. This necessary condition for viability is known as the boundedness condition [11], [12], and is written here:

$$\int_{0}^{\infty} (\ddot{r}(t) - g)e^{-\Omega(t)}dt = \xi(0) \quad (6)$$

Note how this requirement involves both the CoP trajectory $r(t)$ and stiffness trajectory $\lambda(t)$: the former is integrated through $\ddot{r}(t)$, while the latter is embedded in $\Omega(t)$.

## B. Decoupling of the boundedness constraint

Let us express all coordinates in the inertial frame depicted in Figure 1. In what follows, we will use the subscript i (“initial”) to indicate values at $t = 0$, and f (“final”) for stationary values obtained as $t \to \infty$. The origin of the inertial frame is taken at the stationary CoP position $\mathbf{r}_f = 0$ where we want the robot to stop. The stationary CoM position is then $\mathbf{c}_f = z_f e_z$, with $z_f$ the desired stationary CoM height. The two vectors $e_x$ and $e_y$ are horizontal (i.e. orthogonal to gravity), and $e_x$ is chosen aligned with the horizontal projection of $\mathbf{r}_f - \mathbf{c}_i$.

In reality, the center of pressure always lies on the contact surface between the robot and its environment. Therefore, CoPs $\mathbf{r}$ output by our controller must satisfy the feasibility condition of lying on the contact surface. Denoting by $\mathbf{n}$ the contact normal, this means that $\mathbf{r}$ and all its derivatives are orthogonal to $\mathbf{n}$; in particular, $\ddot{\mathbf{r}}(t) \cdot \mathbf{n} = 0$ at all times. Taking the dot product of Equation (6) with $\mathbf{n}$ then yields:

$$\int_{0}^{\infty} e^{-\Omega(t)} dt = \frac{\dot{\bar{z}}_{i} + \omega_{i} \bar{z}_{i}}{g} \quad (7)$$

where $\bar{z} \stackrel{\text{def}}{=} (\mathbf{c} \cdot \mathbf{n})/(e_z \cdot \mathbf{n})$ is the height of the vertical projection of $\mathbf{c}$ onto the contact surface.

Let us now define $\mathbf{p}(t) \stackrel{\text{def}}{=} [r_x(t) \quad r_y(t)]$ and $\bar{\mathbf{c}}_i \stackrel{\text{def}}{=} [x_i \quad y_i]$ the horizontal projections of the CoP and CoM, respectively. The horizontal projection of Equation (6) is:

$$\int_{0}^{\infty} \ddot{\mathbf{p}}(t) e^{-\Omega(t)} dt = (\dot{\bar{\mathbf{c}}}_{i} - \dot{\mathbf{p}}_{i}) + \omega_{i} (\bar{\mathbf{c}}_{i} - \mathbf{p}_{i}) \quad (8)$$

A double integration by parts of the left hand side of this equation yields:

$$\int_{0}^{\infty} \ddot{\mathbf{p}}(t) e^{-\Omega(t)} dt = \int_{0}^{\infty} \mathbf{p}(t) \lambda(t) e^{-\Omega(t)} dt - (\dot{\mathbf{p}}_{i} + \omega_{i} \mathbf{p}_{i})$$

Combining these last two equations, we obtain a temporal formulation of the boundedness condition with separate gravity and CoP components:

### Temporal Boundedness Condition

$$\int_{0}^{\infty} e^{-\Omega(t)} dt = \frac{\dot{\bar{z}}_{i} + \omega_{i} \bar{z}_{i}}{g} \quad (9)$$

$$\int_{0}^{\infty} \mathbf{p}(t) \lambda(t) e^{-\Omega(t)} dt = \dot{\bar{\mathbf{c}}}_{i} + \omega_{i} \bar{\mathbf{c}}_{i} \quad (10)$$

<figure><img src="image_2.png" /><figcaption>Fig. 1. Definition of the inertial frame ($e_x, e_y, e_z$). The frame is rooted at the stationary CoP location $\mathbf{r}_f$. The instantaneous CoM location is denoted by $\mathbf{c}_i$. Both vectors $e_x$ and $e_y$ lie in the horizontal plane, and $e_x$ is aligned with the (horizontal projection of the) desired direction of motion $\mathbf{r}_f - \mathbf{c}_i$.</figcaption></figure>

Equations (9)–(10) are equivalent to (6) as ($e_x, e_y, \mathbf{n}$) is a (non-orthogonal) basis of the 3D Euclidean space.

In the familiar setting of the LIPM, the first equation boils down to:

$$\int_{0}^{\infty} e^{-\omega t} dt = \frac{1}{\omega} = \frac{0 + \omega z}{g} \iff \omega = \sqrt{\frac{g}{z}} \quad (11)$$

Meanwhile, taking $\mathbf{p} = 0$ in the second one implies that the traditional LIPM capture point is located at $\mathbf{r}_f$. This observation illustrates how the boundedness condition (9)–(10) encompasses the well-known model.

### C. Change of variable

Define the adimensional quantity $s(t) = e^{-\Omega(t)}$. This variable ranges from 1 when $t = 0$ to 0 as $t \to \infty$, and its time derivatives are:

$$\dot{s}(t) = -\omega(t)s(t) \quad (12)$$

$$\ddot{s}(t) = \lambda(t)s(t) \quad (13)$$

Thanks to the bijective mapping between $s$ and $t$, we can characterize $\omega$ and $\lambda$ indistinctly as functions of $s$ or $t$. Let us choose the former, and denote by $(\cdot)'$ derivatives with respect to $s$. For instance, $\omega(t) = \omega(s(t))$ implies that:

$$\frac{d\omega}{dt}(t) = \frac{ds}{dt}(t) \frac{d\omega}{ds}(s(t)) \implies \dot{\omega} = \dot{s}\omega' = -s\omega\omega' \quad (14)$$

where we eventually omit arguments $\square(t)$ and $\square(s(t))$ for brevity. Applying the chain rule similarly yields:

$$dt = \frac{ds}{\dot{s}} = - \frac{ds}{\omega(s)s} \quad (15)$$

$$\lambda = \omega^2 - \dot{\omega} = \omega(\omega + s\omega') = \omega(s\omega)' \quad (16)$$

We can therefore formulate the boundedness condition (9)–(10) in terms of the new variable $s$ as:

### Timeless Boundedness Condition

$$\int_{0}^{1} \frac{1}{\omega(s)} ds = \frac{\dot{\bar{z}}_{i} + \omega_{i} \bar{z}_{i}}{g} \quad (17)$$

$$\int_{0}^{1} \mathbf{p}(s)(s\omega)' ds = \dot{\bar{\mathbf{c}}}_{i} + \omega_{i} \bar{\mathbf{c}}_{i} \quad (18)$$
With this change of variable, infinite integrals over time solutions $\omega(t), \mathbf{p}(t)$ are turned into finite integrals over functions $\omega(s), \mathbf{p}(s)$ of the new variable $s$. This approach bears a close resemblance to the recently proposed concept of spatial quantization [15].

### III. SAGITTAL 2D BALANCE WITH FIXED COP

Let us consider first the case of 2D stabilization in the plane $(e_x, e_z)$ with a fixed CoP. Since $r_x = 0$, the boundedness condition becomes:

$$ \omega_i = -\frac{\dot{x}_i}{x_i} \qquad (19) $$

$$ \dot{z}_i + \omega_i z_i = g \int_0^1 \frac{1}{\omega(s)} ds \qquad (20) $$

Equation (19) implies via (5) that $\dot{x} = -\omega x$, which integrates to $x(t) = x_1 e^{-\Omega(t)} = x_1 s(t)$. We see how, in 2D balance control, the coordinates $x$ and $s$ are proportional. Interpolation over $x$ [7], [9] is therefore equivalent to interpolation over $s$, and one can recognize in (20) the same principle as the conservation of orbital energy [9]. The benefit of using $s$ rather than $x$ will appear when we move to 3D control.

#### A. Viability condition

The viability condition $z_{crit} \ge 0$ derived in [7] for $\lambda \ge 0$ can be seen as a consequence of (20). Indeed, the lower-bounding profile $\lambda = 0$ corresponds to $(s\omega)' = 0$, that is to say, $s\omega = \omega_i$. It follows that:

$$ \int_0^1 \frac{ds}{\omega(s)} \ge \int_0^1 \frac{ds}{\omega(s)} = \int_0^1 \frac{s}{\omega_i} ds = \frac{1}{2\omega_i} \qquad (21) $$

$$ z_{\text{crit}} = z_i + \frac{\dot{z}_i}{\omega_i} - \frac{g}{2\omega_i^2} = \frac{g}{\omega_i} \left[ \int_0^1 \frac{ds}{\omega(s)} - \frac{1}{2\omega_i} \right] \ge 0 \qquad (22) $$

In practice, biped robots cannot exert arbitrary large contact forces, and tend to break contact when ground pressure becomes too low. To reflect this, we will thereafter consider the stricter feasibility condition $\lambda \in [\lambda_{\min}, \lambda_{\max}]$. The corresponding viability condition can be derived in a similar fashion by considering lower- and upper-bounding profiles:<fnref n="2" />

$$ \dot{z}_i + \omega_i z_i \le \frac{g}{\lambda_{\max}} \left[ \omega_i + \sqrt{\frac{\lambda_{\max} - \lambda_{\min}}{\lambda_{\min}}} \sqrt{\lambda_{\max} - \omega_i^2} \right] \qquad (23) $$

$$ \dot{z}_i + \omega_i z_i \ge \frac{g}{\lambda_{\min}} \left[ \omega_i - \sqrt{\frac{\lambda_{\max} - \lambda_{\min}}{\lambda_{\max}}} \sqrt{\omega_i^2 - \lambda_{\min}} \right] \qquad (24) $$

#### B. Computing feasible $\omega$ solutions

Let us partition the interval $[0, 1]$ into $N$ fixed segments $0 = s_0 < s_1 < \dots < s_N = 1$, for instance $s_j = j/N$. We compute solutions to the boundedness condition where $\lambda(s)$ is piecewise constant over this subdivision, that is, $\forall s \in [s_j, s_{j+1}], \lambda(s) = \lambda_j$. Define:

$$ \varphi(s) \stackrel{\text{def}}{=} s^2 \omega^2 \qquad \Delta_j \stackrel{\text{def}}{=} s_{j+1}^2 - s_j^2 \qquad (25) $$

Remarking that $\varphi' = 2s\lambda$ from Equation (16), we can directly compute $\varphi(s)$ for $s \in [s_j, s_{j+1}]$ as:

$$ \varphi(s) = \sum_{k=0}^{j-1} \lambda_k \Delta_k + \lambda_j (s^2 - s_j^2) \qquad (26) $$

In what follows, we use the shorthand $\varphi_j = \varphi(s_j)$.

We can now calculate the right-hand side of the boundedness condition (20):

$$ \int_0^1 \frac{ds}{\omega(s)} = \sum_{j=0}^{N-1} \int_{s_j}^{s_{j+1}} \frac{s ds}{\sqrt{\varphi_j + \lambda_j(s^2 - s_j^2)}} \qquad (27) $$

$$ = \sum_{j=0}^{N-1} \int_{0}^{\Delta_j} \frac{dv}{2\sqrt{\varphi_j + \lambda_j v}} \qquad (28) $$

$$ = \sum_{j=0}^{N-1} \frac{1}{\lambda_j} \left[ \sqrt{\varphi_j + \lambda_j \Delta_j} - \sqrt{\varphi_j} \right] \qquad (29) $$

$$ = \sum_{j=0}^{N-1} \frac{\Delta_j}{\sqrt{\varphi_{j+1}} + \sqrt{\varphi_j}} \qquad (30) $$

Note that the latter expression is convex in the variables $\varphi_1, \dots, \varphi_N$ (by definition $\varphi_0 = 0$), as shown in Appendix A.

Besides boundedness, solutions should enforce three conditions:

* **Feasibility:** $\lambda_{\min} \le \lambda \le \lambda_{\max}$, expressed linearly in terms of $\varphi$ as $\Delta_j \lambda_{\min} \le \varphi_{j+1} - \varphi_j \le \Delta_j \lambda_{\max}$;

* **Initial state:** $\omega$ should be equal to $\omega_i$ at the initial index $s=1$, that is to say, $\varphi_N = \omega_i^2$;

* **Stationary state:** a stationary COM height $z_f$ can also be specified via $\varphi_1 = \Delta_0 g / z_f$, or similarly a range of heights $z_{\min} \le z_f \le z_{\max}$ used to approximate kinematic reachability.

Wrapping all four conditions together and adding a regularizing cost function over variations of $\lambda$, we obtain Optimization Problem 1. This problem is “almost” a quadratic program: it has a quadratic cost function and linear constraints, except for Equation (33) which is a one-dimensional nonlinear equality constraint.

#### C. Model predictive control for 2D balance

Solving Problem 1 can be done fast enough for the control loop, on the scale of 1–3 ms using a general-purpose nonlinear solver (see Section V). At each control cycle, we compute the optimum $\varphi_1^*, \dots, \varphi_N^*$ of the problem and extract its initial stiffness via:

$$ \lambda_i^* = \frac{\omega_i^2 - \varphi_{N-1}^*}{\Delta_{N-1}} \qquad (31) $$

This value is then sent as reference to the lower-level leg and attitude controllers until the next control cycle. In the standard model-predictive fashion, the rest of the optimal trajectory is discarded. As a matter of fact, the trajectories $\omega(s)$ and $\lambda(s)$ are never explicitly computed, let alone their time counterparts. The operation is possible (see Appendix B) but not necessary for control. Compared to the controller from [7], this solution enforces feasibility

<footnote n="2">² A more detailed derivation is provided in the supplementary material: https://scaron.info/files/icra-2018/supmat.pdf</footnote>
Optimization Problem 1

2D Balance Control

$$
\underset{\varphi_1, \dots, \varphi_N}{\text{minimize}} \quad \sum_{j=1}^{N-1} \left[ \frac{\varphi_{j+1} - \varphi_j}{\Delta_j} - \frac{\varphi_j - \varphi_{j-1}}{\Delta_{j-1}} \right]^2 \qquad (32)
$$

$$
\text{subject to} \quad \sum_{j=0}^{N-1} \frac{\Delta_j}{\sqrt{\varphi_{j+1} + \sqrt{\varphi_j}}} = \frac{\dot{z}_1 + \omega_i \bar{z}_i}{g} \quad (33)
$$

$$
\varphi_N = \omega_i^2 \tag{34}
$$

$$
\forall j, \lambda_{\min} \Delta_j \le \varphi_{j+1} - \varphi_j \le \lambda_{\max} \Delta_j \quad (35)
$$

$$
\varphi_1 = \Delta_0 g / z_f \quad (36)
$$

constraints $\lambda_{\min} \le \lambda \le \lambda_{\max}$ a priori, as opposed to an a posteriori clipping that may cause free-falling phases in the output trajectory.

IV. 3D BALANCE

In 3D balance where the initial velocity $\dot{c}_i$ is not necessar-
ily coplanar with the stationary state, the CoP cannot be fixed
and the initial damping $\omega_i$ is not determined by Equation (19)
any more. The latter is replaced by a more general condition
over CoP trajectories:

$$
\int_{0}^{1} \mathbf{p}(s)(s\omega)'ds = \dot{\mathbf{c}}_{i} + \omega_{i}\bar{\mathbf{c}}_{i} \quad (37)
$$

Subject to the feasibility conditions:

$$
\mathbf{A} \mathbf{p} \leq \mathbf{b} \qquad (38)
$$

where the matrix **A** and vector **b** form the halfspace-
representation of the contact polygon. This polygon is readily
computed from contact geometry. For example, a rectangular
contact |(**p**·**e**<sub>w</sub>)| ≤ W, |(**p**·**e**<sub>h</sub>)| ≤ H has four inequalities:

$$
\pm \begin{bmatrix} (e_x \cdot e_w) & (e_y \cdot e_w) \\ (e_x \cdot e_h) & (e_y \cdot e_h) \end{bmatrix} \mathbf{p} \le \begin{bmatrix} W \\ H \end{bmatrix} \quad (39)
$$

A. Computing feasible CoP solutions

The structure of Equation (37) suggests a particular solu-
tion: let us take **p**(*s*) = **p***<sub>*i*</sub>*f*(*s*ω), where:

• *f*(*ω*<sub>*i*</sub>) = 1: initially, the CoP is located at *r*<sub>*i*</sub>;

• *f* (0) = 0: eventually, the CoP is located at *r*<sub>*f*</sub>;

* f is increasing: we exclude solutions where the CoP goes back and forth, that we deem suboptimal;

* f is integrable: let F denote its antiderivative such that F(0) = 0. It is positive by monotonicity of f.

With this choice, the boundedness condition (37) becomes

$$
\mathbf{p}_i = \frac{\dot{\mathbf{c}}_i + \omega_i \bar{\mathbf{c}}_i}{F(\omega_i)} \quad (40)
$$

By monotonicity of *f*, the CoP trajectory is feasible if
and only if its initial position **p**<sub>*i*</sub> satisfies the feasibility
condition (38). Considering (40), this can be written:

$$
\mathbf{b}F(\omega_i) - (\mathbf{A}\bar{\mathbf{c}}_i)\omega_i \geq \mathbf{A}\dot{\bar{\mathbf{c}}}_i \quad (41)
$$

Optimization Problem 2

3D Balance Control

$$
\underset{\varphi_1, \dots, \varphi_N}{\text{minimize}} \quad \sum_{j=1}^{N-1} \left[ \frac{\varphi_{j+1} - \varphi_j}{\Delta_j} - \frac{\varphi_j - \varphi_{j-1}}{\Delta_{j-1}} \right]^2 \quad (32)
$$

$$
\text{subject to} \quad \sum_{j=0}^{N-1} \frac{\Delta_j}{\sqrt{\varphi_{j+1} + \sqrt{\varphi_j}}} - \frac{\bar{z}_i}{g} \sqrt{\varphi_N} = \frac{\dot{z}_i}{g} \quad (44)
$$

$$
\omega_{1, \min}^2 \le \varphi_N \le \omega_{1, \max}^2 \qquad (45)
$$

$$
\forall j, \lambda_{\min} \Delta_j \le \varphi_{j+1} - \varphi_j \le \lambda_{\max} \Delta_j \quad (35)
$$

$$
\varphi_1 = \Delta_0 g / z_f \qquad (36)
$$

Compared to the previous fixed-CoP setting where $\omega_i$ was
fully determined, we see how the relaxed polygonal con-
straint **A****p** $\le$ **b** now frees a range of possible choices for $\omega_i$.
At this stage, one can explore different CoP strategies via
the choice of a (preferably convex) function $F$. Let us focus
on the example of a power law, i.e. for some $k > 1$:

$$
f(s\omega) = \left( s \frac{\omega}{\omega_i} \right)^{k-1} \implies F(\omega_i) = \frac{\omega_i}{k} \quad (42)
$$

Equation (41) simplifies to:

$$
\left( \frac{b}{k} - \mathbf{A} \bar{\mathbf{c}}_i \right) \omega_i \geq \mathbf{A} \dot{\mathbf{c}}_i \quad (43)
$$

Each line of this vector inequality provides a lower or upper
bound on $\omega_i$, depending on the sign of the factor in front
of it. These inequalities can then be summed up as $\omega_{i,\min} \le$
$\omega_i \le \omega_{i,\max}$. Note that this computation is only carried out
once from the initial state, i.e. it is not part of the following
numerical optimization.

By definition of $\varphi$, $\omega_i$ is equal to $\sqrt{\varphi_N}$, so that bounds on $\omega$ are mapped directly into the optimization as $\omega_{i,\min}^2 \le \varphi_N \le \omega_{i,\max}^2$. However, previously $\omega_i$ also appeared in the right-hand side of the nonlinear equality constraint (33). In 3D, this constraint becomes:

$$
\sum_{j=0}^{N-1} \frac{\Delta_j}{\sqrt{\varphi_{j+1}} + \sqrt{\varphi_j}} - \frac{\bar{z}_i}{g} \sqrt{\varphi_N} = \frac{\dot{z}_i}{g} \quad (46)
$$

Both the function $x \mapsto -\sqrt{x}$ and the expression from Equation (30) are convex, therefore this new expression is convex as well. Wrapping up these developments, we obtain Optimization Problem 2.

B. Model predictive control for 3D balance

Our pipeline for 3D balance is the same as in Section III.
Once the optimal solution $\varphi_1^*, \dots, \varphi_N^*$ of Problem 2 is found,
stiffness and CoP are extracted as:

$$
\omega_i^* = \sqrt{\varphi_N^*} \quad \lambda_i^* = \frac{\varphi_N^* - \varphi_{N-1}^*}{\Delta_{N-1}} \quad \mathbf{p}_i^* = \frac{\dot{\mathbf{c}}_i + \bar{\mathbf{c}}_i \omega_i^*}{F(\omega_i^*)} \quad (47)
$$

In the case of the power law (42), the CoP solution becomes:

$$
\mathbf{p}^* = k \left[ \mathbf{p}_i + \frac{\dot{\mathbf{p}}_i}{\omega_i^*} \right] \quad (48)
$$
<figure><img src="image_3.png" /><figcaption>Fig. 2. **Saturation behavior of the 3D balance controller.** The horizontal distance from CoM to contact is denoted by *d*, *n* is the contact normal and red discs indicate the initial CoP. The controller keeps the CoM trajectory as close as possible to a LIPM via CoP variations. When this is not enough, height variations are resorted to for additional braking.</figcaption></figure>

Interestingly, we recognize here the expression of the capture-point feedback control law [3], [5], [13], under the usual requirement that $k > 1$, even though we are in the context of 3D balance where $\lambda$ and $\omega$ are time-varying.

Problem 2 is solved as fast as its 2D counterpart, on the scale of 1–3 ms, but it is able to cope with both sagittal and lateral velocity compensation. Its solutions tend to be flatter thanks to a wider range of CoP positions. As a matter of fact, by trying to keep the stiffness $\lambda$ as constant as possible, Problem 2 generates a hierarchical strategy: CoP variations are used first; then, if need be, CoM height variations are resorted to. This behavior is depicted in Figure 2.

### C. Discussion

CoM trajectory generation using 6D contacts is a notori- ously nonconvex problem due to angular momentum. Even when a linear model is used and angular momentum is kept constant [5], nonconvexity lingers in feasibility inequality constraints [16]. Nonlinear optimal control has been explored on direct transcriptions of this problem using e.g. multiple shooting [17]; however, our experience in [18] met with frequent solver failures caused by local optimum switches, which required dedicated countermeasures. To avoid such switches, Ponton et al. [19] proposed a convex relaxation of the direct transcription of centroidal dynamics by bounding the convex and concave parts of the angular momentum.

Compared to these previous works, the study we propose here relies on an indirect transcription, i.e. optimization variables are neither the CoM position nor its derivatives. To reduce further the dimension of the problem, we made the CoP trajectory a consequence $p(s) = p_i f(s\omega)$ of the damping profile $\omega$, as opposed to e.g. a shooting method where $p$ and $\omega$ would be optimized jointly. Choosing a linear CoP trajectory also impacts the existence of feasible solutions, for instance when the feedback gain $k$ is too large. An effect that is by the way also present in all capture-point feedback controllers [3], [4], [5], [10], [20].

Compared to e.g. [16], [17], [18], we did not explicitly model frictional constraints in the present study. We observe however, as noted in [7], that the friction force is maximal at the beginning of the balancing trajectory. It can therefore be constrained (if needed) within the presented framework via additional instantaneous CoP inequalities (38).

## V. STEPPING EXPERIMENTS

We implemented both balance controllers<fnref n="3" /> and evaluated them in *pymanoid*<fnref n="4" />, an extension of OpenRAVE for humanoid robotics. Optimization problems were formulated with a spatial discretization $s_j = j/N$ with $N = 10$. They were subsequently solved using the IPOPT solver,<fnref n="5" /> with Jacobians and Hessians computed by automatic differentiation via CasADi.<fnref n="6" /> Note that IPOPT is a general-purpose nonlinear solver designed for large-scale problems, while the problems at hand are small and have additional structure. A dedicated solver can leverage this structure for improved performance [21].

We ran a benchmark over randomized states and contact locations for Problems 1 and 2. Both solvers were fed the same initial states, and sampling was biased toward viable states using the proxy distribution $z_{crit} \sim \frac{1}{2}z_f + \sigma_z U([0, 1])$. On an Intel Core i7-6500U CPU @ 2.50 Ghz, computation times were identical: 1.8 ± 0.7 ms and 1.7 ± 0.6 ms for the 2D and 3D problem, respectively (averages and standard deviations over 10000 control cycles aggregated over 225 launches from different initial states). These times can be improved by two orders of magnitude using the dedicated solver introduced in the extension [21] of the present work.

We next considered a push recovery scenario for an HRP-4 humanoid evolving in a 3D model of an A350 aircraft under construction. Due to e.g. an unexpected collision, or slippage over a ground obstacle that is only detected after momentum has built up, the robot is imparted with an initial velocity of $1.4 \text{ m s}^{-1}$. Although hand contacts would be equally important in such scenarios, we focus here on the stance leg trajectory. First, a foothold is chosen on the fuselage as the kinematically reachable location with the lowest tilting. The velocity in the resulting frame (Figure 1) consists of roughly $1.3 \text{ m s}^{-1}$ in the desired direction of motion $e_x$, $0.2 \text{ m s}^{-1}$ in the lateral direction $e_y$ and $0.5 \text{ m s}^{-1}$ along the vertical $e_z$. We confirmed that the robot is able to stop in 1.5 s using the CoP strategy (42) with $k = 2$ and a 10 cm CoM height variation. The scenario is depicted in Figure 3 and in the accompanying video.

## VI. CONCLUDING NOTE

We saw how the boundedness condition can lead to an alternative optimization for 3D control of the inverted pendulum model. See [21] for an extension of this approach to 3D bipedal walking, addressing follow-up questions such as contact switches and the efficient resolution of the underlying numerical optimization.

## ACKNOWLEDGMENT

We warmly thank Adrien Escande, Leonardo Lanari, Twan Koolen, Michael Posa, Patrick Wensing and Pierre-Brice Wieber for their helpful suggestions, corrections or comments on this work.

<footnote n="3">³ https://github.com/stephane-caron/3d-balance</footnote>

<footnote n="4">⁴ https://github.com/stephane-caron/pymanoid</footnote>

<footnote n="5">⁵ https://projects.coin-or.org/Ipopt</footnote>

<footnote n="6">⁶ http://casadi.org</footnote>
<figure><img src="image_4.png" /><figcaption>Fig. 3. 3D balance control using both CoP and CoM height variations. The robot recovers from an undesired lateral push by stepping onto the fuselage of an A350 aircraft under construction. An initial velocity of 1.4 m s⁻¹ is absorbed in 1.5 s using both the ankle strategy and a 10 cm CoM height variation.</figcaption></figure>

REFERENCES

[1] S. Kajita, F. Kanehiro, K. Kaneko, K. Yokoi, and H. Hirukawa, “The 3d linear inverted pendulum mode: A simple modeling for a biped walking pattern generation,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, vol. 1, 2001.

[2] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” in IEEE-RAS International Conference on Humanoid Robots, 2006.

[3] T. Sugihara, “Standing stabilizability and stepping maneuver in planar bipedalism based on the best com-zmp regulator,” in IEEE-RAS International Conference on Robotics and Automation, 2009.

[4] M. Morisawa, S. Kajita, F. Kanehiro, K. Kaneko, K. Miura, and K. Yokoi, “Balance control based on capture point error compensation for biped walking on uneven terrain,” in IEEE-RAS International Conference on Humanoid Robots, 2012.

[5] J. Englberger, C. Ott, and A. Albu-Schäffer, “Three-dimensional bipedal walking control based on divergent component of motion,” IEEE Transactions on Robotics, vol. 31, no. 2, 2015.

[6] O. E. Ramos and K. Hauser, “Generalizations of the capture point to nonlinear center of mass paths and uneven terrain,” in IEEE-RAS International Conference on Humanoid Robots, 2015.

[7] T. Koolen, M. Posa, and R. Tedrake, “Balance control using center of mass height variation: Limitations imposed by unilateral contact,” in IEEE-RAS International Conference on Humanoid Robots, 2016.

[8] W. Gao, Z. Jia, and C. Fu, “Increase the feasible step region of biped robots through active vertical flexion and extension motions,” Robotica, vol. 35, no. 7, 2017.

[9] J. E. Pratt and S. V. Drakunov, “Derivation and application of a conserved orbital energy for the inverted pendulum bipedal walking model,” in IEEE-RAS Int. Conf. on Robotics and Automation, 2007.

[10] M. A. Hopkins, D. W. Hong, and A. Leonessa, “Humanoid locomotion on uneven terrain using the time-varying divergent component of motion,” in IEEE-RAS International Conf. on Humanoid Robots, 2014.

[11] L. Lanari, S. Hutchinson, and L. Marchionni, “Boundedness issues in planning of locomotion trajectories for biped robots,” in Humanoid Robots, IEEE-RAS International Conference on, 2014.

[12] L. Lanari and S. Hutchinson, “Inversion-based gait generation for humanoid robots,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2015.

[13] T. Takenaka, T. Matsumoto, and T. Yoshiike, “Real time motion generation and control for biped robot-1st report: Walking gait pattern generation,” in IEEE/RSJ Int. Conf. on Int. Robots and Systems, 2009.

[14] P.-B. Wieber, R. Tedrake, and S. Kuindersma, “Modeling and control of legged robots,” in Springer Handbook of Robotics. Springer, 2016.

[15] S. Kajita, M. Benallegue, R. Cisneros, T. Sakaguchi, S. Nakaoka, M. Morisawa, K. Kaneko, and F. Kanehiro, “Biped walking pattern generation based on spatially quantized dynamics,” in IEEE-RAS International Conference on Humanoid Robots, Nov. 2017.

[16] S. Caron and A. Kheddar, “Multi-contact walking pattern generation based on model preview control of 3d com accelerations,” in IEEE-RAS International Conference on Humanoid Robots, 2016.

[17] J. Carpentier, S. Tonneau, M. Naveau, O. Stasse, and N. Mansard, “A versatile and efficient pattern generator for generalized legged locomotion,” in IEEE Int. Conf. on Robotics and Automation, 2016.

[18] S. Caron and A. Kheddar, “Dynamic walking over rough terrains by nonlinear predictive control of the floating-base inverted pendulum,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2017.

[19] B. Ponton, A. Herzog, S. Schaal, and L. Righetti, “A convex model of humanoid momentum dynamics for multi-contact motion generation,” in IEEE-RAS International Conference on Humanoid Robots, 2016.

[20] A. Pajon, S. Caron, G. De Magistris, S. Miossec, and A. Kheddar, “Walking on Gravel with Soft Soles using Linear Inverted Pendulum Tracking and Reaction Force Distribution,” in IEEE-RAS International Conference on Humanoid Robots, 2017.

[21] S. Caron, A. Escande, L. Lanari, and B. Mallein, “Capturability-based analysis, optimization and control of 3d bipedal walking,” 2018, submitted.

APPENDIX

A. Convexity of Equation (30)

Let us first consider the function $g(x, y) = \frac{1}{\sqrt{x+\sqrt{y}}}$. The trace and determinant of its Hessian $\nabla^2 g$ are:<fnref n="7" />

$$ \mathrm{tr}(\nabla^2 g) = \frac{x^2 + 3(x + y)\sqrt{xy} + y^2}{4(xy)^{3/2}(\sqrt{x} + \sqrt{y})^3} \quad (49) $$

$$ \det(\nabla^2 g) = \frac{3}{16(xy)^{3/2}(\sqrt{x} + \sqrt{y})^4} \quad (50) $$

Both are strictly positive quantities over the domain $x, y > 0$, therefore $\nabla^2 g$ is positive definite and $g$ is convex. Consider now the function $G(\varphi)$ defined by Equation (30) over the vector $\varphi$ of positive values $\varphi_1, \dots, \varphi_N: G(\varphi) = \sum_{j=0}^{N-1} \Delta_j g(\varphi_{j+1}, \varphi_j)$. For any $t \in [0, 1]$, $G(t\varphi + (1-t)\varphi')$

$$ \begin{align} & = \sum_{j=0}^{N-1} \Delta_j g(t\varphi_{j+1} + (1-t)\varphi'_{j+1}, t\varphi_j + (1-t)\varphi'_j) \nonumber \\ & \le \sum_{j=0}^{N-1} \Delta_j [tg(\varphi_{j+1}, \varphi_j) + (1-t)g(\varphi'_{j+1}, \varphi'_j)] \tag{51} \\ & = tG(\varphi) + (1-t)G(\varphi') \tag{52} \end{align} $$

Which establishes that $G$ is convex. ■

B. Computing time trajectories

The piecewise constant values of $\lambda(s)$ are directly given by $\lambda_j = (\varphi_{j+1} - \varphi_j)/\Delta_j$. Computing the time trajectory $\lambda(t)$ is then equivalent to finding the switch times $t_j$ such that $s(t_j) = s_j$. Solving the equation of motion (1) of the IPM with constant $\lambda$, one can establish the recurrence relation:

$$ t_j = t_{j+1} + \frac{1}{\sqrt{\lambda_j}} \log \left( \frac{\sqrt{\varphi_{j+1}} + \sqrt{\lambda_j} s_{j+1}}{\sqrt{\varphi_j} + \sqrt{\lambda_j} s_j} \right) \quad (53) $$

The same relation can be applied to find the map $s(t)$, giving $\omega(t) = \omega(s(t)) = [\varphi_j + \lambda_j(s(t)^2 - s_j^2)]^{1/2}s(t)^{-1}$ for $t \in [t_j, t_{j+1}]$ by Equation (26). Alternatively, one can solve the differential equation (3) from $t_j$ to get directly:

$$ \omega(t) = \sqrt{\lambda_j} \frac{1 - v_j \tanh(\sqrt{\lambda_j}(t - t_j))}{v_j - \tanh(\sqrt{\lambda_j}(t - t_j))} \quad (54) $$

where $v_j = \lambda_j/\omega(s_j) = \lambda_j s_j/\varphi_j$.

<footnote n="7">⁷ To avoid painstaking calculations such as this one, we used the on-line computational-knowledge engine WolframAlpha provided by Wolfram Research, Inc.: https://www.wolframalpha.com/</footnote>