Motion Planning for MultiLimbed Robots on Uneven Terrain

1
Motion Planning for Multi-Limbed Robots on
Uneven Terrain
Jean-Claude Latombe Computer Science Department
Stanford University joint work with David Hsu
(NUS), Tim Bretl (UIUC),Kris Hauser (U.
Indiana), and Ruixiang Zhang (Stanford)
2
Examples
HRP-2
LEMUR
ATHLETE
RHex
Only friction at contacts and internal degrees of
freedom are used to achieve equilibrium
3
Motion Planning

• Given a terrain model and a goal location
• Compute a motion path to reach the goal

Sensing
4
Motion Planning

• Given a terrain model and a goal location
• Compute a motion path to reach the goal

5
Motion Planning

• Given a terrain model and a goal location
• Compute a motion path to reach the goal

6
Key Concept Stance

• Set of fixed robot-environment contacts
• Fs space of feasible robot configurations at
  stance s

7
Key Concept Stance

• Set of fixed robot-environment contacts
• Fs space of feasible robot configurations at
  stance s

Feasible motion at 6-stance of ATHLETE
8
Feasible Space at Given Stance
Feasible space
C(q) ? 0
Configuration space
42-D
Contact submanifold
C(q) 0
27-D
9
Transition Space
C
?2
?1
10
Challenge

• High-dimensional configuration space C (11
  LEMUR, 42 for ATHLETE, 36 for HRP-2, 11 for
  Stanford robot)
• Many possible contacts, hence many stances,
  hence many feasible spacesof different
  dimensionalities
• Problem Find a continuous path through these
  spaces

C
11
Related ProblemManipulation Planning
12
Outline of the rest of the talk

• Motion planning in a configuration space of
  fixed, but high dimensionalityProbabilistic
  Roadmap approach
• Planning across many spaces of different
  dimensionalitiesLazy search approach

13
Basic Motion Planning ProblemMove a rigid or
articulated object from a start configuration to
a goal configuration without collision
Piano Movers Problem
14
Configuration Space

• Issues
• Space dimensionality
• Geometric complexity

15
Probabilistic Roadmap Approach
feasible space
16
Probabilistic Roadmap Approach
feasible space
17
Some Applications of PRM Planning
Animation(Kineo)
Automatic robotprogramming (ABB)
Animation(J. Kuffner, CMU)
Spacerobotics(Stanford)
Reconfigurable robots(M. Yim, A. Casal, Parc)
Navigation throughvirtual worlds(UNC)
Design for manufacturing (GM and GE)
18
Connectivity Issue
19
Connectivity Issue
The ß-lookout of a subset X of F is the set of
all configurations in X that see a ß-fraction of
F\X ß-lookout(X) q ? X µ(V(q)\X) ?
ß?µ(F\X)
20
Expansiveness of F
The ß-lookout of a subset X of F is the set of
all configurations in X that see a ß-fraction of
F\X ß-lookout(X) q ? X µ(V(q)\X) ?
ß?µ(F\X)
F is expansive if each one of its subsets X has a
ß-lookout whose volume is at least a?µ(X)
Hsu et al., 1997
21

• Expansiveness only depends on volumetric ratios
• It is not directly related to the dimensionality
  of the configuration space

In 2-D the expansiveness of the free space
can be made arbitrarily poor
22
Probabilistic Completeness of PRM Planning

• Theorem 1 Hsu, Latombe, Motwani, 1997
• Let F be (e,a,b)-expansive, and s and g be two
  configurations in the same component of F.
  BasicPRM(s,g,N) with uniform sampling returns a
  path between s and g with probability converging
  to 1 at an exponential rate as N increases

23
Probabilistic Completeness of PRM Planning

• Theorem 1 Hsu, Latombe, Motwani, 1997
• Let F be (e,a,b)-expansive, and s and g be two
  configurations in the same component of F.
  BasicPRM(s,g,N) with uniform sampling returns a
  path between s and g with probability converging
  to 1 at an exponential rate as N increases
• Theorem 2 Hsu, Latombe, Kurniawati, 2006
• For any e gt 0, any N gt 0, and any g in (0,1,
  there exists ao and bo such that if F is not
  (e,a,b)-expansive for a gt a0 and b gt b0, then
  there exists s and g in the same component of F
  such that BasicPRM(s,g,N) fails to return a path
  with probability greater than g.

24
Probabilistic Completeness of PRM Planning

• Theorem 1 Hsu, Latombe, Motwani, 1997
• Let F be (e,a,b)-expansive, and s and g be two
  configurations in the same component of F.
  BasicPRM(s,g,N) with uniform sampling returns a
  path between s and g with probability converging
  to 1 at an exponential rate as N increases
• Theorem 2 Hsu, Latombe, Kurniawati, 2006
• For any e gt 0, any N gt 0, and any g in (0,1,
  there exists ao and bo such that if F is not
  (e,a,b)-expansive for a gt a0 and b gt b0, then
  there exists s and g in the same component of F
  such that BasicPRM(s,g,N) fails to return a path
  with probability greater than g.

In general, a PRM planner is unable to detect
that no path exists
25
What does the empirical success of PRM planning
tell us?

• It tells us that F is often favorably expansive
  despite its overwhelming algebraic/geometric
  complexity
• Revealing this property might be the most
  important contribution of PRM planning

26
In retrospect, is this property surprising?

• Not really! Narrow passages are unstable
  features under small random perturbations of the
  robot/workspace geometry

Chaudhuri and Koltun (2006) Given a
configuration space of fixed dimensionand with
polyhedral obstacles bounded byn simplices whose
vertices are perturbed according to a normal
distribution of variance s2, a set of randomly
sampled points with size polynomial in n and 1/s
results in a roadmap that captures the
connectivity of the feasible space with high
probability.
27
Most narrow passages in F are intentional
Alpha puzzle

• but it is not easy to intentionally create
  complex narrow passages in F

28
Sampling Strategies are available
to speedup PRM planning

• Workspace-guided
• Local-feature-based
• Deformation-based

29
Outline of the rest of the talk

• Motion planning in a configuration space of
  fixed, but high dimensionalityProbabilistic
  Roadmap approach
• Planning across many spaces of different
  dimensionalitiesLazy search approach

30
Challenge

• High-dimensional configuration space C (11
  LEMUR, 42 for ATHLETE, 36 for HRP-2, 11 for
  Stanford robot)
• Many possible contacts, hence many stances,
  hence many feasible spacesof different
  dimensionalities
• Problem Find a continuous path through these
  spaces

C

• The feasible spaces have different
  dimensionalities.
• Lower-dimensionality spaces create zero-measure
  passages between other spaces.
• The union of the feasible spaces is not
  expansive.

31
Dealing with Varying Dimensionality

• MSPRMPrecompute a stance graph
• Concurrently generate roadmaps in each feasible
  space by iteratively sampling configurations in
  all feasible spaces and all transition
  spacesuntil the aggregate roadmap connects the
  start and goal configurations

32
Dealing with Varying Dimensionality

• MSPRMPrecompute a stance graph
• Concurrently generate roadmaps in each feasible
  space by iteratively sampling configurations in
  all feasible spaces and all transition
  spacesuntil the aggregate roadmap connects the
  start and goal configurations

33
Dealing with Varying Dimensionality

• MSPRMPrecompute a stance graph
• Concurrently generate roadmaps in each feasible
  space by iteratively sampling configurations in
  all feasible spaces and all transition
  spacesuntil the aggregate roadmap connects the
  start and goal configurations

MSPRM is probabilistically complete with
exponential convergence if every feasible space
is expansive.But the huge number of spaces makes
it impractical.
34
Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

35
Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

36
Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

37
Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

Roadmaps from cycle i-1 are incremented at cycle
i
38
Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

Which sequences to choose?
How much time to spend in each space?
39
Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

Which sequences to choose?
How much time to spend in each space?
40
Transitions are Bottlenecks
41
Improving Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

Transitions are bottlenecks? Learn
probabilistic model Q(s,s) of transition
feasibility between two stances s and s and use
this model to prioritize the search of the
stance graph? Sample one configuration in
each new transition space in Gi (at stance
selection step)
42
Learning Transition Feasibility

• Create a large dataset of labeled transitions
• Train a classifier (SVM and NN) Q transition
  Fs ? Fs ? feasible, non-feasible
• ? 80-85 accuracy on test dataset

43
Improving Lazy-Search Planning

• For i 1, 2, ... do
• Stance selection Generate complete sequences of
  adjacent stances and construct Gi ? Gi-1
• Roadmap generation Run MSPRM on Gi
• until a trajectory is found

During roadmap generation - Sample less spaces
that have already been heavily sampled -
Sample less spaces in which the existing
roadmaps are fully connected
44
Results with Climbing Robots
Lemur
Capuchin
45
Results on Humanoid Robot (HRP-2)
46
HRP-2 Stair Climbing
47
Results on Six-Legged Robot (ATHLETE)
48
Unexpected Result

• Transition spaces are usually connected for
  HRP-2, but highly disconnected for ATHLETE
• ? for HRP-2, major speedup with no loss in
  reliability by sampling a single configuration
  per transition space
• Is it due to wider motion ranges of ATHLETE,
  greater risk of self-colliding?

49
Other Issues

• How to efficiently sample configurations?
• How to create natural-looking trajectories?

50
Conclusion

• PRM planning was adapted to compute motions
  across feasible spaces of different
  dimensionalities and solve legged locomotion
  problems on uneven terrain
• Quick and accurate evaluation of motion
  feasibility is critical. More work is needed
• Important topic for future research planning
  dynamic moves (may not be as hard as it looks)
• Current work (Ruixiang Zhang) completing an
  integrated autonomous climbing robot (Capuchin)
  with on-line planning (and exploratory moves)

51
(No Transcript)
52
Sampling Configurations

• Feasibility constraints
• Stance contacts
• Quasi-static equilibrium
• (Self-)collision avoidance
• Sampling method
• Pick a pose of the chassis at random within reach
  from the contacts
• Use IK to close or almost close contacts
• Use iterative Jacobian-based inverse IK to bring
  configuration into feasible space

53
Simultaneous Enforcement of Contact Equilibrium
Constraints

• Jacobian-based Newton-Raphson method
• . Xc fc(Q) XCM fCM(Q)
• .
• .
• Reject sample if torque limits are exceeded or
  (self-)collision occurs

54
Constraint Enforcement for Paths
55
Other Technical Issues

• How to efficient sample configurations?
• How to create natural-looking trajectories?

56
Non-Natural Looking Trajectory
57
Primitive-Biased Sampling

• Idea
• Create a small library of optimized motion
  primitives
• Select a primitive adapted to a given motion step
• Use this primitive to bias sampling during
  trajectory generation

58
Two Motion Primitives
Place a foot
Remove a foot
59
Primitive-Biased Sampling
60
(No Transcript)
61
Using Primitive to Sample Transition
62
Using Primitive to Sample Transition