Feasible Region: an Actuation-Aware Extension of the Support Region

TL;DR

This paper introduces the Feasible Region, an actuation-aware extension of the support region for legged robots, ensuring static stability and torque feasibility for locomotion on rough terrains.

Contribution

It defines a new feasible region that accounts for actuation limits, improving foothold and trajectory planning for stable legged locomotion.

Findings

Efficient iterative algorithm for feasible region computation

Successful robot experiments negotiating obstacles with payload

Enhanced stability and torque feasibility in locomotion planning

Abstract

In legged locomotion the projection of the robot Center of Mass (CoM) being inside the convex hull of the contact points is a commonly accepted sufficient condition to achieve static balancing. However, some of these configurations cannot be realized because the joint torques required to sustain them would be above their limits (actuation limits). In this manuscript we rule out such configurations and define the Feasible Region, a revisited support region that guarantees both global static stability in the sense of tipover and slippage avoidance and of existence of a set of joint-torques that are able to sustain the robot body weight. We show that the feasible region can be employed for the selection of feasible footholds and CoM trajectories to achieve static locomotion on rough terrains, also in presence of load intensive tasks. Key results of our approach include the efficiency in the computation of the feasible region thanks to an Iterative Projection algorithm. This allowed us to carry out successful experiments on the HyQ robot, that was able to negotiate obstacles of moderate dimensions while carrying an extra 10 kg payload.

Figures

Tables

Table 1. TABLE I : Analogies between 2D regions and 6D polytopes.

constraint	(static)	(dynamic)
type:	2D CoM proj. space	$6D$ CoM wrench
friction	friction/support reg. ${𝒴}_f$	Contact Wrench
		Cone (CWC)
joint-torques	actuation reg. ${𝒴}_a$	Actuation Wrench
		Polytope (AWP)
friction & joint-torques	feasible reg. ${𝒴}_{fa}$	Feasible Wrench
		Polytope (FWP)

Equations

$$n$$

$$N$$

$$n_f$$

$$n_c$$

$$n_l$$

$$i$$

$$n_i$$

$$m$$

$$\mathbf{c}\in {\mathbb{R}}^3$$

$${\mathbf{p}}_i\in {\mathbb{R}}^3$$

$${\mathbf{f}}_i\in {\mathbb{R}}^m$$

$$\mathbf{\tau }\in {\mathbb{R}}^n$$

$$\mathbf{q}\in SE(3)\times {\mathbb{R}}^n$$

$${\dot{\mathbf{q}}}_j\in {\mathbb{R}}^{6+n}$$

$${\mathbf{q}}_i\in {\mathbb{R}}^{n_l}$$

$$s\in {\mathbb{R}}^{6+n}$$

$${\mathbf{I}}_3\in {\mathbb{R}}^{3\times 3}$$

$${\mathcal{Y}}_f$$

$${\mathcal{Y}}_a$$

$${\mathcal{Y}}_{fa}$$

$$\mathbf{\tau }=\mathbf{J}(\mathbf{q}{)}^T\mathbf{w}$$

$$\mathcal{S}_{\tau}=\Big{\{}\mathbf{\bm{\tau}}\in\mathbb{R}^{n}\quad|\quad\mathbf{\bm{\tau}}^{T}\mathbf{\bm{\tau}}\leq 1\Big{\}}$$

$$\mathcal{E}_{\mathbf{\bm{w}}}=\Big{\{}\mathbf{\bm{w}}\in\mathbb{R}^{m}\quad|\quad\mathbf{\bm{w}}^{T}\mathbf{\bm{J}}\mathbf{\bm{J}}^{T}\mathbf{\bm{w}}\leq 1\Big{\}}$$

$$\mathcal{Z}_{\tau}=\Big{\{}\mathbf{\bm{\tau}}\in\mathbb{R}^{n}\quad|\quad\mathbf{\bm{\underline{\tau}}}\leq\mathbf{\bm{\tau}}\leq\mathbf{\bm{\overline{\tau}}}\Big{\}}$$

$$\mathcal{P}_{\mathbf{\bm{w}}}=\Big{\{}\mathbf{\bm{w}}\in\mathbb{R}^{m}\quad|\quad\mathbf{\bm{\underline{\tau}}}\leq\mathbf{\bm{J}}(\mathbf{\bm{q}})^{T}\mathbf{\bm{w}}\leq\mathbf{\bm{\overline{\tau}}}\Big{\}}$$

$${\mathbf{w}}^{lim}=\mathbf{J}(\mathbf{q}{)}^{-T}{\mathbf{\tau }}^{lim}$$

$$\mathbf{M}(\mathbf{q})\dot{\mathbf{s}}+\mathbf{C}(\mathbf{q},\mathbf{s})+\mathbf{g}(\mathbf{q})=\mathbf{S\tau }+\mathbf{T}(\mathbf{q}{)}^T\mathbf{f}$$

$$\underset{\mathbf{M}(\mathbf{q})}{\underbrace{\left[\begin{array}{cc}{\mathbf{M}}_b& {\mathbf{M}}_{bj}\\ {\mathbf{M}}_{bj}^T& {\mathbf{M}}_j\end{array}\right]}}\underset{\dot{\mathbf{s}}}{\underbrace{\left[\begin{array}{c}{\dot{\mathbf{\nu }}}_{\mathbf{b}}\\ {\ddot{\mathbf{q}}}_j\end{array}\right]}}+\underset{\mathbf{C}(\mathbf{q},\mathbf{s})}{\underbrace{\left[\begin{array}{c}{\mathbf{c}}_b\\ {\mathbf{c}}_j\end{array}\right]}}+\underset{\mathbf{g}(\mathbf{q})}{\underbrace{\left[\begin{array}{c}{\mathbf{g}}_b\\ {\mathbf{g}}_j\end{array}\right]}}=\underset{\mathbf{B}}{\underbrace{\left[\begin{array}{c}{0}_{6\times n}\\ {\mathbf{I}}_{n\times n}\end{array}\right]}}\mathbf{\tau }+\underset{\mathbf{T}(\mathbf{q}{)}^T}{\underbrace{\left[\begin{array}{c}{\mathbf{J}}_b^T\\ {\mathbf{J}}_q^T\end{array}\right]}}\mathbf{f}.$$

$${\mathbf{J}}_q=\mathrm{diag}(\mathbf{J}({\mathbf{q}}_1),\dots \mathbf{J}({\mathbf{q}}_{n_c}))$$

$$\displaystyle\mathcal{A}_{i}=\Big{\{}$$

$$\displaystyle\mathbf{\bm{g}}(\mathbf{\bm{q}}_{i})=\mathbf{\bm{\tau}}_{i}+\mathbf{\bm{J}}(\mathbf{\bm{q}}_{i})^{T}\mathbf{\bm{f}}_{i},\quad\mathbf{\bm{\underline{\tau}}}_{i}\leq\mathbf{\bm{\tau}}_{i}\leq\mathbf{\bm{\overline{\tau}}}_{i}\Big{\}}$$

$$\displaystyle\mathcal{A}_{i}=\Big{\{}\mathbf{\bm{f}}_{i}\in\mathbb{R}^{m}\quad|$$

$$\mathbf{g}({\mathbf{q}}_i)={\mathbf{\tau }}_i+\mathbf{J}({\mathbf{q}}_i{)}^T{\mathbf{f}}_i,$$

$${\mathbf{A}}_1=\left[\begin{array}{ccc}{\bar{\mathbf{A}}}_1& \dots & {\bar{\mathbf{A}}}_{n_c}\end{array}\right]\in {\mathbb{R}}^{6\times (m{n}_c)},$$

$${\mathbf{A}}_2=\left[\begin{array}{c}0\\ -m\mathbf{g}\times {\mathbf{P}}^T\end{array}\right]\in {\mathbb{R}}^{6\times 2},\mathbf{P}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]$$

$$\mathbf{u}=\left[\begin{array}{c}-m\mathbf{g}\\ 0\end{array}\right],{\mathbf{b}}_i=\left[\begin{array}{c}({\mathbf{t}}_{1,i}-{\mu }_i{\mathbf{n}}_i{)}^T\\ ({\mathbf{t}}_{2,i}-{\mu }_i{\mathbf{n}}_i{)}^T\\ -({\mathbf{t}}_{1,i}+{\mu }_i{\mathbf{n}}_i{)}^T\\ -({\mathbf{t}}_{2,i}+{\mu }_i{\mathbf{n}}_i{)}^T\end{array}\right]\in {\mathbb{R}}^{4\times 3}$$

$$\mathbf{B}=\mathrm{diag}({\mathbf{b}}_1,\dots ,{\mathbf{b}}_{n_c})\in {\mathbb{R}}^{4n_c\times 3n_c},\mathbf{g}=[0,0,-g{]}^T$$

$$\bar{\mathbf{\bm{A}}}_{i}=\left\{\begin{tabular}[]{ll}\vspace{0.1cm} $\begin{bmatrix}\mathbf{I}_{3}\\ [\mathbf{\bm{p}}_{i}]\times\end{bmatrix}$ $\in\mathbb{R}^{6\times 3{n_{c}}}$&\text{if} \quad$m=3$\\ $\begin{bmatrix}\mathbf{I}_{3}&\mathbf{0}_{3}\\ [\mathbf{\bm{p}}_{i}]\times&\mathbf{I}_{3}\end{bmatrix}$ $\in\mathbb{R}^{6\times 6{n_{c}}}$&\text{if} \quad$m=6$\\ \end{tabular}\right.$$

$$\mathcal{Y}_{f}=\Big{\{}\mathbf{\bm{c}}_{xy}\in\mathbb{R}^{2}\quad|\quad\exists\mathbf{\bm{f}}\in\mathbb{R}^{mn_{c}}\operatorname*{s.t.}(\mathbf{\bm{c}}_{xy},\mathbf{\bm{f}})\in\mathcal{C}\Big{\}}$$

$$\displaystyle\mathcal{C}=\Big{\{}\mathbf{\bm{f}}\in\mathbb{R}^{mn_{c}},$$

$$\displaystyle\mathbf{\bm{B}}\mathbf{\bm{f}}\leq\mathbf{\bm{0}}\Big{\}}$$

$$c_x=\frac{{\tau }_{\mathcal{O}}^y}{mg},c_y=-\frac{{\tau }_{\mathcal{O}}^x}{mg}$$

$$\mathcal{A}_{i}=\Big{\{}\mathbf{\bm{f}}_{i}\in\mathbb{R}^{m}\quad|\quad\mathbf{\bm{J}}(\mathbf{\bm{q}}_{i})^{T}\mathbf{\bm{f}}_{i}=\mathbf{\bm{g}}(\mathbf{\bm{q}}_{i})-\mathbf{\bm{\tau}}_{i},\quad\mathbf{\bm{\underline{\tau}}}_{i}\leq\mathbf{\bm{\tau}}_{i}\leq\mathbf{\bm{\overline{\tau}}}_{i}\Big{\}}$$

Full text

Supplementary Material

• •

Video of simulation and hardware results is available at: https://youtu.be/9pvWO2Qmo9k

• •

Code available (python) at:

https://github.com/orsoromeo/jet-leg

(with source code used for Figs. 6 and 8).

I Introduction

Most state-of-the-art strategies employed for the online motion planning of legged robots work well only under the assumption that the joint-torque limits do not affect the locomotion capabilities of legged robots. This assumption is due to the computational limitations of modern processors that compel to trade-off between the size of the motion planner’s predictive horizon and the accuracy of the considered dynamic model. In order to enlarge the predictive horizon of motion planner for legged robots, therefore, researchers have developed low dimensional (*i.e., *simplified) dynamic models [1] with corresponding stability criteria [2]. Such stability criteria focus on avoiding slippage and tip-over [3]. They are usually characterized by a reference point and a stability region in which the reference point has to lie in order to meet the necessary stability condition. The distance of the considered reference point from the sides of the stability region is used to evaluate the robot’s robustness in static and dynamic gaits.

Renowned 2D examples of such reference points are the CoM projection $\mathbf{\bm{c}}_{xy}$, the Zero-tilting Moment Point (ZMP) $\mathbf{\bm{z}}$ [4] and the more recent Instantaneous Capture Point (ICP) $\mathbf{\bm{\xi}}$ [5]. These 2D reference points represent meaningful descriptors of the system for, respectively, static (neither system’s velocity nor accelerations are considered), semi-dynamic (only instantaneous acceleration explicitly considered) and fully dynamic gaits (both instantaneous velocity and acceleration explicitly considered). Furthermore, the ZMP and the ICP (both derived by the definition of linear inverted pendulum [6]) only hold in presence of co-planar contacts. Higher dimensional reference points have been defined to consider also the vertical and angular accelerations such as, respectively, the 3D virtual repellent point [7] and the 6D aggregated centroidal wrench [8].

As regards the possible definitions of stability region, instead, the support polygon, defined as the convex hull of the contact points, is the most widespread. It is always valid in presence of co-planar contacts for both static and dynamic gaits and it can, therefore, be used in combination with the CoM projection, the ZMP or the ICP. It was proved, however, that the support polygon is not valid in multi-contact scenarios and a new definition, called support region, was provided [9], [10]. Such definition, however, only holds for the evaluation of static postures on arbitrary terrains, for which reason the support region cannot be combined with ZMP or ICP, but only with the CoM projection $\mathbf{\bm{c}}_{xy}$. A dynamic extension of the support region requires to resort to higher dimensional spaces such as 3D volumes [11] or 6D sets [12], [13].

In all of the above mentioned stability criteria, the reference points capture the main dynamics of the robot’s CoM, while the stability regions depend on the location of the contact points, the orientation of the surface normals and on the friction coefficients. Extensions have been made in order to include in the lower-dimensional stability analysis also other feasibility constraints such as the kinematic limits of legged systems [14, 15]. Joint-torque limits, however, have only been mapped into the 6D wrench space [16], but never into the 2D space of the support region.

In this manuscript we focus on the definition of a 2D stability region that includes the joint-torque limits and, because of the static assumption that it involves, it can be used in combination with the CoM projection for the generation of static locomotion for legged robots on complex geometry environments. To the authors’ knowledge this is, therefore, the first example of how a two dimensional feasibility criterion can explicitly consider the actuation constraints of the system.

The efficient computation of the proposed feasible region, enables the concurrent online optimization of actuation-consistent CoM trajectories and foothold positions in rough terrains.

Besides its computational efficiency, the feasible region can be easily represented in 2D, thus representing an intuitive tool for motion planning. This allows us to give a clear and simple answer to legitimate questions like: how does the maximal step length change with the increase of the robot’s mass? And also: which is the height of the highest step that a robot can step on given its body mass and maximal joint-torque/force capabilities?

I-A Contributions

In this manuscript we attempt to give an answer to the above questions in the following way:

We introduce the feasible region, a 2D horizontal convex set that includes all the positions of the CoM projection that are guaranteed to be statically stable and actuation-consistent. This 2D area provides an intuitive yet powerful understanding of the relationship between the task-space locomotion capabilities of a robot and its joint-space actuation limits; 2. 2.

We introduce a modified version of the IP algorithm, initially proposed in [9], able to efficiently compute the feasible region (in about $15ms$); 3. 3.

We employ the proposed feasible region to formulate a motion planning algorithm for legged robots, able to optimize online static trajectories of the CoM projection and footholds on arbitrary terrains for predefined step sequences and phase durations. We report the results of our hardware experiments which show that our motion planner can adapt the footstep locations and the trajectory of the robot’s CoM to the terrain geometry by replanning in a receding horizon fashion at about $15\text{\,}\mathrm{Hz}$.

I-B Outline

The two core building blocks of the work developed in this manuscript are the wrench (or force) polytopes and the IP algorithm which are briefly recalled in Sec. II. Using the described elements we formulate the local actuation region $\mathcal{Y}_{a}$ and the local feasible regions $\mathcal{Y}_{fa}$ in Sec. III. The latter, in particular can be seen as a revisited definition of the well known support region. Section IV presents examples of how the feasible region can be employed to achieve online replanning of CoM and feet trajectories on arbitrary terrains using the height map provided by the robot’s exteroceptive sensors. Simulations and experimental results on the HyQ quadruped robot (see Fig. 1) are finally presented in Sec. V. Section VII draws final conclusions regarding the concepts presented in this manuscript, raises some possible discussion point and anticipates possible future developments.

Notation

List of the most relevant symbols used in this manuscript:

[TABLE]

Acronyms:

[TABLE]

II Background

II-A Wrench Polytopes for Fixed Base Systems

Actuator force/torque limits and their consequences on the overall performance in the task space have been analyzed for decades in the field on mechanical industrial manipulators [17, 18, 19] and, more recently, also on cable driven parallel robots [20] and robotics hands [21].

Wrench ellipsoids (or hyperellipsoids) have been identified as useful tools to assess the control authority at the end-effector of serial mechanical chains. Being always obtained from a hypersphere of unit radius, such ellipsoids do not hold any information relative to the absolute magnitude of the wrench that a mechanical chain can exert. They can be obtained as a consequence of the kinetic energy theorem (or work-energy theorem) that states that the work done by all forces acting on a particle equals the rate of change in the particle’s kinetic energy [22, p. 148]. This leads to the following:

[TABLE]

which represents a static relationship between the generalized task-space wrenches $\mathbf{\bm{w}}\in\mathbb{R}^{m}$ and the generalized joint-space forces $\mathbf{\bm{\tau}}\in\mathbb{R}^{n}$. The matrix $\mathbf{\bm{J}}(\mathbf{\bm{q}})\in\mathbb{R}^{m\times n}$ is the end-effector Jacobian. If we consider (1) in combination with a unit hypersphere $\mathcal{S}_{\tau}$ in the joint-torque space:

[TABLE]

we can then obtain a new set (the wrench ellipsoid) $\mathcal{E}_{\mathbf{\bm{w}}}$ that describes how $\mathcal{S}_{\tau}$ is mapped into the task-space:

[TABLE]

By definition, the force ellipsoid $\mathcal{E}_{\mathbf{\bm{w}}}$ represents the pre-image of the unit hypersphere $\mathcal{S}_{\tau}$ in the joint space under the mapping given by $\mathbf{\bm{J}}(\mathbf{\bm{q}})^{T}$. The lengths of the semiaxes of $\mathcal{E}_{\mathbf{\bm{w}}}$ correspond to the singular values of the Moore-Penrose pseudoinverse of $\mathbf{\bm{J}}(\mathbf{\bm{q}})$ [23, p.285]. The ratio between the greatest and the smallest eigenvalue of $\mathbf{\bm{J}}(\mathbf{\bm{q}})$ is, therefore, used as a measure of anisotropy of the ellipsoid and of the force amplification properties of the mechanical chain.

In a similar fashion, further exploiting  (1), we can then also analyze the pre-image of the joint-torque hypercube $\mathcal{Z}_{\tau}$, *i.e., *the set of all joint-torques $\mathbf{\bm{\tau}}$ comprised within the manipulator’s actuator limits $\mathbf{\bm{\tau}}^{lim}$:

[TABLE]

The vectors $\mathbf{\bm{\underline{\tau}}}=-\mathbf{\bm{\tau}}^{lim}\in\mathbb{R}^{n}$ and $\mathbf{\bm{\overline{\tau}}}=\mathbf{\bm{\tau}}^{lim}\in\mathbb{R}^{n}$ contain in their elements the hardware-dependent lower and upper bounds of the values that limit the generalized joint-torque vector $\mathbf{\bm{\tau}}$111For electric and rotary hydraulic actuators, provided that the actuator’s friction is properly modelled, the dependence of the torque limits on the joint position is negligible. The torque limits of electric actuators also depend on the joint velocity; it was shown, however, that such dependency is often negligible compared to effects of other phenomena such as temperature [24]. In this work, moreover, we focus on static locomotion for which reason the torque limit $\tau_{lim}$ can be identified with the maximum static stall-torque..

The hypercube $\mathcal{Z}_{\tau}$ can be seen also as a system of $2n$ linear inequalities that constrain joint-torques [19] (see Fig. 2). The notation used in  (4) assumes symmetric joint-torque limits222We assume that the maximum joint-torque $\mathbf{\bm{\overline{\tau}}}$ has the same amplitude of the minimum joint-torque $\mathbf{\bm{\underline{\tau}}}$ and opposite sign ($\mathbf{\bm{\overline{\tau}}}=\tau^{lim}$ and $\mathbf{\bm{\underline{\tau}}}=-\tau^{lim}$). and, in this case, $\mathcal{Z}_{\tau}$ is a zonotope centered at the origin (see Appendix VIII-A). In all the other cases the hypercube $\mathcal{Z}_{\tau}$ will still represent a zonotope but its center will not correspond to the origin of joint-torque space.

The task-space wrench polytope $\mathcal{P}_{\mathbf{\bm{w}}}$333Also commonly called force polytope [18], pre-image of $\mathcal{Z}_{\tau}$, can be written as follows (also see Fig. 2):

[TABLE]

While the force ellipsoid $\mathcal{E}_{\mathbf{\bm{w}}}$ can be used as a qualitative metric of the robot’s force amplification capabilities, the wrench polytope $\mathcal{P}_{\mathbf{\bm{w}}}$ also includes a quantitative information about the maximum and minimum amplitude of the wrench that the robot can perform at the end-effector.

Wrench ellipsoids and wrench polytopes have been originally introduced for fixed-base non-redundant serial mechanical chains with $m=n$ where $n$ is the number of actuated joints (dimension of generalized coordinates) and $m$ is the dimension of the end-effector force (or, equivalently, the degree of constraint at the contact). In such cases the Jacobian $\mathbf{\bm{J}}(\mathbf{\bm{q}})$ is thus a square matrix and, except for singular configurations, its transpose $\mathbf{\bm{J}}^{T}$ can be inverted to obtain the vertex representation of the wrench polytope $\mathcal{P}_{\mathbf{\bm{w}}}$:

[TABLE]

where $\mathbf{\bm{\tau}}^{lim}\in\mathbb{R}^{n}$ is a vertex of $\mathcal{Z}_{\mathbf{\bm{\tau}}}$ and $\mathbf{\bm{w}}^{lim}\in\mathbb{R}^{m}$ is a vertex of $\mathcal{P}_{\mathbf{\bm{w}}}$. This is a suitable condition in which a one-to-one relation between joint-space torques and task-space wrenches exists. In the case of an arm with 3 DoFs ($n=3$), for example, a violation of one joint-torque limit will correspond to a point on a facet of $\mathcal{Z}_{\tau}$ and also to another point on a facet of the task-space polytope $\mathcal{P}_{\mathbf{\bm{w}}}$. Similarly, a violation of two (or three) joint-torque limits will correspond to a point on an edge (or a vertex) of the $\mathcal{Z}_{\tau}$ and also to another point on an edge (or a vertex) of the task-space polytope $\mathcal{P}_{\mathbf{\bm{w}}}$. See Appendix VIII-A for the definitions of facets, edges and vertices for $n>3$.

For a more detailed explanation about the effect of gravity on force ellipsoids and on other possible definitions of wrench polytopes for fixed base systems in dynamic conditions please refer to [25, 19, 26].

II-B Wrench Polytopes for Floating Base Systems

In this section we illustrate the procedure to compute the dynamic wrench polytopes $\mathcal{A}$ *i.e., *the set of feasible contact wrenches that a tree-structured, floating base robot can perform at its contact points with the environment while moving [18]. For this, let us consider the Equation of Motion (EoM) of a floating-base robot444We consider the floating-base robot composed by $n_{f}$ branches (*e.g., *number of feet and/or hands), with $n_{c}$ of them in contact with the environment and, each of them having a number $n_{l}$ of actuated DoFs. Therefore, $n=\sum_{k=1}^{n_{f}}n_{l}^{k}$ represents the total number of actuated joints.:

[TABLE]

where $\mathbf{\bm{q}}=\begin{bmatrix}\mathbf{\bm{q}}_{b}^{T}&\mathbf{\bm{q}}_{j}^{T}\end{bmatrix}^{T}\in SE(3)\times\mathbb{R}^{n}$ is the vector of generalized coordinates of the floating-base system, composed of the pose of the floating base $\mathbf{\bm{q}}_{b}\in SE(3)$ and of the coordinates $\mathbf{\bm{q}}_{j}\in\mathbb{R}^{n}$ describing the positions of the $n$ actuated joints. The vector $\mathbf{\bm{s}}=\begin{bmatrix}\mathbf{\bm{\nu}}_{b}^{T}&\mathbf{\bm{\dot{q}}}_{j}^{T}\end{bmatrix}^{T}\in\mathbb{R}^{6+n}$ is the vector of the generalized velocities, $\mathbf{\bm{\tau}}\in\mathbb{R}^{n}$ is the vector of actuated joint-torques while $\mathbf{\bm{C}}(\mathbf{\bm{q}},\mathbf{\bm{s}})$ and $\mathbf{\bm{g}}(\mathbf{\bm{q}})\in\mathbb{R}^{6+n}$ are the Centripetal/Coriolis and gravity terms, respectively. The matrix $\mathbf{\bm{M}}(\mathbf{\bm{q}})\in\mathbb{R}^{(n+6)\times(n+6)}$ is the joint-space inertia matrix, $\mathbf{\bm{S}}\in\mathbb{R}^{(6+n)\times n}$ is the selector matrix whose rows corresponding to the actuated joints are set to ones and whose rows corresponding to the unactuated joints are set to zeros. $\mathbf{\bm{f}}\in\mathbb{R}^{mn_{c}}$ is the vector of contact forces555Note that quadruped robots have nearly point feet, henceforth we thus consider pure forces acting at contact points and no contact torque ($m=3$). that are mapped into joint torques through the stack of Jacobians $\mathbf{\bm{T}}(\mathbf{\bm{q}})\in\mathbb{R}^{mn_{c}\times(6+n)}$. If we split  (7) into its unactuated and actuated parts, we get:

[TABLE]

The vector $\mathbf{\bm{q}}_{j}$ results from the concatenation of the joint positions of all the separate branches $\mathbf{\bm{q}}_{1},\dots\mathbf{\bm{q}}_{n_{c}}$. The bottom block of $\mathbf{\bm{T}}(\mathbf{\bm{q}})$ (bottom $n$ rows), corresponding to the actuated part $\mathbf{\bm{J}}_{q}\in\mathbb{R}^{(mn_{c})\times n}$, is thus block diagonal and it can map joint-torques into contact forces for each leg individually:

[TABLE]

where $\mathbf{\bm{J}}(\mathbf{\bm{q}}_{i})$ with $i=1,\dots n_{c}$ are the Jacobians of the limbs in contact with the ground. Omitting the upper block (first $6$ rows) of  (8) is convenient because it avoids the coupling term $\mathbf{\bm{J}}_{b}$ and one wrench polytope can then be computed for each individual limb.

On a similar line, as the dynamic manipulability polytope $\mathcal{P}_{\mathbf{\bm{w}}}$ defined in [17], we can now define a quantity that we call dynamic wrench polytope $\mathcal{A}_{i}$ as follows:

Definition: for each individual $i^{\text{th}}$ branch of the considered tree-structured robot, $\mathcal{A}_{i}$ corresponds to the set of all contact forces $\mathbf{\bm{f}}_{i}\in\mathbb{R}^{m}$ that satisfies the bottom row of  (8) for all the joint-torques $\mathbf{\bm{\tau}}_{i}\in\mathbb{R}^{n_{l}}$ belonging to $\mathcal{Z}_{\tau}$:

[TABLE]

where $i=1,\dots,n_{c}$ is the contact index and $n_{c}$ is the number of active contacts between the robot and the environment. The vectors $\mathbf{\bm{q}}_{i}\in\mathbb{R}^{n_{l}}$ and $\mathbf{\bm{\tau}}_{i}\in\mathbb{R}^{n_{l}}$ represent the joint-space position and torque of only those joints that belong to the $i^{\text{th}}$ limb while $n_{l}$ represents the number of actuated DoFs of that limb. If $m=3$ then the contact wrench $\mathbf{\bm{f}}_{i}\in\mathbb{R}^{m}$ consists of pure forces while if $m=6$ then a non-zero contact torque is also present. For a partial list of the main symbols employed in this paper and their meaning please refer to the Notation Section Notation.

In Fig. 3 an example of dynamic wrench polytope is drawn superimposed on the Hydraulically actuated Quadruped (HyQ) robot: each limb of this robot has three actuated DoFs ($n_{l}=3$) and each foot can be approximated as a point contact ($m=3$). $\mathcal{A}_{i}$ is then a polytope of $2\cdot 3=6$ facets and $2^{3}=8$ vertices (the mapping of a cube in the joint space).

Equation (10) purposely omits the first line (six equations) of  (8) referring to the unactuated floating base. This corresponds to neglecting the role of the base link as a coupling among the limbs of the tree-structured robot.

The advantage of computing separate individual wrench polytopes $\mathcal{A}_{i}$ for each limb of the robot is that we can then treat the volume of each polytope as an actuation capability measure of the corresponding limb [27, 28].

As a final consideration, we can observe that in static conditions ($\dot{\mathbf{\bm{q}}}_{i}=\ddot{\mathbf{\bm{q}}}_{i}=0$)  (10) can be written as:

[TABLE]

The term $\mathbf{\bm{g}}(\mathbf{\bm{q}}_{i})$ represents the effect of gravity acting on the individual limb $i=0,\dots,n_{c}$. From a geometrical point of view $\mathbf{\bm{g}}(\mathbf{\bm{q}}_{i})$ can also be seen as an offset term that translates the polytope $\mathcal{A}_{i}$ in the same direction of the gravity vector, *i.e., *towards the negative side of the $f_{z}$ direction of the wrench space defined by the axes ($f_{x},f_{y},f_{z},\tau_{x},\tau_{y},\tau_{z}$). For a predefined set of torque limits $\mathbf{\bm{\tau}}_{i}^{lim}$ an increase in the legs’ mass and, as a consequence, a large offset term $\mathbf{\bm{g}}(\mathbf{\bm{q}}_{i})$, will cause a decrease in the set of feasible positive contact forces.

II-C The Iterative Projection Algorithm

The Iterative Projection (IP) algorithm is a method introduced by Bretl *et al. *[9] for the computation of support regions for articulated robots having multiple contacts with the environment in arbitrary locations, having arbitrary surface normals and friction coefficients. In [9], the support region is defined as the horizontal cross section of the convex cylinder that represents the set of CoM positions at which contact forces exist that compensate for gravity without causing slip (for given foot placements with associated friction models).

The IP belongs to a family of cutting-plane methods [29] that allow to approximate a target convex set $\mathcal{Y}$ up to a predefined tolerance value. The tuning of this tolerance allows to conveniently adjust the computational performance of the algorithm: it enables a rough but fast reconstruction of the target set for high tolerance values. On the other hand, it also enables a precise set reconstruction with longer solve times when the tolerance value is low.

Bretl *et al. * [9] have applied this algorithm to the field of legged locomotion with the goal to reconstruct the 2D friction-consistent support region $\mathcal{Y}_{f}$ for the CoM in static equilibrium. This algorithm was also applied in related works to compute multi-contact ZMP support areas [30] or time-optimal trajectory timings [31, 32]. In order to reduce the confusion with other similar regions that will be defined in the upcoming Sections, in the remainder of this manuscript the support region for the CoM in static equilibrium will be referred to simply as the friction region.

Algorithm 1 reports the procedure presented in [9]; one can notice that the algorithm recursively solves a Linear Program (LP) that maximizes the horizontal position of the CoM $\mathbf{\bm{c}}_{xy}\in\mathbb{R}^{2}$ along the direction defined by the unit vector $\mathbf{\bm{a}}_{j}\in\mathbb{R}^{2}$ ($j$ being in this case the iteration index) while satisfying the friction constraints.

The algorithm considers the projection of the robot’s CoM $\mathbf{\bm{c}}_{xy}$, the mass $m$, a set of $n_{c}$ contacts $\mathbf{\bm{p}}_{i}\in\mathbb{R}^{3}$ with corresponding surface normals $\mathbf{\bm{n}}_{i}\in\mathbb{R}^{3}$, tangent vectors $\mathbf{\bm{t}}_{1,i},\mathbf{\bm{t}}_{2,i}\in\mathbb{R}^{3}$ and friction coefficients $\mu_{i}\in\mathbb{R}$ (for $i=0,\dots,n_{c}$). The constraint (III.a) enforces the static equilibrium of the forces and moments acting on the robot due to gravity $\mathbf{\bm{g}}\in\mathbb{R}^{3}$ and to the contact forces $\mathbf{\bm{f}}=[\mathbf{\bm{f}}_{1}^{T},\dots,\mathbf{\bm{f}}_{n_{c}}^{T}]^{T}\in\mathbb{R}^{m{n_{c}}}$. The matrix $\mathbf{\bm{A}}_{1}\in\mathbb{R}^{6\times mn_{c}}$ represents the grasp matrix of the set of point contacts666$[\cdot]\times$ represents the skew-symmetric operator associated to the cross product. $m=6$ for generic contacts and $m=3$ for point contacts., $\mathbf{\bm{A}}_{2}\in\mathbb{R}^{6\times 2}$ computes the $x,y$ angular components $\tau_{\mathcal{O}}^{x}$ and $\tau_{\mathcal{O}}^{y}$ of the wrench generated by the action of gravity on the CoM $\mathbf{\bm{c}}$ of the robot expressed with respect to a fixed frame $\mathcal{O}$. Constraint (III.b) ensures that the friction constraint is satisfied through the matrix $\mathbf{\bm{B}}\in\mathbb{R}^{4n_{c}\times 3n_{c}}$. Note that this represents the friction pyramids as an approximation of the more precise friction cones [33]. Such friction cones could be enforced in (III.b) thus transforming the LP of step (III) into a Second Order Cone Program (SOCP) with a negligible computational overhead.

[TABLE]

where $\mathbf{\bm{P}}\in\mathbb{R}^{2\times 3}$ is a selection matrix that selects the $x,y$ components of the CoM and $\bar{\mathbf{\bm{A}}}_{i}$ is a transformation matrix such that:

[TABLE]

With the above definitions, the friction region $\mathcal{Y}_{f}$ is defined as the set of horizontal CoM coordinates $\mathbf{\bm{c}}_{xy}\in\mathbb{R}^{2}$ for which there exists a set of contact forces that respects both static equilibrium and friction constraints:

[TABLE]

where:

[TABLE]

Fig. 4 shows the process to compute one iteration of the IP algorithm reported in Alg. 1. As it can be seen in step III, the IP does not only maximize the horizontal CoM projection $\mathbf{\bm{c}}_{xy}$ along the direction $\mathbf{a}_{i}\in\mathbb{R}^{2}$, but it also finds a feasible set of contact forces $\mathbf{\bm{f}}$ that fulfills static equilibrium and friction cone constraints (constraints III.a and III.b).

Alg. 1 can also be regarded as a projection of the feasible set $\mathcal{C}$ onto a two-dimensional region whose boundaries represent the limit torques $\tau^{x}_{\mathcal{O}},\tau^{y}_{\mathcal{O}}$, that the robot can exert to balance the effect of gravity acting on its CoM. Exploiting the assumption that the only external force acting on the CoM is gravity we then get a one-to-one mapping between the torque components and the corresponding CoM ($x,y$) coordinates:

[TABLE]

The friction region, as defined in  (14), is a 2D convex set but it is not, in general, a linear set (*i.e., *it is not a polygon). The inner and outer approximations $\mathcal{Y}_{inner}$ and $\mathcal{Y}_{outer}$ used to estimate $\mathcal{Y}_{f}$ are, however, always 2D polygons by construction. For this reason we will therefore refer to $\mathcal{Y}_{f}$ in the rest of this paper with the term friction region rather than friction polygon.

Del Prete *et al. *[10] proposed a Revisited Incremental Projection (IPR) algorithm to test static equilibrium which is shown to be faster than the original IP formulation and than other possible techniques such as the Polytope Projection (PP). However the IPR approach is only suitable for convex cones and, therefore, does not fit well with the projection of bounded polytopes that we address in the next Section.

In the next Section we will see how we modified Alg. 1 in order to obtain a 2D set that does not only respect the static equilibrium and friction cone constraints, but it also respects the actuation capabilities of the system.

III The Feasible Region

In this Section we propose the main contribution of this manuscript which is an extension of Alg. 1 in order to obtain support regions that also consider the robot’s joint-torque limits besides the constraints imposed by the friction cones.

As opposed to Alg. 1, we include the static wrench polytope $\mathcal{A}_{i}$ of every individual end-effector in contact with the environment, as given in  (11). The resulting procedure can be found in Alg. 2.

In contrast to the original IP algorithm, we also retain the possibility of exerting contact torques at the end-effectors; as a consequence we define each individual contact wrench $\mathbf{\bm{f}}_{i}\in\mathbb{R}^{m}$ where $m=3$ if the considered end-effector is perturbed by a pure force and $m=6$ if, instead, also a contact torque component is given.

In order to include in the IP the wrench polytopes $\mathcal{A}_{i}$ of the $i^{\text{th}}$ limb in contact with the environment, we reformulate  (11) as follows:

[TABLE]

Considering that the joint-space torque variable $\mathbf{\bm{\tau}}_{i}$ is limited by its minimum and maximum values $\mathbf{\bm{\underline{\tau}}}_{i}$ and $\mathbf{\bm{\overline{\tau}}}_{i}$, we can then further re-write  (17) to explicitly highlight this dependence:

[TABLE]

Equation (18) thus represents a compact notation for the static wrench polytope initially defined in  (11). We will now exploit the notation in (18) to introduce the matrix $\mathbf{\bm{G}}\in\mathbb{R}^{d\times(mn_{c})}$ and the vector $\mathbf{\bm{d}}$, result of the concatenation of all the matrices $\mathbf{\bm{J}}(\mathbf{\bm{q}}_{i})^{T}$ and vectors $\mathbf{\bm{d}}_{i}=[\mathbf{\bm{\bar{d}}}^{T}_{i},\mathbf{\bm{\underline{d}}}^{T}_{i}]^{T}\in\mathbb{R}^{2n_{l}}$ of all the individual limbs in contact with the environment:

[TABLE]

$\mathbf{\bm{G}}$ and $\mathbf{\bm{d}}$ can now be used to redefine the set $\mathcal{A}$ of actuation-consistent CoM positions and contact forces/wrenches that satisfy all the individual wrench polytopes $\mathcal{F}_{i}$ for $k=1,\dots,n_{c}$:

[TABLE]

In analogy with  (14), we can define a new set of actuation-consistent CoM positions called actuation region:

[TABLE]

As a further observation we notice that we are interested in computing the set of CoM positions $\mathcal{Y}_{fa}$ that simultaneously satisfies both the friction and the actuation constraints (see Fig. 5). This can be obtained by considering the intersection of $\mathcal{C}$ and $\mathcal{A}$:

[TABLE]

Based on  (22), the friction- and actuation-consistent region $\mathcal{Y}_{fa}$, called feasible region, can be defined as:

[TABLE]

In analogy with Alg. 1, Alg. 2 explains how $\mathcal{Y}_{fa}$ can be computed efficiently with an iterative projection.

Simultaneously imposing the inequality constraints (III.b) and (III.c) in Alg. 2 corresponds to performing an intersection of the friction cone $\mathcal{C}_{i}$ with the polytopes $\mathcal{A}_{i}$ of the corresponding contact point. This yields the set of all the contact forces that simultaneously respect both the friction cone constraints and the joint actuation limits of the $i^{\text{th}}$ limb (see for example Fig. 3). Alg. 2, in practice, is equivalent to Alg. 1 with the only difference being the constraint (III.c) relative to the actuation limits.

The actuation region $\mathcal{Y}_{a}$ (that only considers actuation constraints and no friction constraints) can be obtained by simply removing the constraint (III.b) from the LP that is solved at the step III of Alg. 2. Intermediate cases exist where some end-effector present unilateral contacts and other limbs present instead bilateral contacts. This is the case, for example, when a robot is climbing a ladder pushing with its feet and pulling with his hands. Such conditions can be captured by the presented IP modification by enforcing only the wrench polytope constraints on the bilateral contact points and by enforcing both friction pyramids and wrench polytopes on unilateral contacts.

The wrench polytope $\mathcal{A}_{i}$, unlike the friction cones $\mathcal{C}_{i}$, is a pose-dependent quantity and, as a consequence, its vertices will change whenever the robot changes its configuration. The feasible region $\mathcal{Y}_{fa}$ can thus be considered to be accurate only in a neighborhood of the current robot configuration. The distance between the current CoM projection $\mathbf{\bm{c}}_{xy}$ and the edges of $\mathcal{Y}_{fa}$ can be considered as a combined measure of the instantaneous robustness of the robot’s state with respect to the contacts’ stability and joint-space torque limits. This distance (in [m]) can also be seen as a robustness measure against possible external loads being added on top of the robot that may move the robot’s CoM even when its configuration does not change.

Wrench polytopes (and thus the feasible region $\mathcal{Y}_{fa}$), because of their local validity, must be recomputed at every configuration change which makes the motion planning formulation harder compared to cases where only the friction cones are considered. However, this local validity is also the key element of the wrench polytopes that, if properly exploited, can provide an insightful view on the relationship between robot configuration and maximal admissible force at the end-effectors.

III-A 2D Feasible Regions vs. 6D Feasible Polytopes

To achieve a better understanding of feasible regions, it is useful to underline the parallel that exists between them and their 6D counterparts (see Tab. I). In particular, the friction region $\mathcal{Y}_{f}$ can be seen as a particular case of the Contact Wrench Cone (CWC) criterion with only gravity acting on the CoM of the robot. In the same way, also the actuation region $\mathcal{Y}_{a}$ can be seen as a static case of the Actuation Wrench Polytope (AWP) and the feasible region $\mathcal{Y}_{fa}$ can be seen as a static case of the Feasible Wrench Polytope (FWP) [16].

It is possible to show that, for example, a 2D region can be obtained from the relative 6D polytope (*e.g., *Actuation Wrench Polytope (AWP) or Feasible Wrench Polytope (FWP)) by slicing the latter in correspondence of the planes: $f_{x}=0,f_{y}=0,f_{z}=mg$ and $\tau_{z}=0$. In this way only two DoFs are left which correspond to the $\tau_{x}$ and $\tau_{y}$ coordinates of the wrench space. The two-dimensional region that results from this slicing procedure can then be mapped through  (16) into a set of feasible CoM coordinates $\mathbf{\bm{c}}_{xy}$ that corresponds to the relative region (*e.g., *$\mathcal{Y}_{a}$ or $\mathcal{Y}_{fa}$).

Computing the AWP or the FWP, however, can be computationally demanding because of the high dimensionality and large amount of halfspaces and vertices. This is what motivated us to propose a variant of the IP algorithm that allows to directly map joint-torques constraints into 2D CoM limits (without the need of constructing first the whole 6D polytopes). This results in a computation of the 2D feasible region which is at least 20 times faster in presence of three point contacts and 50 times faster in the case of four point contacts. More details about the computational time of the feasible region are provided in the next Section.

III-B Computation Time

The usage of the IP algorithm implies a significant speed up for the computation of the actuation region reaching average computation times in the order of milliseconds (see Fig. 6) which makes it suitable for online motion planning.

The solve time of the IP algorithm depends on the number of inequality constraints embedded in it (only friction constraints, only actuation constraints, or both). The most favorable scenario is when only friction cones are considered (red in Fig. 6): in the case of linearized friction cones with four facets per pyramid, the IP will present $4n_{c}$ inequalities. The least convenient scenario is instead when both friction pyramids and wrench polytope constraints are considered (blue in Fig. 6), in this case the IP will include $(4+2n_{l})n_{c}$ inequalities (assuming that all the limbs in contact with the ground have same number of DoFs $n_{l}$ and that the friction cones are linearized with 4 halfspaces). In the case of the HyQ quadruped this will result in $10$ inequalities per foot contact; in the case of a humanoid robot with 6 DoFs per leg, instead, this will result in $16$ inequalities per foot contact. Figure 7, for example, shows the friction region $\mathcal{Y}_{f}$ (green) and the feasible region $\mathcal{Y}_{fa}$ in the case of the HRP-4 robot standing still in a configuration with non-coplanar contacts.

The last row of Fig. 6 shows that, even in such inconvenient condition where all contacts are subject to both friction and actuation constraints, the solve time is below $10ms$ in a four-stance configuration and below $7.5ms$ in a triple-stance configuration in $99.5\%$ of the computations (blue histogram). This allows the efficient computation of the feasible region at a frequency of, at least, $100\text{\,}\mathrm{Hz}$ in a four stance configuration and $133\text{\,}\mathrm{Hz}$ in a triple stance configuration of a quadruped robot777These computation times, as much as the other performance reported in this manuscript, have been achieved on a 8-core Intel Xeon(R) CPU E3-1505M v6 @ 3.00GHz computer.. These frequencies could be further increased by reducing the tolerance factor of the IP algorithm (the tolerance value we used was $10^{-6}m^{2}$).

III-C The Feasible Region under Different Loading Conditions

Fig. 8 reports various tests of computation of feasible 2D areas for different gravitational loads applied on the CoM of the robot. We can see that the heavier the load on the robot, the smaller the area of the corresponding feasible region. This is analogous to the computation of feasible regions for different percentages of torque limits while keeping the load on the robot fixed. The blue dashed lines represent the classical friction region $\mathcal{Y}_{f}$ as defined by Bretl *et al. *[9].

Figs. 8(a) and 8(c) depict the feasible regions $\mathcal{Y}_{fa}$ for the HyQ robot with four and three coplanar stance feet.

Figs. 8(b) and 8(d) depict the actuation regions $\mathcal{Y}_{a}$ in the same configurations with four and three coplanar stance feet. Such actuation consistent areas $\mathcal{Y}_{a}$ alone are not directly applicable in the field of legged locomotion where robots typically make and break contacts using their feet and have therefore no possibility to grasp the terrain. Feasible regions $\mathcal{Y}_{fa}$ should be used instead since they include the friction constraints that also naturally encode the unilaterality constraint.

As visible in Figs. 8(b) and 8(d) the robot’s CoM might lean outside of the classical friction region $\mathcal{Y}_{f}$ (dashed blue line) depending on the magnitude of the load acting on it: this is a typical condition in which one of the contacts is meant to pull the ground to maintain equilibrium.

As a final consideration, comparing the figures related to the same number of stance feet (Fig. 8(d) compared with 8(c) and Fig. 8(b) compared with 8(a)) one can see that the feasible region $\mathcal{Y}_{fa}$ cannot be obtained by simple intersection of the friction region $\mathcal{Y}_{f}$ and the actuation region $\mathcal{Y}_{a}$. Although this approximation might be accurate under specific conditions, in general the intersection and projection operators do not commute [29]. Let us consider $\mathcal{C}$ to be the set of contact forces and CoM positions $\mathbf{\bm{c}}_{xy}$ that respect static equilibrium and friction constraints (see  (15)); let us then also consider $\mathcal{A}$ defined as the set of contact forces and horizontal CoM positions $\mathbf{\bm{c}}_{xy}$ that respect all static equilibrium, wrench polytopes and friction cones constraints (see  (18)). The friction region [9] can then be defined compactly as:

[TABLE]

the actuation region as:

[TABLE]

and the (actuation- and friction-consistent) feasible region as:

[TABLE]

where $IP$ is the Iterative Projection operator. The projection and intersection are non-commutative operators and, in particular, the following inclusion always holds:

[TABLE]

$\mathcal{Y}_{fa}$ is therefore more conservative than the intersection of $\mathcal{Y}_{f}$ and $\mathcal{Y}_{a}$. Intuitively,  (27) might be explained by considering that there may exist CoM positions that, at same the time provide feasible wrench solutions if the friction cones or wrench polytope constraints are considered individually but they provide unfeasible wrench solutions if these constraints are considered simultaneously. As a consequence, the feasible region $\mathcal{Y}_{fa}$ has to be a subset of the intersection between $\mathcal{Y}_{f}$ and $\mathcal{Y}_{a}$.

IV Center of Mass and Foothold Planning

In this Section we employ the concept of feasible region $\mathcal{Y}_{fa}$ introduced in Section III for the sample-based optimization of feasible footholds and CoM trajectories. The all locomotion scheme proposed in this Section is based on the assumption that a robot is assumed to be statically stable and, simultaneously, a set of admissible joint-torques exists iff:

[TABLE]

where $\mathbf{\bm{c}}_{xy}=\mathbf{\bm{P}}\mathbf{\bm{c}}$ is the CoM projection on the horizontal plane. Robustness against possible modelling errors or external disturbances can then be enforced by considering the minimum distance $r$ between $\mathbf{\bm{c}}_{xy}$ and the edges of the feasible region. This can be found by solving the following LP:

[TABLE]

where $N_{h}$ is the number of edges of $\mathcal{Y}_{fa}$, $\mathbf{\bm{a}}_{i}\in\mathbb{R}^{2}$ is the normal to the $i^{\text{th}}$ edge and $b_{i}\in\mathbb{R}$ is the known term. $r$ is thus the radius of the largest ball centered in $\mathbf{\bm{c}}_{xy}$ and inscribed inside $\mathcal{Y}_{fa}$. and it can also be seen as a static instantaneous measure (*i.e., *a margin) of how far the robot is from slipping or from hitting one of joint-torque limits (actuation limits).

IV-A CoM planning strategy

As we only deal with CoM planning in this Section, we will assume the gait sequence, phase timings and step locations to be predefined. Since the feasible region, at the actual state, is restricted by the quasi-static assumption (an extension to the dynamic case with non-negligible CoM horizontal acceleration is part of future works) a quasi-static gait is a good template to test its applicability.

As the main hardware platform for our experiments is the quadruped robot HyQ, we will consider here a static quadrupedal gait called crawl [34]. The crawl is typically divided in two main phases called swing phase and move-base phase. During the swing phase, the robot does not move its trunk and only one foot at the time is allowed to lift-off from the ground and move to a new foothold while all the other three feet have to be in stance. During the move-base phase, instead, all four feet are in stance and the robot moves its trunk to a target location and orientation.

The most critical phase of this static gait, in terms of stability and margin with respect to the joint-torque limits, is this triple stance phase (*i.e., *the swing phase) because the robot’s weight must be distributed only on three legs. The CoM is meant to move only during the four-stance phase, to enter the future support region which is opposite to the next swing leg. Therefore, after each touchdown, we re-plan a polynomial trajectory that links the actual CoM position with a new target inside the future support region. This enables us to completely unload the swing leg before liftoff and naturally distribute the weight onto the other three stance legs. In our previous work [34] we computed this target heuristically without any awareness of joint-torque limits. Specifically, we were computing the target point at a hand-tuned distance from the main diagonal of the support triangle, in order to sufficiently load the off-diagonal leg. However, this can be inaccurate in complex terrains, because:

the friction region $\mathcal{Y}_{f}$ coincides with the feasible region $\mathcal{Y}_{fa}$ only when every individual limb of the robot is able to carry the total body weight of the robot; 2. 2.

an increased load on the robot or an inconvenient robot configuration can further restrict the feasible region $\mathcal{Y}_{fa}$ making it considerably smaller than the friction region.

Therefore, the heuristic target, since it is not formally taking these aspects into account, might fail in situations that are more demanding due to a complex terrain geometry. Conversely, using the feasible region allows us to select a target position for the robot’s CoM that results in an admissible and statically stable configuration, in the case of:

1. a generic terrain shape (i.e. non coplanar feet, each one with different normal at the contact)
2. different loading conditions.

We plan the CoM target to be inside a scaled feasible region by a predefined scaling factor $s$. This allows us to increase the robustness of our strategy against possible external disturbances and modelling inaccuracies that may arise from the static locomotion assumption that we took in (11)888This has a particular effect on the execution of dynamic motions on electrically-actuated robots whose torque limits (and thus their wrench polytopes) depend on the joint velocity..

At the touch-down instant, we compute the feasible region $\mathcal{Y}_{fa}$, considering as inputs the position of the three stance feet of the future support triangle (the feet sequence is predefined) and the corresponding normals $\mathbf{\bm{n}}_{i}$ at the expected contact points. To evaluate the Jacobians (necessary to map the actuation constraints into a set of admissible contact forces), we also provide the future CoM position predicted by the heuristics. If the projection of the actual CoM $\mathbf{\bm{c}}_{xy}=\mathbf{\bm{P}}\mathbf{\bm{c}}$ is inside $\mathcal{Y}_{fa}$, we then set the target CoM equal to the actual CoM $\mathbf{\bm{c}}\in\mathbb{R}^{3}$. If it is, instead, outside the region $\mathcal{Y}_{fa}$, we set the target CoM equal to the point $\mathbf{\bm{x}}^{*}$ on the boundary of the region (or of the scaled region if we want to provide a certain degree of robustness) that is closest to $\mathbf{\bm{c}}_{xy}$. This allows us to minimize unwanted lateral/backward motions. To obtain the point $\mathbf{\bm{x}}^{*}$ we solve the following QP program:

[TABLE]

where we minimize the Euclidean distance between a generic inner point $\mathbf{\bm{x}}$ and the actual CoM projection $\mathbf{\bm{c}}_{xy}$. $\mathbf{\bm{A}}$ and $\mathbf{\bm{b}}$ matrix represent the half-space description of the polygon $\mathcal{Y}_{fa}$.

The CoM target is depicted as a yellow cube in Fig. 9 while the blue cube represents the heuristic target. In the same picture we show an image of the feasible region $\mathcal{Y}_{fa}$ (light gray) and the scaled feasible region (dark gray) scaled by a factor of $s=0.8$. The dashed triangle represents the friction region $\mathcal{Y}_{f}$999Note that just scaling the value of joint-torque limits (instead of the vertices of the feasible region) might not results in a conservative region. This is because some boundaries of the resulting feasible region could be determined by the friction region itself, thus reducing the joint-torque limits would not result in an increase of robustness with respect to those boundaries. For this reason, it is advisable to scale directly the vertices of the feasible region rather than joint-torque limits used to compute the feasible region.. The scaling procedure can be defined as an affine transformation that preserves straight lines and parallelism relationships among the edges of the feasible region. The scaling can be done with respect to the Chebyshev center (*i.e., *the center of the largest ball inscribed in the feasible region) or with respect to the centroid. The former is more computationally expensive because it requires the solution of an LP; the latter is faster to compute because it can be found analytically as the average of all the vertices. The centroid $v_{c}$ can be considered as a good approximation of the Chebychev center whenever the feasible region presents good symmetry properties101010Whenever the feasible region is not symmetric, however, the centroid might considerably differ from the Chebychev center thus resulting in a value of the robustness margin $r$ lower than desired.. In the case that $v_{c}$ is used, the vertices $\hat{v}$ of the scaled region $\hat{\mathcal{Y}}_{fa}$ can be computed by scaling the vertices $v$ of $\mathcal{Y}_{fa}$ as: $\hat{v}=s(v-v_{c})+v_{c}$ where $s\in(0.0,1.0]$ is the scaling factor.

IV-B Foothold Planning

The foothold planning strategy that we present in this Section represents a sample-based strategy to improve the navigation capabilities of the HyQ quadruped robot on rough terrains. Our strategy employs the height map provided by the perception module and seeks among the terrain samples the foot location that maximizes the area of the corresponding feasible region $\mathcal{Y}_{fa}$.

We exploit the computational efficiency of the IP algorithm as in Alg. 2 in order to plan foothold locations that ensure the robot’s stability and actuation consistency while traversing rough terrains. As in the previous Section, we assume here a static crawl gait with predefined durations of the stance and swing phases. The idea is to find, at each lift-off, the most suitable foothold to maximize the area of the feasible region for the next swing leg. Our strategy consists in sampling a set of $p$ candidate footholds around the default target foothold (from heuristics) located along the direction of motion. We then evaluate the height map of the terrain in those sampled points (*i.e., *correcting the corresponding $z$ coordinate and swing orientation to adapt to the perceived terrain surface 111111To avoid corrections in unwanted directions, we define the sampling direction along the direction of the predicted step, (i.e. in consistency with the desired velocity).) [35, 36]. The default step location is simply a function of the user-defined desired linear and angular velocities of the robot and it neither considers the external map of the surrounding environment, nor the stability and actuation consistency requirements [34]. Fig. 9 shows a foothold planning simulation in which eight different candidate footholds (red spheres) are considered. As additional feature, we discard the footholds that:

1. are close to the edge, 2) would result in a shin collision, 3) are out of the leg’s workspace.

In the simulation shown one out of 8 is discarded because it was too close to the edge of the pallet. The next step consists in computing the feasible regions $\mathcal{Y}_{fa}^{i}$ for the $p=8$ considered foot locations ($i=1,\dots,p$) keeping fixed the set of feet that will be in stance during the following swing phase. Since the feasible region depends on the robot configuration, we consider the future position of the CoM (computed through the heuristics) for the next triple stance phase and obtain the future joints configuration through inverse kinematics. This joint configuration is then used to update the Jacobians needed for the computation of the candidate feasible region $\mathcal{Y}_{fa}^{i}$. The foothold planner then selects, among the reduced set of admissible footholds, the one that maximizes the area of the corresponding feasible region121212Another approach could consist in maximizing the residual radius (i.e. radius of the largest circumference inscribed in the region), however, we noticed that often multiple candidate footholds may return the same residual radius but different areas. This is the case any time that the CoM projection is closer to a friction-limited edge rather than an actuation-limited edge of the feasible region..

In the baseline walking on flat terrain, when joint-torques are far away from their limits, the default foothold is selected. Conversely, on more complex terrain, when the robot is far from a default configuration (*e.g., *when one leg is much more retracted than the other legs), the scaled version $\hat{\mathcal{Y}}_{fa}$ (described in the previous Section) can take on a small area (see Fig. 9). In this case the default step will be corrected (yellow ball in Fig. 9) in order to enlarge this area and, as a consequence, to increase the robustness to model uncertainties and tracking errors. The default target is not visible because, being computed on a planar estimation of the terrain [34], it turns out to be inside the terrain.

V Simulation and Experimental Results

The improvement of a planning strategy based on the feasible regions with respect to our previous heuristic strategy can be demonstrated by either increasing the load acting on the robot during a standard walk on a flat terrain or by addressing challenging terrains. Both scenarios, and any combination of them, bring indeed the robot closer to its actuation limits.

As a first result we report the validation of the feasibility margin defined as the distance between the CoM projection and the edges of the feasible region. We then report some simulation and experimental data of the CoM and foothold strategy that we described above in Sec. IV-A. The results of this strategy can be seen in the accompanying video131313https://youtu.be/9pvWO2Qmo9k.

V-A Validation of the Feasibility Margin

Figure 10 represents the data collected in a simulation where we applied on the CoM of the HyQ robot a vertical increasing force from $0\text{\,}\mathrm{N}$ up to $-600\text{\,}\mathrm{N}$ (upper plot). This force represents a possible external payload applied on the robot and, by increasing its amplitude, we are interested in understanding how the feasible region $\mathcal{Y}_{fa}$ adapts to it.

In the lower plot of Fig. 10 we can see that, as a consequence of this increasing external load, the feasible region gradually shrinks with a consequent reduction of the feasibility margin $r$ from $0.24\text{\,}\mathrm{m}$ to about $0.06\text{\,}\mathrm{m}$. Recall that the feasibility margin $r$ is defined as the minimum distance between the CoM projection $\mathbf{\bm{c}}_{xy}$ and the edges of the feasible region $\mathcal{Y}_{fa}$ (as in  (29)).

For this validation we also introduce the joint-torque limits violation flag $\beta$ whose definition is the following:

[TABLE]

Note that a negative $r$ means that the CoM projection $\mathbf{\bm{c}}_{xy}$ lies outside the edges of the feasible region $\mathcal{Y}_{fa}$. After second $74$ (yellow vertical line) the external load is fixed to $-600\text{\,}\mathrm{N}$ and the robot starts displacing laterally with an increasing $c_{y}$ coordinate. The second plot from above shows that, when the robot has moved laterally of about $0.12\text{\,}\mathrm{m}$ (red vertical line), the feasibility margin $r$ becomes zero and, approximately at the same time, the torque limits violation flag $\beta$ becomes one, meaning that one of joint-torque limits of the robot has been reached (second plot from the bottom).

V-B Walk in Presence of Rough Terrain and External Load

The next simulation result that we report in this Section is a walk over a $0.22\text{\,}\mathrm{m}$ high pallet, where the HyQ robot only lifts two lateral legs on the pallet while the two other legs always remain on the flat ground. The considerable height of the pallet and the asymmetry of the terrain force the robot to take on complex configurations to step up and down the obstacle and, even if no further external load is applied, the robot might easily reach its joint-torque limits. In this scenario we compare the behavior of two different strategies:

friction-region based walk: this motion planning approach combines the foothold selection strategy explained in Sec. IV-B with a CoM motion planning that aims at always keeping the CoM projection inside the scaled friction region $\mathcal{\hat{Y}}_{f}$; 2. 2.

feasible-region based walk: this approach uses the same foothold strategy as above but makes sure that the CoM projection always lies inside the scaled feasible region $\mathcal{\hat{Y}}_{fa}$ rather than $\mathcal{\hat{Y}}_{f}$. In this way therefore both friction and actuation constraints are explicitly considered at the motion planning level and are continuously re-planned for with a receding horizon of one step.

Evaluating the performance of these two strategies using the feasibility margin $r$ would skew the results in favor of the latter method, considering that the planner always makes sure that there exists a minimum feasible margin $r$ itself. For the assessment of the two planners’ performance we therefore define the minimum joint-torque margin $m_{\tau}$. This corresponds to the minimum distance between the torque of each joint of the robot and their corresponding maximum and minimum values:

[TABLE]

where:

[TABLE]

The quantity $m_{\tau}$ measures how well the proposed online motion planner is able to keep the joint-torques away from their limits, while navigating complex geometry environments, being able to reach to the user direction commands or to unexpected disturbances.

It is important to mention that we evaluate $m_{\tau}$ only during during the triple support phases (*i.e., *when only three legs are in contact with the ground and the fourth leg is in swing). This is because the triple support phase is the most critical for joint-torque limits (all the robot’s weight is loaded on three legs rather than four) and because, as a consequence, the CoM planning strategy optimizes the position of the CoM only for this phase. Because of the static assumption that we assumed in  (11), the feasible region computation is only valid when the velocity of the robot’s base is zero, condition which is not respected during the four-stance phase (*i.e., *when the robot’s base moves).

The values of $m_{\tau}$ for the two simulations are reported in the upper plot of Fig. 12. The red line shows the evolution of $m_{\tau}$ in the case of the friction region-based planning over the entire simulation (up to $14s$). The blue line shows instead the evolution of $m_{\tau}$ in the case of the feasible region-based planning over the entire simulation (up to $21s$). The recording of both simulations is stopped when the robot steps down the pallet with all four legs, the different duration of the simulations is therefore due to the different behavior they present during the negotiation of the pallet. We can notice that the minimum joint torque margin reached by the friction region based simulation of 35\text{\,}\mathrm{N}\text{\cdot}\mathrm{m} (dashed red line) occurs towards the conclusion of the experiment when the robot steps down the pallet with the last leg. The feasible region based walk instead performs an increased number of shorter steps before stepping down the pallet, in this way the simulation lasts longer and the minimum joint-torque margin of 39\text{\,}\mathrm{N}\text{\cdot}\mathrm{m} (dashed blue line) is higher than the simulation where only friction was considered.

The lower plot of Fig. 12 refers instead to a hardware experiment where the HyQ robot walks over a moderately rough terrain made of bricks and plastic tiles while also carrying a $10\text{\,}\mathrm{kg}$ extra load on its trunk (snapshots of this test can be seen in Fig. 11). As the robot proceeds with a predefined foothold sequence and timing, the sample-based motion planner allows the robot to adapt its footholds and the trajectory of the CoM to the height map received by the perceptive module. Having a limited number of sampled footholds $p$ allows one to set an upper bound on the maximum computation time required by the planner. In the considered experiment we had $p=9$ which results in 9 sequential repetitions of the IP algorithm for an overall computation time of about $63ms$ (every evaluation of a triple-stance phase takes in average $7ms$). This enables the replanning at a frequency of about $15\text{\,}\mathrm{Hz}$ in order to always include the latest values of the height map or possible changes in the state of the robot141414These computational performance would be significantly further improved by a parallelization of each foothold evaluation..

Also in this case, as in the simulation, the feasible region based approach presents a higher minimum joint-torque margin of 29\text{\,}\mathrm{N}\text{\cdot}\mathrm{m} (dashed blue line) compared to the 21\text{\,}\mathrm{N}\text{\cdot}\mathrm{m} margin that we measured for the friction region based approach (dashed red line). The video of the hardware experiments can be found in the accompanying video.

VI Discussion

In this Section we briefly address a few important aspects of the proposed feasible region such as its possible usage for dynamic gaits, whole-body control performance evaluation and for applications with bilateral contacts:

• •

Relaxation of the quasi-static assumption: a dynamic extension of the feasible region, that considers current configuration, velocity and acceleration of the robot, could be obtained performing the two following actions:

solve Alg. 2 employing the dynamic wrench polytopes, given in (10), rather than the static wrench polytopes given in (11); 2. 2.

include the fictitious forces acting on the robot’s CoM inside the vector $\mathbf{\bm{u}}$ in (12)151515This corresponds to slicing the FWP with a plane orthogonal to the aggregated centroidal wrench (*i.e., *gravito-inertial wrench) [16].

The above can be carried out given the current configuration, velocity and acceleration of the robot for visualization purposes or for testing the feasibility of the current state. Please note that this would also allow to explicitly consider the dependence of electric actuators on the joint velocity. However, using this dynamic extension of the feasible region for online motion planning purposes is more challenging because of the curse of dimensionality. One possibility for achieving semi-dynamic motions would consists in employing the static feasible region, scaled by a factor $s$ to enforce some robustness margin [11], in combination with a dynamic reference point such as the ZMP or the ICP;

• •

Benchmarking of whole-body controllers: Having the CoM inside the feasible region $\mathcal{Y}_{fa}$ provides a formal guarantee of existence of, at least, one set of contact forces that, simultaneously, satisfies the wrench polytope constraints and the friction cone constraints. $\mathcal{Y}_{fa}$ can thus also play a role in the evaluation of the performance of whole-body controllers. The interest is of this approach lies in the fact that constraints with different units ([$\text{\,}\mathrm{N}$] for friction force and [\text{\,}\mathrm{N}\text{\cdot}\mathrm{m}] for joint-torques) are naturally mapped into a single number (in [$\text{\,}\mathrm{m}$]);

• •

Bilateral contacts: actuation-consistent regions $\mathcal{Y}_{a}$ can be useful in other fields of robotics, such as loco-manipulation tasks, or whenever in presence of bilateral contacts (*e.g., *climbing robots with magnetic grippers [37]), heavy-duty walking machines with footsteps anchored to predefined locations (*e.g., *the TITAN series robots [38]) and whenever a gripper/hand attached to the base link might lift an external weight and thus change the robot’s loading conditions.

VII Conclusion

In this paper we presented an approach for projecting joint-torque constraints from the high dimensional joint space ($n+6$ DoFs) of legged robots to the two-dimensional subspace of the CoMhorizontal plane. Despite the static assumption and the local nature of the resulting 2D feasible region, this strategy introduces the possibility to embed torque limitations of the actuators inside the formulation of motion planners based on simplified dynamic models and thus to efficiently plan realistic locomotion at high frequencies.

The feasible region $\mathcal{Y}_{fa}$ does not suffer from limitations related to specific robot morphologies or specific terrains (*e.g., *flat terrains). As a consequence, this friction-and-actuation consistent area $\mathcal{Y}_{fa}$ can be employed for motion planning of legged robots on rough and complex terrains (the only limiting assumption being of static locomotion), where classical simplified models fail.

Thanks to the computational efficiency of the feasible region $\mathcal{Y}_{fa}$ estimation, actuation-consistency and robustness can be tested online at a minimum of $100\text{\,}\mathrm{Hz}$ rate and without any approximation regarding the location and orientation of the contacts. This last point allows our approach to be embedded in a map-based foothold optimization strategy that samples at $15\text{\,}\mathrm{Hz}$ feasible footholds on the height map provided by the vision module (see Fig. 9).

We reported simulations of the HyQ robot crossing a pallet of $0.15\text{\,}\mathrm{m}$ and hardware experiments of the real robot on a terrain of mild roughness (with bricks up to $0.1\text{\,}\mathrm{m}$) while carrying an additional load of $10\text{\,}\mathrm{kg}$. The foothold strategy corrected the steps (*e.g., *making them smaller) when needed to increase the robustness to uncertainty and to disturbances represented by the feasibility margin $r$.

In the two following we present a few possible future developments of the work presented in this paper:

• •

Global feasible region: as we discussed in this paper the feasible region is a configuration dependent quantity, whose validity is thus guaranteed only in a local neighborhood of the considered state. Our future efforts will focus on the computation of a global extension of the feasible region which only depends on the contacts’ pose;

• •

Offline pre-computation and differentiation: it has been shown in the past that CoM-admissible regions that respect feasibility constraints such as kinematic limits and collisions can be learned using proper function approximators [39]. A similar approach could be employed to learn an approximate differentiable relationship between the robot’s state and the feasible region. This would allow us to embed the feasible region as a constraint within, for example, a gradient descent trajectory optimization problem.

VIII Appendix

We recall in this Appendix few of the main concepts and definitions connected to computational geometry that are heavily used in this manuscript. Most definitions are taken from the following sources [40, 41, 42, 43].

VIII-A Generic Bounded and Unbounded Polyhedra Definitions

Main definitions and terminology used in sets representation and adopted in this paper:

• •

A convex polyhedron $\mathcal{H}$ is a subset of $\mathbb{R}^{d}$ that solves a finite set of $m$ linear inequalities. The volume of a polyhedron can therefore be either bounded or unbounded. This is a generic definition that may include both (bounded) polytopes and (unbounded) polyhedral cones.

[TABLE]

with $\mathbf{\bm{A}}\in\mathbb{R}^{m\times d}$ and $\mathbf{\bm{b}}\in\mathbb{R}^{m}$.

• •

A convex polytope $\mathcal{P}$ is a subset of $\mathbb{R}^{d}$ that solves a finite set of $m$ linear inequalities and is bounded.

[TABLE]

with $\mathbf{\bm{A}}\in\mathbb{R}^{m\times d}$ and $\mathbf{\bm{b}}\in\mathbb{R}^{m}$.

• •

A convex polygon $\mathcal{P}$ is a polytope in dimension $d=2$:

[TABLE]

with $\mathbf{\bm{A}}\in\mathbb{R}^{m\times 2}$ and $\mathbf{\bm{b}}\in\mathbb{R}^{m}$.

• •

A convex zonotope $\mathcal{Z}$ is a special kind of polytope in $\mathbb{R}^{d}$ that presents particular symmetry with respect to the its center [44, 45]. A zonotope can therefore be fully described by its center $\mathbf{\bm{c}}\in\mathbb{R}^{d}$ and its $p$ generators $\mathbf{\bm{g}}\in\mathbb{R}^{d}$.

[TABLE]

• •

A convex polyhedral cone $\mathcal{C}$ is a subset of $\mathbb{R}^{d}$ that solves a finite set of $m$ linear inequalities. Geometrically, each linear inequality defines a hyperplane that has to pass through the origin.

[TABLE]

with $\mathbf{\bm{A}}\in\mathbb{R}^{m\times n}$ and $\mathbf{\bm{0}}\in\mathbb{R}^{m}$ is a null vector.

Convex polyhedra, polytopes, zonotopes and cones are called $d$-polyhedra ($d$-polytopes, $d$-zonotopes or $d$-cones) if they have a non-zero interior in $\mathbb{R}^{d}$;

In the computational geometry terminology, a hyperplane $h$ of $\mathbb{R}^{d}$ is a supporting hyperplane of the polyhedron $\mathcal{H}$ if one of the closed halfspaces of $h$ contains $\mathcal{H}$. A face $\mathcal{F}$ of $\mathcal{H}$ is a generic term to indicate either an empty set, $\mathcal{H}$ itself or the intersection between $\mathcal{H}$ and a supporting hyperplane. The faces of dimension $0,1,d-1$ and $d-2$ are usually named vertices, edges, ridges or facets [40].

• •

A half-space is either of the two parts in which a hyperplane divides an affine space.

• •

A generator is a broad term to indicate all the elements of Euclidean space $\mathbb{R}^{d}$ that can be used to represent the considered set. Depending on the considered type of polyhedron, generators may include vertices, rays (or edges).

According to the Minkowski-Weil theorem [42], polyhedra can be equivalently described in terms of their half-spaces ($\mathcal{H}$-description) or in terms of their generators ($\mathcal{G}$-description). The generators, depending on the considered geometrical object, can consist of vertices ($\mathcal{V}$-description), rays ($\mathcal{R}$-description) or an interval ($\mathcal{I}$-description). Polytopes, for example, can be equivalently described in terms of $\mathcal{H}$- and/or $\mathcal{V}$-description. Polyhedral cones $\mathcal{C}$ can be equivalently described in terms of $\mathcal{H}$-description (see  (39)) and/or $\mathcal{R}$-description:

[TABLE]

where $p$ is the number of rays of the set of rays $\mathcal{R}$:

[TABLE]

A cone, however, can not be represented by $\mathcal{V}$-description as it only owns one vertex which is placed in the origin of the reference frame.

VIII-B Minkowsky Sums and Convex Cones

In the following we will discuss the main properties of sum of sets and convex hull algorithm:

• •

Given two convex sets $\mathcal{A}$ and $\mathcal{B}$, their addition (called Minkowski sum), indicated by the operator $\oplus$, another set is defined as the sum of the all elements of $\mathcal{A}$ with all the elements of $\mathcal{B}$:

[TABLE]

which presents a $\mathcal{O}(a\cdot b)$ time (where $a$ is the cardinality of $\mathcal{A}$ and $b$ is the cardinality of $\mathcal{B}$).

• •

For a given convex set $\mathcal{S}=\{\mathbf{\bm{s}}_{1},\dots,\mathbf{\bm{s}}_{n}|\mathbf{\bm{s}}\in\mathbb{R}^{d}\}$ composed of $n$ finite elements of dimension $d$, their convex hull is defined as the set of all the convex combinations of all its elements (e.g. the vertices of a polytope):

[TABLE]

The convex hull distributes over the Minkowski sum, meaning that the following property holds:

[TABLE]

In the worst-case output the complexity of the problem is $\mathcal{O}(n^{[d/2]})$.

For the computation of many locomotion related geometrical objects, such as the Contact Wrench Cone (CWC), it is important to notice that, given the $\mathcal{R}$-representation of two polyhedral cones $\mathcal{C}_{1}$ and $\mathcal{C}_{2}$:

[TABLE]

the $\mathcal{R}$-representation of their Minkowski sum $\mathcal{C}_{sum}$ can be obtained by stacking together (i.e., using the union operator $\cup$) the set of rays $\mathcal{R}_{1}$ and $\mathcal{R}_{2}$ of the two individual cones:

[TABLE]

Despite yielding a redundant representation with internal rays, this property allows a considerable speed-up ($\mathcal{O}(p_{1}+p_{2})$ time) compared to the Minkowski sum of two convex bounded polytopes ($\mathcal{O}(p_{1}\cdot p_{2})$ time).