Motion Planning for Humanoid Robots

1
Motion Planning for Humanoid Robots

• Slides from Li Yunzhen and J. Kuffner

2
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 motion
• J. Kuffner, S. Kagami, M. Inaba, and H. Inoue.
  Dynamically-Stable Motion Planning for Humanoid
  Robots. Proc. Humanoids, 2000

3
(No Transcript)
4
(No Transcript)
5
What is Humanoid Robots its balance constraints?
Human-like Robots
A configuration q is statically-stable if the
projection of mass center along the gravity
vector lies within the area of support.
6
1. Footstep Placement
Goal In an obstacle-cluttered environment,
compute a path from start position to goal region
7
(No Transcript)
8
1.Footstep PlacementSimplifying Assumption

• Flat environment floor
• Stationary obstacles of known position and height
• Foot placement only on floor surface (not on
  obstacles)
• Pre-computed set of footstep placement positions
• Orientation changes allow direction changing
  forward
• and backward motion also

9
Footstep PlacementSimplifying Assumption
Path A sequence of feet placements
trajectories for transition between footstep.

• 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)

Qright stable
10
1.Footstep PlacementOverall algorithm
11
1.Footstep PlacementBest 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

Red Leaf nodes are pruned from the search
12
1.Footstep PlacementCost Heuristic

• Cost of path to current configuration
• Depth of node in the tree
• Penalties for orientation changes, backward
  steps, and poor foot locations
• 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

13
(No Transcript)
14
(No Transcript)
15
1.Footstep PlacementCollision 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 1021 nodes for 18 step path.

16
(No Transcript)
17
(No Transcript)
18
(No Transcript)
19
Full Body Motion

• Challenge dues to
• The high number of degrees of freedom
• Complex kinematic and dynamic models
• Balance constraints that must be carefully
  maintained in order to prevent the robot from
  falling down

20
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.

21
Object Manipulation

• Task move a target object its new location.
• Normally 3 steps
• Reach
• Transfer
• Release

22
Dynamically Stable Motion Planning Using RRTs

• 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.

23
Dynamically Stable Motion Planning Using RRTs
Decoupled Approach Solve for kinematic path,
then Transform path to a dynamically stable path
24
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.

25
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 n 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.

26
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.

27
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

28
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

29
Full Body MotionModified RRT
q
qinit
qgoal

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

30
Full Body MotionModified RRT
q
qinit
qnear
qgoal

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

31
Full Body MotionModified RRT
q
qtarget
qinit
qnear
qgoal

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

32
Full Body MotionModified RRT
q
qtarget
qinit
qnew
qnear
qgoal

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

33
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 )

34
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

35
Full Body MotionDistance Metric

• RRT will need a distance metric to see how
  close two configs are
• Dont 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.

36
Full Body MotionRandom Statically stable postures

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

• Solution Generate a set of stable
  configurations, and randomly choose one from this
  set then do collision checking.

37
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

38
RRT VS Expansive Sampling
Expansive Space Tree Merit Widening Narrow
passage
RRT Merit Random Sampling, can find a solution
very fast if no narrow pasage.
39
Full Body MotionRandom stable postures
40
Full Body MotionExperiment
Dynamically-stable planned trajectory for a
crouching motion
Performance statistics (N 100 trials)
41
Full Body Motion

• Algorithm for computing dynamically-Stable
  collision-free
• trajectories given full-body posture goals.
• Limitations
• Assumes small set of stable configurations
• Paths may need to be smoothed