Whole-Body Walking Generation using Contact Parametrization:
A Non-Linear Trajectory Optimization Approach

• •

  The $i_{th}$ component of a vector $x$ is denoted as $x_{i}$.

• •

  The transpose operator is denoted by $(\cdot)^{\top}$.

• •

  $\mathcal{I}$ is a fixed inertial frame with respect to (w.r.t.) which the robot’s absolute pose is measured. Its $z$ axis points against gravity, while the $x$ direction points forward.

• •

  Given a function of time $f(t)$ the dot notation denotes the time derivative, i.e. $\dot{f}\mathrel{\mathop{\ordinarycolon}}=\frac{\dif f}{\dif t}$. Higher order derivatives are denoted with a corresponding amount of dots.

• •

  $\mathds{1}_{n}\in\mathbb{R}^{n\times n}$ denotes the identity matrix of dimension $n$.

• •

  $0_{n\times n}\in\mathbb{R}^{n\times n}$ denotes a zero matrix.

• •

  $S(\cdot)$ is the skew-symmetric operation associated with the cross product in $\mathbb{R}^{3}$.

• •

  The weighted L2-norm of a vector $v\in\mathbb{R}^{n}$ is denoted by $\|v\|_{W}$, where $W\in\mathbb{R}^{n\times n}$ is a weight matrix.

• •

  ${}^{A}R_{B}\in SO(3)$ and ${}^{A}H_{B}\in SE(3)$ denote the rotation and transformation matrices which transform a vector expressed in the $B$ frame into one expressed in $A$.

• •

  $\mathbf{n}(\cdot),\mathbb{R}^{3}\rightarrow\mathbb{R}^{3}$ returns the direction normal to the walking plane given the argument’s $x$ and $y$ coordinates.

• •

  $\mathbf{t}(\cdot),\mathbb{R}^{3}\rightarrow\mathbb{R}^{3\times 2}$ returns two perpendicular directions normal to $\mathbf{n}(\cdot)$. The composition of $\mathbf{t}(\cdot)$ and $\mathbf{n}(\cdot)$, $\left[\mathbf{t}(\cdot)~\mathbf{n}(\cdot)\right]$, defines the rotation matrix ${}^{\mathcal{I}}R_{plane}$.

• •

  The function $h(p),\mathbb{R}^{3}\rightarrow\mathbb{R}$ defines the distance between $p$ and the walking surface.

• •

  $e_{1}\mathrel{\mathop{\ordinarycolon}}=[1,0,0]^{\top}$, $e_{2}\mathrel{\mathop{\ordinarycolon}}=[0,1,0]^{\top}$ and $e_{3}\mathrel{\mathop{\ordinarycolon}}=[0,0,1]^{\top}$ denote the canonical basis vectors of $\mathbb{R}^{3}$.

• •

  ${}^{D}V_{A,D}\in\mathbb{R}^{6}$ is the relative velocity between frame $A$ and $D$, whose coordinates are defined in frame $D$.

• •

  $diag(\cdot),\mathbb{R}^{n}\rightarrow\mathbb{R}^{n\times n}$ is a function casting the argument into the corresponding diagonal function.

When performing a step, the foot can impact the ground in a not flat configuration, reducing the amount of contact wrenches obtainable from the ground. At the same time, toe-off motions can be used to increase the work-space available during double support phases [21]. Given these reasons, all the various contact configurations should be taken into account when planning step motions.

In order to reduce the complexity, it is possible to consider the foot as composed by a set of points, for example four points located at the corners of the foot [6, 14]. Thanks to this choice, the several contact configurations can be modeled independently, depending on the number of points in contact.

A pure force can be applied on each contact point. In case of four points, twelve variables define a six dimensional quantity, i.e. the resulting contact wrench acting on the foot. This is a drawback that will be addressed later in Sec. III-D.

Define ${}_{i}p\in\mathbb{R}^{3}$ as the $i-$th contact point location in an inertial frame $\mathcal{I}$, and ${}_{i}f\in\mathbb{R}^{3}$ as the force exerted on that point. Such force is expressed on a frame located in ${}_{i}p$ and with orientation parallel to $\mathcal{I}$. Since it results from the interaction of the foot with the ground, it is subject to constraints. Being a reaction force, its normal component with respect to the walking ground is supposed to be non-negative. In particular, $\mathbf{n}({}_{i}p)^{\top}{}_{i}f\geq 0$. Additionally, in order to avoid slippage, friction constraints should be satisfied:

$$\|\mathbf{t}({}_{i}p)^{\top}{}_{i}f\|\leq\rho~\mathbf{n}({}_{i}p)^{\top}{}_{i}f$$

(1)

where $\rho$ is the static friction coefficient.

The contact force ${}_{i}f$ applied to the $i-$th contact point is supposed to be not-null only if the point is in contact with the walking surface. This condition could be represented by the following equality:

$$h({}_{i}p)~\mathbf{n}({}_{i}p)^{\top}{}_{i}f=0.$$

(2)

Such constraint can be difficult to be tackled in an optimization framework. This is due to the fact that the feasible set is only constituted by two lines, namely $h({}_{i}p)=0$ and $\mathbf{n}({}_{i}p)^{\top}{}_{i}f=0$, which are intersecting in the origin. In this point, the constraint Jacobian gets singular, thus violating the linear independence constraint qualification (LICQ), on which most off-the-shelf solvers rely upon [22].

In order to avoid the complications related to Eq. (2), we adopt a simple parametrization. In particular, we assume to have full control over the derivative of both contact point positions and forces:

	$\displaystyle{}_{i}\dot{p}$	$\displaystyle=$	$\displaystyle u_{{}_{i}p}$		(3a)
	$\displaystyle{}_{i}\dot{f}$	$\displaystyle=$	$\displaystyle u_{{}_{i}f},$		(4a)

where $u_{{}_{i}p},~u_{{}_{i}f}\in\mathbb{R}^{3}$ are control inputs. Then, we can impose Eq. (2) dynamically through the following constraints:

	$\displaystyle-M_{f}\leq$	$\displaystyle u_{{}_{i}f}$	$\displaystyle\leq M_{f}$	if h(_i p) = 0,		(5a)
		$\displaystyle u_{{}_{i}f}$	$\displaystyle=-K_{f}{}_{i}f$	if h(_i p) ≠0.		(6a)

When the point is in contact, $u_{{}_{i}f}$ is free to take any value in $\left[-M_{f},M_{f}\right]$ with $M_{f}\in\mathbb{R}^{3}$ defining control bounds (Eq. (5a)). On the other hand, if the contact point is not on the walking surface, the control input makes the contact force decreasing exponentially (Eq. (6a)). Defining $\delta^{*}({}_{i}p)$ as a binary function such that

$$\delta^{*}({}_{i}p)=\begin{cases}1&\quad\text{if }h({}_{i}p)=0\\ 0&\quad h({}_{i}p)\neq 0\end{cases},$$

(7)

it is possible to write Eq. (5a) as a set of two inequalities:

	$\displaystyle-K_{f}\left(1-\delta^{*}({}_{i}p)\right){}_{i}f-\delta^{*}({}_{i}p)M_{f}$	$\displaystyle\leq$	$\displaystyle u_{{}_{i}f}$		(8a)
	$\displaystyle-K_{f}\left(1-\delta^{*}({}_{i}p)\right){}_{i}f+\delta^{*}({}_{i}p)M_{f}$	$\displaystyle\geq$	$\displaystyle u_{{}_{i}f}.$		(9a)

Even if $\delta^{*}({}_{i}p)$ would require the adoption of integer variables, it is possible to use a continuous approximation, $\delta({}_{i}p)$, namely the hyperbolic secant:

$$\delta({}_{i}p)=\text{sech}\left(\mathbf{k_{h}}h({}_{i}p)\right),$$

(10)

where $\mathbf{k_{h}}$ is a user-defined scaling factor. Since $\delta({}_{i}p)=0$ only when ${}_{i}p\rightarrow\infty$, Eq. (8a) satisfies the LICQ condition.

Given Eq. (1), it is enough to apply Eq. (8a) only to the force component normal to the ground: if it decreases to zero, also planar force components have to vanish.

Since contact points are not supposed to penetrate the walking ground, we can impose $h({}_{i}p)\geq 0$.

None of the constraints defined above could prevent the contact points to move on the walking plane when in contact. In fact, even if friction constraints defined in Eq. (1) are satisfied, the contact points are still free to move on the contact surface. Force and position variables are (almost) independent at this stage. It is possible to prevent planar motions when in contact by limiting the effect of the control input $u_{{}_{i}p}$ along the planar components:

$$\mathbf{t}({}_{i}p)^{\top}{}_{i}\dot{p}=\tanh\left(\mathbf{k_{t}}h({}_{i}p)\right)\left[e_{1}~e_{2}\right]^{\top}u_{{}_{i}p}$$

(11)

where $\mathbf{k_{t}}\in\mathbb{R}$ is a user-defined scaling factor. Eq. (11) multiplies the control input along the planar direction to zero when $h({}_{i}p)$ is null and, at the same time, it will reduce the velocity when the contact point is approaching the ground. It is possible to rewrite Eq. (11) as

$${}_{i}\dot{p}=\tau({}_{i}p)u_{{}_{i}p},$$

(12)

where the function $\tau(\cdot)\mathrel{\mathop{\ordinarycolon}}\mathbb{R}^{3}\rightarrow\mathbb{R}^{3\times 3}$ is defined as:

$$\tau({}_{i}p)={}^{\mathcal{I}}R_{plane}~diag\left(\begin{bmatrix}\tanh\left(\mathbf{k_{t}}h({}i_{p})\right)\\ \tanh\left(\mathbf{k_{t}}h({}i_{p})\right)\\ 1\end{bmatrix}\right).$$

(13)

Note that, from now on, $u_{{}_{i}p}$ is assumed to be defined in $plane$ coordinates. Thus, the normal component of the velocity is directly affected by $e_{3}^{\top}u_{{}_{i}p}$. Also, it is necessary to bound this control input, $u_{{}_{i}p}\in\left[-M_{V},M_{V}\right],M_{V}\in\mathbb{R}^{3}$, to properly exploit the effect of the hyperbolic tangent. Note that Eq. (11) allows avoiding the use of complementarity conditions along planar directions.

While each contact point is supposed to be independent from the control point of view, they all need to remain on the same surface and maintain a constant relative distance, since they belong to the same rigid body. At the same time, we want them to be within the workspace reachable by the robot legs. We can achieve both the objectives by enforcing the following constraint on each of the contact points:

$${}_{i}p={}^{\mathcal{I}}H_{\text{foot}}{}^{\text{foot}}{}_{i}p,$$

(14)

where ${}^{\text{foot}}{}_{i}p$ is the (fixed) position of the contact point within the foot surface, expressed in foot coordinates. Here, the transformation matrix ${}^{\mathcal{I}}H_{\text{foot}}$ would depend on the base position ${}^{\mathcal{I}}p_{B}\in\mathbb{R}^{3}$, the base quaternion ${}^{\mathcal{I}}\rho_{B}\in\mathbb{H}$ and the joints configuration $s\in\mathbb{R}^{n}$, with $n$ being the number of joints. As a consequence, the full kinematics of the robot is taken into consideration and the following dynamic equations have to be considered:

	$\displaystyle{}^{\mathcal{I}}\dot{p}_{b}$	$\displaystyle=$	$\displaystyle{}^{\mathcal{I}}R_{B}{}^{B}v_{\mathcal{I},B}$		(15a)
	$\displaystyle{}^{\mathcal{I}}\dot{\rho}_{B}$	$\displaystyle=$	$\displaystyle u_{\rho}$		(16a)
	$\displaystyle\dot{s}$	$\displaystyle=$	$\displaystyle u_{s}.$		(17a)

Here ${}^{B}v_{\mathcal{I},B}\in\mathbb{R}^{3}$, $u_{\rho}\in\mathbb{R}^{4}$ and $u_{s}\in\mathbb{R}^{n}$ are considered control inputs defining the base linear velocity, the quaternion derivative and the joints velocity, respectively. More specifically, ${}^{B}v_{\mathcal{I},B}$ is the linear part of ${}^{B}V_{\mathcal{I},B}\in\mathbb{R}^{6}$ the left-trivialized (i.e. measured in body coordinates) base velocity.

In Sec. II-A, we consider the contact points as if they have the possibility of exerting a force with the environment. We can describe the effect of these contact forces through the momentum, or centroidal, dynamics. This choice is supported by the fact that the momenutm dynamics depends only on the contact forces, their location and on the center of mass (CoM) position, $x_{\text{CoM}}\in\mathbb{R}^{3}$. Define ${}_{\bar{G}}h\in\mathbb{R}^{6}$ as the robot total momentum, with ${}_{\bar{G}}h=\left[{}_{\bar{G}}h^{p\top}~{}_{\bar{G}}h^{\omega\top}\right]^{\top}$ where ${}_{\bar{G}}h^{p}\in\mathbb{R}^{3}$ and ${}_{\bar{G}}h^{\omega}\in\mathbb{R}^{3}$ are respectively the linear and angular momentum. This quantity is expressed in a frame oriented as the inertial frame $\mathcal{I}$, with the origin placed on the CoM position. Such frame is called $G[\mathcal{I}]$ or simply $\bar{G}$. The momentum dynamics has the following form:

	$\displaystyle{}_{\bar{G}}\dot{h}$	$\displaystyle=$	$\displaystyle m\bar{g}+\sum_{i}\begin{bmatrix}\mathds{1}_{3}\\ S({}_{i}p-x_{\text{CoM}})\end{bmatrix}{}_{i}f$		(18a)
	$\displaystyle\dot{x}_{\text{CoM}}$	$\displaystyle=$	$\displaystyle\frac{1}{m}\left({}_{\bar{G}}h^{p}\right)$		(19a)

with $m$ the total mass of the robot, $\bar{g}=\left[\begin{smallmatrix}0&0&-g&0&0&0\end{smallmatrix}\right]^{T}$. We also need to make sure that the integrated CoM corresponds to the one obtained via the joint variables. This is done through an additional constraint:

$$x_{\text{CoM}}=\text{CoM}({}^{\mathcal{I}}p_{B},{}^{\mathcal{I}}\rho_{B},s)$$

(20)

where $\text{CoM}({}^{\mathcal{I}}p_{B},{}^{\mathcal{I}}\rho_{B},s)$ is the function mapping base pose and joint positions to the CoM position. While this constraint defines a link between the linear momentum and the joint variables, the same would not hold for the angular part. To this end, we can exploit the Centroidal Momentum Matrix [5] ($J_{\text{CMM}}$). In fact, the robot angular momentum can be defined as a function of the base and joints velocity:

$${}_{\bar{G}}h^{\omega}=\left[0_{3\times 3}~\mathds{1}_{3}\right]J_{\text{CMM}}\nu$$

(21)

where $\nu=\left[{}^{B}V_{\mathcal{I},B}~u_{s}\right]^{\top}$. Here, the base angular velocity ${}^{B}\omega_{\mathcal{I},B}$ can be substituted with the quaternion derivative through the map $\mathcal{G}$ [23, Section 1.5.4], such that

$${}^{B}\omega_{\mathcal{I},B}=2\mathcal{G}({}^{\mathcal{I}}\rho_{B})u_{\rho}.$$

Some additional constraints can be considered:

	$\displaystyle x_{\text{CoM},z\text{ min}}$	$\displaystyle\leq$	$\displaystyle e_{3}^{\top}{x_{\text{CoM}}}$		(22a)
	$\displaystyle-M_{h_{\omega}}$	$\displaystyle\leq$	$\displaystyle{}_{\bar{G}}h^{\omega}\leq M_{h_{\omega}}$		(23a)

Eq (22a) avoids solutions which would bring the CoM position too close or below the ground. Eq. (23a) provides an upper and lower bound $M_{h_{\omega}}\in\mathbb{R}^{3}$ to the angular momentum. These constraints avoid trajectories that would cause excessive motions or let the robot falling.

While taking steps, we need to make sure that the robot legs do not collide with each other. Self collision constraints are usually hard to be considered and may slow down consistently the determination of a solution. A simpler solution consists in avoiding the left leg to be on the right of the other leg. Consequently, cross steps are forbidden. Let us consider a frame attached to the right foot with the positive $y-$direction pointing toward left. In this case, it is sufficient to impose the $y-$component of the ${}^{r}x_{l}$ (i.e. the relative position of the left foot expressed in the right foot frame) to be greater than a given quantity, i.e. $e_{2}^{\top}{}^{r}x_{l}\geq d_{\text{min}}$.

In order to make the robot moving toward a desired position, we minimize the L2 norm of the error between a point attached to the robot and its desired position in an absolute frame. In particular, we select the centroid of the contact points as target, thus avoiding to specify a desired placement for each foot:

$$\Gamma_{{}_{\#}p}=\frac{1}{2}\|{}_{\#}p-{}_{\#}p^{*}\|^{2}_{W_{\#}}$$

(24)

where $\#$ is the number of contact points in a single foot. Thus, we have ${}_{\#}p=\frac{1}{2\#}\sum_{l,r}\sum^{\#}_{i}{}_{i}p$ and ${}_{\#}p^{*}\in\mathbb{R}^{3}$ is a user-defined reference value.

While walking, we want the robot to keep a constant forward motion. In fact, since the positions of the feet are not scripted, it may be possible to plan two consecutive steps with the same foot. By requiring a constant forward velocity, such phenomena can be avoided. This task is defined as:

$$\Gamma_{{}_{\bar{G}}h^{p}}=\frac{1}{2}\|{}_{\bar{G}}h^{p}-mv_{G}^{*}\|^{2}_{W_{v}}$$

(25)

with $v_{G}^{*}\in\mathbb{R}^{3}$ a desired CoM velocity. The matrix $W_{v}$ selects and weights the different directions separately.

While moving, we want a robot frame to be oriented in a specific orientation ${}^{\mathcal{I}}R^{*}_{\text{frame}}$. In particular, we weight the distance of the rotation matrix ${}^{\mathcal{I}}\tilde{R}_{\text{frame}}={}^{\mathcal{I}}R^{*\top}_{\text{frame}}{}^{\mathcal{I}}R_{\text{frame}}$ from the identity. Having to express this task in vector form, we convert ${}^{\mathcal{I}}\tilde{R}_{\text{frame}}$ into a quaternion (through the function quat which implements the Rodriguez formula) and weight its difference from the identity quaternion $I_{q}$. Namely:

$$\Gamma_{\text{frame}}=\frac{1}{2}\left\|\texttt{quat}\left({}^{A}\tilde{R}_{\text{frame}}\right)-I_{q}\right\|^{2}.$$

(26)

This corresponds to a simplified version of the quaternion difference metric listed in [24], under the assumption for quat to always return a quaternion with positive real value.

While considering each single contact force in a foot as independent, they still belong to a single body part. Thus, we prescribe the contact forces in a foot to be as similar as possible, refraining from using partial contacts if not strictly necessary. This can be obtained through the following:

$$\Gamma_{\text{reg}f}=\sum_{l,r}\sum^{\#}_{i}\frac{1}{2}\left\|{}_{i}f-\frac{1}{\#}\sum^{\#}_{j}{}_{j}f\right\|^{2}.$$

(27)

The joint configuration $s$ is part of the optimization variables. In order to prevent the planner from providing solutions with huge joint variations, we introduce a regularization task for joint variables:

$$\Gamma_{\text{reg}s}=\frac{1}{2}\left\|\dot{s}+K_{s}(s-s^{*})\right\|^{2}_{W_{j}}$$

(28)

with $s^{*}$ the desired joint configurations and $W_{j}$ a weight matrix. The minimum of this cost is when $\dot{s}=-K_{s}(s-s^{*})$, with $K_{s}\in\mathbb{R}^{n\times n}$. When this equality holds, joint values converge exponentially to their reference $s^{*}$. Hence, joint velocities and joint positions are regularized at the same time.

When performing a step, the swing foot clearance usually ensures some level of robustness with respect to ground asperity. Nevertheless, since the soil profile is supposed to be known in advance, a solution satisfying all the equations described in Sec. II may require the swing foot to be raised just few millimeters from the ground. In order to specify a desired swing height, we impose the following cost:

$$\Gamma_{\text{swing}}=\sum_{l,r}\sum^{\#}_{i}\frac{1}{2}\left\|\left(e_{3}^{\top}{}_{i}p-{}_{s}h^{*}\right)\left[e_{1}~e_{2}\right]^{\top}u_{{}_{i}p}\right\|.$$

(29)

It penalizes the distance between the $z$-component of each contact point position from a desired height ${}_{s}h^{*}\in\mathbb{R}$ when the corresponding planar velocity is not null. Trivially, this cost has two minima: when the planar velocity is zero (thus the point is not moving) or when the height of the point is equal to the desired one.

	$\displaystyle\minimize_{\mathcal{X},\mathcal{U}}$	$\displaystyle\left[\Gamma_{{}_{\#}p}~\Gamma_{{}_{\bar{G}}h^{p}}~\Gamma_{\text{frame}}~\Gamma_{\text{reg}f}~\Gamma_{\text{reg}s}~\Gamma_{\text{swing}}\right]^{\top}\mathbf{w}$
	subject to :

$\bullet$ Dynamical Constraints

	$\displaystyle{}_{i}\dot{f}$	$\displaystyle=$	$\displaystyle u_{{}_{i}f}$	∀ contact point		(30a)
	$\displaystyle{}_{i}\dot{p}$	$\displaystyle=$	$\displaystyle\tau({}_{i}p)u_{{}_{i}p}$	∀ contact point		(31a)
	$\displaystyle{}_{\bar{G}}\dot{h}$	$\displaystyle=$	m ¯ g + ∑_i [ 1 3 S( i p - x CoM ) ] _if			(32a)
	$\displaystyle\dot{x}_{\text{CoM}}$	$\displaystyle=$	1 m (_ ¯ G h^p)			(33a)
	$\displaystyle{}^{\mathcal{I}}\dot{p}_{B}$	$\displaystyle=$	$\displaystyle{}^{\mathcal{I}}R_{B}{}^{B}v_{\mathcal{I},B}$			(34a)
	$\displaystyle{}^{\mathcal{I}}\dot{\rho}_{B}$	$\displaystyle=$	$\displaystyle u_{\rho}$			(35a)
	$\displaystyle\dot{s}$	$\displaystyle=$	$\displaystyle u_{s}$			(36a)

$\bullet$ Equality Constraints

	$\displaystyle{}_{i}p$	$\displaystyle=$	$\displaystyle{}^{A}H_{\text{foot}}{}^{\text{foot}}{}_{i}p\quad\forall\text{ contact point}$		(37a)
	$\displaystyle x_{\text{CoM}}$	$\displaystyle=$	$\displaystyle\text{CoM}({}^{\mathcal{I}}p_{B},{}^{\mathcal{I}}\rho_{B},s)$		(38a)
	$\displaystyle{}_{\bar{G}}h^{\omega}$	$\displaystyle=$	$\displaystyle\left[0_{3\times 3}~\mathds{1}_{3}\right]J_{\text{CMM}}\begin{bmatrix}{}^{B}v_{\mathcal{I},B}\\ 2\mathcal{G}({}^{\mathcal{I}}\rho_{B})u_{\rho}\\ u_{s}\end{bmatrix}$		(39a)
	$\displaystyle\|{}^{\mathcal{I}}\rho_{B}\|^{2}$	$\displaystyle=$	$\displaystyle 1$		(40a)

$\bullet$ Inequality Constraints

	$\displaystyle\mathbf{n}({}_{i}p)^{\top}{}_{i}f$	$\displaystyle\geq$	$\displaystyle 0$		(41a)
	$\displaystyle\|\mathbf{t}({}_{i}p)^{\top}{}_{i}f\|$	$\displaystyle\leq$	$\displaystyle\rho~\mathbf{n}({}_{i}p)^{\top}{}_{i}f$		(42a)
	$\displaystyle u_{{}_{i}f}$	$\displaystyle\geq$	$\displaystyle-K_{f}\left(1-\delta({}_{i}p)\right){}_{i}f-\delta({}_{i}p)M_{f}$		(43a)
	$\displaystyle u_{{}_{i}f}$	$\displaystyle\leq$	$\displaystyle-K_{f}\left(1-\delta({}_{i}p)\right){}_{i}f+\delta({}_{i}p)M_{f}$		(44a)
	$\displaystyle-M_{V}$	$\displaystyle\leq$	$\displaystyle u_{{}_{i}p}\leq M_{V}$		(45a)
	$\displaystyle h({}_{i}p)$	$\displaystyle\geq$	$\displaystyle 0$		(46a)
	$\displaystyle e_{2}^{\top}{{}^{r}x_{l}}$	$\displaystyle\geq$	$\displaystyle d_{\text{min}}$		(47a)
	$\displaystyle x_{\text{CoM},z\text{ min}}$	$\displaystyle\leq$	$\displaystyle e_{3}^{\top}{x_{\text{CoM}}}$		(48a)
	$\displaystyle-M_{h_{\omega}}$	$\displaystyle\leq$	$\displaystyle{}_{\bar{G}}h^{\omega}\leq M_{h_{\omega}}$		(49a)

Here, the state variables $\mathcal{X}$ are those derived in time, $\mathcal{U}$ all the others. More specifically:

$$\mathcal{X}=\begin{bmatrix}{}_{i}f\\ {}_{i}p\\ \vdots\\ {}_{\bar{G}}h\\ x_{\text{CoM}}\\ {}^{\mathcal{I}}p_{B}\\ {}^{\mathcal{I}}\rho_{B}\\ s\end{bmatrix},\quad\mathcal{U}=\begin{bmatrix}u_{{}_{i}f}\\ u_{{}_{i}p}\\ \vdots\\ {}^{B}v_{\mathcal{I},B}\\ u_{\rho}\\ u_{s}\end{bmatrix}$$

(50)

where the symbol $\vdots$ represents the repetition of variables for each contact point. The optimal control problem is solved using a Direct Multiple Shooting method [22]. The system dynamics, defined in Eq. (30a), is discretized adopting an implicit trapezoidal method with a fixed integration step. The corresponding optimization problem is solved thanks to Ipopt [25].

The walking trajectories are generated using the Receding Horizon Principle [26], adopting a fixed prediction window.

The optimal control problem described in Sec. IV-A is built such that (almost) no constraint is task specific. As a consequence, it is particularly important to define the cost function carefully since the solution will be a trade-off between all the various tasks. On the other hand, the detailed model of the system allows achieving walking motions without specifying a desired CoM trajectory or by fixing the angular momentum to zero. Nevertheless, due to the limited time horizon, it is better to prevent the solver from finding solutions which would bring the robot to unfeasible states in future planner iterations. To this end, Eq. (22a) and Eq. (23a) have been added, using reasonably large bounds.

Another possible effect resulting from the application of the Receding Horizon principle, is the emergence of “procrastination” phenomena. Due to the moving horizon, the solver may continuously delay in actuating motions, since the task keeps being shifted in time. A simple fix to this phenomena is to increase the weights $\mathbf{w}$ with time, such that it is more convenient to reach a goal position earlier.

Finally, given that the problem under consideration is non-convex, the optimizer will find a local minimum. This may result in a sub-optimal solution for the given tasks.

During the first iteration, the solver is initialized by simply translating the whole robot in the desired position. In successive iterations, the solver is warm-started with the solution previously computed.