NUS CS5247

Motion Planning for Motion Planning for Humanoid RobotsHumanoid Robots

Slides from Li Yunzhen and J. Slides from Li Yunzhen and J. KuffnerKuffner

NUS CS5247

Papers:

Footstep placement J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue. Footstep Planning Among Obstacles for Biped Robots. Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2001.

Full-body motionJ. Kuffner, S. Kagami, M. Inaba, and H. Inoue. Dynamically-Stable Motion Planning for Humanoid Robots. Proc. Humanoids, 2000

NUS CS5247

NUS CS5247

NUS CS5247

What is Humanoid Robots & its balance constraints?

area of support

gravity vector

center of mass

projection of mass center

A configuration q is statically-stable if the projection of mass center along the gravity vector lies within the area of support.

Human-like Robots

NUS CS5247

1. Footstep Placement

Goal:In an obstacle-cluttered environment, compute a path from start position to goal region

NUS CS5247

NUS CS5247

1.Footstep Placement—Simplifying Assumption

1. Flat environment floor2. Stationary obstacles of known position and height3. Foot placement only on floor surface (not on obstacles)4. Pre-computed set of footstep placement positions5. Orientation changes allow direction changing; forward and backward motion also

NUS CS5247

Footstep Placement—Simplifying Assumption

• Pre-calculate statically stable motion trajectories for transition between footstep and use intermediate postures to reduce the number of transition trajectories.

• Define intermediate postures Qright, Qleft, that are single foot stable. All transitions are Q1, Qleft, Q2, Qright, Q3, etc.

• Thus, problem is reduced to how to find

feet placements (collision-free)

Path = A sequence of feet placements + trajectories for transition between footstep.

Qright == stable

NUS CS5247

1.Footstep Placement—Overall algorithm

NUS CS5247

1.Footstep Placement—Best First Search

1. Generate successor nodes from lowest cost node

2. Prune nodes that result in collisions

3. Continue until a generated successor node falls in goal region

4. Number of possible footstep placements is branching factor of the tree

Initial Configuration

Red Leaf nodes are pruned from the search

NUS CS5247

1.Footstep Placement—Cost Heuristic

I. Cost of path to current configuration Depth of node in the tree Penalties for orientation changes, backward

steps, and poor foot locations

II. Cost estimate to reach goal Straight-line steps from current to goal Weights favor fewer steps, forward motion weighting factors are empirically-determined D() is path length so far, rho() is penalty for

rotating or going backward, X() is estimate of remaining cost to goal

NUS CS5247

NUS CS5247

NUS CS5247

1.Footstep Placement—Collision Checking

• 2D polygon-polygon intersection test foot vs. attempted position

• 3D polyhedral minimum-distance determination Robot vs. Obstacle

• Lazy collision checking used: as new candidate state is chosen, check for collision

• 15 discrete foot placements, 20 obstacles• Search tree has 6,700 nodes. Brute force search

would require 10^21 nodes for 18 step path.

NUS CS5247

NUS CS5247

NUS CS5247

NUS CS5247

Full Body Motion

Challenge dues to :The high number of degrees of freedomComplex kinematic and dynamic modelsBalance constraints that must be carefully maintained in order to prevent the robot from falling down

NUS CS5247

Full Body Motion

Given two statically-stable configurations, generate a statically stable, collision-free trajectory between them

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

NUS CS5247

Object Manipulation

Task: move a target object its new location. Normally 3 steps:ReachTransferRelease

Decoupled Approach:

Solving the problem by first computing a kinematic path, and subsequently transforming the path into a dynamic trajectory.

State-Space Formulation:

Searching the system state-space directly by reasoning about the possible controls that can be applied.

NUS CS5247

Dynamically Stable Motion Planning Using

RRT’s

NUS CS5247

Dynamically Stable Motion Planning Using

RRT’s

Decoupled Approach: Solve for kinematic path, then Transform path to a dynamically stable path

Assumptions

Environment Model: Assume that the robot has access to a 3D model of the surrounding environment to be used for collision checking. This model could have been obtained through sensors such as laser rangefinding or stereo vision, or given directly in advance.

Initial Posture: The robot is currently balanced in a collision-free, statically-stable configuration supported by either one or both feet.

Goal Posture: A full-body goal configuration that is both collision-free and statically-stable is specified.

The goal posture may be given explicitly by a human operator, or computed via inverse kinematics or other means. (e.g., for extending a limb towards a target location or object).

Support Base: The location of the supporting foot (or feet in the case of dual-leg support) does not change during the planned

motion.

NUS CS5247

Problem Statement Robot: a collection of rigid links, with transforms T_i between links.

Configuration is q_i, set of joint values, dimension = N (number of DOFs)

Obstacles: Modeled as polygonal meshes. C_free is collision free configurations

C_stable = configs where the robot has 1) COM over support polygon of feet, 2) motor joint torques not exceeded in this configuration.

C_valid = C_free ∩ C_stable is set of configs in C_free that are also stable

Trajectory T: interval over [t_0, t_1] where each configuration q(t) ϵ C_valid (collision free and stable)

Dynamic Stability: Given T, assure dynamic stability during transitions between q(t). Post processing step.

Distance Metric RRT will need a distance metric to see how “close”

two configs are With many joints, can take the sum of the absolute

differences between joint sets for two configs Problem: some joints not as important(e.g arm

positions while navigating) Solution: weight each joint as to its importance. Favor links with greater mass and proximity to

torso – ad hoc weightings.

NUS CS5247

Planning Algorithm It is unlikely that a configuration picked at random will be collision-

free and statically-stable Use RRT, but instead of finding random path in C_free, and then

checking for stability we limit search to random set of C_stable configs

Offline we pre-compute a set of stable configurations, and do the random path planning between these configs

If set of configs fails, can compute more configs. Single-Leg: Generate random config, assume right foot is on

ground, check for static stability. If stable add to C_stable, do same with symmetry for left foot.

For Dual-leg support, assume 1 leg on ground, then find Inverse Kinematics solution for other leg in similar ground contact position

NUS CS5247

NUS CS5247

Rapidly-exploring Random Tree (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

NUS CS5247

Full Body Motion—Modified RRT

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

qinit

qgoal

q

NUS CS5247

Full Body Motion—Modified RRT

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

qinit

q

qnear

qgoal

NUS CS5247

Full Body Motion—Modified RRT

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

qinit

q

qnear

qtarget

qgoal

NUS CS5247

qnear

Full Body Motion—Modified RRT

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

qinit

qqtarget

qnew

qgoal

NUS CS5247

Extend(T,q)

qnear NEAREST_NEIGHBOR(q,T);

If NEW_CONFIG(q,qnear,qnew) then

T. add_vertex(qnew);

T. add_edge(qnear,qnew);

if qnew = q then return Reached;

else return Advanced;return Trapped )

NUS CS5247

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;

NUS CS5247

Full Body Motion—Distance Metric RRT will need a distance metric to see how “close” two configs are Don’t want erratic movements from one step to the next. With many joints, can take the sum of the absolute differences

between joint sets for two configs

Encode in the distance metric:

Assign higher relative weights to links with greater mass and proximity to trunk.

A general relative measure of how much the variation of an individual joint parameter affects the overall body posture.

Post Processing the Trajectory Smoothing: Once we have a trajectory, we can

refine it Attempt to connect different configurations along

the path with “straight line” segments in C_valid Typically eliminates many unnatural poses (e.g.

large arm movements) Dynamics filtering: Body is adjusted iteratively in

configurations to achieve dynamic balance If failure, we slow the robot down to find dynamic

stability

NUS CS5247

NUS CS5247

Full Body Motion—Random stable postures

NUS CS5247

Full Body Motion—Experiment

Dynamically-stable planned trajectory for a crouching motion

Performance statistics (N = 100 trials)

NUS CS5247

Full Body Motion

Algorithm for computing dynamically-Stable collision-freetrajectories given full-body posture goals.

Limitations:•Assumes small set of stable configurations•Paths may need to be smoothed