IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 2 ...IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 2, APRIL 2012 427 Fast Humanoid Robot Collision-Free Footstep Planning Using Swept Volume

IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 2, APRIL 2012 427

Fast Humanoid Robot Collision-Free FootstepPlanning Using Swept Volume Approximations

Nicolas Perrin, Olivier Stasse, Leo Baudouin, Florent Lamiraux, and Eiichi Yoshida

Abstract—In this paper, we propose a novel and coherent frame-work for fast footstep planning for legged robots on a flat groundwith 3-D obstacle avoidance. We use swept volume approxima-tions that are computed offline in order to considerably reducethe time spent in collision checking during the online planningphase, in which a rapidly exploring random tree variant is used tofind collision-free sequences of half-steps (which are produced by aspecific walking pattern generator). Then, an original homotopy isused to smooth the sequences into natural motions, gently avoidingthe obstacles. The results are experimentally validated on the robotHRP-2.

Index Terms—Footstep generation, humanoid robots, motionplanning, obstacle avoidance.

I. INTRODUCTION

ARGUABLY, the one thing that most differentiates hu-manoid robots from their wheeled counterparts is their

intrinsic ability to step over obstacles on the ground. For thisreason, a lot of work has been done on the problem of humanoidrobot walk planning, with the aim of best exploiting this uniquecapability. Since humanoid robots combine high dimensional-ity with underactuation, two properties that tend to drasticallyincrease the complexity of motion planning, this problem is noteasy to solve. Nevertheless, and although there is no completelysatisfying solution so far, a lot of promising techniques and toolshave been introduced over the past decade.

Probably, the most successful approaches are based on theuse of the A* algorithm with a finite transition model, i.e., a rel-atively small set of possible steps (see, e.g., [6], [7], and [23]).For each step, a corresponding configuration space trajectory isknown, and it is possible to check quite quickly whether a given

Manuscript received December 12, 2010; revised July 26, 2011; acceptedOctober 10, 2011. Date of publication November 11, 2011; date of current ver-sion April 9, 2012. This paper was recommended for publication by AssociateEditor T. Asfour and Editor G. Oriolo upon evaluation of the reviewers’ com-ments. This work was supported by a grant from the RBLINK Project underContract ANR-08-JCJC-0075-01.

N. Perrin is with the CNRS/LAAS, Universite de Toulouse UPS, INSA,INP, ISAE, F-31077 Toulouse, France, and also with CNRS-AIST JointRobotics Laboratory, UMI3218/CRT, 305-8568 Tsukuba, Japan (e-mail:[email protected]).

O. Stasse is with CNRS/LAAS, Universite de Toulouse UPS, INSA, INP,ISAE, F-31077 Toulouse, France (e-mail: [email protected]).

L. Baudouin and F. Lamiraux are with CNRS/LAAS, Universite deToulouse UPS, INSA, INP, ISAE, F-31077 Toulouse, France (e-mail:[email protected]; [email protected]).

E. Yoshida is with the CNRS-AIST Joint Robotics Laboratory,UMI3218/CRT, 305-8568 Tsukuba, Japan (e-mail: [email protected]).

Color versions of one or more of the figures in this paper are available onlineat http://ieeexplore.ieee.org.

Digital Object Identifier 10.1109/TRO.2011.2172152

step will avoid the obstacles or not. Since those steps need tobe connectable at will, however, it often requires the initial andfinal speed of the robot bodies to be zero for all the steps ofthe transition model. At least some parts of the gaits producedare, thus, static. This is, for example, the case in [1] and [23].Chestnutt et al. avoid it in [7] by using a search space which con-sists in sequences of two consecutive steps. However, since thissearch space has a higher dimensionality, in order to be expres-sive, transition models need to be much larger than when onlyisolated steps are considered. Yet, the use of the A* algorithmstrongly constrains the size of the transition model. Even whenthe transitions are isolated steps, the stepping capabilities areoften limited because the complexity of the A* search quicklyincreases with the size of the transition model. Recently, though,some interesting refinements have been considered in order toenhance the stepping capabilities while keeping a small transi-tion model. In [9], for example, the steps of a set of referenceactions (i.e., the transition model) can be slightly adjusted toavoid bad terrain locations.

In this paper, we replace the A* search by a sampling-basedalgorithm in order to directly deal with a large transition modeland add several other improvements to the standard {A* + finitetransition model} approach. Here are our main contributions.

1) Thanks to a walking pattern generator that is specificallydesigned, we obtain a low-dimensional search space whichcan be densely covered by relatively few points. With anautomatically generated finite transition model of about300 points in this search space, we are able to obtainvery expressive stepping capabilities. To deal with sucha large transition model, we use, instead of the classicalA* search, a specific rapidly exploring random tree (RRT)algorithm.

2) Each point in the transition model corresponds to a con-figuration space trajectory of the robot. Through extensiveoffline computations, for each of them, we approximatethe volume swept in the workspace by a part of the robotlower body (from the knees down) during the executionof the trajectory and store it in an efficient data structure.It helps to drastically reduce the time consumed by theonline planning phase when checking for collisions withthe environment.

3) Finally, with a simple homotopy, we quickly smooth andaccelerate the trajectories that are obtained after the plan-ning phase, and as a result, the final motions producedare fully dynamic: a feature that is often lacking with cur-rent approaches. On top of that, there is no incoherencebetween the planning phase and the smoothing phase;therefore, we have the guarantee that if the planner returns

1552-3098/$26.00 © 2011 IEEE

428 IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 2, APRIL 2012

a collision-free solution, then the robot will execute a se-quence which will also be collision-free (this guaranteeis up to some details—discrepancies between simulationand real world, errors of approximation, errors due to dis-cretization, etc.).

1) Pattern generation and smoothing homotopy: One of thekey elements of our framework is the combination between aspecific walking pattern generator based on “half-steps” and asimple homotopy that can quickly smooth sequences of (half-)steps. We present both in Section II (we introduced them in [32]).Before the use of the homotopy, the generated sequences arecalled “raw” and simply correspond to concatenations of iso-lated half-steps. Isolated half-steps are obtained by fixing theposition of the swing foot when it is at its maximum height: Thisputs us in the conditions of [23] where two “via point configu-rations” Qright and Qleft (corresponding to balanced postures)are fixed and divide steps into two parts: an upward half-stepand a downward half-step. In [23], this restriction was used inorder to reduce the number of trajectories to consider; we useit in order to reduce the dimensionality of the input space. Thesimple homotopy that we use to smooth sequences of half-stepsis, to our knowledge, new in the field of humanoid robotics (butit is based on the same principle as the techniques introducedin [28] and [29]).

2) Finite transition model and swept volume approximations:Our pattern generator benefits from an input space of dimen-sion only 3, and therefore, we can cover it with a dense gridof only relatively few points. Each point corresponds to a se-quence of two half-steps. For each point of the grid, we firstsimulate the sequence of half-steps and check that it is feasible,i.e., it contains no self-collision and does not violate the jointslimits. The points which correspond to feasible trajectories willbe the elements of our transition model. In Section III, we ex-plain the construction of this transition model and show how, foreach of its elements, we approximate the volume swept by therobot lower body during the execution of the corresponding tra-jectory. By speeding up collision checks, these approximationswill enable us to save a considerable computation time online.

At first, it might seem strange to combine precomputed sweptvolumes and a smoothing homotopy that modifies trajectories,but in fact, in the whole process, the homotopy is only applied toone feasible trajectory returned by the planning process duringwhich the swept volume approximations are extensively used.When the homotopy is applied, we do not use precomputedswept volumes for the collision checks.

Several efficient swept volume approximation algorithms ex-ist, such as, for example, the ones introduced in [17] and [22].Using such advanced specific algorithms will be part of our fu-ture work, but in this paper, we validate our framework witha simpler approach. Since the highest priority is the evaluationtime (because approximations are used multiple times at eachiteration of the RRT algorithm), we use a generic approximationalgorithm that stores the results in compact tree structures that,in our case, can be used to very quickly check for collisions withobstacles of the environment. This algorithm is a slight variantof the one introduced in [31]; the variant is presented in detailin [30]. The use of swept volumes is widespread in robotics,

especially for path planning (see [15] and [34]), but relativelyabsent in the field of humanoid robotics, where, for the sake ofcomputational efficiency, simpler bounding volumes are oftenpreferred (see [10] and [39]).

3) Rapidly exploring random tree variant for footstepplanning: The last part of our framework is the planning phase.Since we have a large transition model, the traditional A* searchwould perform poorly. Alternatives to A* have already beenproposed. For example, in [13], Harada uses a probabilisticroadmap method (PRM, see [21]) approach to plan footsteps: atree of “milestone configurations” is grown from an initial con-figuration to a goal configuration. At first, collisions are checkedonly at milestone configurations, and only once a candidate pathhas been found is the full trajectory verified. An issue of thisapproach is that even though the milestones are collision-free,collisions might occur in the candidate path. Thus, the processmight have to be restarted several times, leading to lengthycomputations.

The idea of using an RRT algorithm [26] for footstep planningwas introduced in [38], where a single-node-extending and amultinode-extending RRT methods are proposed. In Section IV,we follow the single-node-extending method and present a newvariant of the RRT algorithm for footstep planning, where wedeal separately with the sets of left and right footsteps. When anew transition (i.e., a new footstep) is considered by the RRT al-gorithm, we test the corresponding approximated swept volumeagainst all the points of the objects that are close enough (wesuppose that the environment is known and obstacles are repre-sented by point clouds: Each object is contained in a boundingbox, and a finite set of points is covering the object exteriorsurface). If one of the points lies inside the swept volume, thetransition is discarded. Using point clouds for collision detectionis certainly neither the safest nor the most efficient approach,but we believe that it illustrates well the performance of ourframework: Indeed, it is important to show that we are able torapidly plan motions even if during each iteration of the RRT al-gorithm, the number of collision queries is high, because in realapplications, unknown obstacles are often acquired as untreatedsets of voxels or large triangle soups or meshes. Preliminaryexperiments are presented in Section V, where the robot HRP-2quickly solves complicated footstep planning problems in envi-ronments cluttered with 3-D obstacles.

In Section VI, we improve our implementation by usingmeshes to represent the swept volume approximations and thePQP algorithm [24] for collision checks. This yields a furtherspeedup that enables us to perform some experiments of real-time replanning.

Section VII contains a brief discussion on an extension of ourframework to a continuous transition model, and Section VIIIconcludes this paper.

II. WALKING PATTERN GENERATOR BASED ON HALF-STEPS

AND A SMOOTHING HOMOTOPY

We use a classical simplified model of the robot dynamics:the linear inverted pendulum model (see [19]). In this model, themass of the robot is assumed to be concentrated in its center of

PERRIN et al.: FAST HUMANOID ROBOT COLLISION-FREE FOOTSTEP PLANNING USING SWEPT VOLUME APPROXIMATIONS 429

mass (CoM), which is supposed to be rigidly linked to and abovethe robot waist at all time. Besides, the robot is supposed to haveonly point contacts with the walking surface. The contact pointsare coplanar on a horizontal plane. Thus, it behaves like aninverted pendulum, and an analysis of the subsequent equationsleads to a further approximation which enables the decouplingof the dynamic differential equations for the x-axis and y-axis.They can be written as follows:

px = Z(x) (1)

py = Z(y) (2)

with Z�= Id − zc

g

d

dt2. (3)

(x, y) are the (x-axis,y-axis) coordinates of the CoM of the robot,and zc is the height of the robot CoM which is supposed constantduring the step. Let us notice that Z is a linear operator actingon functions of time. (px, py ) are the (x-axis,y-axis) coordinatesof the virtual zero-moment point (ZMP). A classical balancecriterion for biped walking is that the ZMP should stay at alltime inside the polygon of support (see [37]).

In [14], Harada et al. show how analytical trajectories for boththe CoM and the ZMP can be derived from these equations. TheZMP trajectory is a polynomial of the time variable t, and the

CoM trajectory

(x(t)y(t)

)has the general following form:

cosh

(√g

zc· t

)(Vx

Vy

)+sinh

(√g

zc· t

)(Wx

Wy

)+

(rx(t)ry (t)

)

(4)where rx(t) and ry (t) are polynomials entirely determined bypx(t) and py (t), respectively.

From this equation, we see that for a given ZMP profile, thereare just enough free parameters (Vx, Vy ,Wx, and Wy ) to set theinitial horizontal position and speed of the CoM:

(x(0)y(0)

)=

(Vx + rx(0)Vy + ry (0)

)(5)

(x(0)y(0)

)=

⎛⎝

√gzc

· Wx + rx(0)√gzc

· Wy + ry (0)

⎞⎠ . (6)

Using these equations, next, we show how to produce theconfiguration space (C-space) trajectory corresponding to anisolated half-step. We just need to obtain a unique C-space tra-jectory from a small number of half-step parameters (as we willsee; in our case, it will be three parameters). If each of the robotlegs has six degrees of freedom or more (the redundancy canbe treated using generalized inverse kinematics; see [27]), thisproblem can be reduced to the generation of trajectories forthe waist and the feet. Besides the compulsory constant waistheight, we also made a few arbitrary and convenient restrictions(which reduce the number of parameters): The pitch and rollparameters of the waist orientation will stay at zero, and sim-ilarly, the swing foot will always stay parallel to the walkingsurface. Thus, the lower body trajectory is entirely defined byseven functions of the time:

1) the waist horizontal position: x(t), y(t) (we recall that thewaist and CoM are rigidly fixed);

2) the waist orientation: θ(t);3) the swing foot position: SFx(t), SFy (t), SFz (t);4) the swing foot orientation: SFθ (t).

A. Producing Isolated Half-Steps

In this section, we only consider upward half-steps, but themethod for the generation of downward half-steps trajectoriesis similar.

Therefore, let us consider an upward half-step. In order toreduce the dimensionality of the parameter space, we makeseveral assumptions. First, we fix and denote by T the durationof any half-step. Then, we assume that the initial and final speedof the ZMP and swing foot are 0, but we do not assume that theCoM initial and final speed are zero

px(0) = py (0) = px(T ) = py (T ) = 0 (7)

θ(0) = θ(T ) = 0 (8)

˙SFx(0) = ˙SFy (0) = ˙SFz (0) = ˙SFθ (0) = 0 (9)

˙SFx(T ) = ˙SFy (T ) = ˙SFz (T ) = ˙SFθ (T ) = 0. (10)

Second, the initial vertical projection on the ground of the CoMis equal to the ZMP initial position, i.e., at the barycenter of thefoot centers. Taking the center of the support foot as the originof the Euclidean space, it gives us that

x(0) = px(0) =SFx(0)

2(11)

y(0) = py (0) =SFy (0)

2. (12)

We also assume that the final horizontal position of the CoM andZMP coincide at the center of the support foot and that the finalswing foot orientation and the initial and final orientation of thewaist are equal to the support foot orientation (at this stage, theorientation of the waist changes only during downward half-steps). Besides, the line passing through the centers of the finalpositions of the feet is orthogonal to this orientation

x(T ) = px(T ) = 0 (13)

y(T ) = py (T ) = 0 (14)

θ(0) = θ(T ) = SFθ (T ) = 0 (15)

SFx(T ) = 0. (16)

As a consequence of these equations, the final and initialconfigurations are entirely determined by five parameters (asshown in Fig. 1)

SFx(0), SFy (0), SFθ (0), SFy (T ), and SFz (T ).

Besides concerning the derivatives at the boundaries, the onlyfree parameters are x(0), x(T ), y(0), and y(T ). This adds up toa total of nine free parameters.

Now, we show how the ZMP trajectory is defined. An upwardhalf-step is divided into three phases: During the first one, of du-ration t1 , the ZMP stays at the barycenter of the feet (and the feet


Fig. 1. Here, we show an upward half-step from above. It is fully determinedby the five parameters SFx (0), SFy (0), SFθ (0), SFy (T ), and SFz (T ). Adownward half-step is also fully determined by five parameters.

keep their positions as well); therefore, we have px(t) = SFx (0)2 ,

py (t) = SFy (0)2 , and thus, px(t) = py (t) = 0. Then, there is the

“shift” phase, during which the ZMP quickly shifts from itsinitial position to its final position, reached at time t2 . Then,from t2 to T , the ZMP stays at its final position; therefore,we have px(t) = py (t) = px(t) = py (t) = 0. During the “shift”phase, we set px and py as third-degree polynomials deter-

mined by the respective boundary conditions px(t1) = SFx (0)2 ,

px(t2) = px(t1) = px(t2) = 0, and py (t1) = SFy (0)2 , py (t2) =

py (t1) = py (t2) = 0. For the downward half-step, even if thephase of double support and single support are inverted, we keepthe same durations: The ZMP shift occurs between time t1 andt2 . In practice, we set t1 = T − t2 .

Thanks to (4), if we fix SFx(0), SFy (0), x(0), and y(0), wecan get an analytical expression of the unique C2 solution forx(t) and y(t) over [0, T ]. The solution is unique because duringthe first phase, Vx , Vy , Wx , and Wy are fixed by the followingequations [which are obtained from (5) and (6)]:

Vx =SFx(0)

2− rx(0) (17)

Vy =SFy (0)

2− ry (0) (18)

Wx =√

zc

g(x(0) − rx(0)) (19)

Wy =√

zc

g(y(0) − ry (0)) . (20)

Moreover, the unique solution during the first phase leads tounique values for x(t1), y(t1), x(t1), and y(t1). This fixes thefree parameters of the unique C2 extension of the solution on[t1 , t2 ] and, subsequently, the free parameters of the unique C2

extension over [t2 , T ]. Nevertheless, those two unique C2 so-lutions might violate the constraints x(T ) = 0 and y(T ) = 0

Fig. 2. We consider the upward half-step of Fig. 1 and show the correspondingZMP trajectory along the y-axis: py (t) (solid line). To this trajectory corresponds

an infinity of C 2 solutions for y(t) which all verify y(0) = py (0) = S Fy (0)2 ,

each of them being fully defined by y(0). We show several such C 2 solutions(dotted lines); the thick dotted line is the solution retained. It is the unique oneverifying y(T ) = 0.

[see (13) and (14)]. Analyzing the impact of x(0) and y(0)in the analytical solutions, we can see that they have a mono-tonic influence over, respectively, x(T ) and y(T ) and that onevalue of x(T ) (respectively, y(T )) corresponds a unique valuex(0) (respectively, x(0)). We implemented a dichotomic searchfor those values, and with simple methods avoided problemsof numerical unstability (using the fact that with only oneZMP shift and the boundary conditions CoM(0) = ZMP(0) andCoM(T ) = ZMP(T ), the solution CoM trajectories x and y arenecessarily monotone).

Fig. 2 considers the half-step of Fig. 1, and it shows the trajec-tory of the ZMP along the y-axis, as well as several C2 solutionsfor y(t), for different values of y(0). Only one solution is re-tained, the one with y(T ) = 0. If the durations t1 and T − t2are long enough, the values that are obtained for x(0), x(T ),y(0), and y(T ) can be neglected, and thus, the CoM trajectoriesobtained are supposed to be C2 continuous over (−∞,∞). Per-forming tests on a real humanoid robot empirically validated thisassumption: Time discretization of the control law itself makesthe neglected velocity unnoticeable. For the trajectories otherthan x(t) and y(t) (θ(t), SFx(t), SFy (t), SFz (t), SFθ (t)), wesimply use polynomials of degree 3 that ensure C2 smoothnessand satisfying profiles, with a few specific constraints (e.g., inour implementation, the swing foot always leaves the groundand lands vertically). Therefore, we can completely define ahalf-step with five parameters (whether it is an upward half-step or a downward half-step). In our application, we decidedto fix the maximum height of the swing foot (SFz (T )), and thehorizontal distance between the feet when the maximum heightis reached (which fixes SFy (T )). This puts us in the condi-tions of [23], where two “via point configurations” Qright andQleft are fixed. With these constraints, only three parameters areneeded to completely define a half-step. Once these parametersare set, we are capable of generating unique analytical solutionsfor the seven functions of time that are required to produce thelower body trajectory in the C-space.


Fig. 3. Progressively increasing the overlap between two half-steps.

B. Smoothing a Sequence of Half-Steps

Using the results of the previous section, we can generateC-space trajectories for isolated half-steps. Since they startand finish with zero speed, we can simply join them to pro-duce sequences of half-steps. Alternating upward and down-ward half-steps will produce a walking motion. Each half-steptrajectory is dynamic in the sense that the inertial forces playan important role in maintaining the balance (the trajectoriesare not quasi-static). However, at the end of each half-step,a balanced posture is reached with zero speed. This is not asatisfactory result because between each half-step, the robotcomes to a stop; therefore, the walk motion is not visuallysmooth and rather slow. Recent walking pattern generatorsachieve much better results by using preview control (see [19]).In this section, we show how to continuously modify a se-quence of half-steps using a simple homotopy in order to makeit faster and smoother along the same footstep sequence. We firstshow how to do so for a sequence of two half-steps and startwith the case of an upward half-step followed by a downwardhalf-step.

1) Upward Then Downward: We consider an upward half-step followed by a downward half-step. Together, the two half-steps make a classical full step: double support phase, then singlesupport phase, and then double support phase again.

We recall that the whole C-space trajectory of the lower bodyduring one half-step is generated by inverse geometry fromseven functions of the time. Since here we are dealing with twoconsecutive half-steps (with the same support foot), we haveto consider 14 functions. Let us first consider, for example, theposition of the waist (or CoM) along the y-axis, respectively, forthe upward half-step: y1(t) and the downward half-step: y2(t).We have y1(T ) = y2(0) = 0. Let us define two operators g1

Δand g2

Δ such that

g1Δ(f)(t)=

{f(t), for t ∈ (0, T )f(T ), for t ∈ (T, 2T − Δ) (21)

g2Δ(f)(t)=

{0, for t ∈ (0, T−Δ)f(t − T + Δ) − f(0), for t ∈ (T − Δ, 2T−Δ).

(22)

g10 (y1) + g2

0 (y2) corresponds to the simple concatenation ofy1 and y2 without overlap. Knowing that py1 = Z(y1), py2 =Z(y2), and y1(T ) = y2(0) = 0, it is quite easy to verifythat for any 0 ≤ Δ ≤ T , g1

Δ(py1 ) = Z(g1Δ(y1)), g2

Δ(py2 ) =Z(g2

Δ(y2)). In addition, since Z is a linear operator

g1Δ(py1 ) + g2

Δ(py2 ) = Z(g1Δ(y1) + g2

Δ(y2)). (23)

Operators g1Δ and g2

Δ enable us to obtain new combined CoMand ZMP trajectories that still verify the linear inverted pen-dulum equations [see (1) and (2)]. Starting with Δ = 0 andprogressively increasing the value of Δ continuously mod-ifies the CoM trajectory (starting from the initial trajectoryg1

0 (y1) + g20 (y2)) to make the second ZMP shift (the one of

py2 ) happen earlier, creating an overlap of duration Δ betweenthe two trajectories y1 and y2 . Fig. 3 illustrates this effect: Whenwe increase the value of Δ, we can see that the position of theCoM does not need to reach the center of the support foot.

We use the same operators, i.e., g1Δ and g2

Δ , to produce anoverlap between the functions of time corresponding to the waistorientation and swing foot position and orientation. Since theinverse geometry for the legs is a continuous function as long aswe stay inside the joint limits, these operators used on the bodiestrajectories actually implement a simple homotopy that contin-uously deforms the initial C-space trajectory into a smoother,more dynamic trajectory. The linearity of simplified differentialequations has already been used in a similar way to producemixtures of motions (see [28] and [29]), but the purpose was tocreate new steps and not to smooth them nor speed them up.

In the case of an upward half-step followed by a downwardhalf-step, increasing Δ reduces the duration of the single supportphase, and therefore, it increases the speed of the swing foot. Tolimit this effect, we must bound Δ. Besides, if Δ is too large,undesirable phenomena can occur, such as a negative swing foot


Fig. 4. We illustrate the “smoothing” of a raw sequence of half-steps. On theinitial raw sequence (left), the support paths of the ZMP and CoM trajectoriesare superimposed. Then, after adjusting the overlaps, the ZMP support pathstays the same, but the CoM support path becomes smoother (right). We cansmooth even more, but it reduces the duration of the single support phase thatis directly linked with the swing foot speed. Therefore, limitations on the swingfoot speed constrain the smoothing process.

height. To avoid these problems, we set an upper bound suchthat the maximum overlap results in a moderately fast gait.

2) Downward Then Upward: We can apply the same tech-nique to produce an overlap in the case of a downward half-stepfollowed by an upward half-step. Since the last phase of thedownward half-step and the first phase of the upward half-stepare double support phases, the constraint on the swing foot mo-tion disappears and the maximum bound on Δ becomes simplyT (that is, if t1 = T − t2, and it results in a double supportphase whose duration is t2 − t1).

For longer sequences of half-steps, we can simply repeat theprocedure to smooth the whole trajectory, setting the overlapsone by one. Fig. 4 shows the results that are obtained withan example of raw sequence. After the smoothing, the CoMtrajectory is visually smoother and besides, the new trajectoryis much faster (about three times faster).

Changing overlaps inside a sequence of half-steps modifiesthe whole C-space trajectory: not only the CoM and ZMP but theswing foot trajectory, as well. When the overlap is increased,the swing foot tends to move faster and closer to the ground.If one property must be preserved (for instance absence of col-lision), it must be checked after every modification. Since thesmoothing by overlap is a continuous operator, we can use di-chotomies to quickly find large acceptable values of overlaps.Let us consider an example for two consecutive half-steps. Wepredefine a maximum overlap Δmax and, first, we simulate thepart of the trajectory modified by the overlap Δmax and checkfor collisions, self-collisions, and joint limit violations. If noneof these events occur, we set the overlap to Δmax . Otherwise, weuse a dichotomy starting at Δmax/2 to quickly converge towardthe largest “good” overlap value below Δmax . Fig. 5 shows theeffect of the smoothing process on the swing foot trajectory:With the dichotomy, we can quickly find a large overlap thatkeeps the trajectory collision-free.

Fig. 5. (Left) Raw sequence of two half-steps avoiding a box on the ground.We can see that the swing foot reaches a high position. (Right) After smoothing,the trajectory has been modified so that the foot moves very close to the obstacle.

Fig. 6. Thanks to the two via point configurations Qle ft and Qright , a rawsequence of two half-steps can be entirely defined by only three parameters: x,y, and θ. Qle ft and Qright were chosen such that the swing foot is quite high.This provides good obstacle avoidance capabilities to raw sequences of steps,and in the absence of obstacles, smoothing significantly reduces the height.

III. BUILDING THE TRANSITION MODEL AND THE SWEPT

VOLUME APPROXIMATIONS

A. Transition Model

Thanks to the walking pattern generator that is described inthe previous section, we can produce isolated half-steps withonly three parameters. If we join a downward half-step withthe corresponding upward half-step, we obtain a trajectory thatgoes from Qleft to Qright or Qright to Qleft , and which is entirelydefined by only three parameters, as shown in Fig. 6. We denotesuch a trajectory (expressed in the frame of the left foot) by〈Qleft , (x, y, θ), Qright〉 or (expressed in the frame of the rightfoot) 〈Qright , (x, y, θ), Qleft〉. We also denote that

Tl = {〈Qlef t , (x, y, θ), Qright〉 | (x, y, θ) ∈ R3}

and

Tr = {〈Qright , (x, y, θ), Qlef t〉 | (x, y, θ) ∈ R3}.

We will interchangeably call the elements of Tl or Tr points(because of the bijection with R

3), transitions (because the tran-sition model will be a finite set of elements of Tl), sequences(each element corresponds to a downward half-step–upwardhalf-step sequence), or trajectories. By concatenating alterna-tively trajectories from Tl and trajectories from Tr , we obtain


Fig. 7. Initial grid of 600 points covers the input space. To each of the 120values of (x, y) correspond five possible orientations. All the correspondingtrajectories (which are generated by the walking pattern generator presented inSection II) are sequences of two half-steps. We test each of them, checking forself-collisions and joint limit violations, and remove all the unfeasible ones. The276 remaining points form the transition model Ml used for planning.

walk motions. With a symmetric robot (like HRP-2), Tl and Tr

are symmetric in the sense that the feasibility of a sequence〈Qleft , (x, y, θ), Qright〉 is equivalent to the feasibility of the se-quence 〈Qright , (x,−y,−θ), Qleft〉, and that the correspondingswept volumes are symmetric. Therefore, only one transitionmodel was built, on the space Tl , but it can be used by symme-try on Tr . To build it, as explained in Fig. 7, we first covereda reasonably large domain of Tl with regularly spaced points.Considering the robot (HRP-2) dimensions and joint limits, thisdomain was defined as the following box:

Bl = {〈Qleft , (x, y, θ), Qright〉 | x ∈ [−0.35 m,+0.35 m]

y ∈ [−0.37 m,−0.02 m], θ ∈ [−30◦,+30◦]}.

We covered the box Bl with 600 points (15 possible valuesfor x, eight possible values for y, and five possible values forθ), and for each point, using discretized trajectories—one foreach body of the robot legs—we verified the feasibility of thecorresponding downward half-step–upward half-step sequence.If any self-collision (which were checked using the algorithmthat is introduced in [3]) or joint limit violation occurred, thepoint was discarded.

The 276 remaining points all correspond to feasible se-quences, and they form the transition model.

We denote byMl ⊂ Bl this finite transition model;Mr ⊂ Br

is defined by symmetry. We denote by S(Ml ,Mr ) the set offinite feasible sequences (s1 , s2 , . . . , sn ), alternating left footand right foot support.

Fig. 8. Example of swept volume approximation. The data structure obtainedis a bit similar to an adaptively sampled distance field (see [11]).

B. Swept Volume Approximations

For each of the 276 points of the transition model, we build anapproximation of the volume swept by the lower part of the robot(from the knees down) during the corresponding downward half-step–upward half-step sequence. The algorithm used is the onedescribed in [30]: Given a transition z ∈ Ml , it learns throughadaptive sampling the sign of the mapping Cz (p), which returnsthe distance (minus a fixed margin, 1 cm in our case) betweena point p of the Euclidean space and the finite set of polyhedraconsisting of all the configurations of the robot legs bodies alongtheir discretized trajectories during the sequence correspondingto z. Fig. 8 illustrates an example of this process. The importantproperty of the approximation algorithm used is that it storesthe result in a tree structure which can be evaluated extremelyquickly. The computation time saved is considerable: With theapproximation, checking whether a point is outside or insideone of the swept volumes, we consider is done in 4 μs. Thisis about 2000 times faster than with the normal evaluation ofCz (p).

For a transition z = 〈Qleft , (x, y, θ), Qright〉 ∈ Ml , we de-note by Vz (p) the corresponding swept volume approximation(Vz (p) > 0 if and only if p is outside the approximated sweptvolume). If z′ = 〈Qleft , (x,−y,−θ), Qright〉 ∈ Mr , we caneasily obtain the approximation Vz ′ by applying a symmetry toVz ; thus, only 276 swept volume approximations are needed.With an Intel(R) Xeon(R) 2.00-GHz CPU, it took a bit lessthan 48 h to generate them all, but we believe that by usingstate-of-the art swept volume approximation algorithms (andmay be only afterward apply our algorithm to obtain reapprox-imations that can be evaluated very fast), we should be able tosignificantly reduce this offline computation time.

Fig. 9 shows five of the 276 swept volume approximations.


Fig. 9. Three-dimensional representations of five swept volumes approximations.

IV. FOOTSTEP PLANNING WITH A VARIANT OF RAPIDLY

EXPLORING RANDOM TREE

In this section, we present a simple adaptation of the RRTalgorithm for footstep planning, which is quite similar to theone introduced in [38].

Let us first define the search space. Since, in our formal-ism, we connect single support phases, the search space isS = {(q, x, y, θ) | q ∈ {Qleft , Qright}, (x, y, θ) ∈ R

3}, whereq is the support foot, (x, y) is the position of the support foot(relatively to a fixed reference), and θ is its orientation (relativelyto a fixed reference). The transition model being an alternationbetween Ml and Mr , we can apply transitions to states of thesearch space using the operator δ:

δ ((q, x, y, θ), 〈q, (x′, y′, θ′), q〉)= (q, x′cos(θ) − y′sin(θ), x′sin(θ) + y′cos(θ), θ + θ′)

where Qleft = Qright and Qright = Qleft . In practice, we willuse only a compact subset of the search space, depending onthe environment E . We denote it by S|E . For example, if therobot stays in a 5 m × 5 m room, we naturally use these dimen-sions to define S|E and bound x and y. Considering the classicalRRT algorithm (see [26]), the only operation that cannot bestraightforwardly adapted to the context of footstep planning isthe extension toward random samples (to find the nearest neigh-bor, we use the Euclidean metric, ignoring the orientations). Let(q, x, y, θ) ∈ S be a random sample of the search space and(q′, x′, y′, θ′) the nearest state in the search tree. In [38], two op-tions are considered: Either add to the tree all the successors of(q′, x′, y′, θ′) or just one random successor. Because of the sizeof our transition model, we chose to follow the latter strategy.Fig. 10 shows one issue of this approach: In some cases, it isdifficult to extend the search tree toward a given region. To copewith this problem, many options are possible. We simply choseto alternatively look for nearest states with left support foot andnearest states with right support foot. It leads to our RRT variantthat is presented in Algorithm 1 (we stop the while loop whena path to the goal region has been found or when a sufficientlyshort path has been found). We based our implementation ona fast and modular open-source code by Karaman and Fraz-zoli which uses kd-trees for fast nearest neighbor queries (thiscode implements RRT and RRT*, i.e., the algorithms introducedin [20]).

Fig. 10. Advantage of separating left and right support feet during nearestneighbor queries. (Left) (global nearest neighbor): All the points in the grayregion have the same nearest neighbor (Qright , x, y, θ), but no successor of(Qright , x, y, θ) is inside the gray region. Therefore, numerous samples are re-quired before extending the search tree toward the gray region. (Right) (alternatenearest neighbors): When only states with left support foot are considered, thenearest neighbor will not be (Qright , x, y, θ) but may be one of its successors.With the alternation strategy, the search tree is more likely to quickly growinside the gray region.

Further analyses and improvements of the variants of RRTfor footstep planning can probably help to obtain faster resultsbut are out of the scope of this paper.

V. PRELIMINARY EXPERIMENTS

The framework presented in this paper was experimentallytested on the robot HRP-2.

We studied the two experimental setups that are describedin Fig. 11, where 2-D obstacles (i.e., holes in the ground) arecombined with 3-D obstacles. The 3-D obstacles that are shownin Fig. 11 have the same size as the ones in the real environment(see Fig. 12) but are smaller than the ones used for the collisionchecks (a margin is needed because of the robot drift during thereal-world experiments).

The construction of the solution trajectory is divided intotwo parts: First, during the planning phase, just as explained inthe previous section, we use a specific variant of RRT to finda sequence (s1 , s2 , . . . , sn ) ∈ S(Ml ,Mr ) which reaches thegoal. Then, we use the homotopy of Section II-B to smooth the


Fig. 11. Experimental setups and results of the planning. The computationsare made with an Intel(R) Xeon(R) 2.00-GHz CPU. Remark: In Setup 1, theexterior surface of the 3-D obstacles (the boxes on the ground) is covered by250 points. In Setup 2, the exterior surface of the 3-D obstacles is covered by75 points.

Fig. 12. Experimental results: the robot HRP-2 executing planned trajectories.(Above) So-called toy problem of walking in a child’s bedroom avoiding toyson the ground.

sequence (s1 , s2 , . . . , sn ) to obtain the final fast and dynamictrajectory that will be performed by the robot.

A. Planning Phase: Rapidly Exploring Random Tree Versus A*

We implemented a classical A* search algorithm and com-pared it with the RRT variant that is introduced in the previoussection. For the costs required by A*, we used a simple heuris-tic where the estimated remaining cost is derived from the Eu-clidean distance, and the cost of a path is the sum of each (fixed)transition cost. Better heuristics can often be obtained, such as,for example, heuristics derived from a mobile robot planner thatlooks for continuous paths between the initial position and thegoal, but because they do not take stepping over capabilitiesinto account, such heuristics tend to severely misjudge costs invery constrained environments like the ones that we considerhere (for a review on the association { A* + heuristic }, see[5, ch. 8]). Finding a robust heuristic that would perform wellin challenging environments is as hard as solving the problemwithout using A*: That is why we tried to directly apply RRT.Other approaches of interest include planning algorithms thatare based on inflated heuristics (see [12]): They usually findsolutions faster than a classical A* search, but they are not asefficient as RRT to avoid local minima. Their main advantageover RRT is that they provide suboptimality bounds; however,because of the particularity of the problem of footstep planning,it is not clear whether such bounds can still be obtained in ourcontext. Finally, it might be interesting to try to adapt control-based strategies such as [35], but the adaptation would be farfrom straightforward.

In Setups 1 and 2, we fixed an upper bound and stopped theexecution of RRT or A* as soon as a path of cost smaller thanthis upper bound was found.

As shown by the results in Setup 1, without strong localminima, the time that is needed by RRT and A* to find a solutionis approximately the same, but A* finds a better trajectory cost(it finds solutions with fewer steps).


On the other hand, we can see with the results in Setup 2that when the transition model is large, A* seems much moresensitive to local minima than RRT: Indeed, A* fails to find asolution on Setup 2, whereas the RRT method consistently findssolutions in less than 40 s (and 29.8 s on average).

This is easily explainable because A* usually has to explorea subtree of fixed height h (which depends on the heuristic costsused) before being able to avoid a local minimum. Therefore,

it will try about (|M|h − 1)(

|M||M|−1

)transitions (|M| being

the size of the transition model) before overcoming the localminimum. This can be done if both |M| and h are relativelysmall, but since, in our case, |M| = 276, the complexity canquickly become insurmountable.

As a randomized approach, RRT does not have this caveat,and that is why, we think it is more suitable than A* when thetransition model is large.

A remark on the time saved thanks to the swept volumeapproximations: In Setup 1, whose environment contain a lot ofpoints (i.e., 250), we can see that during their execution, both theRRT variant and A* make about 200 000 calls to a swept volumeapproximation every second. Without the approximations, these200 000 calls would be replaced by more than 26 min spent incollision checking.

B. Smoothing Phase

Once a trajectory avoiding the obstacles has been found bythe planner, since it consists in a concatenation of isolated half-steps, we can use the homotopy that is described in Section II-Bto smooth it. One overlap parameter has to be set for each pairof consecutive half-steps, and since the overlaps are indepen-dent, they can be set sequentially. This means that we can startto execute the trajectory on the robot even if only a few initialoverlaps have been set, the next overlaps being computed duringthe execution of the trajectory. Let us notice that the dichotomysearch for the best overlap time is an “any-time process” that canbe interrupted if computation time is too long, the current resultbeing no worse than the initial raw motion. Another importantremark follows: Since we cannot know in advance the sweptvolumes for the trajectories that are involved in the smoothingprocesses, we have to use classical collision checks. We mea-sured the overlaps computation time for ten raw sequences ofhalf-steps that are obtained in Setup 1 and ten raw sequences thatare obtained in Setup 2. In all cases, the duration of the smooth-ing was less than the final trajectory execution time. For thesolutions in Setup 1, the average time needed for the smoothingwas 14.4 s, and the average execution time of the final trajectorywas 31.1 s. For the solutions in Setup 2, the average duration ofthe smoothing was 13.4, and the average execution time of thefinal trajectory was 41.6 s. Fig. 13 illustrates the effect of thesmoothing on the foot trajectories.

VI. MORE ADVANCED IMPLEMENTATION

The way we deal with collisions in the preliminary experi-ments is clearly not optimal: We represent obstacles by coveringthem with points on their exterior surface, and all the points are

Fig. 13. (Above) Raw sequence of two half-steps. (Below) Smoothed se-quence. When there are no obstacles, the swing foot trajectory of the smoothedsequence depends on the minimum time between two ZMP shifts, which is fixedin advance in order to bound the speed of the feet.

always taken into account. The results showed that the sweptvolume approximations can be called a great number of timesin a short period, proving that significant speedup can be ob-tained compared with frequent collision checks along a prioriunknown trajectories. What is more, in some case, point cloudsare a very natural input, and it would be interesting to see ifwe can organize them in a good structure so that to use ourapproximation functions in an efficient way. This is beyond thescope of this paper, but we can already obtain better results byusing state-of-the-art collision detection algorithms. First, wecan notice that our swept volume approximations are defined byintersections of small boxes with planes. Thus, it is easy to con-struct meshes that describe the swept volume approximations(we actually use simplified meshes, i.e., they have a slightly sim-pler geometry than the initially precomputed approximations).With these 276 meshes, we will use the PQP algorithm [24] forcollision checks. The main advantage we obtain by doing sois that when the obstacles are represented by classical meshesas well, PQP stores them in bounding volume hierarchies thatreduce the complexity of collision checks.

With this method, a significant speedup is reached: With theSetup 2 in Fig. 11, we performed 1000 trials with a slightlyfaster CPU (Intel(R) Xeon(R) 2.40-GHz) but overall in similarconditions. A solution was always found, and the average timerequired was only 1.60 s, which is almost 20 times faster than thepreliminary results. The average number of steps of the solutionwas 28.5 steps, and on average, 18 000 collision checks wereneeded before finding a solution.

With this new implementation, we tested our algorithm inmore complex environments in simulation and also used it toperform real-time replanning in experiments, where the posi-tion of the robot and obstacles is acquired by motion capture.The details of the framework used for these experiments are de-scribed in [2]. Fig. 14 shows two simulations, and Fig. 15 showsan experiment during which a bar placed 5 cm above the ground


Fig. 14. (Left) Sequence of steps found in a complex environment. (Right) Weshow for one sequence of steps the concatenation of the swept volumes, whichare simplified meshes obtained from the original swept volume approximations.For the upper body, simpler bounding boxes are used for the collision checks.

Fig. 15. (1) HRP-2 starts to execute the sequence initially found. (2) Bar issuddenly moved, and the current sequence of step would lead to collisions. (3)While walking, HRP-2 is able to compute a new sequence of steps toward thegoal (we show the concatenation of the swept volumes which indeed avoid thebar). (4) Robot finally steps over the bar while, at the same time, it tries tooptimize the rest of the path towards the goal. Remark: Due to uncertainty onpositions, we use a model of bar that is thicker than the actual bar.

is moved, while the robot is executing its initial plan. The robotis then able to quickly find a new plan and successfully stepsover the bar in its new configuration before reaching the goal.

VII. DISCUSSION ON AN EXTENSION TO CONTINUOUS

TRANSITION MODELS

Even if the expressiveness of a continuous transition modelcan be approached by the one of a large finite transition model,a continuous transition model would still be preferable.

Several useful techniques would be easier to apply with a con-tinuous transition model: local footstep modifications (see [8]and [9]), extraction of convex regions in the transition model inorder to use optimization techniques to determine foot place-ments [16], path deformation, [18], etc.

RRT and other sampling-based algorithms (e.g., PRM, see[21]) would be easier to adapt with a continuous transitionmodel; therefore, it would cause no problem at the planningphase. Besides, it would not be difficult to approximate thefeasibility regions so as to obtain continuous transition modelsMl and Mr (although it might be hard to obtain the guaran-tee that all transitions are indeed feasible). However, then, themain issue would be the need to approximate swept volumeswhich depend on a continuous parameter z ∈ Ml ; instead ofapproximating (the sign of) Cz (p) for a finite set of values of z,we would need to approximate C(z,p), which depends on sixparameters. It no longer corresponds to the approximation of asingle swept volume; therefore, the state-of-the-art algorithmsfor swept volume approximation cannot be directly used, andwe would probably need to keep a generic approximation al-gorithm, like the one used in this paper. Since it took alreadyalmost 48 h to approximate the swept volumes of the finitetransition model, for a continuous transition model, an accurateapproximation would probably be excessively time consuming.In that case, it is likely that instead of trying to compute theswept volumes more efficiently, other collision detection rou-tines should be taken into account, such as continuous collisiondetection [40], GPU-based approaches [25], or other variants(see, e.g., [33] and [36]).

VIII. CONCLUSION

In this paper, we have described a novel and coherent frame-work for footstep planning, which includes a walking patterngenerator based on half-steps, a simple homotopy for trajec-tory smoothing, swept volume approximations for fast collisionchecking, and an RRT variant for footstep planning. We usedthis framework on the robot HRP-2 to quickly plan dynamicsequences of walk in environments cluttered with 3-D and 2-Dobstacles. Although computed in a few seconds and with thetheoretical guarantee that they actually avoid the obstacles, theexecuted trajectories seem very natural: no pauses, no exagger-ated motions to avoid small obstacles, and a large diversity offoot placement.

REFERENCES

[1] Y. Ayaz, K. Munawar, M. B. Malik, A. Konno, and M. Uchiyama, “Human-like approach to footstep planning among obstacles for humanoid robots,”in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2006, pp. 5490–5495.

[2] L. Baudouin, N. Perrin, T. Moulard, O. Stasse, and E. Yoshida. (2011).“Real-time replanning using 3D environment for humanoid robot,” inProc. 11th IEEE-RAS Int. Conf. Humanoid Robots, [Online]. Available:http://homepages.laas.fr/nperrin/submitted/humanoids11-lbaudouin.pdf.


[3] M. Benallegue, A. Escande, S. Miossec, and A. Kheddar, “Fast c1 prox-imity queries using support mapping of sphere-torus-patches boundingvolumes,” in Proc. IEEE Int. Conf. Robot. Autom., 2009, pp. 483–488.

[4] J.-M. Bourgeot, N. Cislo, and B. Espiau, “Path-planning and tracking in a3D complex environment for an anthropomorphic biped robot,” in Proc.IEEE Int. Conf. Intell. Robots Syst., 2002, pp. 2509–2514.

[5] J. Chestnutt, “Navigation planning for legged robots,” Ph.D. dissertation,Carnegie Mellon Univ., Pittsburgh, PA, 2007.

[6] J. Chestnutt, J. Kuffner, K. Nishiwaki, and S. Kagami, “Planning bipednavigation strategies in complex environments,” in Proc. IEEE Int. Conf.Humanoid Robots, 2003. [CD-ROM].

[7] J. Chestnutt, M. Lau, G. Cheung, J. Kuffner, J. Hodgins, and T. Kanade,“Footstep planning for the honda ASIMO humanoid,” in Proc. IEEE Int.Conf. Robot. Autom., 2005, pp. 631–636.

[8] J. Chestnutt, P. Michel, K. Nishiwaki, J. Kuffner, and S. Kagami, “Anintelligent joystick for biped control,” in Proc. IEEE Int. Conf. Robot.Autom., 2006, pp. 860–865.

[9] J. Chestnutt, K. Nishiwaki, J. J. Kuffner, and S. Kagami, “An adaptiveaction model for legged navigation planning,” in Proc. IEEE/RAS Int.Conf. Humanoid Robots, 2007, pp. 196–202.

[10] M. Elmogy, C. Habel, and J. Zhang, “Online motion planning for HOAP-2humanoid robot navigation,” in Proc. IEEE/RSJ Int. Conf. Intell. RobotsSyst., 2009, pp. 3531–3536.

[11] S. F. Frisken, R. N. Perry, A. P. Rockwood, and T. R. Jones, “Adaptivelysampled distance fields: A general representation of shape for computergraphics,” in Proc. 27th Annu. Conf. Comput. Graph. Interact. Tech., 2000,pp. 249–254.

[12] J. P. Gonzalez and M. Likhachev, “Search-based planning with provablesuboptimality bounds for continuous state spaces,” in Proc. 4th Annu.Symp. Combinatorial Search, 2011.

[13] K. Harada, “Motion planning for a humanoid robot based on a bipedwalking pattern generator,” in Motion Planning for Humanoid Robots,New York: Springer-Verlag, 2010, pp. 192–197.

[14] K. Harada, S. Kajita, K. Kaneko, and H. Hirukawa, “An analytical methodfor real-time gait planning for humanoid robots,” Int. J. Humanoid Robot.,vol. 3, no. 1, pp. 1–19, 2006.

[15] T. Hasegawa, K. Nakagawa, and K. Murakami, “Collision-free path plan-ning of a telerobotic manipulator based on swept volume of teleoperatedmanipulator,” in Proc. 5th IEEE Int. Symp. Assembly Task Planning, 2003,pp. 259–263.

[16] A. Herdt, N. Perrin, and P.-B. Wieber, “Walking without thinking aboutit,” in Proc. IEEE Int. Conf. Intell. Robots Syst., 2010, pp. 190–195.

[17] J. C. Himmelstein, E. Ferre, and J.-P. Laumond, “Swept volume approx-imation of polygon soups,” IEEE Trans. Autom. Sci. Eng., vol. 7, no. 1,pp. 177–183, Jan. 2010.

[18] L. Jaillet and T. Simeon, “Path deformation roadmaps,” in Proc. 7th Work-shop Algorithmic Found. Robot., 2006.

[19] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, and K. Yokoi,“Biped walking pattern generation by using preview control of zero-moment point,” in Proc. IEEE Int. Conf. Robot. Autom., 2003, pp. 1620–1626.

[20] S. Karaman and E. Frazzoli, “Incremental sampling-based algorithms foroptimal motion planning,” in Proc. Robot. Sci. Syst. VI Conf., 2010.

[21] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Prob-abilistic roadmaps for path planning in high-dimensional configurationspaces,” IEEE Trans. Robot. Autom., vol. 12, no. 4, pp. 566–580, Aug.1996.

[22] Y. J. Kim, G. Varadhan, M. C. Lin, and D. Manocha, “Fast swept volumeapproximation of complex polyhedral models,” in Proc. 8th ACM Symp.Solid Model. Appl., 2003, pp. 11–22.

[23] J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue, “Footstepplanning among obstacles for biped robots,” in Proc. IEEE/RSJ Int. Conf.Intell. Robots Syst., 2001, pp. 500–505.

[24] E. Larsen, S. Gottschalk, M. C. Lin, and D. Manocha, “Fast proximityqueries with swept sphere volumes,” in Proc. IEEE Int. Conf. Robot.Autom., 2000, pp. 3719–3726.

[25] C. Lauterbach, Q. Mo, and D. Manocha, “Gproximity: Hierarchical GPU-based operations for collision and distance queries,” Comput. Graph. Fo-rum, pp. 419–428, 2010.

[26] S. M. LaValle and J. J. Kuffner, “Rapidly-exploring random trees: Progressand prospects,” in Proc. 4th Workshop Algorithmic Found. Robot., 2000,pp. 293–308.

[27] Y. Nakamura and H. Hanafusa, “Optimal redundancy control of robotmanipulators,” Int. J. Robot. Res., vol. 6, pp. 32–42, 1987.

[28] K. Nishiwaki, K. Nagasaka, M. Inaba, and H. Inoue, “Generation of re-active stepping motion for a humanoid by dynamically stable mixture

of pre-designed motions,” in Proc. IEEE Int. Conf. Syst., Man, Cybern.,1999, pp. 902–907.

[29] K. Nishiwaki, T. Sugihara, S. Kagami, M. Inaba, and H. Inoue, “Onlinemixture and connection of basic motions for humanoid walking controlby footprint specification,” in Proc. IEEE Int. Conf. Robot. Autom., 2001,pp. 4110–4115.

[30] N. Perrin, O. Stasse, F. Lamiraux, and E. Yoshida. (2010).Adaptive sampling-based approximation of the sign of multivari-ate real-valued functions. Tech. Rep. [Online]. Available: http://hal.archives-ouvertes.fr/docs/00/54/48/91/PDF/approx.pdf.

[31] N. Perrin, O. Stasse, F. Lamiraux, and E. Yoshida, “Approximation offeasibility tests for reactive walk on HRP-2,” in Proc. IEEE Int. Conf.Robot. Autom., 2010, pp. 4243–4248.

[32] N. Perrin, O. Stasse, F. Lamiraux, and E. Yoshida, “A biped walkingpattern generator based on ”half-steps” for dimensionality reduction,” inProc. IEEE Int. Conf. Robot. Autom., 2011, pp. 1270–1275.

[33] H. Schmidl, N. Walker, and M. C. Lin, “CAB: Fast update of OBB treesfor collision detection between articulated bodies,” J. Graph. Tools, vol. 9,pp. 1–9, 2004.

[34] F. Schwarzer, M. Saha, and J.-C. Latombe, “Exact collision checking ofrobot paths,” in Proc. 5th Workshop Algorithmic Found. Robot., 2002.

[35] I. A. Sucan and L. E. Kavraki, “Kinodynamic motion planning by interior-exterior cell exploration,” in Proc. 8th Workshop Algorithm. Found.Robot., 2008.

[36] M. Tang, Y. J. Kim, and D. Manocha, “CCQ: Efficient local planning usingconnection collision query,” in Proc. 9th Workshop Algorithm. Found.Robot., 2010, pp. 229–247.

[37] M. Vukobratovic and B. Borovac, “Zero-moment point—Thirty five yearsof its life,” Int. J. Humanoid Robot., vol. 1, no. 1, pp. 157–173, 2004.

[38] Z. Xia, G. Chen, J. Xiong, Q. Zhao, and K. Chen, “A random sampling-based approach to goal-directed footstep planning for humanoid robots,”in Proc. IEEE/ASME Int. Conf. Adv. Intell. Mechatronics, 2009, pp. 168–173.

[39] E. Yoshida, I. Belousov, C. Esteves, and J.-P. Laumond, “Humanoid mo-tion planning for dynamic tasks,” in Proc. IEEE/RAS Int. Conf. HumanoidRobots, 2005, pp. 1–6.

[40] X. Zhang, S. Redon, M. Lee, and Y. J. Kim, “Continuous collision de-tection for articulated models using Taylor models and temporal culling,”ACM Trans. Graph., vol. 26, no. 3, art. 15, 2007.

Nicolas Perrin was a student at the Ecole NormaleSuperieure de Lyon, Lyon, France, between 2005 and2009 from which he received the M.Sc. degree inmathematical logic and foundations of computer sci-ence from the University of Paris VII, Paris, France,in 2008. He is currently working toward the Ph.D.degree with LAAS-CNRS and INP, Universite deToulouse, Toulouse, France.

During his Ph.D. studies, he worked with CNRS-AIST Joint Robotics Laboratory, Tsukuba, Japan,from 2008 to 2011. His current research interests

include humanoid robots and motion planning.

Olivier Stasse received the Ph.D. degree in intelli-gent systems from the University of Paris 6, Paris,France, in 2000.

He is currently a Senior Researcher (CR-1) withLAAS-CNRS, Universite de Toulouse, Toulouse,France. He was an Assistant Professor of computerscience with the University of Paris 13. His researchinterests include humanoids robots and, more specif-ically, motion generation motivated by vision. From2003 to 2011, he was with the Joint French–JapaneseRobotics Laboratory, Tsukuba, Japan.

Dr. Stasse was a Finalist for the Best Paper Award at the 2007 InternationalConference on Advanced Robotics and a Finalist for the Best Video Award at the2007 International Conference on Robotics and Automation, and he receivedthe Best Paper Award at the 2006 International Conference on Mechatronicsand Automation.


Leo Baudouin received the M.Sc. degree in masterimage and vision from the Blaise Pascal Universityof Clermont-Ferrand, Clermont-Ferrand, France, in2011. He is currently working toward the Engineer’sdegree with the French Institute of Advanced Me-chanics, which is an engineering school of mecha-tronics in Clermont-Ferrand.

In 2011, he has been with the CNRS-AISTJoint Robotics Laboratory, Tsukuba, Japan, and withLAAS-CNRS, Universite de Toulouse, Toulouse,France, working on humanoid robot real-time

replanning.

Florent Lamiraux received the Graduate degreefrom the Ecole Polytechnique Paris, Paris, France,in 1993 and the Ph.D. degree in computer sciencefrom the Institut National Polytechnique de Toulouse,Toulouse, France, in 1997 for his research on mobilerobots.

He was with Rice University, Houston, TX, as aResearch Associate for two years. He is currently theDirecteur de Recherche with LAAS-CNRS, Univer-site de Toulouse, where he is involved with humanoidrobots.

Eiichi Yoshida received the M.E. and Ph.D. degreesin precision machinery engineering from the Gradu-ate School of Engineering, the University of Tokyo,Tokyo, Japan, in 1993 and 1996 respectively.

He joined the former Mechanical EngineeringLaboratory, Tsukuba, Japan, in 1996. He is currentlya Senior Research Scientist with Intelligent SystemsResearch Institute, National Institute of AdvancedIndustrial Science and Technology (AIST), Tsukuba.From 1990 to 1991, he was a Visiting Research Asso-ciate with the Swiss Federal Institute of Technology,

Lausanne, Switzerland. He served as a Co-Director of the AIST/IS-CNRS/ST2IJoint French-Japanese Robotics Laboratory at LAAS-CNRS, Toulouse, France,from 2004 to 2008. He has been a Co-Director of the CNRS-AIST Joint RoboticsLaboratory since 2009. His research interests include robot task and motionplanning, modular robotic systems, and humanoid robots.

Documents

IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 2 ...IEEE TRANSACTIONS ON ROBOTICS, VOL. 28, NO. 2, APRIL 2012 427 Fast Humanoid Robot Collision-Free Footstep Planning Using Swept Volume