Simultaneous Contact, Gait and Motion Planning for Robust Multi-Legged Locomotion via Mixed-Integer Convex Optimization

TL;DR

This paper introduces a mixed-integer convex optimization approach for multi-legged robot locomotion that plans contact, gait, and motion simultaneously, enabling robust and efficient traversal of complex terrains without flat terrain or fixed gait assumptions.

Contribution

It presents a novel convex formulation that couples contact, gait, and motion planning, overcoming limitations of previous methods requiring fixed gaits or flat terrains.

Findings

Successfully validated on HyQ robot across challenging terrains

Achieves robust locomotion with low computation time

Increases motion planning flexibility and generality

Abstract

Traditional motion planning approaches for multi-legged locomotion divide the problem into several stages, such as contact search and trajectory generation. However, reasoning about contacts and motions simultaneously is crucial for the generation of complex whole-body behaviors. Currently, coupling theses problems has required either the assumption of a fixed gait sequence and flat terrain condition, or non-convex optimization with intractable computation time. In this paper, we propose a mixed-integer convex formulation to plan simultaneously contact locations, gait transitions and motion, in a computationally efficient fashion. In contrast to previous works, our approach is not limited to flat terrain nor to a pre-specified gait sequence. Instead, we incorporate the friction cone stability margin, approximate the robot's torque limits, and plan the gait using mixed-integer convex constraints. We experimentally validated our approach on the HyQ robot by traversing different challenging terrains, where non-convexity and flat terrain assumptions might lead to sub-optimal or unstable plans. Our method increases the motion generality while keeping a low computation time.

Tables

Table 1. TABLE I : Variables of the trajectory optimization

$𝐫={[r_x,r_y,r_z]}^T$	Center of Mass (CoM) in the world frame
$𝐤={[k_x,k_y,k_z]}^T$	Centroidal angular momentum
${𝐩}_l={[p_{l_x},p_{l_y},p_{l_z}]}^T$	End-effector positions in the world frame
${𝝀}_l={[{\lambda }_{l_x},{\lambda }_{l_y},{\lambda }_{l_z}]}^T$	Contact force at the $l^{th}$ end-effector
$N_t,N_k$	Number of time-slots and knots per swing
$N_f,N_r$	Number of contacts and convex regions

Table 2. TABLE II : Computation time for a locomotion cycle for various terrain conditions and gaits

Experiment	convex surfaces	Gait	mean time (s)
Exp. 1	3	Walk	0.47
Exp. 2	3	Walk	0.64
Exp. 3	4	Walk	0.44
Exp. 4	3	Walk	0.48
Exp. 4	3	Trot	0.51
Exp. 4	3	Free	1.62

Equations

$$\left[\begin{array}{c}m\ddot{\mathbf{r}}\\ \dot{\mathbf{k}}\end{array}\right]=\left[\begin{array}{c}m\mathbf{g}+{\sum }_{l=1}^{n_l}{\mathbf{\lambda }}_l\\ {\sum }_{l=1}^{n_l}({\mathbf{p}}_l-\mathbf{r})\times {\mathbf{\lambda }}_l\end{array}\right],$$

$$\sum _{j=1}^{N_t}{\mathbf{T}}_{ij}=1,\forall i=1,..,N_f.$$

$${\mathbf{t}}_j>{\mathbf{t}}_{j-n_l},\forall j=n_l+1,\dots ,N_t.$$

$$\mathbf{f}=(f_x,f_y,f_z,\theta ),$$

$$\sum _{\mathrm{r}=1}^{N_r}{\mathbf{H}}_{i\mathrm{r}}=1,$$

$${\mathbf{H}}_{i\mathrm{r}}\Rightarrow A_{\mathrm{r}}{\mathbf{f}}_i\le b_r,$$

$$\left|{\mathbf{f}}_i-\left[{\mathbf{r}}_{T(i)}+L_i\left(\begin{array}{c}\cos ({\theta }_i+{\varphi }_i)\\ \sin ({\theta }_i+{\varphi }_i)\end{array}\right)\right]\right|\le d_{lim},$$

$$\sum _{s=1}^{N_s}{\mathbf{S}}_{is}=1\sum _{s=1}^{N_s}{\mathbf{C}}_{is}=1,$$

$${\mathbf{S}}_{ik}\Rightarrow \{\begin{array}{l}{\psi }_{k-1}\le {\theta }_i\le {\psi }_k\\ s_i=m_{s_k}{\theta }_i+n_{s_k}\end{array}{\mathbf{C}}_{ik}\Rightarrow \{\begin{array}{l}{\gamma }_{k-1}\le {\theta }_i\le {\gamma }_k\\ c_i=m_{c_k}{\theta }_i+n_{c_k}\end{array},$$

$${\mathbf{T}}_{ij}\Rightarrow {\mathbf{p}}_{l(i)\gamma (j,N_k)}={\mathbf{f}}_i,$$

$$\sum _{i\in C(l)}{\mathbf{T}}_{ij}=0\Rightarrow {\mathbf{p}}_{l\gamma (j,t)}={\mathbf{p}}_{l\gamma (j,1)}\forall t\in [2,\dots ,N_k],$$

$${\text{d}}_{-}<{\mathbf{r}}_j-\frac{{\sum }_{l=1}^{n_l}{\mathbf{p}}_{lj}}{n_l}<{\text{d}}_{+},$$

$$\sum _{i\in C(l)}{\mathbf{T}}_{ij}=1\Rightarrow {\mathbf{\lambda }}_{l\gamma (j,t)}=0,\forall t\in NC(j),$$

$${\mathbf{\lambda }}_{lj}\in {\mathbb{FC}}_{\mathrm{r}}\Rightarrow {\mathbf{\lambda }}_{lj}=\sum _{e=1}^{N_e}{\rho }_e{\mathbf{v}}_{{\mathrm{r}}_e},{\rho }_1,\dots ,{\rho }_{N_e}>0,$$

$$\alpha =\text{arg}\underset{\bar{\alpha }}{\max }\text{ s.t }{\mathbf{\lambda }}_{lj}-\bar{\alpha }{\hat{\mathbf{n}}}_{\mathrm{r}}\in {\mathbb{FC}}_{\mathrm{r}},$$

$${\mathbf{T}}_{ij}\text{and}{\mathbf{H}}_{ri}\Rightarrow {\mathbf{\lambda }}_{l(i)\gamma (j)}-{\alpha }_{l(i)\gamma (j)}{\hat{\mathbf{n}}}_{\mathrm{r}}\in {\mathbb{FC}}_{\mathrm{r}},\alpha \ge 0.$$

$$\sum _{l\in C(i)}\sum _{t\in NS(j)}{\mathbf{T}}_{lt}=0,$$

$$\Rightarrow {\mathbf{\lambda }}_{l(i)\gamma (j)}-{\alpha }_{l(i)\gamma (j)}{\mathbf{n}}_{\mathrm{r}}\in {\mathbb{FC}}_{\mathrm{r}},$$

$$\mathbf{ab}=\frac{{\mathbf{u}}^{+}-{\mathbf{u}}^{-}}{4}{\mathbf{u}}^{+}\ge (\mathbf{a}+\mathbf{b}{)}^2{\mathbf{u}}^{-}\ge (\mathbf{a}-\mathbf{b}{)}^2.$$

$${\mathbf{J}}_{l,j}^T{\mathbf{\lambda }}_{l,j}\le {\mathbf{\tau }}_{max},$$

$${{\mathbf{J}}^{\ast }}_{l,j}^T{\mathbf{\lambda }}_{l,j}\le {\mathbf{\tau }}_{max},$$

$$\underset{\mathbf{r},{\mathbf{k}}_o,{\mathbf{p}}_l,{\mathbf{\lambda }}_l}{\min }{g}_T+\sum _{k=1}^{N}g(k),$$

$$g(k)=\Vert {\ddot{\mathbf{r}}}_k{\Vert }_{{\mathbf{Q}}_v}+\Vert {\mathbf{\lambda }}_{l,k}{\Vert }_{{\mathbf{Q}}_F}+q_u{\mathbf{U}}_k+q_t{t}_k-q_{\alpha }{\alpha }_k,$$

$$g_T=\Vert {\mathbf{r}}_T-{\mathbf{r}}_G{\Vert }_{{\mathbf{Q}}_g},$$

$${\ddot{\mathbf{r}}}^r$$

$${\dot{\mathbf{\omega }}}_b^r$$

$${\mathbf{x}}^{\ast }=$$

$$s.t.$$

$$\underline{\mathbf{d}}<\mathbf{Cx}<\bar{\mathbf{d}}$$

$$g_{err}(\mathbf{x})=\left\|\begin{array}[]{c}\ddot{\mathbf{r}}-\ddot{\mathbf{r}}^{r}\\ \dot{\boldsymbol{\omega}}_{b}-\dot{\boldsymbol{\omega}}_{b}^{r}\end{array}\right\|_{\mathbf{S}},$$

$$\mathbf{G}$$

$${\mathbf{\tau }}_{ff}^{\ast }=\left[\begin{array}{cc}{\mathbf{M}}_{bj}^T& {\mathbf{M}}_j\end{array}\right]{\ddot{\mathbf{q}}}^{\ast }+{\mathbf{h}}_j-{\mathbf{J}}_{cj}^T{\mathbf{\lambda }}^{\ast }$$

Full text

I Introduction

Planning motions for multi-legged robots can be a challenging task, as it involves both the discrete choice of the gait sequence, and continuous decisions on foot location and robot dynamics. Because of this, most traditional approaches [1, 2, 3, 4] simplify the planning problem by decoupling it into two stages: 1) find a sequence of contact locations; and 2) generate body trajectories based on a stability metric, like the Zero Moment Point (ZMP) [5] or the generalized Contact Wrench Cone (CWC) [6, 7] stability criterion. Ignoring the dynamics in the first stage might restrict the number of feasible motion on the second one, when dynamics are considered. Hence, in this paper we combine these two stages into one single problem, in order to efficiently generate complex behaviors.

Trajectory Optimization (TO) has emerged as a solution to the coupled motion planning problem [8, 9, 10, 11, 12]. Unfortunately, even state of the art approaches are often restricted to simple environments, a fixed gait, or require intractable computation time. Furthermore, none of them can guarantee global optimality, and can easily fall into local minima, since these are posed as non-convex optimization problems. On the other hand, Mixed-Integer Convex Programming (MICP) proved to be an efficient tool to solve multi-contact motion planning problems. In [1, 3], the authors first find a set of safe surfaces, and then use integer variables to assign each contact to one surface. Further works incorporate a convex or mixed-integer convex dynamic model in the formulation [13, 14], which can efficiently optimize contacts and motions. Unfortunately, these approaches do not incorporate torque constraints nor maximize any generalized stability margin. Moreover, they do not reason about the sequence of contacts, being limited to a fixed gait sequence.

In this work, we introduce a novel MICP formulation for non-gaited multi-legged locomotion on challenging terrain. Here, we formulate a single mixed-integer convex optimization problem that simultaneously plans contacts and motions. In contrast to other approaches, our method guarantees dynamic feasibility and optimizes gaits sequences in challenging terrains. For this, we incorporate convex representations of friction cones, torque limits, and gait planning constraints. For the friction cone constraints, we exploit the segmentation of the terrain and pose the stability margin computation as a convex constraint. This optimization problem can then be solved efficiently to its global optimum, using off-the-shelf optimization solvers. We validated our approach by traversing different challenging terrains with the Hydraulically actuated Quadruped (HyQ) robot, shown in Fig. 1. Our approach generates robust motions, even during gait transitions and non-coplanar contact scenarios.

The rest of this paper is organized as follows: Section II presents our simultaneous contact and motion planning approach. Section III presents our whole-body controller. Section IV presents a set of experiments on HyQ traversing challenging terrains, and Section V discusses and concludes on the contributions of this work.

II Simultaneous Contact and Motion Planning

In this section, we describe our formulation of the simultaneous contact and motion planning problem using Mixed-Integer Convex Optimization.

II-A Approach Overview

Let us consider a robot with $n_{l}$ legs and a locomotion plan, discretized on $N$ time-knots. We formulate a MICP to find the optimal contact locations and motions, given a goal position $\mathbf{r}_{G}$ and a convex segmentation of the terrain. An illustration of this approach is shown in Fig. 2.

We describe the dynamic evolution of the system with a centroidal model (Section II-B). We include gait planning as part of the optimization (Section II-C), describing the motion through time-slots over which legs swing. This evolution is subject to robot and environmental constraints such as: approximated kinematic reachability (Section II-D), friction cone constraints (Section II-F), and approximated torque limits (Section II-H). describing the motion through time-slots over which we will generate leg swings. We adopt the formulation in [13] to further address the non-linearities of the angular dynamics by relying on a convex decomposition of bilinear terms, making our optimization problem convex (Section II-G).

II-B Centroidal Dynamics

We describe the evolution of the system using a centroidal dynamics model [15, 16], as depicted in Fig. 3. In this case, the dynamics of the system are written as:

[TABLE]

where variables are named as specified in Table I, $m$ is the mass of the robot, and g is the gravity vector. This formulation of the dynamics is entirely convex, except for the cross product to compute the torque at center of mass.

Remark 1: Note that (1) neglects the effect of contact torques. This is done because most multi-legged platforms have approximate point contacts, without support surface to generate moments.

Furthermore, the dynamics of the model are discretized in knots of $\Delta t$ seconds, over which we will perform backward Euler integration.

II-C Gait Sequence

In order to plan gait transitions, we represent the motion through $N_{f}$ contact locations and $N_{t}$ time-slots over which the legs will swing between adjacent contacts , and introduce a binary transition matrix $\mathbf{T}\in\{0,1\}^{N_{f}\times N_{t}}$. Here, $\mathbf{T}_{ij}=1$ means that the robot will move to the $i^{th}$ contact location at the $j^{th}$ time-slot, as shown in Fig. 4. Since each contact location in the plan is reached once, we enforce that:

[TABLE]

Additionally, we enforce that each cycle of $n_{l}$ contacts must be reached before the next transfer cycle starts. For this, we define a vector $\mathbf{t}=\mathbf{T}\begin{bmatrix}1&\ldots&N_{t}\end{bmatrix}^{T}$ which computes the corresponding slot assigned to each movement. Furthermore, since the gait follows a sequentially ordered contact plan, we enforce the following constraint:

[TABLE]

Terrain heuristics can also be added in order to guide the gait through different environments. These can be incorporated in the formulation as shown in [3].

II-D Contact Location

In order to simplify the formulation, we only optimize the contact locations and contact timing, leaving the end-effector trajectory as an interpolation between adjacent contacts in the plan. Since swing phases do not contribute to the centroidal dynamics, as force in the leg becomes null, this simplification has no effect on the resulting plan.

As part of the decision variables, we describe contact locations for $n_{l}$ end-effectors using an array of $N_{f}$ vectors in $\mathbb{R}^{4}$, ordered by end-effector number, of the form:

[TABLE]

representing the position of each contact in Cartesian space and the yaw orientation of the trunk when transitioning to that contact, neglecting roll and pitch positions.

II-D1 Safe-region assignment

Here, we will invert the problem of avoiding obstacles by constraining the contacts to lie within one of $N_{r}$ convex safe contact surfaces, shown colored in Fig. 5 (left). Each surface is represented as a polygon $\mathcal{R}=\{\mathbf{c}\in\mathbb{R}^{3}|A_{\mathrm{r}}\mathbf{c}\leq b_{\mathrm{r}}\}$. The assignment of contacts to these surfaces is done through a binary decision matrix $\mathbf{H}\in\{0,1\}^{N_{f}\times N_{r}}$. The constraints, for the $i^{th}$ contact, are:

[TABLE]

[TABLE]

where the $\Rightarrow$ (implies) operator is represented with big-M formulation [17]. Such surfaces can be easily obtained with segmentation algorithms.

II-D2 Kinematic constraints

In order to ensure kinematic reachability, we must account for the workspace of each independent leg. To do this, we constrain each contact location within the biggest square inscribed in the leg workspace, as shown in Fig. 5 (right). Algebraically:

[TABLE]

where $\mathbf{r}_{T(i)}$ is the CoM location after transitioning to the contact (given by the gait matrix $\mathbf{T}(i)$), $d_{lim}$ is the square diagonal, $L_{i}$ is the approximate distance from the trunk to the leg, and $\phi_{i}$ is a known offset for each foot. Here, the trigonometric functions are decomposed in terms piecewise linear approximations of the trunk orientation functions.

To have them expressed as mixed-integer convex, we will follow the approach of [1] and replace the trigonometric functions of (6) with piecewise linear approximations of $N_{s}$ segments $s$ and $c$. We define binary matrices $\mathbf{S}$ and $\mathbf{C}$ in $\{0,1\}^{N_{f}\times N_{s}}$ to assign linear segments, which is done with the following constraints:

[TABLE]

[TABLE]

where $\psi$ and $\gamma$ represent the boundaries between each linear segment, and $m$ and $n$ represent its slope and intersection.

II-E End-effector Trajectories

As mentioned in Section II-D, the end-effector trajectories are defined by the gait transition matrix $\mathbf{T}$. For simplicity, we define the function $\gamma(j,t)$ to reference the knots over which an end-effector swings between two adjacent contacts in the plan, where $j$ indicates the time-slot used for the swing and $t\in[1,\dots,N_{k}]$ indicates the knot, where $N_{k}$ is the number of knots allocated for each slot. Then, the end-effector motions are governed by the following constraint:

[TABLE]

where $l(i)$ is the leg number for the $i^{th}$ contact. This constraint enforces that the leg reaches the contact position $\mathbf{f}_{i}$ at the end of the $j^{th}$ slot. Also, it is important to constrain that the leg remains stationary when there is no transition. This is enforced using the following constraint over the $l^{th}$ leg:

[TABLE]

where $C(l)$ are the contact indexes assigned to the $l^{th}$leg. Further constraints can be added to make the end-effector follow a specific swing trajectory. Moreover, we ensure kinematic feasibility by constraining the CoM position with respect to the end-effectors (Fig. 6). Here, we use the bounding box constraint:

[TABLE]

where $\textbf{d}_{-}$ and $\textbf{d}_{+}$ are the bounding box limits.

II-F Contact Dynamics

We model the dynamic interaction of the forces through activation and friction cone constraints, which ensure stability in the motion. Here, we will describe how to pose these constraints as mixed-integer convex.

II-F1 Activation constraints

In order to ensure dynamic consistency, it is important that no force is present on a leg when it breaks contacts. This is constrained, for the $l^{th}$ leg, as:

[TABLE]

where $NC(j)$ is the set of knots in the $j^{th}$ slot used for the swing. This relation can be seen as a complementarity constraint, which can be also used to activate contacts [11, 18], between $\sum_{i\in C(l)}\mathbf{T}_{ij}$ and $\boldsymbol{\lambda}_{l\gamma(j,i)}$.

II-F2 Dynamic stability constraints

In order to maintain stability in non-coplanar contact conditions, the contact forces must remain within their friction cones [7]. We exploit the convex segmentation of the terrain in order to linearly approximate the friction cone constraints at each segment. This allows us to maintain the convexity in the problem, while also providing formal robustness guarantees.

Given that the $\mathrm{r}^{th}$ surface has a normal unit vector $\hat{\mathbf{n}}_{\mathrm{r}}$, we approximate its friction cone as a polyhedron with $N_{e}$ edges $\mathbf{v}_{r_{1}},\dots,\mathbf{v}_{r_{N_{e}}}$. Therefore, each force remains in its respective contact cone $\mathbb{FC}_{\mathrm{r}}$ through the following constraint:

[TABLE]

where $\rho_{e}$ are positive multipliers on each cone edge, as shown in Fig. 7 (left). Then, in order to add robustness to the motion, we maximize the distance between the nonlinear friction cone boundary and the force vector. This distance can be computed as $\alpha\sin\theta$, where $\theta$ is the half-angle of the cone, and $\alpha$ is defined as

[TABLE]

namely, $\alpha$ is the distance from the contact force $\boldsymbol{\lambda}_{lj}$ to the boundary of the friction cone, along the normal force direction [19], as shown in Fig. 7 (right). Therefore, we seek to maximize the value of $\alpha$ at each knot, which increases the stability margin, and we introduce the following linear constraint over each safe surface:

[TABLE]

Since the contact cone must not change when it is in stance phase, we also add the constraint:

[TABLE]

[TABLE]

where $NS(j)$ is the set of time-slots succeeding $j$. Note that this ensures that the cone constraint holds for all the succeeding knots in which the leg is stationary.

II-G Convex Decomposition of Angular Dynamics

The angular dynamics of the centroidal model are non-convex due to the cross product $(\mathbf{p}_{l}-\mathbf{r})\times\boldsymbol{\lambda}_{l}$. Other mixed-integer approaches have used McCormick envelopes to approximate these bilinear relations [14]. Nevertheless, this increases significantly the complexity of the problem. Here, we exploit the relation noted in [13], namely that this non-convexity can be addressed by reformulating bilinear constraints as a decomposition of quadratic terms:

[TABLE]

This convex decomposition is represented via quadratic constraints, turning the problem into Mixed-Integer Quadratically Constrained Quadratic Programming (MIQCQP) and, thus, a convex optimization. We penalize the norm of $\mathbf{u}^{+}$ and $\mathbf{u}^{-}$ to bound the rate of the centroidal angular momentum. For more details, the readers can refer to [13].

II-H Approximate Torque Limits

In order to improve the feasibility of the motion, we approximate torque limits by relying on a quasi-static motion assumption. This allows us to represent torques by projected the contact forces at each joint, compensating gravity terms, with respect to each leg frame:

[TABLE]

where $\mathbf{J}_{l,j}\in\mathbb{R}^{3\times 3}$ is the operational space foot Jacobian for the $l^{th}$ leg at the $j^{th}$ knot, and $\boldsymbol{\tau}_{max}$ are the joint torque limits of the leg. Unfortunately, convex computation of the Jacobian requires prior knowledge of the joint positions. Thankfully, the invariance of the operational space allows us to approximate it around a nominal value $\mathbf{J}_{l,j}^{*}$. Therefore, we approximate joint limits with the following constraint:

[TABLE]

This constraint approximates the Actuation Wrench Polytope (AWP) to define an approximation of the Feasible Wrench Polytope (FWP), as defined in [20]. In practice, this approximation is useful for most motions, close to the nominal position and without aggressive speeds. For further precision, one could use robust optimization [21] to constrain over the variations of the Jacobian ${\mathbf{J}^{*}}_{l,j}\pm\delta{\mathbf{J}}_{l,j}$.

II-I Trajectory Optimization

Given the constraints stated above, we formulate a convex trajectory optimization, as follows:

[TABLE]

where $T$ is the total number of knots, $g(k)$ is a running cost along the plan and $g_{T}$ is a terminal cost. Our running cost $g(k)$ maximizes the stability of the motion, while seeking for the fastest and smoothest gait. For this, the objectives are:

Minimize the CoM acceleration $\ddot{\textbf{r}}$. 2. 2.

Minimize the contact forces magnitude $\|\boldsymbol{\lambda}\|$. 3. 3.

Minimize the upper bound of quadratic terms $\textbf{U}=(\mathbf{u}^{-},\mathbf{u}^{+})$ used in the convex angular dynamics model. 4. 4.

Maximize the stability margin $\alpha$. 5. 5.

Minimize the execution time, by minimizing the sum of the elements in the time vector t.

The running cost $g(k)$ is defined as:

[TABLE]

On the other hand, the terminal cost $g_{T}$ biases the plan towards its goal, the terminal position $\mathbf{r}_{G}$, as:

[TABLE]

where $q$ are positive weights, $\mathbf{Q}$ are positive-semidefinite weighting matrices, and $\|\mathbf{v}\|_{\mathbf{Q}}$ stands for the weighted squared-$L_{2}$ norm $\mathbf{v}^{T}\mathbf{Q}\mathbf{v}$. In practice, we add a small cost to $\|\dot{k}\|_{Q_{k}}$ in order to generate smoother motions.

III Whole-body Control

The CoM motion, body attitude and swing motions are controlled by a trunk controller. It computes the feed-forward joint torques $\boldsymbol{\tau}_{ff}^{*}$ necessary to achieve a desired motion without violating friction, torques or kinematic limits. To address unpredictable events (e.g. limit foot divergence in case of slippage on an unknown surface), an impedance controller computes in parallel the feedback joint torques $\boldsymbol{\tau}_{fb}$ from the desired joint motion $(\mathbf{q}^{d}_{j},\dot{\mathbf{q}}^{d}_{j})$. Note that the desired body and joint motions have to be consistent with each other in order to prevent conflicts with the trunk controller.

To achieve compliantly desired trunk motions, we compute a reference CoM acceleration ($\ddot{\mathbf{r}}^{r}\in\mathbb{R}^{3}$) and body angular acceleration ($\dot{\boldsymbol{\omega}}^{r}_{b}\in\mathbb{R}^{3}$) through a virtual model:

[TABLE]

where ($\mathbf{r}^{d},\dot{\mathbf{r}}^{d},\ddot{\mathbf{r}}^{d})\in\mathbb{R}^{3}$ are the desired CoM position, velocity and acceleration respectively, $e(\cdot):\mathbb{R}^{3\times 3}\rightarrow\mathbb{R}^{3}$ is a mapping from the rotation matrix into the associated rotation vector, $\boldsymbol{\omega}_{b}\in\mathbb{R}^{3}$ is the angular velocity of the trunk. $\mathbf{K_{r}},\mathbf{D_{r}},\mathbf{K}_{{\theta}},\mathbf{D}_{{\theta}}\in\mathbb{R}^{3\times 3}$ are positive-definite diagonal matrices of proportional and derivative gains, respectively.

The target of our trunk controller is to minimize the error between the reference and actual accelerations while enforcing friction, torque and kinematic constraints. As mentioned above, the reference accelerations are computed from (18). We formulate the problem using Quadratic Programming (QP) with the generalized accelerations and the contact forces as decision variables, i.e. $\mathbf{x}=[\ddot{\mathbf{q}}^{T},\boldsymbol{\lambda}^{T}]^{T}\in\mathbb{R}^{6+n+3n_{l}}$:

[TABLE]

where $n$ represents the number of active Degrees of Freedom (DoFs). The first term of the cost function (19) penalizes the tracking error:

[TABLE]

while the second one is a regularization factor to keep the solution bounded or to pursue additional criteria. Both costs are quadratic-weighted terms. As the CoM acceleration is not a decision variable, we compute them from the contact forces using the centroidal dynamic model. We then re-write the tracking cost (20) as $\|\mathbf{Gx}-\mathbf{g}_{0}\|$ where:

[TABLE]

and $\mathbf{I}_{k}$ representing an identity matrix for the $k^{th}$ end-effector. The equality constraints $\mathbf{Ax}=\mathbf{b}$ encodes dynamic consistency, stance condition and swing task. On the other hand, the inequality constraints $\underline{\mathbf{d}}<\mathbf{Cx}<\bar{\mathbf{d}}$ encode friction, torque, and kinematic limits.

We map the optimal solution $\mathbf{x}^{*}$ into desired feed-forward joint torques $\boldsymbol{\tau}_{ff}^{*}\in\mathbb{R}^{n}$ using the actuated part of the full dynamics of the robot as:

[TABLE]

where $\mathbf{M}_{bj}\in\mathbb{R}^{(6+n)\times n}$ represents the coupled inertia between the floating-base and joints, $\mathbf{M}_{j}\in\mathbb{R}^{n\times n}$ the joint contribution to the inertia matrix, $\mathbf{h}_{j}\in\mathbb{R}^{n}$ is the force vector that accounts for Coriolis, centrifugal, and gravitational forces to the joint torque, and $\mathbf{J}_{cj}\in\mathbb{R}^{3n_{l}\times n}$ is a stack of Jacobians of the $n_{l}$ end-effectors.

Finally, the feed-forward torques $\boldsymbol{\tau}_{ff}^{*}$ are summed with the joint PD torques (i.e. feedback torques $\boldsymbol{\tau}_{fb}$) to form the desired torque command $\boldsymbol{\tau}^{d}$:

[TABLE]

which is sent to a low-level joint-torque controller. For more information on this controller the reader can refer to [22]

IV Experimental Validation

We validated our approach on the HyQ robot [23], a 85 kg hydraulically actuated quadruped robot, traversing various challenging terrains while also optimizing the gait transition 111video with the experiments: https://youtu.be/s4PMXpbwUes. The HyQ robot is fully-torque controlled and equipped with high-precision joint encoders, a Multisense SL sensor (Carnegie Robotics) and an Inertial Measurement Unit (KVH). HyQ has approximately the dimensions of a goat: 1.0 m$\times$0.5 m$\times$0.98 m (length$\times$width$\times$height). The planned motions are computed off-board using the Drake Toolbox [24] in MATLAB 2015a, on a Dual-Core Laptop running Mac OS X Sierra. We use Gurobi 6.0.5 [25] as our MIQCQP solver. The whole-body controller and state estimation run fully on-board and in real-time. Our state estimation is based on a modular inertial-driven Extended Kalman Filter which incorporates a rugged probabilistic leg odometry component with additional inputs from stereo vision and LIDAR registration, as described in [26].

IV-A Locomotion on rough terrain

In our first experiments we showcase the different capabilities of our approach by navigating over various challenging terrains composed of gaps, ramps, and steps with different terrain heights (Fig. 8). Our method allows the robot to successfully cross those terrains because of the use of a) a convex model of robot’s dynamics and terrain, b) an approximation of the torque limits, and c) stability maintenance capabilities in non-coplanar contact conditions.

IV-A1 Convex model of robot’s dynamics and terrain

Traversing a gap while climbing requires to accommodate properly the body position because of the robot’s kinematic limitations. In Fig. 8 (top), we show that our method exploits properly the leg reachability, requiring fewer contact locations to reach the goal compared with [8]. In fact, having a convex model of the robot’s dynamics, and especially of the terrain, avoids to fall into a local minimum because of the global optimality guarantees. This convex terrain model works well for most scenarios, however, it might lose validity when the terrain has significant non-linear curvature.

IV-A2 Approximate torque constraints

We compare two different planned motions, with and without approximate torque constraints in the optimization (see Fig. 9). While the robot climbed up successfully the slope in both cases, it was able to cross the gap only when the torque limits were considered. In fact, in these scenarios, the admissible set of contact forces is reduced due to the friction cones pointing outwards from the robot’s CoM. Here, only the approximate torque constraints (highlighted in blue in Fig. 9) account for this reduction of the feasible wrench set, and as result, the contact forces are distributed accordingly, see Fig. 9 (bottom)222Note that HyQ’s torque limits depend on the joint position, resulting in limit differences in the yellow shade on Fig. 9 (bottom), which are not accessible using the centroidal dynamics and the quasi-static assumption.. However, if we do not consider the approximate torque constraints, the robot reaches torque limits three times, red shade in Fig. 9 (top), and it falls while climbing down, see Fig. 9 (top-right).

IV-A3 Stability in non-coplanar contact conditions

Crossing a gap and ascending up stairs is a challenging task, because the robot has to maintain stability while considering potential non-coplanar contacts, see Fig. 8 (bottom). For instance, ZMP-based approaches such as [8, 9] cannot ensure stability in such terrains. We experimentally found an increment on the robustness of the planned motion compared to the aforementioned methods. This robustness is especially important because we rely on a reference for the robot attitude modulation as in [8]. This is done because we restrict our trajectories to small angular momentum changes, because of limitations in the convex model (see Section II-G).

IV-B Gait transitions

Afterwards, we tested the gait planning and transitioning capabilities of our approach. In the optimization, we seed a gait sequence by first optimizing only over the linear dynamics. For this, we run trials over a challenging terrain with non-coplanar contacts to compare two different scenarios: a) crossing with a fixed gait that is predefined, walk and trot, and b) crossing while the gait sequence is optimized. Figure 10 shows snapshots of these experimental trials.

IV-B1 Fixed walking and trotting gaits

In these cases, as expected, the motions are executed robustly across the entire trial. For walking, we obtained motions of quality similar to those of the previous experiments. In the case of the trotting, we increased the execution speed compared to the walking case (from 18 cm/s to 31 cm/s). This fast motion is effectively tracked by our controller, despite relatively slow corrections, from the visual sources, in the state estimation.

IV-B2 Automatic gait discovery

In this trial, we showcase the trade-offs involved when optimizing the gait transitions Fig. 10 (bottom). First, in order to minimize acceleration, our motion planner selects a walking gait before smoothly transitioning to a dynamic trotting gait. As expected, the robot continues trotting until it faces a significant change of terrain. At that moment, the robot transitions to a slower gait (i.e. walking gait) in order to accommodate for the terrain conditions. For comparison, Fig. 11 shows the minimum $\alpha$ margin at each time-step, normalized by its contact force. Finally, it returns to a trotting gait as is detailed in Fig. 11. In contrast to [3], we do not include any terrain heuristics in the planning stage, resulting in an automatic gait discovery based on the centroidal dynamics of the robot.

IV-C Discussion

The experimental trials presented above showcase the main aspects of our approach. As expected, all of our plans result in successful executions. Thanks to our incorporation of approximate torque limits, we are able to push the limits of our hardware, reaching faster walking speeds (around 15 cm/s) than previous approaches executed on the same robot [2, 8, 9]. Additionally, we have shown that our approach is able to handle gait transitions robustly even on challenging terrain. In our experiments, the convex approximation of the angular dynamics helps the planner to generate natural motions. Nonetheless, we found that this model does not extend well to more dynamical gaits, such as bounding or pace, since it cannot impose limits in the angular momentum, causing divergence.

Computation time is one of the main concerns in any optimization problem. In Table II, we report the required computation time (for a single locomotion cycle one stride) of the above scenarios. We plan motion at least two orders of magnitude faster than previous work on similar terrains [8, 12] because of our convex formulation using MIQCQP. The kinematic constraints imposed help the solver to quickly discard infeasible surfaces. This captures the main advantage of our method, that it is able to efficiently plan motions through 3D environments with obstacles, while maintaining stability despite changes in the terrain.

Other factors might influence the computation time. For instance, leaving the gait as a decision variable increases computation time significantly. As in [1], we found that the incorporation of piecewise-affine trigonometric functions has a negative effect on the computation time. Likewise, quadratic constraints in (15) also influence the computation time, resulting in increases of around 500%. However, we believe that our method can potentially be extended to receding horizon fashion.

V Conclusions

We have presented a novel approach for simultaneously planning contacts and motions on multi-legged robots based on MICP. Our approach is able to handle complex terrain, while also providing formal robustness guarantees on the plan. Moreover, we incorporate the gait sequences as a decision variable, which allows for automatic gait discovery. Our experimental trials use both a state-of-the-art whole-body controller and state estimation [22, 26]. We demonstrate the capabilities of our approach by traversing challenging terrains with the HyQ robot. To our knowledge, these hardware experiments constitute the first experimental validation of non-gaited uneven locomotion via friction cone stability margin. Moreover, our implementation is able to plan locomotion cycles in less than a second, even in complex scenarios, which is at least two orders of magnitude faster than previous non-convex approaches [8, 10] in similar environments.

In future work, we plan to expand on the capabilities of real-time planning that this approach might offer, possibly reaching similar computation time to [9]. To this end, friction cone constraints could be relaxed to vertex-based ZMP constraints [9], sacrificing stability guarantees but improving computation time. Here, we have relied on a convex decomposition of the angular dynamics. This approach works well for simple motions where angular momentum rates are often low. However, it can cause the angular momentum to diverge when the contact forces vary significantly. Because of this, we are interested in exploring other models of angular dynamics, like those proposed in [14], so as to generate more aggressive behaviors. In this work, we have approximated torque limits with a nominal operational space Jacobian that is updated iteratively. While this works well for the experiments performed, it loses validity for more aggressive motions. For this reason, we are interested in studying other approximations of torque limits. We are further interested in exploiting the piecewise affine structure of the multi-contact dynamics in the receding horizon.