PRM adaptations for closed chain systems

1
PRM adaptations for closed chain systems

• A Probabilistic Roadmap Approach for Systems with
  Closed Kinematic Chains, Steven M. LaValle,
  Jeffery H. Yakey, Lydia E. Kavraki.
• A Kinematics-Based Probabilistic Roadmap Method
  for Closed Chain Systems, Li Han, Nancy M. Amato.

2
History

• A Probabilistic Roadmap Approach for Systems with
  Closed Kinematic Chains, Steven M. LaValle,
  Jeffery H. Yakey, Lydia E. Kavraki.
• The first paper applying applying PRM techniques
  to closed kinematic chains.

3
History

• A Kinematics-Based Probabilistic Roadmap Method
  for Closed Chain Systems, Li Han, Nancy M. Amato.
• This extension to the LaValle paper from Amatos
  lab provided speed enhancements sufficient to
  make the earlier techniques practical.

4
The Saga Continues

• In Yogis presentation, we saw Computer Scientist
  Man create a protein blocker to thwart the evil
  plans of Mr. X. But how would CS Man deliver his
  world-saving antidote?
• In this exciting installment, we see how closed
  kinematic chains can be used to represent bipedal
  locomotion, but first this short break for some
  theory

5
Why are closed kinematic chains useful?

• Closed kinematic chains go beyond simple
  kinematic chains to represent closed loop
  molecules, grasping, gaming graphics.

6
Why are closed kinematic chains useful?

• Closed kinematic chains go beyond simple
  kinematic chains to represent closed loop
  molecules, grasping, gaming graphics.

7
Why are closed kinematic chains useful?

• Closed kinematic chains go beyond simple
  kinematic chains to represent closed loop
  molecules, grasping, gaming graphics.

8
Why are closed kinematic chains useful?

• Closed kinematic chains go beyond simple
  kinematic chains to represent closed loop
  molecules, grasping, gaming graphics.

9
Why are closed kinematic chains useful?

• Closed kinematic chains go beyond simple
  kinematic chains to represent closed loop
  molecules, grasping, gaming graphics.

10
Why are they so hard to work with? A PRM
refresher.

• A PRM does two things
• It randomly samples the configuration space to
  create nodes in a graph.
• It uses a fast local planner to connect the
  samples in free space to create the edges.
• The goal is to create a graph that encodes the
  topology of the C-Space sufficiently well that a
  path can be derived, while at the same time
  remaining sufficiently sparse that the resources
  of the computer are not overwhelmed.

11
Why are they so hard to work with? Sampling

• For a rigid body, this is as simple as picking a
  random location from n-dimensions.

12
Why are they so hard to work with? Sampling

• For a kinematic chain, the dimensionality of the
  C-space is higher, but the process is essentially
  the same.
• A chain can be specified as its base
  configuration and its joint variables.

13
Why are they so hard to work with? Sampling

• The base configuration can be specified by the
  Euclidean rigid body transformation from the
  world frame Fw to the body frame FBi. I.E.
• FBi gwbi (Positionwbi, Orientationwbi) ?
  SE(d)
• Where d ? 2, 3 is the dimension of the
  workspace, Positionwbi ? Rd, and Orientationwbi ?
  SO(d), both relative to the world frame.

14
Why are they so hard to work with? Sampling

• The joint variables can be specified as a vector
  ß (ß1, , ßn)
• For revolute joints, ßi ? 0, 2p)
• For prismatic joints, ßi ? R along a directed
  axis.

15
Why are they so hard to work with? Sampling

• The C-Space of a multi-link robot is represented
• C(gwb1, ß1 ,, gwbk, ßk )
• gwb1 ? SE(d), ßi ? Sri X Rpi, i 1 k
• where ri and pi are the number of revolute
  joints and prismatic joints for link i
  respectively.
• Randomly sampling a configuration means picking a
  value for each of the variables.

16
Why are they so hard to work with? Sampling

• For closed kinematic chains, theres an extra
  constraint the first and last links must
  connect.
• In other words,
• f(q) gwe1 gwe2 0

17
Why are they so hard to work with? Sampling

• CFree is the set of all configurations where the
  links are neither in self-collision nor in
  collision with objects in the workspace CClosure
  is the set of all configurations that satisfy the
  additional constraint of closure.
• I.E. CClosure q q ? C and f(q) 0

The chance that picking values at random for
each of the C-Space variables will result in a
configuration that satisfies the closure
constraint is ZERO.
18
Why are they so hard to work with?

• A PRM does two things
• It randomly samples the configuration space to
  create nodes in a graph.
• It uses a fast local planner to connect the
  samples in free space to create the edges.
• Weve seen node creation is hard connecting
  nodes is similarly difficult.

19
Why are they so hard to work with? The local
planner

• Connecting nodes is hard for much the same reason
  that creating them is.
• For a rigid robot, connecting two configurations
  in C-space means the robot has to translate. For
  a closed kinematic chain, connection means a
  translation and/or a reconfiguration of the
  links, while satisfying the closure constraint.

qa ? qb
20
The first solution - Overview
First, break the kinematic chains. Now its
relatively easy to pick configurations at random,
as in the traditional PRM.
21
The first solution - Overview
However, the solutions that are picked are in
CFree rather than CClosure . To move them into
CClosure, attempt to minimize the Euclidian
distance between the links that need to be
connected.
22
Minimizing the gap

• The gap minimization algorithm is iterative
  Create random nearby configurations, and if the
  gap is smaller, take it.
• e is the numeric tolerance
• i is the max. number of search steps
• j is the max. number of consecutive failures.

23
Connecting the nodes

• Once the nodes have been created, they need to be
  connected.
• Neighbors are chosen by calculating the sum of
  the squares of the Euclidean displacement of all
  of the joints ?.
• A randomized descent is used to minimize ?.

24
Connecting the nodes

• The connect vertices algorithm is similar in
  nature to the generate vertex algorithm. New,
  iterative intermediate configurations are
  created. If closure is successful, the
  configuration is kept.
• K is the max. number of consecutive attempts to
  reduce ?.
• ?0 is the distance from the endpoint that
  constitutes success.

25
In practice

• Although this technique was enough to get the
  ball rolling, and prompt other researchers to
  look at the problem, the technique itself was so
  slow as to be impractical. Even in a 2D world
  with a fixed base, each of these examples took
  several hours to compute.

26
Plan B.

• Two observations and a trick were used by the
  Amato group to create a new algorithm
• Only the joint variables determine if a
  configuration is in closure or not.
• A given closure configuration is calculated
  relative to the base the base in turn can be
  placed at many physical locations in SE(3).

27
Observation 1

• Only the joint variables determine if a
  configuration is in closure or not.
• This means that you can create valid closed
  configurations outside the workspace ignore
  obstacles, and work with a fixed, mobile base.
• This is where the trick comes in

28
The trick

• Creating random, closed kinematic chains is hard.
  Creating both random and fixed open kinematic
  chains is much easier.
• The trick is to break a closed chain into two
  open chains. One can be created at random,
  called the active chain. The other, the passive
  chain, is (partially) determined by the fact that
  it shares its endpoints with the endpoints of the
  active chain.

29
Brief diversion - kinematics

• Forward kinematics
• If you know the location of the first link
  position and orientation and you know the
  length and attachment angle of every other link,
  you can create a closed form equation to
  calculate the location of the tip of the last
  link.
• Xf(q)

30
Brief diversion - kinematics

• Inverse Kinematics
• If you know the location of the first link
  position and orientation and you know the
  position and orientation of the end effecter and
  the lengths of the links, what are the attachment
  angles?
• Unfortunately, theres no easy answer. In fact,
  for more than two links, there are several or
  many not easy answers.

31
Brief diversion - kinematics

• Inverse Kinematics
• Fortunately, the angles can be computed, and for
  three- and four- link chains there exist boiler
  plate formulae to calculate them.
• For this discussion, suffice it to say that the
  angles can be computed.
• For more info, see
• http//www.cs.unc.edu/baxter/courses/290/html/img
  0.htm

32
Back to the trick

• The new technique to create a random kinematic
  chain that satisfies the closure constraint is
• Split the closed chain into two open ones.
• For the active chain, pick the attachment angles
  at random. Use forward kinematics to calculate
  the endpoint.
• For the passive chain, apply inverse kinematics
  to the start- and end-points of the active chain
  to determine a set of possible attachment angles.

33
Observation 1 Creating nodes

• Only the joint variables determine if a
  configuration is in closure or not.
• Applying this technique to observation 1, it
  becomes possible to efficiently build not only
  configurations that satisfy the closure
  constraint, but paths between them.
• In other words, before even looking at the
  workspace, the algorithm applies an abridged
  version of PRM to the robot itself and creates
  multiple configurations that avoid self-collision
  and that satisfy the closure constraint.

34
Observation 1 How to connect configurations?

• To build the edges between the configurations, a
  straight-line local planner can be used to
  connect the variables of the active chain,
  worrying only about self-collision.
• Note that weve now efficiently built a roadmap.
  The map doesnt yet capture the C-space, but it
  has already gone a long way toward overcoming the
  limitations of PRM with closed kinematic chains.

35
Observation 2 Capturing the C-Space

• A given closure configuration is calculated
  relative to the base the base in turn can be
  placed at many physical locations in SE(3).
• Now, the algorithm turns to the C-space. Random
  locations are computed in the SE(3) portion of
  C-Space, and copies of the entire roadmap are
  dropped in, including the nodes and the edges.
  Collisions between nodes and edges are detected,
  and offending portions of the map are removed.

36
Observation 2 Capturing the C-Space

• Note that there are now multiple copies of each
  of the closure structures, C1, C2, etc., in the
  roadmap.
• If the chain were to be moved from one C1 to
  another, it would represent a simple rigid body
  translation. As such, edges between identical
  closure structures can again be added by a simple
  straight-line planner.

37
Observation 2 Capturing the C-Space

• Note that there are now multiple copies of each
  of the closure structures, C1, C2, etc., in the
  roadmap.
• If the chain were to be moved from one C1 to
  another, it would represent a simple rigid body
  translation. As such, edges between identical
  closure structures can again be added by a simple
  straight-line planner.

38
Observation 2 Capturing the C-Space

• Voila!

39
Details, details

• OK creating the closure structures isnt quite
  as easy as it appeared. Two cautions
• Its possible to chose a configuration for the
  active chain for which there is no configuration
  of the passive chain that provides closure.
• A link can be in more than one loop.

40
Observation 3 3?!

• A randomly generated value of ?a can result in a
  closure configuration, i.e., there exist passive
  joint variable values satisfying the closure
  constraint, if and only if the end-frame
  configuration of the active chain, gba, falls in
  the workspace of the passive chain, i.e. gba ?
  Wp.
• Prob(closure) Volume(Wp n Wa) / Volume(Wa), in
  SE(d)
• Or, since multiple joint values in the active
  chain can result in the same end-frame
  configuration, use inverse kinematics on the
  active chain to get
• Prob(closure) Volume(g-1(Wp n Wa)) /
  Volume(g-1(Wa)), in C-space
• g-1() is the set of inverse kinematic solutions

41
Observation 3

• Observation 3 can be used to guide the choice of
  a joint partition scheme, i.e. where you split
  the loop.
• However, actually calculating the probability is
  way too complex to be practical.
• Except to note that chains of roughly equal
  length have roughly equal workspace volumes.

42
Details, details

• A link can be in more than one loop.
• In this case, the loops have to be solved
  sequentially.
• The trick is to avoid breaking a previously
  solved loop when solving the next.
• Treat links that are part of a previously solved
  loop as fixed.
• Note that when a loop has more determined joint
  values, it is more constrained and harder to
  close.
• Therefore, solve the loop with the largest number
  of common joints first.

43
Results!

• P S Planar and spatial.
• cfg CC number of nodes and connected
  components.

44
Results!

• P S Planar and spatial.
• cfg CC number of nodes and connected
  components.

45
Conclusion

• Much faster than previous techniques
• Much better than previous techniques (fewer
  connected components)
• Clearly doesnt work well for longer chains,
  indicating the need for further optimization.

46
Future work

• More research is needed to determine the
  limitations of the current algorithm.
• More sophisticated planning techniques can be
  added, e.g. Ariadnes Clew algorithm.
• New applications can be sought such as
  manipulation and regrasp planning.

47
The real conclusion

• In the exciting conclusion to our story, CS Man
  decided it wasnt worth the bother and flew the
  antidote there himself.

48
References

• A Probabilistic Roadmap Approach for Systems with
  Closed Kinematic Chains. Steven M. LaValle,
  Jeffrey Yakey, and Lydia Kavraki, IEEE
  International Conference on Robotics and
  Automation, 1999.
• A Kinematics-Based Probabilistic Roadmap Method
  for Closed Chain Systems. Li Han and Nancy M.
  Amato, Proceedings of the Workshop on Algorithmic
  Foundations of Robotics (WAFR'00), March 2000.
• Fast Numerical Methods for Inverse Kinematics,
  http//www.cs.unc.edu/baxter/courses/290/html/img
  0.htm