Planning forHumanoid Robots

Presented by Irena PashchenkoCS326a, Winter 2004

What is different?

High Dimensionality ( >30 DOF )

Requires careful control to maintainstatic and dynamic stability.

Two problems ( I )

Given two statically-stable configurations, qinit and qgoal, generate a dynamically stable, collision-free trajectory from qinit to qgoal.

Kuffner, et. al, Dynamically-stable Motion Planning for Humanoid Robots, 2000.

Two problems ( II )

In an obstacle-cluttered environment, walk toward a goal position.

Kuffner, et. al, Footstep Planning Among Obstacles for Biped Robots, IROS 2001

Dynamically-Stable Motion Planning for Humanoid Robots, Kuffner, et. al, 2000.

qinit

qnear

qtarget

qnew

q

Restricted Configuration Space

Cvalid = Cstable Cfree

Cstable - set of statically stable

configurations

Given qinit Cvalid and qgoal Cvalid find

find trajectory s.t. (t) Cvalid

Justification – statically stable trajectory can be transformed into dynamically-stable by arbitrarily slowing down the motion.

gravity vector

center of mass X(q)

projection of X(q)

area of support

Random-Exploring Trees (LaValle ’98)

Efficient randomized planner for high-dim. spaces with Algebraic constraints (obstacles)

and Differential constraints (due to

nonholonomy & dynamics)

Biases exploration towards unexplored portions of the space

Rendering of an RRT by James Kuffner

Modified RRT

Build two trees from the start and goal configurations

To obey dynamic constraints – introduce dynamic balance compensator in Connect/Extend phase

qgoal

qinit

Modified RRT - EXTEND( T, q )

Randomly select a collision-free, statically stable configuration q.

qgoal

qinit

q

Modified RRT - EXTEND( T, q )

Select nearest vertex to q already in RRT (qnear).

qgoal

qinit

qnear

q

Modified RRT - EXTEND( T, q )

Make a fixed-distance motion towards q from qnear (qtarget).

qgoal

qinit

qnear

qtarget

q

qtarget

q

Modified RRT - EXTEND( T, q )

Generate qnew by filtering path from qnear to qtarget through dynamic balance compensator and add it to the tree.

qgoal

qinit

qnear

qnew

RRT_CONNECT_STABLE( qinit, qgoal)

1. Ta.init( qinit ); Tb.init( qgoal);

2. For k = 1 to K do

3. qrand RANDOM_STABLE_CONFIG();

4. if not( EXTEND(Ta, qrand = Trapped )

5. if( EXTEND(Tb, qnew) = Reached )

6. return PATH(Ta, Tb);

7. SWAP(Ta, Tb);

8. return Failure;

Distance Metric

To avoid erratic movements from one step to the next.

Encode in the distance metric:

(q,r ) = wi ( qi – ri )

Higher relative weights to links with greater mass and proximity to trunk.

Random Static Postures

It is unlikely that a configuration picked at random will be collision-free and statically-stable.

Solution: generate a set of configurations, and randomly sample this set at run time.

Experimental Results

Dynamically-stable planned trajectory for a crouching motion.

Performance statistics ( N = 100 trials ).

Task DescriptionComputation Time

Crouch near table

min max avg std

176 620 304 133 3-12 min

Footstep Planning Among Obstacles for Biped Robots, Kuffner, et. al, 2001.

Simplifying Assumptions

Stationary obstacles of known position and height

Foot placement only on floor surface (not on obstacles)

Pre-computed set of footstep placement positions (15)

Simplifying Assumptions

Statically-stable transition trajectories, pre-calculated

Statically-stable intermediate postures fewer trajectories

Thus, problem is reduced to best-first search of feet placements (collision-free)

Best-First Search

Initial Configuration Generate successor nodes

from lowest cost node

Prune nodes that result in collisions

Continue until a generated successor node falls in goal region

Cost Heuristic

Cost of path taken so far Depth of node in the tree Penalties for orientation changes and backward

steps

Cost estimate to reach goal Straight-line steps from current to goal

Empirically-determined weighting factors

Obstacle Collision Checking

Two-level:

2D projections of foot and obstacle on walking surface

3D polyhedral minimum-distance determination (V-clip)

Experimental Results

Path Computed in 12 seconds on an800 MHz Pentium II

Future Work

Allow stepping on obstacles

Environments with uneven terrain

Incorporate sensor feedback

Different heuristics

Dynamic stepping motions (including hopping or jumping)

Conclusion

General task involves a high-dimensional space Use random planning [Kuffner 2000] Restrict motion to a finite set [Kuffner 2001]

Randomized planner is more general, and could solve [Kuffner 2001] problem, but takes longer time

Could integrate the two planners to form a more efficient, comprehensive planner Use 2001 planner to move to goal location, and 2000

planner to achieve final posture