An efficient acyclic contact planner for multiped robots

Steve Tonneau, Andrea del Prete, Julien Pettré, Chonhyon Park, Dinesh
Manocha, Nicolas Mansard

To cite this version:

Steve Tonneau, Andrea del Prete, Julien Pettré, Chonhyon Park, Dinesh Manocha, et al.. An efficient
acyclic contact planner for multiped robots. IEEE Transactions on Robotics, 2018, 34 (3), pp.586-601.
�10.1109/TRO.2018.2819658�. �hal-01267345v3�

HAL Id: hal-01267345

https://hal.science/hal-01267345v3
Submitted on 11 Oct 2017

HAL is a multi-disciplinary open access L’archive ouverte pluridisciplinaire HAL, est

archive for the deposit and dissemination of sci- destinée au dépôt et à la diffusion de documents
entific research documents, whether they are pub- scientifiques de niveau recherche, publiés ou non,
lished or not. The documents may come from émanant des établissements d’enseignement et de
teaching and research institutions in France or recherche français ou étrangers, des laboratoires
abroad, or from public or private research centers. publics ou privés.
 1

An efficient acyclic contact planner for multiped

robots
Steve Tonneau, Andrea Del Prete, Member, IEEE, Julien Pettré, Member, IEEE, Chonhyon Park, Student
Member, IEEE, Dinesh Manocha, Member, IEEE, and Nicolas Mansard, Member, IEEE,

Abstract—We present a contact planner for complex legged follows the right foot). Efficient tools such as the capture
locomotion tasks: standing up, climbing stairs using a handrail, point [2] can be used to compute the next contact location.
crossing rubble and getting out of a car. The need for such a In the general case planning complex contact interactions is
planner was shown at the Darpa Robotics Challenge, where such
behaviors could not be demonstrated (except for egress). extremely challenging: At any given time a contact must be
Current planners suffer from their prohibitive algorithmic chosen between infinitely many possibilities (often a combina-
complexity, because they deploy a tree of robot configurations torial discrete choice for the effector and contact surface, and
projected in contact with the environment. a continuous choice for the contact location). Furthermore, a
We tackle this issue by introducing a reduction property: contact choice constrains kinematically and dynamically the
the reachability condition. This condition defines a geometric
approximation of the contact manifold, which is of low dimension, possible motions, and there is no analytical way to verify
presents a Cartesian topology, and can be efficiently sampled whether this choice brings the robot one step closer to the
and explored. The hard contact planning problem can then be desired goal or to a dead end, especially in the presence of
decomposed into two sub-problems: first, we plan a path for the obstacles; we say that the contact manifold is foliated [3]. The
root without considering the whole-body configuration, using a foliation prevents the use of efficient sampling-based planners
sampling-based algorithm; then, we generate a discrete sequence
of whole-body configurations in static equilibrium along this path, for two reasons. (i) First, each sub-manifold of the foliation
using a deterministic contact-selection algorithm. has a zero measure and cannot be directly sampled. A sample
The reduction breaks the algorithm complexity encountered in is rather obtained by sampling a free flying configuration and
previous works, resulting in the first interactive implementation explicitly projecting it in contact (which is a costly numerical
of a contact planner (open source). While no contact planner has operation). (ii) Second, the foliated topology turns the explo-
yet been proposed with theoretical completeness, we empirically
show the interest of our framework: in a few seconds, with high ration by spreading a graph of configurations (probabilistic
success rates, we generate complex contact plans for various road-map, rapidly exploring random trees) into an inefficient
scenarios and two robots, HRP-2 and HyQ. These plans are random process, where many useless nodes are sampled on
validated either in dynamic simulations, or on the real HRP- parallel sub-manifolds. The total algorithmic complexity of
2 robot. classical contact planners comes from both the number of
Index Terms—Multi contact locomotion, centroidal dynamics, graph nodes sampled during exploration (ii) and the cost of
Humanoid robots, legged robots, motion planning the projection when sampling new configurations (i).

I. I NTRODUCTION
For this reason previous contributions having demonstrated

L EGGED robots move by sequentially creating contacts

with the environment. After years of research, such robots
can autonomously walk on flat ground, but struggle to navigate
acyclic contact locomotion on a real robot are too computa-
tionally expensive [4]. As a result at the DARPA Robotics
Challenge, the participants stated that except for egress, the
more complex environments. Deciding where to create a robots did not use multi contact strategies: they relied on
contact with its feet and possibly its hands is nontrivial, e.g. unsafe bipedal walking to climb stairs, instead of using the
to climb stairs using a handrail. provided handrails to facilitate the motion [5].
Most of the complexity of this problem lies in the contact
planning, i.e. the underlying decomposition of the trajectory
into contact phases where specific points of the robot body Our work aims at breaking the complexity of the acyclic
are exerting forces on specific locations of the environment. contact planning problem. To do so we deal sequentially
Tackling this complexity is the main objective of this paper. with the two main issues associated to our problem: the
Once the contact plan is known, efficient approaches exist null measure of the contact manifold, and the combinatorics
to generate a dynamically feasible motion [1]. In the specific of the contact selection problem. First we introduce a low-
(and simple) case of gaited biped locomotion on flat ground, dimensional space, called the contact reachable space, that
choosing the effector with which to create a contact is trivial can be sampled and mapped efficiently to the contact manifold.
because walking follows a cyclic pattern (the left foot always Then, given a path computed in the contact reachable space,
we propose a deterministic algorithm to generate a contact
S. Tonneau, A. Del Prete and N. Mansard are with LAAS-CNRS / sequence along the path. This decoupling presents pros and
Université de Toulouse, France e-mail: ([email protected])
J. Pettré is with Inria, Rennes, France cons discussed in previous related literature, summarized in
C. Park and D. Manocha are with UNC, Chapel Hill, USA the following.
 2

P1 P2

a b c
Fig. 1. Overview of our two-stage framework. Given a path request between start and goal positions (left image), P1 is the problem of computing a guide
path in the space of equilibrium feasible root configurations. We achieve this by defining a geometric condition, the reachability condition (abstracted with the
transparent cylinders on the middle image). P2 is then the problem of extending the path into a discrete sequence of contact configurations using an iterative
algorithm (right image).

A. State of the art be decoupled to reduce the complexity. The feasibility and
Additionally to robotics, acyclic motion planning is also interest of the decoupling has been shown by Escande et
a problem of interest in neurosciences, biomechanics, and al. [17] who manually set up a rough root guide path (i.e.
virtual character animation. Early contributions in the latter an ad-hoc solution to P1 ), and then addressed P2 as the
field rely on local adaptation of motion graphs [6], or ad-hoc combinatorial computation of a feasible contact sequence in
construction of locomotion controllers [7]. These approaches the neighborhood of the guide. A solution could then be
are by definition not able to discover complex behaviors in found, but at the cost of prohibitive computation times (up
unforeseen contexts. to several hours) for constraining scenarios. This approach
is suboptimal because the quality of the motion depends
The issue of planning acyclic contacts was first completely
on the quality of the guide path. Bouyarmane et al. [18]
described by Bretl [4]. The issue requires the simultaneous
precisely focused on automatically computing a guide path
handling of two sub-problems, P1 : planning a guide path for
with guarantees of equilibrium feasibility, by extending key
the root of the robot in SE(3); and P2 : planning a discrete
frames of the path into whole-body configurations in static
sequence of equilibrium configurations along the path. A third
equilibrium. Randomly-sampled configurations are projected
nontrivial problem, P3 , then consists in interpolating a com-
to the contact manifold using an inverse-kinematics solver, a
plete motion between two postures of the contact sequence. A
computationally-expensive process (about 15 minutes to com-
key issue is to avoid combinatorial explosion when considering
pute a guide path in the examples presented). Moreover this
at the same time the possible contacts and the potential paths.
explicit projection is insufficient to guarantee the feasibility
Bretl’s seminal paper proposes a first effective algorithm,
between two key postures in the path. Chung and Khatib [19]
able to handle simple situations (such as climbing scenarios),
also proposed a decoupled approach, with a planning phase
but not applicable to arbitrary environments. Following it,
based on the reachable workspace of the robot limbs, used
several papers have applied this approach in specific situations,
to judge the ability to make contact with a discretized envi-
limiting the combinatorial by imposing a fixed set of possible
ronment. This planning phase does not account for collisions,
contacts [8], [9].
implying that re-planning is required in case of failure. In
Most of the papers that followed the work of Bretl have
highly-constraining cases such as the car egress scenario we
explored alternative formulations to handle the combinatorics.
address, we believe that including collision constraints in the
Two main directions have been explored. On the one hand,
planning is a requirement [20], [21]. A limitation with these
local optimization of both the root trajectory P1 and the
approaches (including our method) is that the existing planners
contact positions P2 has been used, to trade the combinatorial
only address a subset of the problem, because their ability
aspect of the complete problem for a differential complexity,
to find a solution depends on the existence of quasi-static
at the cost of local convergence [10]. A complete example of
equilibrium configurations along a feasible path, which is too
the potential offered by such approaches was proposed by [11]
restrictive in the general case. Other contributions to legged
and successfully applied to a real robot [12]. To get reasonable
locomotion, not directly related to multi-contact motion, are
computation times, the method uses a simplified dynamic
worth mentioning, as they rely on a similar decomposition
model for the avatar. Still, the method is far from real-time
of the problem. First a discrete sequence of contact sets is
(about 1 minute of computation for 20 contacts). A similar
planned (at the so-called footstep planning phase), using a
approach has been considered for manipulation by [13]. Deits
low-dimensional abstraction of the robot to account for its
and Tedrake proposed to solve contact planning globally as
kinematic constraints [22], [23]. In [23], a “pose certificate”
a mixed-integer problem, but only cyclic, bipedal locomotion
is obtained by generating a whole-body configuration for each
is considered, and equilibrium is not considered [14]. Dai et
set through inverse kinematics, as done in P2 . Then a motion is
al. [15] extended the work of Posa et al. [16] to discover
generated along the sequence through the use of optimization
the contact sequence for landing motions, but need to specify
techniques. The solutions proposed are designed for cyclic
the contacts manually for complex interactions. In addition to
locomotion in quasi-flat scenarios, where the support polygon
the limits of the current implementations, optimization-based
is a relevant method for equilibrium checks. They thus cannot
approaches only converge locally.
be generalized to multi contact locomotion. However, some
On the other hand, the two problems P1 and P2 might
 3

contributions on specific parts of the problem could be applied completeness regarding the contact planning problem.
directly in our case. For instance, learning “terrain costs”,
based on expert knowledge as proposed in [24], could define Comparison with our previous work: The present paper is
good heuristics to compute the next contact location for an an extension of our ISRR conference paper [20]. As such, our
effector. Although we did not try to include such formulation solution to address P1 and P2 is the same motion planner
in this paper, it would be straightforward to integrate such as the one presented at ISRR (reformulated in Section IV
heuristic in our planner. and V). However three important novelties have been added to
As far as robotics applications are concerned, none of the planner: the pseudo-code of the algorithm (Section V-B2),
the existing multi-contact planners is interactive1 . However, a novel criterion for static equilibrium (Section VI), and the
recent contributions to the interpolation between contact poses release of the source code of the planner (Section VII).
(problem P3 ) have brought promising preliminary solutions The other novelty of the paper is a rigorous experimental
[25], [26], [27], [1]. In particular, our algorithm proposed in validation of the approach on actual robot models (Sec-
[1] is interactive. Therefore, a planner capable of efficiently tion VIII). To validate our contact plans we introduced a
solving P1 and P2 could outperform all existing planners if complete framework for multi-contact motion synthesis. This
coupled with an interpolation method solving P3 . The main framework additionally comprises an interpolation method
contribution of this paper is exactly this planner. to solve the problem P3 , based on a reformulation of our
previous work [1]. Our solution to P3 allows us to verify that
the synthesized motions are physically consistent, using our
B. Contributions implementation of a state-of-the-art simulation algorithm [28].
Our solution belongs to the class of decoupled approaches, These aspects of the framework are presented in details in the
i.e. we propose specific algorithms to efficiently solve both paper, but are not novel per se. The novelty lies in the complete
P1 and P2 while relying on state-of-the-art solution to P3 validation of the contact planner with real robot models, for
to obtain the whole movement. Our main contribution is the which the planning is much harder with respect to the avatars
definition of a reduction property, the reachability condition. used in [20], because of more restrictive kinematic constraints.
Compared to previous approaches, our solution has two
main novelties: II. OVERVIEW
Regarding P1 , we propose a fast guide path planning algo- Figure 1 illustrates our work flow. P1 and P2 are addressed
rithm. The key to its efficiency is that it does not sample sequentially: From a given problem (a) we first plan a root
directly the contact manifold, but an approximation of the guide path (b), before extending it into a sequence of static
contact reachable space. The contact reachable space is a equilibrium configurations (c). In the case where step (c)
low-dimensional space for which there exists a mapping to fails, our framework invalidates the computed guide path, and
the contact manifold. restarts the planning from (b).
Regarding P2 , we propose a fast method to extend a contact
reachable path into a sequence of whole-body configurations A. Computation of a guide path — P1 (Section IV)
in static equilibrium. This requires the explicit computation of
contact configurations. It is guided by dedicated heuristics that We first consider the problem of planning a root guide path
quickly synthesize feasible configurations. (Figure 1–P1 ). The dimension of the path is equal to the
The reachability condition is the key to the strict separa- number of degrees of freedom (DoFs) of the root of the robot.
tion between P1 and P2 , hence to the low complexity of Similarly to previous work [18] the path must be equilibrium
our planner. However the reduction can result in failures. feasible: there must exist a joint configuration that results in
We demonstrate empirically its interest, through an extensive static equilibrium for each root configuration2 . Previous works
experimental validation with real robot models on a dynamic verify equilibrium feasibility by explicitly computing such
simulator. The high success rate and low computation times a configuration. We preserve the low dimensionality of the
we obtain allow us to plan (and re-plan upon failure) multi- problem by approximating equilibrium feasibility with contact
contact sequences at interactive rates. reachability, illustrated in the following.
An intuitive description of contact reachable configurations
To further demonstrate the validity of our approach, we
is “close, but not too close” to the environment: close, because
show that the generated contact plans can be successfully
a contact surface must be partially included in the reachable
executed (problem P3 ), either in simulation or on the real
workspace of the robot to allow contact creation; not too close,
HRP-2 robot. For HRP-2, we detail the complete computation
because the robot must avoid collision. More precisely, a root
times to address sequentially the three problems, and compare
configuration is contact reachable if the root scaled by a user-
them to related work, demonstrating that our method is orders
defined factor s ≥ 1 is not in collision (Figure 2 - red shape),
of magnitude faster.
while the reachable workspace is in collision (Figure 2 - green
Finally, we provide an extensive discussion on the
shapes). To plan a root path, we then use an RRT planner
consequences of our approach in terms of efficiency and
2 Enforcing static equilibrium is a classical, conservative approach to reduce
1 We define an interactive planner as one for which the time to plan a motion the search space and considering states with non-zero accelerations and
is in the same order of magnitude as the time to execute it. For instance, velocities, which can’t be connected trivially. This does not mean that the
computing one contact change should take about one or two seconds. final motion will necessary be quasi-static.
 4

Fig. 3. Reachable workspace and torso bounding box of HyQ. Each green
Fig. 2. The reachability condition is verified by the right configuration: the
shape represent a reachable workspace W k of a limb. The red shape is W 0 .
trunk (red) is free of collisions while the limbs reachable workspace (green)
intersect the environment.

where pk denotes the end-effector position (in the root frame)

where, instead of checking collision to validate a configuration, of Rk (translation only) for q0 = 0 being the null dis-
we verify the reachability condition. k
placement, and Cj.lim is the space of admissible limb joint
In the remainder of this paper, we use the terms contact Sl
configurations. We also define W = k=1 W k .
reachable and equilibrium feasible to qualify either a root or
a whole-body configuration, or a set of such configurations. The environment O is defined as the union of the obstacles
Oi that it contains. O is represented as a polygon soup (or
mesh), where the normal of each surface is known. No further
B. Generating a discrete sequence of contact configurations requirement is needed by our approach. In this work, we
— P2 (Section V) assume the environment is fully known. State uncertainty is
The second stage extends the guide path into a sequence out of the scope of the paper.
of contact configurations (Figure 1–P2 ). To achieve this the
root path is first decomposed into a sequence of discrete Finally we define some relevant subsets of the configuration
root configurations, according to a user-defined discretization space C. CContact is the set of whole body-configurations in
k
step. Each root configuration is then extended into a whole- contact and collision-free. CContact ⊂ CContact is the set of
body configuration in static equilibrium. The algorithm thus whole body-configurations where at least Rk is in contact.
proceeds iteratively, starting from the whole-body initial con- CEquil ⊂ CContact is the set of whole body-configurations
figuration of the robot. It takes advantage of the fact that in static equilibrium and collision-free.
each root configuration is fixed to generate the contact by
considering each limb individually. 0
For any set CX , we define CX :
n o
0
III. N OTATION AND DEFINITIONS CX = q0 , ∃q0 : q0 ⊕ q0 ∈ CX

A vector x is denoted with a bold lower-case letter. A matrix

A is denoted with a bold upper-case letter. A set C is denoted
with an upper-case italic letter. Scalar variables and functions IV. ROOT PATH PLANNING IN THE CONTACT REACHABLE
are denoted with lower-case italic letters, such as r or f (x). SPACE

A robot is a kinematic tree R composed of: a root R0 , and During the root path planning we only consider the root
l limbs Rk , 1 ≤ k ≤ l, attached to the root. The root has configuration q0 defined in the previous Section, as well as
r ≥ 6 DoFs: for instance, HRP-2 has two extra DoFs in the the environment O.
torso, such that we have r = 8. Thus R is fully described by Given start and goal configurations, we aim at computing a
a configuration (a vector of joint values) q ∈ Rr+n , with n guide path q0 (t) : [0, 1] −→ Rr verifying:
the number of joint DoFs. q is decomposed as follows:
• qk is a configuration of the limb Rk ; ∀t ∈ [0, 1], q0 (t) ∈ CEquil
0

• qk is a vector of joint values of R not related to Rk . We

define for convenience q = qk ⊕ qk ; This means that any root configuration must be extended into
0
• q0 ∈ Rr is the world coordinates vector of R0 . a whole-body, static equilibrium configuration. CEquil cannot
be described analytically.
We then define a set of 3d volumes W i , 0 ≤ i ≤ l, each The main hypothesis of this work is that for a large
attached to one joint of the root, such that W i (q0 ) describes 0
variety of locomotion tasks, we can define a space CReach ≃
the world position of W i for the root configuration q0 . 0
CContact , such that
W 0 is a volume encompassing R0 (Figure 3), or equal to it3 .
W k is the reachable workspace of a limb Rk : ∀t ∈ [0, 1], q0 (t) ∈ CReach
0
⇒ q0 (t) ∈ CEquil
0
(2)

W k = x ∈ R3 : ∃ qk ∈ Cj.limk
, pk (qk ) = x (1) 0
We call CReach the contact reachable workspace, and detail its
construction in the following. The validity of this hypothesis
3W 0 is typically a low-polygonal bounding shape of R0 for performance. is discussed in depth in Section IX.
 5

A. Conditions for contact reachability V. F ROM A GUIDE PATH TO A DISCRETE SEQUENCE OF

CONTACT CONFIGURATIONS (P2 )
The contact reachable workspace is defined as a compro-
mise between two necessary and a sufficient condition for In the second phase, we compute a discrete sequence
contact creation. of static equilibrium configurations Q0 given a root path
necessary conditions: For a contact to be possible, an ob- q0 (t) : [0, 1] −→ CReach
0
. This contact planner uses a contact
stacle Oi ⊂ O necessarily intersects the reachable workspace generator, used to generate static equilibrium configurations.
W (q0 ) of the robot. Also the torso of the robot W 0 (q0 ) must We first describe the contact planning algorithm, before de-
necessarily be collision-free. Therefore we can define an outer scribing the contact generator.
0 0
approximation CNec ⊃ CContact as:
A. Definition of a contact sequence
0 0 0 0 0
CNec = {q : W (q ) ∩ O 6= ∅ and W (q ) ∩ O = ∅} (3) In previous contributions [17], a contact plan is defined as
a sequence of quasi-static equilibrium configurations for each
sufficient condition: Similarly we can define an inner contact phase. For instance, a walk cycle would be described
0 0
approximation CSuf ⊂ CContact by considering a bounding by three key configurations: a double-support configuration,
Suf
volume B encompassing the whole robot in a given pose, a single-support configuration (a contact is broken), and an-
except for the effector surfaces. other double-support configuration (a contact is created). Our
0
definition of contact plan differs: between two consecutive
CSuf = {q0 : W (q0 ) ∩ O 6= ∅ and B Suf (q0 ) ∩ O = ∅} (4) configurations we allow both a contact break and a contact
creation—if they are on the same effector. In the previous
example, our contact plan would simply consist of the two
B. The compromising reachability condition double-support configurations. This representation is sufficient
The ideal shape B ∗ , W 0 ⊂ B ∗ ⊂ B Suf would define to describe all the contact phases, because the single support
a necessary and sufficient condition for contact creation. It phase is implicitely described. Furthermore it removes the
would guarantee that any root configuration q0 ∈ B ∗ would need to compute a single-support quasi-static configuration
result in a contact configuration, while any q0 ∈
/ B ∗ could not. as in the example. Indeed, there might be a case where
To our knowledge B ∗ has no explicit definition. Therefore, we no quasi-static solution exists for the single support phase
0 (because of the environment), but there exists a dynamic
approximate B ∗ to define the contact reachable space CReach .
0 0
We define Ws as the volume W subject to a scaling motion connecting the two double support states. Such motion
transformation by a factor s ∈ R+ . We then consider the will be computed by our framework, because the quasi-static
spaces Cs0 constraint is only required at the contact planning phase; as
shown in the companion video, and explained in Appendix C,
Cs0 = {q0 : W (q0 ) ∩ O 6= ∅ and Ws0 (q0 ) ∩ O = ∅} (5) our framework is able to produce dynamic motions.

The parametrization of s defines a trade-off: If s = 1, then B. Contact planning algorithm

0
Ws0 = W 0 , such that C10 = CNec . By increasing s, the condition Starting from an initial whole-body configuration, we com-
can become sufficient, but less and less necessary. Eq. 5 thus pute a sequence of whole-body configurations Q0 along the
defines the reachability condition. We fix a value s∗ for s and root path q0 (t). We first give an intuition of the algorithm,
0
define CReach = Cs0∗ . The computation of s∗ is detailed in before providing its complete pseudo-code.
Section VIII-C1. In Appendix A, we give a generic method to 1) Algorithm overview: First, the root path q0 (t) is dis-
0
compute the W volumes appearing in the definition of CReach . cretized into a sequence of j key configurations:
Q0 = [q00 ; q0i ; ..., q0j−1 ]
0
C. Computing the guide path in Creach where q00 and q0j−1 are the start and goal configurations.
0
CReach can be sampled efficiently thanks to Eq. 5, and can j depends on a user-defined variable, called the discretization
thus be used with any standard motion planner. Our current step. It corresponds to the ratio between the length of the path
implementation uses the Bi-RRT planner [29] provided by the q0 (t)4 , and the number of configurations selected along it to
HPP software [30]. Our implementation is exactly the same create the contact configurations. Each root configuration of
as the pseudo-code of the original planner (which does not Q0 is then extended into a whole-body configuration such that:
detail the configuration validation method). With respect to a • At most one contact is not maintained (broken) between
“classic” implementation, the only difference is that instead two consecutive configurations.
of validating a configuration using collision detection, we • At most one contact is added between two consecutive
validate it with the reachability condition. configurations.
• Each configuration is in static equilibrium.
This Section has presented a guide path planner for the • Each configuration is collision-free.
geometric root of a robot, implemented as a low-dimensional 4 The length of the path is computed as the weighted 6D Euclidian distance
sampling-based algorithm. Given start and goal configurations, travelled along the it, with a weight of 0.7 for the translation part, and 0.3
it outputs a continuous path for the robot’s root. for the orientation part.
 6

Algorithm 1 Discretization of a path

1: function I NTERPOLATE (s0,Q0 , M AX T RIES)
2: list <State> states = [s0]
3: nb f ail = 0
1 2 3 4
4: i = 1; /*Current index in the list*/
5: while i < length(Q0 ) do
Fig. 4. Contacts are maintained if joint limits and collisions constraints are 6: State pState = last element(states)
respected (2). They are broken otherwise(3,4). The green line represents the
root path. The blurred character represents the previous contact configuration. 7: State s = G EN F ULL B ODY(pState, Q0 [i])
8: if s! = N U LL then
9: nb f ail = 0
a) Maintaining a contact in the sequence: If kinemati- 10: i+ = 1
cally possible, a limb in contact at step i−1 remains in contact 11: return q0
at step i (Figure 4). Otherwise the contact is broken and a 12: else
collision-free configuration is assigned to the limb. If two or 13: nb f ail+ = 1
more contacts can’t be maintained between two consecutive 14: if nb f ail == M AX T RIES then
configurations, one or more intermediate configurations are 15: return F AILU RE
added, to ensure that at most one contact is broken between 16: s =I NTERMEDIATE C ONTACT S TATE(pState)
two sequential configurations. 17: push back(states, s)
b) Creating contacts: Contacts are created using a FIFO
18: return states
approach: we try first to create a contact with the limb that
has been contact-free the longest. If the contact creation does
Algorithm 2 Full body contact generation method
not succeeds, the limb is pushed on top of the queue, and
will only be tried again after the others. 1: function G EN F ULL B ODY (pState,q0 )
2: State newState
3: newState.q0 = q0
2) Pseudo-code of the Algorithm: First, we define an 4: newState.f reeLimbs = pState.f reeLimbs
abstract structure State, that describes a contact configuration. 5: /*First try to maintain previous contacts*/
The use of queues allows a FIFO approach regarding the order 6: nbContactsBroken = 0
in which contacts are tested: we try to replace older contacts 7: for each Limb k in pState.contactLimbs do
first when necessary. Thus the algorithm is deterministic even 8: if !M AINTAIN C ONTACT(pState, q0 , k) then
though it can handle acyclic motions. 9: nbContactsBroken+ = 1
10: if nbContactsBroken > 1 then
11: return N U LL
S t r u c t Limb 12: push(newState.f reeLimbs, k)
{ 13: else
/ / Limb C o n f i g u r a t i o n 14: push(newState.contactLimbs, k)
C o n f i g u r a t i o n qk ;
15: for each Limb k in pState.f reeLimbs do
/ / Effector position in
16: if G ENERATE C ONTACT(q0 , k) then
/ / world c o o r d i n a t e s
17: push(newState.contactLimbs, k)
v e c t o r 6 pk ;
18: remove(newState.f reeLimbs, k)
};
19: return newState
20: if I S I N S TATIC E QUILIBRIUM(newState) then
Struct State
21: return newState
{
22: else
/ / root location
23: return N U LL
C o n f i g u r a t i o n q0 ;
/ / L i s t of limbs not in c o n t a c t
queue<Limb> f r e e L i m b s ;
switch occurring. Otherwise, the method I NTERMEDIATE -
/ / L i s t of limbs in c o n t a c t
C ONTACT S TATE is called. It repositions one end effector
queue<Limb> c o n t a c t L i m b s ;
(either a free limb, or the oldest active contact) towards a
};
new contact position if possible. This repositioning allows to
From the start configuration, given as an input by the user, increase the odds that the contact can be maintained at the next
we create the initial state s0. Algorithm 1 is then called with step. Algorithm 2 gives the pseudo code for G EN F ULL B ODY.
s0, as well as the discretized path Q0 , as input parameters. The method M AINTAIN C ONTACT(pState, q0 , k) performs
At each step, G EN F ULL B ODY is called with the previous inverse kinematics to reach the previous contact position for
state as a parameter, as well as a new root configuration. the Limb. If it succeeds, the new limb configuration is assigned
G EN F ULL B ODY returns a new contact configuration, if it to k. If it fails, a random collision free configuration is
succeeded in computing a configuration with only one contact assigned to k.
 7

Algorithm 3 Adds or repositions a contact for one limb the contact sequence. Second, we rely on off-line generation
1: function I NTERMEDIATE C ONTACT S TATE (state) of configuration candidates.
2: i=0 ǫ
We define CContact ⊃ CContact as the set of configurations
3: while i < length(states.f reeLimbs) do such that the minimum 3D distance between an effector and
4: Limb k = pop(states.f reeLimbs) an obstacle is less than ǫ ∈ R. We then apply the following
5: if G ENERATE C ONTACT(state.q0, k) then steps:
6: push(newState.contactLimbs, k)
1) Generate off-line N valid sample limb configurations
7: return
qki , 0 ≤ i < N (We choose N = 104 );
8: else
2) Using the end-effector positions p(qki ) as indices, store
9: i+ = 1
each sample in an octree data structure;
10: push(states.f reeLimbs, k)
3) At runtime, when contact creation is required, intersect
11: i=0 the octree and the environment5 to retrieve the list of
12: while i < length(states.contactLimbs) do ǫ
samples S ⊂ CContact close to contact (Figure 5 (b)
13: Limb k = pop(states.contactLimbs) and (c));
14: Limb copy = k 4) Use a user-defined heuristic h to sort S;
15: i+ = 1 5) If S is empty, stop (failure). Else select the first con-
16: if G ENERATE C ONTACT(state.q0, k) then figuration of S. Project it onto contact using inverse
17: push(newState.contactLimbs, k) kinematics. (Figure 5 (d) and (e));
18: return 6) If Eq. 6 is verified, stop (success). Otherwise remove
19: else the element from S and go to step 5.
20: push(newState.contactLimbs, copy)
/*Fails if impossible to relocate any effector*/ Because the distance ǫ does not account for the variation
ǫ
21: return F AILU RE in orientation, several samples of CContact may turn out to
be unfeasible at the time of projection. One could consider
ǫ
additionally filtering CContact based on the orientation with
The method I S I N S TATIC E QUILIBRIUM returns whether a respect to the obstacle normal, but in our experience we did
given state is in static equilibrium. not notice any significant improvement in the computational
The pseudo code for the method I NTERMEDIATE C ONTACT- performances of the planner, so we do not perform this
S TATE is given by Algorithm 3. additional step.
G ENERATE C ONTACT(q0 , k) is a call to the contact gen- In all our experiments, the heuristic h is implemented
erator presented in the following Section V-C. It generates as a variation of a manipulability-based heuristic [31]. The
a contact configuration in static equilibrium, and assigns manipulability is a real number that quantifies how “good” a
the corresponding configuration to k. If it fails, k remains configuration is to perform a given task, based on the analysis
unchanged if it is collision free, otherwise it is assigned a of the Jacobian matrix. With such heuristics, a configuration
random collision free configuration. can be chosen because it is far from singularities, and thus
allows mobility in all directions. On the contrary, it can be
chosen because it is particularly efficient to exert a force in
C. Contact generator a desired direction. In our experiments, the former solution is
Given a configuration of the root and the list of effectors usually chosen for computing leg contacts, while the latter is
that should be in contact, the contact generator computes the used for computing hand contacts. We recall the manipulability
configuration of the limbs such that contacts are properly measure and its derivatives in Appendix B.
satisfied and the robot is in static equilibrium: Finally, to verify that a configuration is in static equilibrium,
we use a new robust LP formulation. It replaces the compu-
tationally inefficient double description approach used in our
qk −→ qk , (qk ⊕ qk ) ∈ CEquil and qk ∈ CContact
k
(6)
previous work [20], and presented in the following Section VI.
In previous works [17], [18], the generation of contact is
typically implemented by randomly sampling configurations
VI. A CRITERION FOR ROBUST STATIC EQUILIBRIUM
and projecting the whole robot configuration onto the closest
surfaces with an inverse kinematics solver. In case of failure We first give a linear program (LP) that verifies whether a
of the projection, the process would randomly iterate. contact configuration allows for static equilibrium. This LP is
We propose two modifications of this general algorithm the same that was proposed in [32]. From this formulation
principle. First our contact generator handles each limb Rk we derive a new LP that quantifies the robustness of the
independently. By handling each limb separately, we reduce equilibrium to uncertainties in the contact forces. In turn, from
the complexity of the generation of contact configurations. this value we can either choose the most robust candidate, or
This is made possible thanks to the reachability condition set a threshold on the required robustness.
in P1 that produces a root path that we can afford not to
modify in P2 , and because we allow both a contact break and 5 this operation is achieved natively by the fcl library https://flexible-
a contact creation between two consecutive configurations of collision-library.github.io/
 8

a b c d e

Fig. 5. Generation of a contact configuration for the right leg of HRP-2. (a): Selection of reachable obstacles. (b): Entries of the limb samples database
(with N = 4). (c): With a proximity query between the octree database and the obstacles, configurations too far from obstacles are discarded. (d): The best
candidate according to a user-defined heuristic h is chosen. (e): The final contact is achieved using inverse kinematics.

1) Conditions for static equilibrium: We first define the

variables of the problem, for e contact points, expressed in find β ∈ R4e
world coordinates: subject to Gβ = Dc + d (9)
3 β≥0
• c ∈ R is the robot center of mass (COM);
• m ∈ R is the robot mass;
T 2) Formulation of a robust LP: Let b0 ∈ R be a scalar
• g = [0, 0, −9.81] is the gravity acceleration;
value. We now define the following LP:
• µ is the friction coefficient;
• for the i-th contact point 1 ≤ i ≤ e: find β ∈ R4e , b0 ∈ R
– pi is the contact position; minimize − b0
– fi is the force applied at pi ; (10)
subject to Gβ = Dc + d
– ni , γ i1 , γ i2 form a local Cartesian coordinate system
centered at pi . ni is aligned with the contact surface β ≥ b0 1
normal, and the γ i s are tangent vectors. We observe that if b0 is positive then (9) admits a solution,
According to Coulomb’s law, the non-slipping condition is and b0 is proportional to the minimum distance of the contact
verified if all the contact forces lie in the friction cone defined forces to the boundaries of the friction cones. If b0 is negative,
by the surface. As classically done, we linearize the friction the configuration is not in static equilibrium, and b0 indicates
cone in a conservative fashion with a pyramid included in it, “how far” from equilibrium the configuration is. We thus use
described by four generating rays of unit length. We choose b0 as a measure of robustness. A simple approach to robustness
for instance: consists in choosing a smaller friction coefficient, to constrain
 T the forces to lie away from the boundaries of the real cone.
Vi = ni + µγ i1 ni − µγ i1 ni + µγ i2 ni − µγ i2 However, this would result in a small safety margin for forces
of low magnitude, and an excessively large safety margin for
Any force belonging to the linearized cone can thus be
large forces as the boundaries grow more and more apart.
expressed as a positive combination of its four generating rays.
In comparison, our margin b0 is constant, and provides a
helpful mean to compare the robustness of different contact
∀i ∃βi ∈ R4 : βi ≥ 0 and fi = Vi βi , configurations.
where βi contains the coefficients of the cone generators. We In our implementation, rather than solving directly (10),
can then stack all the constraints to obtain: we solve an equivalent problem of smaller dimension that we
get by taking the dual of (10) and eliminating the Lagrange
∃β ∈ R4e , β ≥ 0 and f = Vβ, (7) multipliers associated to the inequality constraints:

where V = diag({V1 , . . . , Ve }), and f = (f0 , ..., fe ). find ν ∈ R6

From the Newton-Euler equations, to be in static equilib- maximize − (Dc + d)T ν
rium the contact forces have to compensate the gravitational (11)
subject to GT ν ≥ 0
forces:
1T G T ν = 1
      Indeed, from Slater’s conditions [33], we know that the
I3 . . . I3 03×3 −mg
V β, = c+ (8) optimal values of an LP and its dual are equal. Therefore
p̂1 . . . p̂e mĝ 0
| {z } | {z } | {z } the optimal value ν ∗ gives the optimal value b∗0 through the
G D d equality b∗0 = (Dc + d)T ν ∗ .
3×3
where x̂ ∈ R is the cross-product matrix associated to x.
If there exists a β ∗ satisfying (7) and (8), it means that the VII. S OURCE CODE OF OUR PLANNER
configuration is in static equilibrium. The problem can then Our planner is implemented using the Humanoid Path
be formulated as an LP: Planner (HPP) software, introduced in [30]. HPP is an open
 9

source motion planning framework developed by the Gepetto

team at LAAS-CNRS. HPP implements the standard tools
and algorithms used in motion planning, such as the Bi-RRT
planner from which RB-RRT is derived.
The robot models used in our experiments are described
using the standard urdf file format, compatible with HPP.
Our implementation of the planner is also open
source. Both HPP and our planner can be simply
downloaded and compiled by following the instructions on
https://humanoid-path-planner.github.io/hpp-doc/download.
html?branch=rbprm.

VIII. R ESULTS
In this section we present some of the results obtained
with our planner. The complete sequences are shown in the
companion video. Specifically, we demonstrate the planner for
two legged robots, in a large variety of environments: the
humanoid HRP-2 and the quadruped HyQ.
Our contact plans are then interpolated with a dedicated
solution to the interpolation problem P3 . This allows us to
validate the obtained motions in a dynamic simulator. This
validation is an important contribution as it increases the Fig. 6. HRP-2 in the standing scenario.
confidence that the contact plans we compute can effectively
result in feasible motions on the real robot. One motion is
demonstrated on the real HRP-2 robot.
At the end of this section, we discuss the role of the
parameters of our framework. We then provide the interactive
computation times obtained in each case. We also compare the
times obtained with HRP-2 with respect to previous works.

A. Experimental validation of the contact plans

To generate continuous movements from our contact plans
Fig. 7. Selected frames from the car egress scenario.
we used either the framework proposed in [1], or our own
implementation of a P3 solver (Appendix C). The resulting
movements have been validated either on the real HRP-2 the goal. In each scenario we detail the contacts involved and
robot (details can be found in [1]), or with our dynamic the heuristics chosen (either hEFORT , hvel or hw , all of which
simulator, based on a state-of-the-art algorithm [28]. In the are defined in the Appendix B).
simulations we controlled the robot with a standard inverse- 1) HRP-2 – Standing up (Figure 6): From a bent configu-
dynamics controller [34]. The code source of the simulator is ration, the robot has to stand up using a wall as support, and
available at https://github.com/andreadelprete/pinocchio inv climbing a 25-cm high step.
dyn/releases/tag/rbprm. This controller tries to follow the given Contacts involved: All (both feet and hands).
whole-body trajectories, giving higher priority to the center- Heuristics: hw for the feet, hEFORT for the hands.
of-mass and end-effectors tracking with respect to the joint 2) HRP-2 – Car egress (Figure 7): In this scenario inspired
tracking. The controller also makes sure that the resulting from the DRC car egress HRP-2 has to step out of a car.
contact forces lie inside the specified friction cones (we used a Contacts involved: All (both feet and hands).
friction coefficient of 0.3), and that the joint position, velocity Heuristics: hw .
and torque limits are satisfied. The companion video shows 3) HRP-2 – Staircase with high steps (Figure 8): The goal
the obtained motions. is to climb three 15-cm high steps.
Contacts involved: Feet and right arm.
Heuristics: The manipulability hw is chosen for the feet;
B. Description of the scenarios hEFORT is chosen for the right arm.
In all the scenarios considered, the formulation of the 4) HyQ – DRC-style rubble (Figure 9): The quadruped
problem is always the same: a start and goal root config- robot must cross a rubble composed of bricks rotated at
uration are provided as input (except in the stair climbing different angles and directions.
scenario where the start whole body configuration is given). Contacts involved: All (the 4 legs).
The framework computes the initial contact configuration, and Heuristics: hw for all legs. The robustness threshold b0 is set
outputs a sequence of contact configurations connecting it to to 20.
 10

Fig. 8. HRP-2 in the stair climbing scenario.

Fig. 9. Robust crossing of rubbles by HyQ.

Fig. 11. Crossing a hole contact sequence for HyQ.

Fig. 10. HyQ crossing a narrow bridge.

5) HyQ – Obstacle race (Figure 10 and 11): In this long

scene, HyQ has to cross a 55-cm large hole, followed by a
narrow “bridge”, only 25-cm large.
Contacts involved: All (the 4 legs).
Heuristics: hw for all legs. The robustness threshold b0 is set
to 10.
6) HRP-2 – Path re-planning (Figure 12): In this long
scene, HRP-2 plans a path through several obstacles. The
scene is edited during the execution of the motion: a stair
is added, some stepping stones are removed, and part of the
final staircase is deleted. All these modifications require re- Fig. 12. HRP-2 in the re-planning scenario. After the red step stones are
planning. removed, a new sequence of contacts is re-planned. Hand contacts are not
presented here for readability.
Contacts involved: Feet and the right arm.
Heuristics: hw for all legs. hEFORT for the right arm. The
robustness threshold is set to 2.
 11

Value of s Sensitivity Specificity Path planning success rate

1 76% 100 % Stairs 100%
1.1 88% 96% Standing 68%
1.15 93% 94% Car 77%
1.2 97% 92.5% Rubble 97%
1.25 98% 91.7% Race 88.0%
1.5 99% 90.5% TABLE II
TABLE I P ERCENTAGE OF SUCCESSFUL COMPLETE CONTACT PLANNING RATES FOR
S ENSITIVITY AND SPECIFICITY VALUES OF THE REACHABILITY EACH SCENARIO , ROUNDED TO THE FIRST DECIMAL .
CONDITION , DEPENDING ON THE SCALING VALUE s OF W 0 .

Equilibrium Kinematic Equilibrium

success rate failure failure
High stairs 99.5% 0.1% 0.4%
C. Role of the main parameters Standing up 87.8% 6.1% 6.1%
We discuss the factors that influence the outcome of our Car egress 66.2% 15.9% 17.9%
Rubble 97.54% 0.16% 2.3%
planner: the root scaling factor s (Section IV-B), the heuristics Obstacle race 92.4% 0.15% 7.45%
for contact generation (Appendix B), and lastly, the discretiza- TABLE III
S UCCESS RATES OBTAINED FOR THE GENERATION OF STATIC
tion step for the guide path. The appropriate value for these EQUILIBRIUM CONTACT CONFIGURATIONS FOR EACH SCENARIO ,
parameters is computed empirically based on use-case analysis ROUNDED TO THE FIRST DECIMAL . C OLUMN 1 INDICATES INDICATES THE
or trials and errors. RATE OF CONTACT GENERATION THAT SUCCEEDED . I N THE CASES WHERE
THE GENERATION FAILS , IT CAN BE EITHER A KINEMATIC ISSUE ( COLUMN
1) Choosing the scaling factor s: For several values of 2), OR BECAUSE NO CONTACT CONFIGURATION LED TO A STATIC
s, we generated 10000 configurations. We then computed EQUILIBRIUM CONFIGURATION ( COLUMN 3). N OTE THAT A FAILURE IN
the sensitivity and specificity of the reachability condition. THE CONTACT GENERATION IS NOT EQUIVALENT TO A FAILURE OF THE
CONTACT PLANNING ALGORITHM .
In this context, the sensitivity refers to the percentage of
0 0
configurations in CReach , effectively belonging to CContact .
0
If a sampled configuration is in CReach , but our method is
unable to generate a contact configuration from it, as a result
the sensitivity decreases. The sensitivity thus illustrates the on the output of the planner: if too large steps are taken, the
confidence we have that any configuration in CReach 0
will planner may fail since we impose the constraint that only one
effectively lead to a contact configuration. Conversely, the contact change might occur between two consecutive steps. On
specificity refers to the percentage of configurations not in the other hand, a small step will not impact the success rate
0
CReach 0
, effectively not belonging to CContact . If a sampled of the planner, but may generate unnecessary states. In most
0
configuration is not in CReach , but our method is able to scenarios the torso of HRP-2 moves about 15 cm between two
generate a contact configuration from it, as a result the speci- postures, but only 3 cm for the car egress scenario to handle
ficity decreases. The specificity thus illustrates the confidence the geometry of the car. For future work, we would like to
we have that all configurations that allow contact creation automatically adapt the size of the discretization step to the
0
belong to CReach (or informally, the confidence that we are complexity of the environment.
not missing valid solutions). We thus look for a compromise
between sensitivity and specificity. D. Performance analysis
The obtained results for HRP-2 are shown in Table I, To analyze performance, we ran the planner 1000 times for
averaged over all scenes (except for the car egress: in this each scenario. We measured the computation time spent in
scenario, statistical tests are not really conclusive since we each part of the algorithm, and analyzed success rate.
are only interested in a small area of the environment). 1) Success rates (Table II): Despite the complexity of the
As it can be expected, the scaling results in a high increase scenarios and the approximations made in our formulation, our
of the sensitivity, with a decrease of the specificity. For HRP-2 planner succeeded in the large majority of cases.
we decided to set s∗ = 1.2. Table III presents the rate of successful contact generation.
2) Choosing the heuristics: In our conference paper [20], Note that a failure in contact generation for a root configura-
the computed motions were generated using the EFORT tion is not equivalent to a failure in the contact plan. It simply
heuristic. EFORT is designed for tasks requiring large magni- means that another limb was tested for contact generation for
tude contact forces (such as pushing / pulling / climbing). In the same root configuration. As expected, a more constrained
locomotion tasks, such as the stair scenario, one issue with scenario such as the car egress provides less satisfying results,
EFORT is that it tends to generate configurations close to despite the high success rate of the planner.
singularities (and joint limits). While this did not significantly 2) Computation times (Table IV): For HRP-2, most of the
impact the generation of the plan, the resulting interpolation time was spent performing inverse kinematics. This is not
turned out to be harder. For this reason, we prefer to use our surprising considering the number of calls to the methods: IK
manipulability-based heuristic for the legs of the robot, but we projection is used intensively to maintain contact continuity
still use EFORT for the arms, which results in fewer contact between two postures; it is also applied every time a new
repositionings. candidate needs to be evaluated. In particular for the car
3) Discretization of the guide path: The discretization step egress scenario, the kinematic constraints are very demanding
is a user-defined, fixed parameter. The step has an influence to avoid collisions.
 12

Time
Scenario Complete guide Static equilibrium Inverse Kinematics Total generation time per
Collision (ms)
(nb steps) generation (ms) (ms) (ms) (ms) step
(ms)
Stairs (18) 5 – 6 – 18 13 – 32 – 329 1 – 4 – 38 26 – 127 – 1345 92 – 261 – 2174 15
Standing 65 – 1086 –
27 – 144 – 338 2 – 12 – 37 144 – 1046 – 2374 371 – 2257 – 7671 94
(24) 5227
320 – 6971 – 409 – 1766 – 3154 – 15323 –
Car (86) 297 – 1187 – 8483 5834 – 31391 – 281000 365
44002 14752 165541
Rubble
37 – 573 – 1685 583 – 2714 – 9459 491 – 1971 – 6273 269 – 706 – 3118 1811 – 7195 – 23241 86
(82)
455 – 1359 –
Race (134) 14 – 51 – 125 397 – 923 – 9924 228 – 471 – 5415 1436 – 3343 – 41446 25
21045
TABLE IV
MINIMUM , AVERAGE AND WORST TIME ( IN MS ) SPENT IN THE GENERATION PROCESS FOR EACH SCENARIO AND EACH CRITICAL PART OF THE
GENERATION PROCESS ( NOT ALL PARTS ARE TIMED , THUS THE AVERAGE TOTAL COMPUTATION TIME IS HIGHER THAN THE SUM OF EACH PART ). T HE
LAST COLUMN INDICATES THE AVERAGE TIME NECESSARY TO COMPUTE ONE CONTACT TRANSITION . T HE C OLLISION COLUMN TIMES INCLUDES THE
( NEGLIGEABLE ) OCTREE INTERSECTION OPERATION NECESSARY TO RETRIEVE THE CANDIDATE SAMPLES .

Scenario Method Computation time

Hauser [8] 5.42 min that allows any reader to reproduce our results. Furthermore,
Stair 20 cm Mordatch et al.[11] 2 to 10 min P3 remains challenging in the presence of obstacles. The only
Ours + [1] < 2s valid scenarios addressed completely in previous works are
Hauser [8] 4.08 min
Stair 30 cm Mordatch et al.[11] 2 to 10 min
thus the stair-climbing scenarios of different heights proposed
Ours < 2s by Hauser in [8], and the table-egress scenario by Escande
Hauser [8] 10.08 min et al. in [17], which we consider to be of similar complexity
Stair 40 cm Mordatch et al.[11] 2 to 10 min
Ours < 5s
with respect to the car-egress scenario (we did not consider
Bouyarmane et al. [18], [17] 3.5 hours the stairs in the scene). Both scenarios are tested with HRP-2.
Table (car) egress
Ours < 60 s Table V presents the computation times for these scenarios,
TABLE V
C OMPARISON BETWEEN THE COMPUTATION TIMES OBTAINED BY OUR clearly demonstrating that our approach is order of magnitude
METHOD AND PREVIOUS ONES FOR ADDRESSING THE WHOLE PROBLEM . faster than previous works.

IX. D ISCUSSION : VALIDITY AND PURPOSE OF OUR

CONTACT PLANNER
On the other hand for HyQ most of the time is spent testing As demonstrated in the results section, the main purpose
the static equilibrium of the candidate configurations. of our method is the reduction of the algorithmic complexity
In all scenarios, one can observe that the average compu- of the problem, which leads to an interactive application.
tation time for one single step is largely below one second, This property is critical for online applications with the robot
thus enabling interactive applications and online autonomous and was not proposed by any of the previous contributions.
planning of the robot motion. Our method addresses highly constrained environments while
Conclusion: These results confirm that our approach pro- improving the search time by orders of magnitude. This high
vides a satisfying compromise between completeness and performance is reached at the cost of some approximations
efficiency, thus enabling online planning while controlling the that we discuss here.
robot. Indeed, when the contact planning fails, it fails rapidly.
The first approximation is the verification of contact reach-
This allows us to rapidly re-plan with a reasonable chance
ability (q0 ∈ CContact
0
). Our reachability condition (q0 ∈
of success. The most efficient (and immediate) approach to 0
CReach ) is computationally efficient and provides an accurate
obtain a valid contact plan as fast as possible would be to 0
approximation of CContact (Section IV-B). This is demon-
launch in parallel several instances of the planner (our current
strated by the second column of Table III, and illustrated
implementation is single-threaded) and to use any successful
by Figure 13. Indeed, in the large majority of cases, (84%
result as a plan for solver P3 .
in the worst car egress case), we are able to find a contact
0
configuration for any configuration in CReach .
E. Comparison with previous work Another source of computational cost identified in previous
We did our best to provide a fair comparison of the works is the verification of equilibrium feasibility. The main
computation complexity of our method with the state of the assumption of our work is that for the class of problems we
art. However existing benchmarks for motion planning algo- consider contact reachability implies equilibrium feasibility.
rithms [35] do not yet encompass contact planning. Moreover, Our scenarios show that the assumption is verified in the
the source code of the previous methods of the state of the majority of cases when at least one contact surface is quasi
art is often not available. Providing a fair comparison with flat [32], that is when the friction cone of the contact surface
the algorithms performing on the same computer and on the contains the direction opposite to the gravity. Figure 13
same scenarios is yet out of reach. A step in this direction is illustrates this observation, demonstrated empirically by the
the open-source release of our source code (see Section VII) third column of Table III. In the worst case, in our experiment
 13

Our contribution to P2 is a fast contact generation scheme

that can optimize user-defined criteria.
Our results demonstrate that our method allows a pragmatic
compromise between three criteria that are hard to reconcile:
generality, performance, and quality of the solution, making
it the first acyclic contact planner compatible with interactive
applications.
Regarding generality, the reachability condition, coupled
with an approach based on limb decomposition, allows the
method to address automatically arbitrary legged robots. Re-
garding performance, our framework is efficient in address-
ing both P1 and P2 . This results in interactive computation
times. Regarding the quality of the paths, we are able
to compute equilibrium feasible paths in all the presented
scenarios, with high success rates. As for [18], failures can
still occur, due to the approximate condition used to compute
0 0 0 the guide path. The low computational burden of our frame-
CEquil ⊂ CContact ≈ CReach
work however allows for fast re-planning in case of failure.
Fig. 13. Illustration of several root configurations sets used in this paper Furthermore, because of this approximation, the guide search
in a 2D scene. Obstacles are violet, and units are in meters. To show the is not complete. The choice is deliberate, because we believe
sets in a 2D representation, all the rotational joints of HRP-2 are locked in
the shown configuration, such that a torso configuration is only described
that it is necessary to trade completeness for efficiency at all
by two positional parameters (x and y). The root of the robot is indicated stages of the planner. However, one direction for future work is
0
with a black cross. To compute the reachable workspace, the point on the to focus on a more accurate formulation of Creach to improve
0
ankle indicated by a green cross was used. CEquil 0
is included in CContact .
0 0
the approximation.
CReach approximates CContact . Depending on a parametrization, we can
0
obtain CContact 0
⊂ CReach . Considering the configurations around the top Our method applies to any scenario where at least one
0
obstacle, we can observe a similarity between CEquil 0
and CContact when contact friction cone contains the direction opposed to the
the reachable workspace of the legs includes quasi-flat surfaces. gravity (i.e. quasi-flat). This class of scenarios include all
the problems proposed at the DARPA Robotics Challenge.
One way to further extend its range of application, which
the assumption was verified for 82% of the total amount we consider for future work, is to include the equilibrium
of trials that verified contact reachability. In the example criterion when solving P1 . Considering the set of obstacles
of [18], the verification of equilibrium feasibility implies a intersecting with the reachable workspace for a given root
constructive demonstration by exhibiting a valid q0 , requiring configuration as candidate surfaces, we can use them to verify
several minutes of planning. Our method, in comparison, takes the equilibrium criterion. This would give us a necessary
from a few milliseconds to several seconds. condition for equilibrium feasibility.
These results clearly justify our pragmatic approach. While we have exhibited complete multi-contact locomotion
obtained with our contact planner, our main concern for future
X. C ONCLUSION work is to address the interpolation between contact sequences
(P3 ), which remains an open issue in highly-constrained sce-
In this paper we consider the multi-contact planning prob- narios. Solving P3 requires addressing efficiently the collision
lem, formulated as three sub-problems P1 , P2 , P3 , addressed avoidance problem in the interpolation phase, an issue not
sequentially. While we propose a global framework that han- addressed by existing frameworks. We aim at providing our
dles all these problems, our contribution focuses on P1 and plans with transition certificates, that would define constraints
P2 . The first problem P1 consists in computing an equilibrium on P3 , under which the transition between two contact con-
feasible guide path for the root of the robot; the second figurations is feasible and collision-free. Finally, we aim at
problem P2 is the computation of a discrete sequence of performing kinodynamic planning to remove the constraint
whole-body configurations along the root path. We believe that that configurations be in static equilibrium. We believe that
this decomposition is currently the most promising approach the most promising direction in this regard is to integrate the
towards a global resolution of the problem. We also claim to notion of Admissible Velocity Propagation [36]. Addressing
have achieved a significant step towards this objective thanks these two issues is essential to bridge the gap between the
to the dimensionality reduction provided by the reachability planning and control aspects of legged locomotion.
condition. With our results and the release of our source code,
we hope to inspire further research in this direction.
A PPENDIX A
Our contribution to P1 is the introduction of a low-
0 G ENERATING THE W VOLUMES FOR HRP-2
dimensional space Creach , an approximation of the space
0
of equilibrium feasible root configurations. Creach can be We detail our method to generate the volumes W used
efficiently sampled and has a low-dimension. For these reasons in RB-RRT, with the example of HRP-2. The kinematic tree
we are able to solve P1 much faster than previous approaches. is split into four limbs Rk . The arms are connected to the
 14

Velocity ellipsoid Force ellipsoid (scale 0.5)

Fig. 16. Examples of velocity and force ellipsoids for a manipulator composed
of 2 DoFs and 2 segments. Only the horizontal and vertical speeds are shown
(not the rotation speeds).

Fig. 14. The W volumes computed for HRP-2. The red shapes are W 0 . The
green shapes represent the W k . and often not contact reachable, as illustrated in Figure 13,
where the exterior boundaries of the reachable workspace
0
appear red, thus not belonging to CContact ). We choose again
to be less complete but more efficient, regarding the number
of collision tests to be performed by RB-RRT. In step 1 on
the other hand, selecting the convex hull (Figure 15–middle)
instead of a minimum encompassing shape (Figure 15–left)
may introduce false positives. Concretely, because the false
positive set intersects with W 0 , the scaling volume of the
robot torso, the induced error is compensated, as verified by
the results shown by Table III.

B. Step 2: computing the torso scaling workspace W 0 of the

robot
Fig. 15. Different approximations of the range of motion of the right arm of To define the volume W 0 of HRP-2, we proceed in an
HRP-2. Left: non convex-hull, computed with the powercrust algorithm [37].
Middle: convex hull of the reachable workspace. Right: Simplified hull used empirical manner. First, we compute the bounding boxes of
in our experiments. the robot torso, head, and upper legs (Figure 14 – red shapes).
Then, we perform a scaling of these boxes by a factor s.
The higher s is, the more likely sampled configurations are
shoulders, and the legs to the root. The obtained volumes W to be feasible, but the less complete is the approach. To
are shown in Figure 14. compute the appropriate value of s, we proceed as described
in Section VIII-C1, and choose empirically s∗ = 1.2 as the
A. Step 1: computing the reachable workspace W k of a limb appropriate value for HRP-2.
To generate a volume W k , we proceed as follows:
A PPENDIX B
1) Generate randomly N valid limb configurations for Rk , M ANIPULABILITY- BASED HEURISTICS FOR CONTACT
for N really large (say 100000); SELECTION
2) For each configuration, store the 3D position of the end
effector joint relatively to the root of Rk ; then compute This Section proposes two heuristics to select a contact
the convex hull of the resulting point cloud; that optimizes desired capabilities. For instance, one can be
3) The resulting polytope can contain a very large number interested in configurations that allow to efficiently exert a
of faces. A last step is thus to simplify it with the blender force in the global direction of motion, or to stay away from
decimate tool (http://wiki.blender.org/index.php/Doc:2. singular configurations. We derive these heuristics from the
4/Manual/Modifiers/Generate/Decimate). This tool re- work by [31], recalled here.
moves a user-defined amount of vertices (and faces) of 1) The force and velocity ellipsoids: We consider: a limb
the polytope, thus resulting in a convervative approxi- configuration qk ; its end effector position pk ; its Jacobian
mation of the original shape. For HRP-2 we apply the matrix Jk (qk ); a force f exerted by the end effector. For clarity
operator with a ratio of 0.06, resulting in a polytope of in the rest of the section we omit the k indices and write
38 faces for the arms and the legs. Jk (qk ) as J. Yoshikawa [31] defines the velocity( 12) and
force (13) ellipsoids:
Figure 15 illustrate the obtained W k for HRP-2. Regarding
the procedure, we can see that step 2 is conservative (Fig-
ṗT (JJT )−1 ṗ ≤ 1 (12)
ure 15–right), which is acceptable, especially because the lost
set essentially relates to configurations close to singularity
(they are close to the boundaries of the reachable workspace, f T (JJT )f ≤ 1 (13)
 15

They describe the set of end-effector velocities (respectively where L̇ is the angular momentum expressed at the com.
forces) that can be reached under the constraint ||q̇||2 ≤ 1 for Eq. 16 defines a 6-dimensional cone K [40], [41]. For a
the current configuration. The longer the axis of the ellipsoid given set of contacts, this cone determines all the admissible
is, the more important the velocity (resp. force) of the end- wrenches w that can be generated by contact forces inside
effector the direction of the axis can be (Figure 16). their friction cones. The face form of K can be computed using
2) Manipulability-based heuristics: From these definitions, the double description method [42], resulting in the following
we can derive two useful heuristics, that all account for the linear inequalities:
environment and the task being performed. The first one,
EFORT, was introduced by [38]; the second one derives the
K = {w, Aw ≤ b} (17)
manipulability measure proposed by Yoshikawa [31].
With EFORT, we define the efficiency of a configuration as
the ability of a limb to exert a force in a given direction. We The objective is then to plan a trajectory for the center of
thus consider the force ellipsoid as a basis for our heuristic. mass such that the generated w always verifies Eq. 17. We
In a given direction ρ, the length of the ellipsoid is given by now consider two contact configurations q0 and q1 computed
the force-transmission ratio [39]: by our planner: in the general case one contact is broken and
1
one created to get from q0 to q1 . We manually define the
fT (q, ρ) = [ρT (JJT )ρ]− 2 duration of each of the three contact phases. In each phase
s the centroidal wrench w is constrained to lie inside a cone
In our problem, to compare candidate configurations, we
Ku , u = 0 . . . 2. We call the total duration of the motion T ,
include the quality of the contact surface, and choose ρ as the
and formulate the following optimization problem:
direction opposite to the local motion (given by the difference
between two consecutive root positions):
2 Z
X tu +∆tu
1
hEFORT (q, ρ) = [ρT (JJT )ρ]− 2 (µnT ρ) (14) minimize ℓ(c̈(t), L̇(t))dt
c̈(t),L̇(t) tu
u=0
where µ and n are respectively the friction coefficient and the
subject to Au w(t) ≤ bu , ∀t ∈ [tu , tu + ∆tu [ , ∀u
normal vector of the contact surface.
hEFORT will favor contacts that allow large efforts. EF ORT Yu c(t) ≤ yu , ∀t ∈ [tu , tu + ∆tu [ , ∀u
(18)
in particular is useful for tasks such as standing up, pushing / c(0) = cq0
pulling. In other less demanding cases, manipulability can also c(T ) = cq1
be considered to avoid singularities. To do so, we can consider
c(0) = ċ(0) = c̈(0) = 0
the manipulability measure hw , also given by Yoshikawa [31]:
q c(T ) = ċ(T ) = c̈(T ) = 0
hw (q) = det(JJT ) (15)
The cost function ℓ is a weighted sum of the angular mo-
hw measures the “distance” of a given configuration to sin- mentum and center-of-mass acceleration variation over the
gularity. When hw is equal to 0, the configuration is singular; whole trajectory. The center-of-mass positions and velocities
the greater hw is, the further away the configuration is from c(t), ċ(t) are internal variables, obtained through the double
singularity. integration of c̈(t). Then w(t) is obtained directly from these
variables. cq0 and cq1 are the center-of-mass positions for
A PPENDIX C configurations q0 and q1 respectively. Yu and yu denote
A DDRESSING P3 stacked kinematic constraints on the center of mass position,
determined by the active contact locations. The inequalities
The stair climbing and the standing up scenarios were
for each contact are determined in the same way that the
validated with the trajectory optimization scheme provided in
reachable workspace is computed in Appendix A, with the
[1]. To address the other scenarios, we propose a new imple-
effector serving as root.
mentation, entirely open source (https://github.com/stonneau/
This formulation can trivially be extended over the whole
python sandbox/releases/tag/tro paper ), which can be inte-
contact sequence. In our implementation, the problem is
grated directly in our motion planner software. This for-
discretized using time steps of 100 ms.
mulation uses the center of mass acceleration and angular
momentum as input variables, while previously the contact The output of this optimization problem is an admissible
forces were used. The center-of-mass trajectory resulting from center-of-mass trajectory. To compute the whole body motion,
the optimization is then turned into a collision-free whole-body we use a two-step approach.
trajectory. First, we plan a kinematic motion for the robot, subject to
We rewrite Eq. 8 in the general case: the contact constraints. We also constrain the center of mass
to follow the computed trajectory. This is achieved using a
  constraint-based RRT planner [30]. As a result we obtain a
m(c̈ − g)
Gβ = (16) collision-free whole-body motion.
mc × (c̈ − g) + L̇
| {z } The entire resolution takes approximatively 1.5 seconds for
w a complete contact transition.
 16

ACKNOWLEDGEMENTS [20] S. Tonneau, N. Mansard, C. Park, D. Manocha, F. Multon, and J. Pettré,

“A reachability-based planner for sequences of acyclic contacts in
This research is supported by Euroc (project under FP7 cluttered environments,” in Int. Symp. Robotics Research (ISRR), Sestri
Levante, Italy, 2015.
Grant Agreement 608849); Entracte (ANR grant agreement [21] M. X. Grey, A. D. Ames, and C. K. Liu, “Footstep and motion
13-CORD-002-01); the ARO Contract W911NF-14-1-0437; planning in semi-unstructured environments using possibility graphs,” in
and the NSF award 1305286. ICRA’17. IEEE International Conference on Robotics and Automation,
2017 (Submitted). IEEE, 2017, available at https://arxiv.org/pdf/1610.
00700v1.pdf.
[22] N. Perrin, O. Stasse, L. Baudouin, F. Lamiraux, and E. Yoshida, “Fast
R EFERENCES humanoid robot collision-free footstep planning using swept volume
approximations,” IEEE Transactions on Robotics, vol. 28, no. 2, pp.
[1] J. Carpentier, S. Tonneau, M. Naveau, O. Stasse, and N. Mansard, 427–439, April 2012.
“A versatile and efficient pattern generator for generalized legged [23] M. Zucker, N. Ratliff, M. Stolle, J. Chestnutt, J. A. Bagnell,
locomotion,” in Proc. of IEEE Int. Conf. on Robot. and Auto (ICRA), C. G. Atkeson, and J. Kuffner, “Optimization and learning for rough
Stockholm, Sweden, 2016. terrain legged locomotion,” The International Journal of Robotics
[2] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture Point: A Step Research, vol. 30, no. 2, pp. 175–191, 2011. [Online]. Available:
toward Humanoid Push Recovery,” 2006 6th IEEE-RAS International https://doi.org/10.1177/0278364910392608
Conference on Humanoid Robots, 2006. [24] N. D. Ratliff, D. Silver, and J. A. Bagnell, “Learning to search:
[3] T. Siméon, J. Laumond, J. Cortes, and A. Sahbani, “Manipulation plan- Functional gradient techniques for imitation learning,” Auton. Robots,
ning with probabilistic roadmaps,” The Int. Journal of Robot. Research vol. 27, no. 1, pp. 25–53, Jul. 2009. [Online]. Available: http:
(IJRR), vol. 23, no. 7-8, pp. 729–746, 2004. //dx.doi.org/10.1007/s10514-009-9121-3
[4] T. Bretl, “Motion planning of multi-limbed robots subject to equilibrium [25] K. Hauser, “Fast interpolation and time-optimization with contact,” The
constraints: The free-climbing robot problem,” The Int. Journal of Robot. Int. Journal of Robot. Research (IJRR), vol. 33, no. 9, pp. 1231–1250,
Research (IJRR), vol. 25, no. 4, pp. 317–342, Apr. 2006. Aug. 2014.
[5] C. G. Atkeson, B. P. W. Babu, N. Banerjee, D. Berenson, C. P. Bove, [26] A. Herzog, N. Rotella, S. Schaal, and L. Righetti, “Trajectory gen-
X. Cui, M. DeDonato, R. Du, S. Feng, P. Franklin, M. Gennert, J. P. eration for multi-contact momentum-control,” in Humanoid Robots
Graft, P. He, A. Jaeger, K. K. J. Kim, L. Li, X. L. C. Liu, T. Padir, (Humanoids), 15h IEEE-RAS Int. Conf. on, Nov. 2015.
F. Polido, G. G. Tighe, and X. Xinjilefu, “What happened at the darpa [27] C. Park, J. S. Park, S. Tonneau, N. Mansard, F. Multon, J. Pettré, and
robotics challenge, and why?” Carnegie Mellon University, Pittsburgh, D. Manocha, “Dynamically balanced and plausible trajectory planning
USA, Tech. Rep., 2015. for human-like characters,” in To appear in Proc. of I3D ’16, Seatle,
[6] L. Kovar, M. Gleicher, and F. Pighin, “Motion graphs,” in ACM Trans. USA, 2016.
Graph., vol. 21, 2002, pp. 473–482. [28] D. M. Kaufman, S. Sueda, D. L. James, and D. K. Pai, “Staggered pro-
[7] J. Pettré, J.-P. Laumond, and T. Siméon, “A 2-stages locomotion planner jections for frictional contact in multibody systems,” ACM Transactions
for digital actors,” in Proc. of the 2003 ACM SIGGRAPH/Eurographics on Graphics, vol. 27, no. 5, p. 1, 2008.
symp. on Comp. animation, Granada, Spain, 2003, pp. 258–264. [29] S. LaValle and J. Kuffner, J.J., “Randomized kinodynamic planning,” in
[8] K. Hauser, T. Bretl, K. Harada, and J.-C. Latombe, “Using motion Proc. of IEEE Int. Conf. on Robot. and Auto (ICRA), vol. 1, Detroit,
primitives in probabilistic sample-based planning for humanoid robots.” Michigan, USA, 1999, pp. 473–479 vol.1.
in WAFR, ser. Springer Tracts in Advanced Robot., S. Akella, N. M. [30] J. Mirabel, S. Tonneau, P. Fernbach, A. K. Seppl, M. Campana,
Amato, W. H. Huang, and B. Mishra, Eds., vol. 47. Springer, 2006. N. Mansard, and F. Lamiraux, “Hpp: A new software for constrained
motion planning,” in 2016 IEEE/RSJ International Conference on Intel-
[9] M. Stilman, “Global Manipulation Planning in Robot Joint Space With
ligent Robots and Systems (IROS), Oct 2016, pp. 383–389.
Task Constraints,” IEEE Trans. on Robot., vol. 26, no. 3, Jun. 2010.
[31] T. Yoshikawa, “Manipulability of robotic mechanisms,” The Int. Journal
[10] K. Yunt and C. Glocker, “Trajectory optimization of mechanical hybrid
of Robot. Research (IJRR), vol. 4, no. 2, pp. 3–9, 1985.
systems using sumt,” in 9th IEEE International Workshop on Advanced
[32] A. Del Prete, S. Tonneau, and N. Mansard, “Fast Algorithms to Test
Motion Control, 2006., 2006, pp. 665–671.
Robust Static Equilibrium for Legged Robots,” in To appear in Proc. of
[11] I. Mordatch, E. Todorov, and Z. Popović, “Discovery of complex be- IEEE Int. Conf. on Robot. and Auto (ICRA), Stockholm, Sweden, 2016.
haviors through contact-invariant optimization,” ACM Trans. on Graph., [33] S. Boyd and L. Vandenberghe, Convex Optimization. Cambridge
vol. 31, no. 4, pp. 43:1–43:8, 2012. University Press, 2004.
[12] I. Mordatch, K. Lowrey, and E. Todorov, “Ensemble-CIO: Full-Body [34] A. Del Prete and N. Mansard, “Robustness to Joint-Torque Tracking
Dynamic Motion Planning that Transfers to Physical Humanoids,” in Errors in Task-Space Inverse Dynamics,” IEEE Transaction on Robotics,
Proc. of IEEE Int. Conf. on Robot. and Auto (ICRA), Seattle, USA, vol. 32, no. 5, pp. 1091 – 1105, 2016.
2015. [35] M. Moll, I. A. Sucan, and L. E. Kavraki, “An extensible bench-
[13] M. Gabiccini, A. Artoni, G. Pannocchia, and J. Gillis, “A computational marking infrastructure for motion planning algorithms,” arXiv preprint
framework for environment-aware robotic manipulation planning,” in arXiv:1412.6673, 2014.
Int. Symp. Robotics Research (ISRR), Sestri Levante, Italy, 2015. [36] Q. Pham, S. Caron, and Y. Nakamura, “Kinodynamic planning in the
[14] R. Deits and R. Tedrake, “Footstep planning on uneven terrain with configuration space via admissible velocity propagation,” in Robotics:
mixed-integer convex optimization,” in Humanoid Robots (Humanoids), Science and Systems IX (RSS), Berlin, Germany, 2013.
14th IEEE-RAS Int. Conf. on, Madrid, Spain, 2014. [37] N. Amenta, S. Choi, and R. K. Kolluri, “The power crust,” in Proc. of
[15] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planning Sixth ACM Symposium on Solid Modeling and Applications (SMA), Ann
with centroidal dynamics and full kinematics,” in Humanoid Robots Arbor, Michigan, USA, 2001, pp. 249–266.
(Humanoids), 14th IEEE-RAS Int. Conf. on, Madrid, Spain, 2014, pp. [38] S. Tonneau, J. Pettré, and F. Multon, “Using task efficient contact con-
295–302. figurations to animate creatures in arbitrary environments,” Computers
[16] M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory & Graphics, vol. 45, no. 0, 2014.
optimization of rigid bodies through contact,” The Int. Journal of Robot. [39] S. Chiu, “Control of redundant manipulators for task compatibility,” in
Research (IJRR), vol. 33, no. 1, pp. 69–81, Jan. 2014. Proc. of Int. Conf. on Robot. and Auto (ICRA), vol. 4, 1987, pp. 1718–
[17] A. Escande, A. Kheddar, S. Miossec, and S. Garsault, “Planning Support 1724.
Contact-Points for Acyclic Motions and Experiments on HRP-2,” in [40] Z. Qiu, A. Escande, A. Micaelli, and T. Robert, “Human motions
ISER, ser. Springer Tracts in Advanced Robot., O. Khatib, V. Kumar, analysis and simulation based on a general criterion of stability,” in
and G. J. Pappas, Eds., vol. 54. Springer, 2008, pp. 293–302. Int. Symposium on Digital Human Modeling, 2011.
[18] K. Bouyarmane, A. Escande, F. Lamiraux, and A. Kheddar, “Potential [41] S. Caron, Q.-C. Pham, and Y. Nakamura, “Leveraging Cone Double
field guide for humanoid multicontacts acyclic motion planning,” in Description for Multi-contact Stability of Humanoids with Applications
Proc. of IEEE Int. Conf. on Robot. and Auto (ICRA), Kobe, Japan, 2009, to Statics and Dynamics,” in Robotics, Science and Systems (RSS), 2015.
pp. 1165 – 1170. [42] K. Fukuda and A. Prodon, Double description method revisited. Berlin,
[19] S.-Y. Chung and O. Khatib, “Contact-consistent elastic strips for multi- Heidelberg: Springer Berlin Heidelberg, 1996, pp. 91–111.
contact locomotion planning of humanoid robots,” in Proc. of IEEE Int.
Conf. on Robot. and Auto (ICRA), May 2015, pp. 6289–6294.