IEEE Robotics and Automation Letters (RAL) paper presented at the

2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

Madrid, Spain, October 1-5, 2018

Application of Wrench based Feasibility Analysis to the Online

Trajectory Optimization of Legged Robots
Romeo Orsolino1 , Michele Focchi1 , Carlos Mastalli1,2 ,
Hongkai Dai3 , Darwin G. Caldwell1 and Claudio Semini1
Abstract— Motion planning in multi-contact scenarios has The present paper tackles this issue using simplified
recently gathered interest within the legged robotics community, dynamic models that still contain sufficient details of the
however actuator force/torque limits are rarely considered. We system. The use of the centroidal dynamics [4] coupled with
believe that these limits gain paramount importance when the
complexity of the terrains to be traversed increases. We build the CWC-based planning represents a step in this direction,
on previous research from the field of robotic grasping to allowing to remove the limitation of having coplanar con-
propose two new six-dimensional bounded polytopes named the tacts (as for Zero Moment Point (ZMP) based approaches)
Actuation Wrench Polytope (AWP) and the Feasible Wrench and thus increasing the complexity of motions that can be
Polytope (FWP). We define the AWP as the set of all the generated [5], [6]. This has also led to the formulation of
wrenches that a robot can generate while considering its
actuation limits. This considers the admissible contact forces algorithms that can efficiently verify robots stability in multi-
that the robot can generate given its current configuration contact scenarios [7], [8]. Such approaches however still
and actuation capabilities. The Contact Wrench Cone (CWC) fail to capture some properties of the robot - such as the
instead includes features of the environment such as the contact actuation limits, the joints kinematic limits and the possible
normal or the friction coefficient. The intersection of the AWP self-collisions. These properties become more and more
and of the CWC results in a convex polytope, the FWP, which
turns out to be more descriptive of the real robot capabilities important with the increasing complexity of the environment
than existing simplified models, while maintaining the same and we believe that they should not be neglected in motion
compact representation. We explain how to efficiently compute planning. To the best of the author’s knowledge, while
the vertex-description of the FWP that is then used to evaluate actuation constraints have been considered at the control
a feasibility factor that we adapted from the field of robotic level [9], [10], this is the first time that a framework for the
grasping [1]. This allows us to optimize for robustness to
external disturbance wrenches. Based on this, we present an formulation of actuation consistent online motion planners is
implementation of a motion planner for our quadruped robot provided. As later explained, the strategy consists in devising
HyQ that provides online Center of Mass (CoM) trajectories CoM trajectories that are guaranteed to respect the actuation
that are guaranteed to be statically stable and actuation- and friction constraints, without explicitly optimizing neither
consistent. the joint torques nor the contact forces.
I. I NTRODUCTION
A. Contribution
Legged locomotion in rough terrains requires the careful
selection of a contact sequence along with a feasible motion In this paper we address the problem of devising actuation-
of the CoM. In case of an unexpected event (e.g. changes in consistent motions for legged robots and, in particular, we
the terrain conditions, human operator commands, external propose the four following contributions: a) first, we in-
force disturbance, inaccuracies in the state estimation and in troduce the concept of Actuation Wrench Polytope (AWP)
the terrain mapping, etc.) replanning is an important feature which complements the CWC, adding the robot-related con-
to avoid accumulation of errors. As a consequence, ideal straints such as its configuration and actuation capabilities.
motion planners for complex terrains should be fast but The consideration of both the environment-related constraints
accurate. Approaches that use simplified dynamic models (the CWC) and the robot-related constraints (the AWP) leads
are especially fast but they only capture the main dynamics to the definition of a second convex polytope that we call
of the system [2]. On the other hand, other approaches use Feasible Wrench Polytope (FWP). This can be seen as a
the whole-body model of the robot and provide particularly development of the Grasp Wrench Space (GWS) proposed
accurate joint torques and position trajectories, but are not for robotic grasping [1]. Disregarding the constraints due to
suitable for online applications in arbitrary terrains. A third self-collision and kinematic joints limits, the FWP can then
option consists in offline learning primitives and behaviors be used as a sufficient criterion for legged robots stability;
generated with the more accurate whole-body models that b) second, we exploit recent advancements in computa-
can be later quickly realized in real-time [3]. tional geometry [11] to compute the vertex-description (V-
description) of the FWP, drastically reducing the computation
1 Department of Advanced Robotics, Istituto Italiano di Tecnologia, Gen- time with respect to the double-description (D-description)
ova, Italy. email: {romeo.orsolino, michele.focchi, based methods; c) third, we adapt the vertex-based feasibility
darwin.caldwell, claudio.semini}@iit.it factor, as in [1], to evaluate online the feasibility of a motion
2 CNRS-LAAS, University of Toulouse, Toulouse, France email:
[email protected]
plan for legged robots. d) Finally, we exploit this feasibility
3 Toyota Research Institute (TRI), Los Altos, USA email: factor for the online generation of CoM trajectories that are
[email protected] statically stable and actuation-consistent.
B. Outline of successful experimental implementations on the hardware
The rest of this paper is organized as follows: we first is mainly due to the fact that often the desired complex
discuss the previous research in the field of wrench based movements require the torques to be beyond the limits of
feasibility analysis with a special consideration for the robot the actuators. Indeed, the actuation capabilities become even
stability and actuation-consistency (Section II). We then more critical when the robot interacts with an environment of
introduce the computation of the AWP [12] and an efficient complex geometry. Therefore, an accurate evaluation of the
strategy to calculate the V-description of the FWP (Sec- robot actuation capabilities takes on paramount importance.
tion III). Section IV introduces the FWP-based feasibility
metric and Section V describes how this can be used for
online motion planning. Section VI presents the simulations
and experimental results we obtained by implementing our
strategy on the Hydraulically actuated Quadruped (HyQ)
robot [13]. Finally, Section VII draws the conclusions with
a brief discussion on the results and on future developments.

II. R ELATED W ORK

Wrench based feasibility analysis is not a novel idea in
robotics. In the field of Cable-Driven Parallel Robots (CDPR)
the set of all the configurations that can be realized respecting
the maximum tension in the ropes is indicated under the
name of Wrench-Feasible Workspace (WFW) [14], [15]. The
WFW is used to analyze the robot’s capability to carry loads,
but it does not consider constraints that might arise from
Fig. 1. Planar example: unbounded friction cones (top-left) give origin
the interaction with the environment, such as unilaterality to unbounded feasible wrench sets (top-right). Bounded friction polytopes
and friction. The idea of modeling the wrench admissible (bottom-left), instead, generate bounded feasible wrench sets (bottom-right).
region is also present in the field of mechanical fixtures and III. W RENCH - BASED A NALYSIS
tolerance analysis [16] where reciprocity of twists and screws Considering the actuation limits significantly affects the
is exploited to characterize the mobility conditions of any wrench margin of a legged robot. As an example let us
couple of faces in tolerance chains. consider a human(oid) trying to climb a vertical chimney
In the robotic grasping community it is common to consider shown in Fig. 1. Here, the CWC is obtained through the
sets of wrenches respecting frictional constraints [17]. When Minkowski sum of the friction cones, represented by the
considered, actuation limits take the form of an upper bound pink areas (Fig. 1 top-left). In the CWC-based approach the
on the magnitude of the normal contact force. Composing margin is quantified as the minimal distance between the
the contribution of each contact friction cone, a GWS can gravito-inertial wrench wGI and the boundary of the CWC.
be defined [1], representing a subset of the task wrench space The CWC margin represents the maximum allowed wrench
in which the a robust grasp against external disturbances is that can be applied (or rejected in case of a disturbance)
guaranteed. Such actuation constraints, however, may depend in order to keep the system stable. The CWC margin s has
on the joint configuration but they usually disregard the fact an infinite value s = ∞, i.e. the force closure condition is
that the maximal normal force cannot be coupled with any achieved (Fig. 1 top-right). This happens because the friction
tangential force component. cone representation assumes that a contact force with an
In legged locomotion the seminal work of Takao et al. has infinite normal component can be realized at the contact.
studied the problem of finding Feasible Solution of Wrench This misleading result is the consequence of not taking the
(FSW) in multi-contact configurations [18]. Wrench sets actuation limits into account.
have appeared with the CWC-based margin [5], which is On the other hand, imposing the actuation limits can be
a stability criterion for locomotion that is suitable for non- rephrased as adding a further constraint on the magnitude
coplanar contacts and finite friction coefficients. Dai et al. of the admissible contact forces with a force polytope that
in [19], [6] have shown how to exploit a CWC margin depends on the actuation capabilities and on the current
to obtain a convex optimization formulation that can plan configuration Fig. 1(bottom-left). This limits the set of ap-
CoM and joints trajectories of legged robots on complex plicable body wrenches (feasible wrenches) that the hu-
terrains. On a similar line, Caron et al. [20] have focused man(oid) can apply on its own CoM to keep himself stable.
on improving the real-time performances of 3D motion In Fig. 1(bottom-right) the bounded volume represents the
planning, either exploiting the double-description of the 6D result of the Minkowski sum of the four bounded friction
polyhedra or by considering lower dimensional projections polytopes after considering the torque contribution of each
of the CWC defining full-support areas. The latter, coupled maximal contact force. This convex region is a subset of the
with a linear pendulum model, led to the definition of the CWC that we call Feasible Wrench Polytope (FWP). This is
pendular support area [21]. computed as intersection of CWC and AWP, for more details
Despite the excellent results shown in this field, the lack see Section III-B. According to our definition, the margin s is
 0 Friction cone [N]
limited to a finite value, as in Fig. 1(bottom-right), showing Friction cone [N]
Force polytope [N]
Force polytope[N]
0 -0.2 Bounded friction cone[N]
that the human(oid) might fall if his limbs are not strong
-0.4
enough to support his body’s weight. -0.5
-0.6

A. The Actuation Wrench Polytope (AWP) -1

-0.8

In this section we illustrate the procedure to compute the 0.5 -1

0 0.5 -1 -0.5 0 0.5 1
0
AWP, the wrench polytope devoted to taking actuation limits -0.5 -0.5

into account. Let us consider the Equation of Motion (EoM)

Fig. 2. Representation of the force polytopes (blue) and of the friction
of a floating-base robot with nl branches (e.g. legs) in contact cones (pink) of each single leg in 3D (left) and projected on the (Fx , Fz )
with the environment, each of them with aPnumber na of plane (right). The offset fleg is due to the bias term δ in (3).
nl
actuated Degree of Freedoms (DoFs), n = k=1 nka being
the total number of actuated joints: lead to solutions that actually violate the bounds given by the
torque limits. In such cases, solving an inequality-constrained
M(q)q̈ + c(q, q̇) + g(q) = Bτ + JTs (q)f
(1)
quadratic program is a viable solution [22].
T
where q = qb qTj
 T 
∈ SE(3)×Rn represents the pose of τi lim ∈ Rna is a vector that contains the upper and lower
the floating-base system, composed of the pose of the base- bounds of the joint torques of each single leg. Considering all
frame qb ∈ SE(3) and of the generalized coordinates qj ∈ their combinations, these bounds result in 2na values of filim
Rn describing the positions of the n actuated joints. The that form the vertices of the force polytope Fk . In the case of
T
vector q̇ = vT q̇Tj

∈ Rn+6 is the generalized velocity, our quadruped robot HyQ, Fk is a polytope with 8 vertices
n
τ ∈ R is the vector of actuated joint torques while c(q) and its shape changes nonlinearly with the joint configuration
and g(q) ∈ R6+n are the centrifugal/Coriolis and gravity because of the nonlinearities in Ji 2 . As an example, we
terms, respectively. B ∈ R(6+n)×n is the matrix that selects compute the force polytope for each leg in a quadruped robot.
the actuated joints of the system. f ∈ R3nl is the vector of Fig. 2(left) shows the four force polytopes (together with
contact forces1 that are mapped into joint torques through the friction cones) obtained for a typical quadruped robot.
the stack of Jacobians Js (q) ∈ R3nl ×(6+n) . If we split (1) Fig. 2(right) shows a lateral view that depicts the same force
into its underactuated and actuated parts, we get: polytopes projected onto the (Fx , Fz ) plane.

Mb Mbj
      
v̇ cb gb 06×n
  T
Jsb To compute the AWP, the next step is to add the torque values
+ + = τ + f that are generated in correspondence to the maximum pure
MTbj Mj q̈j cj gj In×n JTsq
| {z } | {z } |{z} | {z } | {z } | {z } contact forces:
M(q) q̈ c(q,q̇) g(q) B Js (q)T  lim

fi,k
(2) wi,k = lim with k = 1, . . . , 2na (4)
By inspecting the actuated part (n bottom equations), result- pi × fi,k
ing from the concatenation of the equations of motions of where pi ∈ R3 represents the position of the i − th foot
all the branches, we see that Jsq ∈ R3nl ×n is block diagonal and wi,k ∈ R6 represents the wrench that can be realized
and we can use it to map joint torques into contact forces at that foot, both quantities are expressed in a fixed frame.
for each leg separately. We will see in Section V-A that this Therefore, the set of admissible wrenches that can be applied
is convenient because it avoids using the coupling term Jsb . at the CoM by the i − th foot/end-effector is:
For motion planning we are interested in estimating the maxi-
Wi = ConvHull(wi,1 , . . . , wi,2na ) (5)
mum f max ∈ R3nl and minimum f min ∈ R3nl contact forces
that the end-effectors are able to apply on the environment. with i = 1, . . . , nl . We now have nl wrench polytopes Wi ,
This quantity can be estimated by considering the maximal one for each limb in contact with the environment. Finally,
torques achievable by the actuation system τ lim ∈ Rn in the AWP corresponds to the Minkowski sum of all the nl
a given configuration qj ∈ Rn of the actuated joints. As wrench polytopes:
previously anticipated, we can estimate the maximal and AW P = ⊕ni=1
l
Wi (6)
minimal admissible contact force filim ∈ R3 for each end-
As defined above, the AWP is a bounded convex polytope
effector i separately, by considering a subset of the EoM
in R6 (Fig. 3 left) that contains all the admissible wrenches
(represented by the i sub-script) describing the dynamics of
that can be applied to the robot’s CoM that do not violate the
the actuacted joints of that specific leg:
# actuation limits of the limbs in contact with the environment.
filim = JTi MTbi v̇ + Mi q̈i + ci + gi −τilim


(3) Note that the force polytopes Fk are not zonotopes because
| {z } #
δ they result from the sum of a zonotopic term −JTi τ lim
where (.)# is the Moore-Penrose pseudoinverse, and Ji ∈ #
and of a nonzero singleton JTi δ as in Eq. 3. This differs
R3×na is the Jacobian matrix for the i−th foot. In our robot, from what is generally done for CDPR and for the GWS,
each leg has three DoFs (na = 3), thus a simple inversion where inertial and Coriolis effects are usually ignored and
is sufficient since Ji is square. Note that, for redundant and the ropes/fingers are considered light-weighted [23].
for underactuated limbs, the use of the pseudo-inverse may
2 The torque limits τ lim may depend on the joint positions, making the
i
1 Note that our robot has nearly point feet, thus we only consider pure dependency of the polytope from the joints configuration even more complex
forces at the contact point and no contact torque. (e.g. a revolute joint made of a linear actuator with nonlinear lever-arm).
B. The Feasible Wrench Polytope (FWP) IV. FWP- BASED F EASIBILITY METRIC
Note that the AWP does not include the constraints The CWC margin has been proven to be a universal
imposed by the environment, namely, the terrain normal, criterion for dynamic legged stability [5]. However, the CWC
the friction coefficient and the unilateral contact condition still lacks knowledge of robot’s feasibility constraints such
(e.g. the legs can not pull on the ground). However, those as self-collisions, actuation and kinematic joint limits. These
constraints can be accounted by the CWC [5] (Fig. 3 center): constraints become even more important when the roughness
CW C = ConvexCone(êki ) k = 1, . . . ne (7) of the terrain increases. Therefore, in order to plan complex
and i = 1, . . . nl . Here ne is the number of edges of the motions in unstructured environments we will introduce a
linearized friction cone and: more restrictive metric, that we generically call the feasibility
metric. This metric incorporates all the properties of the
eki
 
k
êi = ∈ R6 , with k = 1, . . . , ne (8) CWC criterion, and additionally the robot’s actuation limits.
pi × eki
In order to obtain the feasibility metric we first need to
where eki ∈ R3 is the k − th edge of the contact point i. We compute the robot’s gravito-inertial wrench wGI ∈ R6 in
subsequently perform the intersection of the CWC with the the specific robot state that we want to evaluate:
AWP obtaining a convex polytope that we define as Feasible
wGI = ḣ − wG (14)
Wrench Polytope (FWP) (Fig. 3 right):
with:
F W P = CW C ∩ AW P. (9) 
mc̈
 
mg

However, performing the intersection of polytopes in 6D is ḣ = , wG = (15)
k̇W c × mg
an expensive operation that requires the D-description [11]
of both operands. We propose a more efficient approach where kW ∈ R3 is the robot’s angular momentum and c
for the computation of the FWP that: 1) first computes is its CoM position (both expressed in the fixed coordinate
the intersection between the friction cones Ci and the force frame W). The criterion of feasibility can then be defined
polytopes Fi obtaining, for each i−th contact, a 3D bounded as:
friction cone Bi (Fig. 2 right) with µ vertices bki ∈ R3 : wGI ∈ F W P. (16)
Bi = ConvHull(bki ), with k = 1, . . . , µ (10) The definition of feasibility metric depends on the type of the
representation chosen for the FWP, i.e. half-plane description
2) then composes the wrench by adding the torque, as in (4): (H-description) or a vertex description (V-description).
bki
 
b̂ki = ∈ R6 with k = 1, . . . , µ (11) A. Half-plane description
pi × bki
In the H-representation, the FWP set can be written in
obtaining in this way the intermediate sets Gi ∈ R6 : terms of half-spaces as:
Gi = ConvHull(b̂ki ), with k = 1, . . . , µ (12) F W P = {w ∈ R6 |âTj w ≤ 0, j = 1, . . . nh } (17)
3) finally, the FWP is computed through the Minkowski sum where nh is the number of half-spaces of the FWP and âj ∈
of the Gi of all the nl contacts: R6 is the normal vector to the j − th facet. The feasibility
F W P = ⊕ni=1
l
Gi (13) criterion expressed in (16) can thus be written as:

The advantage of this proposed method is that the inter- HT wGI ≤ 0 (18)
section is performed in 3D rather than in 6D, which is where H ∈ R6×nh is the matrix whose columns are the
computationally faster. This is advantageous also for the final normals to all the half-spaces of the FWP and ≤ is a
step in (13) because it avoids computing vertices that will component-wise operator. The columns of H can be divided
be removed later (e.g. all the vertices from the AWP with into two blocks Hc and Ha in order to differentiate the
negative contact forces are removed by intersecting with the CWC half-spaces from the AWP half-spaces, respectively:
CWC). Additionally, the Minkowski sum can be efficiently H = [Hc |Ha ]. If HTc wGI > 0 but HTa wGI ≤ 0 then the
obtained using the V-description only as in [11]. robot’s state is consistent with its actuation capabilities but its
contact condition is unstable (e.g. friction limits are violated).
Viceversa, if HTc wGI ≤ 0 but HTa wGI > 0 then the system
C. Polytope representation for a Planar model has stable contacts but it does not respect the actuation limits.
To achieve a better understanding of the nature of these In the latter case, the legged system might still not fall but
polytopes, let us consider the simplified case of a planar it will not be able to realize the desired task.
dynamic model, as in Fig. 2 (right), where each point of the If the H-description of the FWP is given, we can provide
space is represented through the (x, z) coordinates. In this a definition of robustness that extends the properties of the
case the wrench space has three coordinates (Fx , Fz , τy ) and CWC margin. In the same line with [6] the feasibility metric
can be represented in 3D. Fig. 3 depicts the AWP (left), the can be defined as the margin m, i.e. the distance, of the point
CWC (center) and the FWP (right) for this simplified model. wGI from the boundaries of the FWP. This corresponds to
 T LS

500
AWP 1500
CWC 1500
FWP

1000

Fz [N ]
1000

Fz [N ]
0
=y

500 500

-500
500 0
2000
-1000
0 0 -2000 0 0
0 1500 1000 500 -1000
0 -500 -1000 -1500 -3000 1500 1000 500 0 -2000
-500 -1000 -1500 -3000
Fx -500 -2000 Fz Fx [N ] =y [N m] Fx [N ] =y [N m]
Fig. 3. The Actuation Wrench Polytope (AWP) (left), the Contact Wrench Cone (CWC) (center) and the Feasible Wrench Polytope (FWP) (right). These
drawings refer to a planar dynamic model where the only non zero wrench components are (Fx , Fz , τy ) and that can therefore be represented in 3D.

finding the biggest disturbance wrench wd ∈ R6 that the The FWP’s vertices v̂i are linked to the vertices of P s
system can reject. This is equivalent to computing the largest through the feasibility factor s:
residual radius m such that the m-ball Bm (centered in wGI ) v̂is = v̂i (1 − s), −∞ < s ≤ 1 (22)
still lies within the FWP: s
If, for instance, s = 1 then P shrinks into the origin. If
Bm ∈ F W P (19) we impose λi = (1 − s)λsi , then we can write the shrunk
polytope P s in terms of the vertices v̂i of the FWP, i.e.:
where Bm is defined as: n nv
X o
P s = w ∈ R6 |w = λi v̂i , λi ≥ 0, kλk1 = 1−s (23)
Bm = {wGI + T(pwd )wd | wdT Qwd ≤ m} (20) i=1

pwd is the disturbance application point and T(pwd ) is the We can see the feasibility factor as the scalar s that corre-
adjoint spatial transform that expresses it in the frame W sponds to the smallest shrunk polytope containing the point
[6]. Q is a positive definite matrix that is used to make the wGI . The problem of finding s can be formulated as a Linear
units of the wrench homogeneous. Program (LP) that can be carried out by any general-purpose
solver: max s
B. Vertex description
λ,s
If only a V-description of the FWP is available, the s. t. Vλ = wGI
distance between wGI and the faces of the FWP cannot be (24)
kλk1 = 1 − s s ∈ (−∞, 1]
computed anymore and a different definition of feasibility
metrics is needed. We decide to employ in this case a λi ≥ 0 i = 1, . . . , nv
feasibility scalar factor s ∈ (−∞, 1] adapted by the scaling Note that the larger is s, the more robust is the system against
factor defined in [1] used to measure a robotic grasp quality. disturbances. A negative s means that the point is out of the
Let us consider a matrix V ∈ R6×nv whose columns are polytope and the wrench is unfeasible. When s becomes zero,
the vertices vi of the FWP, and a vector λ ∈ Rn+v of non- it means the point is on the polytope boundary and that either
negative weights where nv is the number of vertices of the the friction or actuation limits are violated. Table I shows the
FWP. Every point inside the FWP can P be described with computation time of a Intel(R) Core(TM) i5-4440 CPU @
nv 3.10GHz with 4 cores for three and four contacts scenarios.
a combination of weights λi such that i=0 λi = 1. We
therefore define the robot to be in a feasible state if, for The feasibility factor s, unlike the margin m, is not sensitive
the corresponding wrench wGI , there exists a λ such that to the fact of having different units in the wrench space since
Vλ = wGI , with kλk1 = 1 and λi ≥ 0 i = 1, . . . , nv . it does not encode the concept of distance. On the other
A preliminary step for the computation of the feasibility hand, for the very same reason, the factor s of a given joint
factor consists in subtracting the centroid vc from all the configuration cannot be compared to another configuration,
FWP vertices vi , obtaining new translated vertices v̂i (v̂i = because of the different scaling of the two polytopes.
vi − vc ). This has the effect of shifting the origin of the V. O NLINE T RAJECTORY O PTIMIZATION (TO)
wrench space in the centroid, that, in a V-representation, is The FWP factor can be used to devise a motion planner
a good approximation of the most “robust” point (e.g. the that provides robust CoM trajectories. Hereafter, we present
Chebishev centre). We then define a new shrunk polytope a brief description of how we used the proposed criterion
P s centered in the origin (which is now also the centroid of
the FWP). P s can be expressed in terms of its own vertices TABLE I
v̂is and of a set of multipliers λsi : COMPUTATION TIME OF THE FEASIBILITY FACTOR s
n nv
X o 3 non-coplanar contacts 4 non-coplanar contacts
s 6
P = w ∈ R |w = λsi v̂is , λsi ≥ 0, kλs k1 = 1 (21) FWP vertices 436 1118
i=1 variables 437 1119
constraints 7 7
For a better understanding Fig. 4 (left) illustrates the idea LP time [ms] 90 350
of the shrunk polytope for a 2D representation.
 TABLE II
to plan online motions for our quadruped robot, that do not
FWP’ S V - AND H- DESCRIPTION COMPUTATION TIME WITH P OLITOPIX [26].
violate the actuation limits.
2 contact points 3 contact points 4 contact points
We further extended the capabilities of the locomotion V-description 0.03s 0.15s 0.49s
framework [24], [25] by replacing the original (heuristic) H-description 0.04s 1.03s 30.21s
planner with a trajectory optimizer that exploits the proposed TABLE III
feasibility criterion. This framework realizes a statically TO OF THE VARIABLES Γ USING THE FWP V - DESCRIPTION
stable crawling gait where a base motion phase and a swing 3 non-coplanar contacts 4 non-coplanar contacts
motion phase are alternated (therefore the base of the robot timesteps 10 10
does not move when a leg is in swing). FWP vertices 436 1118
variables Γ 41 41
To be able to compare with the heuristic planner, we opted constraints 24 24
for a decoupled planning approach where the footholds and time [ms] 75 85
the CoM trajectory of the robot are determined sequentially.
the states Γ of the system and the number of vertices will
Our online TO computes during every swing phase the CoM
influence the size of the TO problem only marginally (see
trajectory to be realized in the next base motion phase using a
Table III). We noticed that adding a bias term in the nullspace
one-step horizon. The decision variables of the optimization
of V to “drive” the solution λ[k] toward λ0 ∈ Rnv was
problem are the X, Y components of the CoM positions, the
giving satisfactory results:
velocities and the duration of the base motion phase ∆tbm :
Γ = {cx [k], cy [k], ċx [k], ċy [k], ∆tbm } with k = 1, · · · , N . λ[k] = V# wGI [k] + NV λ0 , λ ∈ Rnv (27)
The trajectory is discretized in N equally spaced knots (at
time intervals h = ∆tbm /N ). Note, that we here do not where NV ∈ Rnv ×nv is the null-space projector associated
optimize the angular dynamics nor the coordinate z, parallel to V. Indeed, if we set λ0 = [1/nv , · · · , 1/nv ] as the
to gravity. This is because, for quasi static motions, the geometric center of the FWP, the constraints kλ0 k1 =
predominant acceleration term acting on the system is gravity 1, λ0i > 0 are satisfied by construction. Thanks to the one-to-
itself, and therefore its influence on the stability or on the one correspondence between the gravito-inertial wrench and
joint torques is limited compared to the role of the X and the weights λ, penalizing the deviation of λ ∈ Rnv from λ0
Y components. We aim to maximize the FWP factor s, as is equivalent to maximizing the feasibility factor. Therefore,
in Eq. 24, while we enforce back-ward Euler integration we formulate hthe running costi computation as:
constraints along the trajectory and zero velocity at the L (c[k], ḣ[k], V) = kλ[k] − λ0 k2 (28)
trajectory extremes: A. Computational issues and approximations
N
X Despite the remarkable computational speed-up obtained
min L(c[k], ḣ[k], V) by the use of the V-description (see Table II), the evaluation
Γ
k=1
of the FWP still represents the most time-consuming (a) of
s. t. ċx [k + 1] = (cx [k + 1] − cx [k])/h (25)
this pipeline (about 150ms needed for a triple-stance config-
ċy [k + 1] = (cy [k + 1] − cy [k])/h uration). The subsequent step (b), i.e. the solution of the TO
ċx [0] = ċy [0] = ċx [N ] = ċy [N ] = 0 problem for a given FWP, requires instead about 75 − 85ms
for 10 nodes trajectory. In theory we should recompute step
As a first step, we evaluate the FWP polytope considering a and step b iteratively, concurrently optimizing the vertices
the robot contact configuration. By assuming a quasi-static of the FWP and the trajectory inside it. We decided instead
condition (q̈ = q̇ = 0), Eq. 3 reduces to: to compute the FWP only once per step, assuming that its
 
filim = Ji (q0i )−T g(q0i ) − Bτ lim (q0i ) (26) vertices do not change during the execution of each phase,
as explained in the following paragraph.
Exploiting the approximation later explained in Section V-A We analyze the influence on the estimated maximum and
we compute the FWP just once at the beginning PN of the minimum contact force when the Jacobian matrix is ap-
optimization. Then, to compute the running cost k=1 L, proximated to be constant along a body motion of the HyQ
for each optimization loop, we evaluate the CoM acceleration robot. In Fig. 5 we can see the contact force boundaries (Z
along the trajectory (h·c̈x,y [k+1] = ċx,y [k+1]− ċx,y [k], ∀k) component only) when a foot spans its workspace (the foot
and evaluate the gravito-inertial wrench at each knot through covers all the X and Y positions on a plane located at Z =
(14). In order to exploit the V-description, for each node, −0.6m). The green surfaces represent the real boundaries of
we should add the λ vector as decision variable and the the vertical contact force considering the correct leg Jacobian
constraints in (24). However, the amount of decision vari- and correct piston lever-arm for each considered position.
ables would significantly increase due to the high number The red surfaces show instead the same force boundaries
of vertices in the polytope (i.e. λ may have hundreds of when the correct piston lever-arm and a constant Jacobian
elements for each optimization knot) leading to computation is evaluated. We can see that in a neighborhood of the de-
times that do not meet the requirements for online planning. fault foot configuration([0.3, 0.2, −0.6]m with respect to the
Thus we tackled this problem by computing the set of λ base frame of the robot) the approximation is accurate and
through a simple Moore-Penrose pseudo-inversion: λ[k] = becomes rough in proximity of the workspace boundaries.
V# wGI [k]. In this way the decision variables will be only We chose to use a constant Jacobian matrix corresponding
 Lateral shift /y test Vertical disturbance dz test
0

dz [N ]
/y [m]
0.3 -500
0.2
-1000
0.1 Heuristic plan
0 0.5 1
Heuristic
1.5
plan
0 2 4 6 8
CWC plan10 CWC plan
0.4 FWP plan 0.4 FWP plan

s
0.2

s
0.2
0 0
0 2 4 6 8 10 0 0.5 1 1.5
400 150

= [Nm]
Fz [N]
200 100 torques limit
0 50
0 2 4 6 8 10 0 0.5 1 1.5
time [s] time [s]

Fig. 4. A 2D pictorial representation of the shrunk polytope P s (left): we see that the factor s can be seen as the shrinkage rate of the reduced polytope
with respect to the FWP; simulation data of the horizontal displacement test (center) and vertical disturbance test (right).
to a joint configuration q0 of the trajectory coming from the with the knee joint torque of the LF leg and the feasibility
heuristic plan. In this way the Jacobian remains constant and factors s in the three cases. We can see that, in the case of the
we remove the AWP’s dependency from the joints position. static configuration found with the FWP planner, the torque
As a further simplification we assume a quasi-static motion limit is reached for a higher amplitude of the disturbing
as in Eq. 26. These assumptions allow us to compute the force (about −1100N compared to −900N ), showing that
FWP only once at each stance change. Note that all the this is more robust against external disturbance forces than
wrenches are expressed in the fixed frame. the configurations selected by the heuristic and by the CWC
VI. E XPERIMENTAL R ESULTS planners. We can see that in all the cases the feasibility factor
In this section we present simulation results and real s goes to zero when a torque limit is violated.
experiments with the HyQ robot. The first two simulations
validate the feasibility factor formulation based on the V-
description of the FWP. After that we highlight the differ-
ences between our proposed feasibility metric and the state-
of-the-art stability metric. Finally we present a few examples
of the behaviors we can obtain with the TO presented in
Section V.
A. FWP-based feasibility factor validation
In a first test, we consider three different motion planners:
the heuristic planner, a CWC planner (that incorporates only
frictional constraints) and our FWP planner. We have the
robot crawling where the CoM trajectory is planned by Fig. 5. Contact force limits (Z component) on the left-front (LF) leg of
HyQ as a function of the foot position computed with real torque limits
the three different methods and, in all cases, we stop the (green) and with jacobian approximation (red).
robot during a triple-stance phase (being more critical for
robustness than four-stance phases). We then make the robot B. CWC-margin vs. FWP-factor Comparison
displace laterally with an increasing offset δy = 0.5 m The last test highlights the main differences between the
( ∈ R increases linearly from 0 to 1). The objective is to feasibility metrics s and the traditional stability measures. As
obtain a gradual unloading of the lateral legs and therefore state-of-the-art stability metric we consider the CWC-margin,
violate the unilaterality constraints. Figure 4 (center) shows which is obtained by applying Eq. 24 on the V-description
the evolution of the displacement δy and of the normal of the CWC, rather than the FWP as explained in Section
component of the contact force Fz at the left-front (LF ) IV-B. Fig. 6 (above) shows the results when a crawl gait
leg. As expected the plot shows that for all the cases s drops is evaluated using this method. The red line represents the
to zero when the leg LF becomes unloaded (Fz = 0). value of the CWC-margin during the triple-stance phase of
In the second test the robot is again stopped during a walk a crawling gait. The dashed blue line represents the same
in a three-legs stance configuration. This time a vertical walk, evaluated again with the CWC-margin, in the case that
disturbance force was applied at the origin of the base link an external load of 20kg is applied on the CoM of the robot
(i.e. the geometric center of the torso). The force is vertical during its walk. The factor referring to the four-legs stance is
and pointing downwards with increasing magnitude dz = not directly comparable to the triple-stance phase because the
−1000 N where  ∈ R is linearly increasing from 0 to vertices of the FWP have a different scaling. For this reason
1. The joint torques will increase because of the action of we only show the values referring to the triple stance. We can
this force, eventually making one (or more) of them hit the see that the same two trials, with and without external load,
limits. Since the test is performed in a static configuration provide a completely different result if evaluated with the
and the disturbance force is always vertical, the Center of FWP-factor (Fig. 6 bottom): the blue-dashed line shows that
Pressure (CoP) of the system will not change, being the the feasibility is lower in the case with external load. This
robot always statically stable. Fig. 4 (right) shows a plot implies that, when the load increases, even if the stability
of the magnitude of the vertical pushing force dz together might improve, the risk of hitting the torque limits is higher.
 CWC margin [2] C. Mastalli, M. Focchi, I. Havoutis, A. Radulescu, S. Calinon,
0.2 J. Buchli, D. G. Caldwell, and C. Semini, “Trajectory and Foothold
0.1 Optimization using Low-Dimensional Models for Rough Terrain Lo-
0 comotion,” in International Conference on Robotics and Automation
0 2 4 6 8 10 12
(ICRA), 2017.
FWP margin with load
[3] J. Carpentier, R. Budhiraja, and N. Mansard, “Learning Feasibility
0.2 without load
Constraints for Multi-contact Locomotion of Legged Robots,” in
0.1 Robotics Science and Systems (RSS), 2017.
0 [4] D. Orin, A. Goswami, and S. Lee, “Centroidal Dynamics of a
0 2 4 6 8 10 12
Humanoid Robot,” Autonomous Robots, vol. 35, pp. 161–176, 2013.
time [s] [5] H. Hirukawa, K. Kaneko, S. Hattori, F. Kanehiro, K. Harada, K. Fu-
Fig. 6. Evaluation of the same heuristic crawl gait (with and without 20kg
load)
10 with the CWC margin (top) and with the FWP factor (bottom). jiwara, S. Kajita, and M. Morisawa, “A Universal Stability Criterion
of the Foot Contact of Legged Robots - Adios ZMP,” in ICRA, 2006.
[6] H. Dai and R. Tedrake, “Planning robust walking motion on uneven
0
C. Crawling Simulations terrain via convex optimization,” in Humanoids, 2016.
3 [7] H. Audren and A. Kheddar, “3D robust stability polyhedron in multi-
-10As shown in the accompanying video , we report a few contact,” February 2017, eprint hal:lirmm-01477362.
0
simulation and50hardware experiments
100 150 200
of HyQ performing [8] A. Del Prete, S. Tonneau, and N. Mansard, “Fast algorithms to test
a crawling gait. At first we see that the heuristic crawl robust static equilibrium for legged robots,” in ICRA, 2016.
easily hits the torque limits while crawling on a flat ground [9] B. Aceituno-Cabezas, C. Mastalli, D. Hongkai, M. Focchi, A. Rad-
ulescu, D. G. Caldwell, J. Cappelletto, J. C. Grieco, G. Fernando-
while carrying an external load of 20kg (about 25% of the Lopez, and C. Semini, “Simultaneous Contact, Gait and Motion Plan-
robot total weight) placed on its CoM. We can then see that ning for Robust Multi-Legged Locomotion via Mixed-Integer Convex
the FWP planner, as explained in Section V, finds a new Optimization,” in IEEE Robotics and Automation Letters (RA-L), 2018.
[10] V. Samy, S. Caron, K. Bouyarmane, and A. Kheddar, “Post-Impact
duration ∆tbm of the base motion phase and a new CoM Adaptive Compliance for Humanoid Falls Using Predictive Control of
trajectory that avoids hitting the torque limits at all times, a Reduced Model,” July 2017, eprint hal:01569819.
while maintaining the desired linear speed. [11] V. Delos and D. Teissandier, “Minkowski sum of polytopes defined
by their vertices,” Journal of Applied Mathematics and Physics, 2015.
Final simulations show the capability of the planner to [12] R. Orsolino, M. Focchi, C. Mastalli, H. Dai, D. G. Caldwell, and
optimize feasible trajectories when the robot has a hindered C. Semini, “The Actuation-consistent Wrench Polytope (AWP) and
joint (i.e. when a specific joint can only realize a significantly the Feasible Wrench Polytope (FWP),” in arXiv:1712.02731, 2017.
[13] C. Semini, N. Tsagarakis, E. Guglielmino, M. Focchi, F. Cannella,
smaller torque than the other joints), or when we limit the and D. Caldwell, “Design of HyQ – a Hydraulically and Electrically
normal force that a specific leg can realize on the ground. The Actuated Quadruped Robot,” J. of Systems and Control Eng., 2011.
video also shows hardware experiments of HyQ crawling on [14] P. Bosscher, A. T. Riechel, and I. Ebert-Uphoff, “Wrench-feasible
workspace generation for cable-driven robots,” TRO, vol. 22, 2006.
a rough terrain without hitting the torque limits. [15] S. Bouchard, C. Gosselin, and B. Moore, “On the ability of a cable-
VII. C ONCLUSION driven robot to generate a prescribed set of wrenches,” in ASME
The complexity of a motion increases with the complexity Journal of Mechanisms and Robotics, 2010.
of the terrain to be traversed. Moreover, there is a need for [16] S. Arroyave-Tobon, D. Teissandier, and V. Delos, “Applying screw
theory for summing sets of constraints in geometric tolerancing,”
online motion replanning to avoid error accumulation. For Mechanism and Machine Theory, 2017.
these reasons, in this paper, we presented the concepts of [17] A. Bicchi, “On the closure properties of robotic grasping,” in Inter-
AWP and FWP and a method to efficiently compute their V- national Journal of Robotics Research, 2015, pp. 319–334.
[18] T. Saida, Y. Yokokohji, and T. Yoshikawa, “FSW (feasible
description. We then adapted a feasibility factor, originally solution of wrench) for multi-legged robots,” in ICRA, 2003.
proposed for grasping [1], to the V-description of the FWP [Online]. Available: http://ieeexplore.ieee.org/lpdocs/epic03/wrap-
in order to study the stability and the actuation-consistency per.htm?arnumber=1242182
[19] H. Dai, “Robust multi-contact dynamical motion planning using con-
of a given motion plan. Finally we showed how this factor tact wrench set,” Ph.D. dissertation, MIT, 2016.
can be used in a CoM trajectory optimization not only for [20] S. Caron, Q.-C. Pham, and Y. Nakamura, “Leveraging Cone Double
feasibility evaluation but also for motion planning. Description for Multi-contact Stability of Humanoids with Applica-
tions to Statics and Dynamics,” in RSS, 2015.
Thanks to the efficiency of the vertex-based approach we [21] S. Caron and A. Kheddar, “Dynamic Walking over Rough Terrains by
are able to perform online TO where the robot plans during Nonlinear Predictive Control of the Floating-base Inverted Pendulum,”
each swing the trajectory for the next base motion phase. Our in eprint arXiv:1703.00688, 2017.
[22] P. Chiacchio, Y. Bouffard-Vercelli, and F. Pierrot, “Force Polytope
approach does not take any assumption on the environment and Force Ellipsoid for Redundant Manipulators,” Journal of Robotic
and it is therefore suitable for complex terrain scenarios. Systems, vol. 14, no. 8, pp. 613–620, 1997.
Future works will concentrate on the removal of the approx- [23] F. Guay, P. Cardou, A. L. Cruz Ruiz, and S. Caro, “Measuring how
well a structure supports varying external wrenches,” in New Advances
imations mentioned in Section V-A and on the integration of in Mechanisms, Transmissions and Applications, 2014, pp. 385 – 392.
planned and reactive locomotion behaviors [27]. [24] M. Focchi, A. Del Prete, I. Havoutis, R. Featherstone, D. G. Caldwell,
ACKNOWLEDGMENT and C. Semini, “High-slope terrain locomotion for torque-controlled
The authors are grateful to Vincent Delos and Prof. Denis quadruped robots,” Autonomous Robots, 2017. [Online]. Available:
http://dx.doi.org/10.1007/s10514-016-9573-1
Teissandier of the I2M lab., University of Bordeaux, for the [25] M. Focchi, R. Featherstone, R. Orsolino, D. G. Caldwell, and C. Sem-
fruitful discussions about computational geometry. ini, “Viscosity-based Height Reflex for Workspace Augmentation for
Quadrupedal Locomotion on Rough Terrain,” in IROS, 2017.
R EFERENCES [26] V. Delos, “Politopix,” http://i2m.u-bordeaux.fr/politopix.html, 2015.
[1] R. Krug, A. J. Lilienthal, D. Kragic, and Y. Bekiroglu, “Analytic Grasp [27] V. Barasuol, M. Camurri, S. Bazeille, D. Caldwell, and C. Semini, “Re-
Success Prediction with Tactile Feedback,” in ICRA, 2016. active trotting with foot placement corrections through visual pattern
classification,” in IEEE/RSJ International Conference on Intelligent
3 https://youtu.be/vUx5b5kfRfE
Robots and Systems (IROS), 2015.