In Proc. 2001 IEEE Int’l Conf. on Robotics and Automation (ICRA 2001) 1
Motion Planning for Humanoid Robots Under Obstacle
and Dynamic Balance Constraints
James Kuffner, Koichi Nishiwaki, Satoshi Kagami, Masayuki Inaba, Hirochika Inoue
Dept. of Mechano-Informatics
The University of Tokyo
{kuffner,nishi,kagami,inaba,inoue}@jsk.t.u-tokyo.ac.jp
Abstract
We present an approach to path planning for
humanoid robots that computes dynamically-stable,
collision-free trajectories from full-body posture goals.
Given a geometric model of the environment and a
statically-stable desired posture, we search the configu-
ration space of the robot for a collision-free path that si-
multaneously satisfies dynamic balance constraints. We
adapt existing randomized path planning techniques by
imposing balance constraints on incremental search mo-
tions in order to maintain the overall dynamic stability
of the final path. A dynamics filtering function that con-
strains the ZMP (zero moment point) trajectory is used
as a post-processing step to transform statically-stable,
collision-free paths into dynamically-stable, collision-free
trajectories for the entire body. Although we have fo-
cused our experiments on biped robots with a humanoid
shape, the method generally applies to any robot subject
to balance constraints (legged or not). The algorithm is
presented along with computed examples using the hu-
manoid robot “H6”.
1 Introduction
Recently, significant progress has been made in the de-
sign and control of humanoid robots, particularly in the
realization of dynamic walking in several full-body hu-
manoids [11, 37, 27]. As the technology and algorithms
for real-time 3D vision and tactile sensing improve, hu-
manoid robots will be able to perform tasks that involve
complex interactions with the environment (e.g. grasp-
ing and manipulating objects). The enabling software for
such tasks includes motion planning for obstacle avoid-
ance, and integrating planning with visual and tactile
sensing data. To facilitate the deployment of such soft-
ware, we are currently developing a graphical simulation
environment for testing and debugging [19].
This paper presents an algorithm for automati-
cally generating collision-free dynamically-stable mo-
tions from full-body posture goals. It expands upon the
preliminary algorithm developed in [21], and has been
tested and verified on the humanoid robot hardware plat-
form “H6”. Our approach is to adapt techniques from
Figure 1: Dynamically-stable motion for retrieving an
object (top: simulation, bottom: real robot hardware).
an existing, successful path planner [20] by imposing bal-
ance constraints upon incremental motions used during
the search. Provided the initial and goal configurations
correspond to collision-free, statically-stable body pos-
tures, the path returned by the planner can be trans-
formed into a collision-free and dynamically-stable tra-
jectory for the entire body. To the best of our knowledge,
this paper represents the first general motion planning
algorithm for humanoid robots that has also been exper-
imentally confirmed on real humanoid robot hardware.
2 Background
Due to the complexity of motion planning in its gen-
eral form [32], the use of complete algorithms [5, 12, 33]
is limited to low-dimensional configuration spaces. This
has motivated the use of heuristic algorithms, many of
which employ randomization (e.g., [1, 2, 3, 4, 14, 15, 17,
20, 26]). Although these methods are incomplete, many
have been shown to find paths in high-dimensional con-
figuration spaces with high probability.
Motion planning for humanoid robots poses a par-
ticular challenge. Developing practical motion planning
algorithms for humanoid robots is a daunting task given
that humanoid robots typically have 30 or more degrees
of freedom. The problem is further complicated by the
fact that humanoid robots must be controlled very care-
fully in order to maintain overall static and dynamic
stability. These constraints severely restrict the set of
allowable configurations and prohibit the direct applica-
tion of existing motion planning techniques. Although
efficient methods have been developed for maintaining
dynamic balance for biped robots [36, 31, 30, 16], none
consider obstacle avoidance.
Motion planning algorithms that account for system
dynamics typically approach the problem in one of two
ways: 1) decoupling the problem by first computing a
kinematic path, and subsequently transforming the path
into a dynamic trajectory, or 2) searching the system
state-space directly by reasoning about the possible con-
trols that can be applied. The method presented in this
paper adopts the first approach. Other methods using
one of these two planning strategies have been devel-
oped for off-road vehicles[35, 6], free-flying 2D and 3D
rigid bodies[23, 24], helicopters and satellites[8], and for
a free-flying disc among moving obstacles[18]. None of
these previous methods have yet been applied to com-
plex articulated models such as humanoid robots. One
notable exception is the VHRP simulation software un-
der development[28], which contains a path planner that
limits the active body degrees of freedom for humanoid
robots for simultaneous obstacle avoidance and balance
control. Since the space of possible computed motions is
limited however, this planner is not fully general.
3 Dynamically-stable Motion Planning
Our approach is to adapt a variation of the random-
ized planner described in [20] to compute full-body mo-
tions for humanoid robots that are both dynamically-
stable and collision-free. The first phase computes
a statically-stable, collision-free path, and the sec-
ond phase smooths and transforms this path into a
dynamically-stable trajectory for the entire body. The
planning method (RRT-Connect) and its variants uti-
lize Rapidly-exploring Random Trees (RRTs) [22, 24] to
connect two search trees, one from the initial configura-
tion and the other from the goal. This method has been
shown to be efficient in practice and converge towards a
uniform exploration of the search space.
Robot Model and Assumptions: We have based
our experiments on an approximate model of the H6
humanoid robot (see Figure 1), including the kinemat-
ics and dynamic properties of the links. Although we
have focused our experiments on biped robots with a
humanoid shape, the algorithm generally applies to any
robot subject to balance constraints (legged or not).
Aside from the existence of the dynamic model, we make
the following assumptions:
1. Environment model: We assume that the robot
has access to a 3D model of the surrounding envi-
ronment to be used for collision checking.
2. Initial posture: The robot is currently balanced in
a collision-free, statically-stable configuration sup-
ported by either one or both feet.
3. Goal posture: A full-body goal configuration that
is both collision-free and statically-stable is speci-
fied. The goal posture may be given explicitly by a
human operator, or computed via inverse kinemat-
ics or other means.
4. Support base: The location of the supporting foot
(or feet in the case of dual-leg support) does not
change during the planned motion.
3.1 Problem Formulation:
Our problem will be defined in a 3D world W in which
the robot moves. W is modeled as the Euclidean space
<
3
(< is the set of real numbers).
Robot: Let the robot A be a finite collection of p rigid
links L
i
(i = 1, . . . , p) organized in a kinematic hierar-
chy with Cartesian frames F
i
attached to each link. We
denote the position of the center of mass c
i
of link L
i
relative to F
i
. A pose of the robot is denoted by the set
P = {T
1
, T
2
, . . . , T
p
}, of p relative transformations for
each of the links L
i
as defined by the frame F
i
relative
to its parent link’s frame. The base or root link trans-
formation T
1
is defined relative to some world Cartesian
frame F
world
. Let n denote the number of generalized
coordinates or degrees of freedom (DOFs) of A. A con-
figuration is denoted by q ∈ C, a vector of n real numbers
specifying values for each of the generalized coordinates
of A. Let C be the configuration space or C-space of A.
C is a space of dimension n.
Obstacles: The set of obstacles in the environment
W is denoted by B, where B
k
(k = 1, 2, . . .) represents
an individual obstacle. We define the C-obstacle region
CB ⊂ C as the set of all configurations q ∈ C where one
or more of the links of A intersect (are in collision) with
another link of A, any of the obstacles B
k
. We also re-
gard configurations q ∈ C where one or more joint limits
are violated as part of the C-obstacle region CB. The
open subset C \ CB is denoted by C
free
and its closure
by cl(C
free
), and it represents the space of collision-free
configurations in C of the robot A.
Balance and Torque Constraints: Let X (q) be a
vector relative to F
world
representing the global position
of the center of mass of A while in the configuration q.
A configuration q is statically-stable if: 1) the projection
of X (q) along the gravity vector g lies within the area of
support SP (i.e. the convex hull of all points of contact
2
between A and the support surface in W), and 2) the
joint torques Γ needed to counteract the gravity-induced
torques G(q) do not exceed the maximum torque bounds
Γ
max
. Let C
stable
⊂ C be the subset of statically-stable
configurations of A. Let C
valid
= C
stable
∩ C
free
denote
the subset of configurations that are both collision-free
and statically-stable postures of the robot A. C
valid
is
called the set of valid configurations.
Solution Trajectory: Let τ : I 7→ C where I is an
interval [t
0
, t
1
], denote a motion trajectory or path for
A expressed as a function of time. τ (t) represents the
configuration q of A at time t, where t ∈ I. A trajectory
τ is said to be collision-free if τ(t) ∈ C
free
for all t ∈
I. A trajectory τ is said to be both collision-free and
statically-stable if τ(t) ∈ C
valid
for all t ∈ I. Given
q
init
∈ C
valid
and q
init
∈ C
valid
, we wish to compute a
continuous motion trajectory τ such that ∀t ∈ [t
0
, t
1
],
τ(t) ∈ C
valid
, and τ (t
0
) = q
init
and τ(t
1
) = q
goal
. We
refer to such a trajectory as a statically-stable trajectory.
Dynamic Stability: Theoretically, any statically-
stable trajectory can be transformed into a dynamically-
stable trajectory by arbitrarily slowing down the motion.
For these experiments, we utilize the online balance com-
pensation scheme described in [16] as a method of gen-
erating a final dynamically-stable trajectory after path
smoothing (see Section 3.4).
Planning Query: Note that in general, if a
dynamically-stable solution trajectory exists for a given
path planning query, there will be many such solution
trajectories. Let Φ denote the set of all dynamically-
stable solution trajectories for a given problem. The
query P lanner(A, B, q
init
, q
goal
) −→ τ , accepts as in-
put the robot and obstacle models along with the initial
and goal postures, and attempts to calculate a solution
trajectory τ ∈ Φ. If no solution is found, τ will be empty
(a null trajectory).
3.2 Path Search
Unfortunately, there are no currently known methods
for explicitly representing C
valid
. The obstacles are mod-
eled completely in W, thus an explicit representation of
C
free
is also not available. However, using a collision
detection algorithm, a given q ∈ C can be tested to de-
termine whether q ∈ C
free
. Testing whether q ∈ C
stable
can also be checked verifying that the projection of X (q)
along g is contained within the boundary of SP , and that
the torques Γ needed to counteract gravitational torques
G(q) do not exceed Γ
max
.
Distance Metric: As with the most planning algo-
rithms in high-dimensions, a metric ρ is defined on C.
The function ρ(q, r) returns some measure of the dis-
tance between the pair of configurations q and r. Some
axes in C are weighted relative to each other, but the
general idea is to measure the “closeness” of pairs of
configurations with a positive scalar function.
For our humanoid robot models, we employ a metric
that assigns higher relative weights to the generalized co-
ordinates of links with greater mass and proximity to the
trunk (torso): ρ(q, r) =
P
n
i=1
w
i
||q
i
− r
i
||. This choice of
metric function attempts to heuristically encode a gen-
eral relative measure of how much the variation of an
individual joint parameter affects the overall body pos-
ture. Additional experimentation is needed in order to
evaluate the efficacy of the many different metric func-
tions possible.
Planning Algorithm: We employ a randomized
search strategy based on Rapidly-exploring Random
Trees (RRTs) [23, 20]. For implementation details and
analysis of RRTs, the reader is referred to the original
papers or a summary in [24]. In [21], we developed an
RRT variant the generates search trees using a dynamics
filter function to guarantee dynamically-stable trajecto-
ries along each incremental search motion.
The algorithm described in this paper is more general
and efficient than the planner presented in [21], since it
does not require the use of a dynamics filtering func-
tion during the path search phase. In addition, it can
handle either single or dual-leg support postures, and
the calculated trajectories have been verified using real
robot hardware. The basic idea is the same as the RRT-
Connect algorithm described in [20]. The key difference
is that instead of searching C for a solution path that
lies within C
free
, the search is performed in C
stable
for
a solution path that lies within C
valid
. In particular, we
modify the planner variant that employs symmetric calls
to the EXTEND function as follows:
1. The NEW
CONFIG function in the EXTEND opera-
tion checks balance constraints in addition to check-
ing for collisions with obstacles (i.e. q
new
∈ C
valid
).
2. Rather than picking a purely random configuration
q
rand
∈ C at every planning iteration, we pick a
random configuration that also happens to corre-
spond to a statically-stable posture of the robot (i.e.
q
rand
∈ C
stable
).
Pseudocode for the complete algorithm is given in Fig-
ure 2. The main planning loop involves performing a
simple iteration in which each step attempts to extend
the RRT by adding a new vertex that is biased by a
randomly-generated, statically-stable configuration (see
Section 3.3). EXTEND selects the nearest vertex already
in the RRT to the given configuration, q, with respect
to the distance metric ρ. Three situations can occur:
Reached, in which q is directly added to the RRT, Ad-
vanced, in which a new vertex q
new
6= q is added to the
RRT; Trapped, in which no new vertex is added due to
the inability of NEW CONFIG to generate a path seg-
ment towards q that lies within C
valid
.
NEW CONFIG attempts to make an incremental mo-
tion toward q. Specifically, it checks for the existence of
a short path segment δ = (q
near
, q
new
) that lies entirely
within C
valid
. If ρ(q, q
near
) < , where is some fixed
incremental distance, then q itself is used as the new
configuration q
new
at the end of the candidate path seg-
ment δ (i.e. q
new
= q). Otherwise, q
new
is generated at
3
EXTEND(T , q)
1 q
near
← NEAREST NEIGHBOR(q, T );
2 if NEW CONFIG(q, q
near
, q
new
) then
3 T .add vertex(q
new
);
4 T .add edge(q
near
, q
new
);
5 if q
new
= q then Return Reached;
6 else Return Advanced;
7 Return Trapped;
RRT CONNECT STABLE(q
init
, q
goal
)
1 T
a
.init(q
init
); T
b
.init(q
goal
);
2 for k = 1 to K do
3 q
rand
← RANDOM STABLE CONFIG();
4 if not (EXTEND(T
a
, q
rand
) =Trapped) then
5 if (EXTEND(T
b
, q
new
) =Reached) then
6 Return PATH(T
a
, T
b
);
7 SWAP(T
a
, T
b
);
8 Return Failure
Figure 2: Pseudocode for the dynamically-stable motion
planning algorithm.
a distance along the straight-line from q
near
to q.
1
All
configurations q
0
along the path segment δ are checked
for collision, and tested whether balance constraints are
satisfied. Specifically, if ∀q
0
∈ δ(q
near
, q
new
), q
0
∈ C
valid
,
then NEW
CONFIG succeeds, and q
new
is added to the
tree T . In this way, the planner uses uniform samples
of C
stable
in order to grow trees that lie entirely within
C
valid
.
Convergence and Completeness: Although not
given here, arguments similar to those presented in [20]
and [24] can be constructed to show uniform coverage
and convergence over C
valid
.
Ideally, we would like to build a complete planning al-
gorithm. That is, the planner always returns a solution
trajectory if one exists, and indicates failure if no solu-
tion exists. As mentioned in Section 2, implementing a
practical complete planner is a daunting task for even
low-dimensional configuration spaces (see [13]). Thus,
we typically trade off completeness for practical perfor-
mance by adopting heuristics (e.g. randomization).
The planning algorithm implemented here is incom-
plete in that it returns failure after a preset time limit is
exceeded. Thus, if the planner returns failure, we cannot
conclude whether or not a solution exists for the given
planning query, only that our planner was unable to find
one in the allotted time. Uniform coverage and conver-
gence proofs, though only theoretical, at least help to
provide some measure of confidence that when an algo-
rithm fails to find a solution, it is likely that no solution
1
A slight modification must be made for the case of dual-leg
support. In this case, when interpolating two stable configurations,
inverse kinematics for the leg is used to force the relative position
between the feet to remain fixed. The same s technique is also
used for generating random statically-stable dual-leg postures (see
Section 3.3).
exists. This is an area of ongoing research.
3.3 Random Statically-stable Postures
For our algorithm to work, we require a method of
generating random statically-stable postures (i.e. ran-
dom point samples of C
stable
). Although it is trivial to
generate random configurations in C, it is not so easy
to generate them in C
stable
, since it encompasses a much
smaller subset of the configuration space.
In our current implementation, a set Q
stable
⊂ C
stable
of N samples of C
stable
is generated as a preprocess-
ing step. This computation is specific to a particular
robot and support-leg configuration, and need only be
performed once. Different collections of stable postures
are saved to files and can be loaded into memory when
the planner is initialized. Although stable configura-
tions could be generated “on-the-fly” at the same time
the planner performs the search, pre-calculating Q
stable
is preferred for efficiency. In addition, multiple stable-
configuration set files for a particular support-leg config-
uration can be saved independently. If the planner fails
to find a path after all N samples have been removed
from the currently active Q
stable
set, a new one can be
loaded with different samples.
Single-leg Support Configurations: For configura-
tions that involve balancing on only one leg, the set
Q
stable
can be populated as follows:
1. The configuration space of the robot C is sampled by
generating a random body configuration q
rand
∈ C.
2. Assuming the right leg is the supporting foot, q
rand
is tested for membership in C
valid
(i.e. static stabil-
ity, no self-collision, and joint torques below limits).
3. Using the same sample q
rand
, a similar test is per-
formed assuming the left leg is the supporting foot.
4. Since most humanoid robots have left-right symme-
try, if q
rand
∈ C
valid
in either or both cases, we can
“mirror” q
rand
to generate stable postures for the
opposite foot.
Dual-leg Support Configurations: It is slightly
more complicated to generate statically-stable body con-
figurations supported by both feet at a given fixed rela-
tive position. In this case, populating Q
stable
is very sim-
ilar to the problem of sampling the configuration space
of a constrained closed-chain system (e.g. closed-chain
manipulator robots or molecular conformations [25, 10]).
The set Q
stable
is populated with fixed-position dual-leg
support postures as follows:
1. As in the single-leg case, the configuration space of
the robot C is sampled by generating a random body
configuration q
rand
∈ C.
4
2. Holding the right leg fixed at its random configura-
tion, inverse kinematics is used to attempt to po-
sition the left foot at the required relative position
to generate the body configuration q
right
. If it suc-
ceeds, then q
right
is tested for membership in C
valid
.
3. An identical procedure is performed to generate
q
lef t
by holding the left leg fixed at its random con-
figuration derived from q
rand
, using inverse kinemat-
ics to position the right leg, and testing for mem-
bership in C
valid
.
4. If either q
right
∈ C
valid
or q
lef t
∈ C
valid
, and the
robot has left-right symmetry, additional stable pos-
tures can be derived by mirroring the generated sta-
ble configurations.
3.4 Trajectory Generation
If successful, the path search phase returns a continu-
ous sequence of collision-free, statically-stable body con-
figurations. All that remains is to calculate a final solu-
tion trajectory τ that is dynamically-stable and collision-
free. Theoretically, any given statically-stable trajectory
can be transformed into a dynamically-stable trajectory
by arbitrarily slowing down the motion. However, we can
almost invariably obtain a smoother and shorter trajec-
tory by performing the following two steps:
Smoothing: We smooth the raw path by making sev-
eral passes along its length, attempting to replace por-
tions of the path between selected pairs of configurations
by straight-line segments in C
valid
.
2
This step typically
eliminates any potentially unnatural postures along the
random path (e.g. unnecessarily large arm motions).
The resulting smoothed path is transformed into an in-
put trajectory using a minimum-jerk model [7].
Filtering: A dynamics filtering function is used in or-
der to output a final, dynamically-stable trajectory. We
use the online balance compensation scheme described in
[16], which enforces constraints upon the zero moment
point (ZMP) trajectory in order to maintain overall dy-
namic stability. The output configuration of the filter is
guaranteed to lie in C
stable
. Collision-checking is used to
verify that the final output trajectory lies in C
valid
, with
the motion made slower in the case of collision.
Although this method has generated satisfactory results
in our experiments, it is by no means the only option.
Other ways of generating dynamically-stable trajectories
from a given input motion are also potentially possible
to apply here (e.g. [37, 29]). It is also possible to em-
ploy variational techniques, or apply algorithms for com-
puting time-optimal trajectories [34]. Calculating the
globally-optimal trajectory according to some cost func-
tional based on the obstacles and the dynamic model is
an open problem, and an area of ongoing research.
2
When interpolating dual-leg configurations, inverse kinematics
is used to keep the relative position of the feet fixed.
Task Description Computation Time (seconds)
min max avg stdev
Reach under chair 171 598 324 138
Lift leg over box 26 103 48 21
Reach over table 194 652 371 146
Table 1: Performance statistics (N = 25 trials).
4 Experiments
This section presents some preliminary experiments
performed on a 270 MHz SGI O2 (R12000) worksta-
tion. We have implemented a prototype planner in C++
that runs within a graphical simulation environment [19].
An operator can position individual joints or use inverse
kinematics to specify body postures for the virtual robot.
The filter function can be run interactively to ensure
that the goal configuration is statically-stable. After
specifying the goal, the planner is invoked to attempt
to compute a dynamically-stable trajectory connecting
the goal configuration to the robot’s initial configuration
(assumed to be a collision-free, stable posture).
We have tested the output trajectories calculated by
the planner on an actual humanoid robot hardware plat-
form. The “H6” humanoid robot (33-DOF) is 137cm tall
and weighs 51kg (including 4kg of batteries). Figure 1
shows a computed dynamically-stable motion for the H6
robot moving from a neutral standing position to a low
crouching position in order to retrieve an object from be-
neath a chair. Figure 3 shows a different view of the real
robot executing the same motion. Figure 4 shows a mo-
tion for positioning the right leg above the top of a box
while balancing on the left leg. The motion in Figure 5
was not executed on the real robot, but is interesting in
that it involves reaching for an object placed on top of a
cabinet while avoiding both the cabinet and the shelves
behind the robot. The robot is required to balance on
one leg in order to extend the arm far enough to reach
the obstacle on the table.
Each of the scenes contains over 9,000 triangle primi-
tives. The 3D collision checking software used for these
experiments was the RAPID library based on OBB-Trees
developed by the University of North Carolina[9]. The
total wall time elapsed in solving these queries ranges
from under 30 seconds to approximately 11 minutes. A
summary of the computation times for repeated runs of
25 trials each is shown in Table 1.
5 Discussion
This paper presents an algorithm for computing
dynamically-stable collision-free trajectories given full-
body posture goals. Although we have focused our ex-
periments on biped robots with a humanoid shape, the
algorithm is general and can be applied to any robot
subject to balance constraints (legged or not). There are
many potential uses for such software, with the primary
one being a high-level control interface for automatically
computing motions to solve complex tasks for humanoid
5
