Configuration Space

for Motion Planning

RSS Lecture 10
M d
Monday, 8MMarchh 2010
Prof. Seth Teller
Siegwart & Nourbahksh S 6.2
(Thanks to Nancy Amato, Rod Brooks, Vijay Kumar,
and Daniela Rus for some of the figures)

Last Time
• Planning for point robots
– Visibility graph method
– Intermittent obstacle contact
• Ad hoc method of handling
non-point robots
– Represent robot as a (2-DOF) disk
– Discretize Cartesian space, conservatively
(Some feasible paths not identified by search)
• Today: “configuration space” methods
– Reason directly in space with dimension = #DOFs
– Transform, solve problem, transform back

1
 Today
• Configuration space
– Intuition
• Preliminaries
– Minkowski sums
– Convexity, convex hulls
• Configuration space
– Definition
– Construction
• Rigid (low-DOF) motion
– Deterministic methods
• Articulated (high-DOF) motion
– Randomized methods

Intuition
• Suppose robot can move only by translating in 2D
• How can it move in the presence of an obstacle?
• How to describe infeasible placements of robot origin?

Obstacle

Robot Robot
origin

2
 Infeasibility Under Translation

Obstacle

Robot Locus of infeasible

placements of
robot origin
Robot origin

What if Robot can also Rotate?

Obstacle

Robot at +/6
orientation

Robot origin

3
 Infeasibility under 3-DOF Motion

Obstacle

Robot at +/6
orientation

Robot origin Locus of infeasible

placements of origin
with robot at +/6
orientation

Configuration Space
For a robot with k total motion DOFs, C-space is a
coordinate system with one dimension per DOF

(Latombe 1991)

In C-space, a robot pose is simply a point!

… and a workspace obstacle is a complex shape

4
Motion Planning Transformation
Workspace C-space
( x, y ) ( x, y, )


C-obst C-obst

obst
obst

obst C-obst C-obst

obst

y
x
Robot Robot
Path is swept volume Path is space curve

Configuration Space Idea

Interaction Constraints
Robot
(difficult to due to obstacle
geometry characterize))
geometry

Transformation to
equivalent problem
in higher dimension

Interaction
Point Transformed
(simple to
geometry characterize) constraints

5
 C-space Summary, Examples
C-obst • Define space with one dimension
C-obst C-obst
per robot motion (or pose) DOF
• Map robot to a point in this space
C-obst • C-space = all robot configurations
• C-obstacle = locus of infeasible
C-obst
configurations due to obstacle
Some example configuration spaces: Molecule with n
Translation + Translation + 3-link fixed-length bonds
rotation in 2D rotation in 3D arm

 


3D C-space 6D C-space 3D C-space 2n-D C-space

(x, y, ) (x, y, z, ψ, φ) () (1, 1, 2, 2, . . . ,  n,  n)

Convexity
• A set S is convex if and only if every line segment
connecting two points in S is contained within S
• Which of these
are convex?

Yes No

No No

6
 Convex Hull of a Set of Points
• Intuition: shrink wrap or rubber band around points

Convex Hull: Formal Definitions

v =  ci . pi, ci ≥ 0,  ci = 1

• Which of these are constructive / algorithmic?

7
 Computing 2D Convex Hull
• Input: set S of N points (xi, yi) in 2D
• Output: polygonal boundary of convex hull of S

S Convex(S)

• How can Convex(S) be computed (efficiently)?

The Leftof Predicate

• Input: three points p, q, r
• Function Leftof (p, q, r) // argument order matters
• Output: 1 iff r is left of directed line pq,
pq otherwise -1
1

r How to implement Leftof()?

1. Compute sign of determinant

q 1 rx ry
p
1 px py
r
1 qx qy
r-p

q 2. Equivalently, find sign of z

p q-p component of (q-p) x (r-p)

8
 Brute Force Solution
Identify point pairs that form edges of Convex(S)
I.e. for each pair p, q ‫ א‬S, if ‫ ׊‬r ‫ א‬S – {p, q}, r lies
left of the directed line pq,
pq emit boundary edge pq

Running time for input of n points? O(n2 x n) = O(n3)

Can do better: O(n2), O(n log n), O(nh), O(n log h) !

Jarvis March Algorithm

pivot = leftmost point in S; i = 0 // leftmost point must be on convex hull
repeat
H[ i ] = pivot // store hull vertices in output point list H[ i ], 0  i < h
endpoint = S[0] // check candidate hull edge [pivot .. endpoint]
for j from 1 to |S|-1
if (Leftof (pivot, endpoint, S[ j ]))
H[1]
endpoint = S[ j ] H[2]
H[0]
pivot = endpoint; i++ S[2]
until endpoint == H[0] …
H[h-1]
S[1]
Outer loop runs h times; S[0]

inner loop does O(n) work

Running time for input
set of n points? O(nh) “Output-sensitive” algorithm.

9
 Minkowski Addition
• Given two sets A,B ‫ א‬Rd, their Minkowski sum,
denoted A ْ B, is the set { a + b | a ‫ א‬A, b ‫ א‬B }
– Result of adding each element of A to each element of B
• If A & B convex, just add vertices & find convex hull:

y y y
AْB

B B

A A

x x x

Computation of C-obstacles
• Inputs: robot polygon R and obstacle shape S
• Output: c-space obstacle c-obstacle(S, R)

y
robot
obstacle obstacle
x
y y
c-obstacle

x x

10
 C-obstacle Computation
1. Reflect robot R about its origin to produce R’
2. Compute Minkowski sum of R’ and obstacle S
y

x R’ R’
R’
obstacle

y
1. R’ R’
2
2. c obstacle
c-obstacle
y
robot
x x
Sanity check: can robot origin enter c-obstacle? No.

C-obstacles with Rotations

How do we compute this object?

11
 Back to Motion Planning
• Given robot and set of obstacles:
– Compute C-space representation of obstacles
– Find path from robot start pose to goal pose (point)

• Unfortunately, we have a rather serious problem:

– We have constructed a representation of the obstacles
– But we need to search a representation of the freespace!

Computational Complexity
• The best deterministic motion planning algorithm
known requires exponential time in the C-space
dimension [Canny 1986]
• D goes up fast – already 6D for a rigid body in
3-space; articulation adds many more DOFs
• Simple obstacles have
complex C-obstacles
p
• Impractical to compute
p
explicit representation
of freespace for robot
with many DOFs
• What to do? Approximate and/or randomize.

12
 Strategies
• Approximate: use regular subdivision of freespace
• Randomize: sample and evaluate C-space poses
• Trade away completeness for gains in efficiency

goal
C-obst

C-obst C-obst

C-obst
C obst

C-obst
start

Example: Exact Decomposition

13
 Approximate Cell Decomposition

• Advantage: recasts complex original problem as

search within space of many, simpler motion plans

Probabilistic Road Maps for Motion

Planning [Kavraki et al. 1996]
C-space
Roadmap
p Construction (Pre-processing)
( p g)
goal
1. Randomly generate robot configurations (nodes)
C-obst - Discard invalid nodes (how?)

C-obst 2. Connect pairs of nodes to form roadmap edges

C-obst - Use simple, deterministic local planner
- Discard invalid edges (how?)
C-obst

C-obst
start
Plan Generation (Q
(Query
ypprocessing)
g)
1. Link start and goal poses into roadmap
2. Find path from start to goal within roadmap
3. Generate motion plan for each edge used
Primitives Required:
1. Method for sampling C-Space points
2. Method for “validating” C-space points and edges

14
 PRMs: Pros and Cons
goal Advantages
C-obst
1. Probabilistically complete
C-obst C-obst 2. Easily applied to high-dimensional C-spaces
3 Support fast queries (w/ enough preprocessing)
3.
C-obst
Many success stories in which PRMs were
C-obst applied to previously intractable problems
start

goal
Disadvantages
C-obst C-obst
PRMs don
don’tt work well for some problems:
– Unlikely to sample nodes in narrow passages
– Hard to connect nodes along constraint surfaces
C-obst C-obst

start

Sampling Around Obstacles:

OBPRM [Amato et al. 1998]
To Navigate Narrow Passages we must sample in them
Most PRM nodes lie where planning is easy, not where it’s hard
PRM Roadmap OBPRM Roadmap
goal goal

C-obst C-obst C-obst

C-obst

C-obst C-obst C-obst C-obst

start start

Idea: Can we sample nodes near C-obstacle surfaces?

• We cannot explicitly construct the C-obstacles, but...
• We do have models of the (workspace) obstacles!

15
Finding Points on C-obstacles
2
3

4 Basic Idea (for workspace obstacle S)

1. Find a point in S’s C-obstacle
(robot placement colliding with S)
2. Select random direction in C-space
3. Find freespace point in that direction
1 4. Find boundaryy point
p between ppoints
C-obst using binary search (collision checks)

Note: we can use more sophisticated

approaches to try to “cover” C-obstacle

Summary
• Introduced drastically simplifying transformation
– Based on two useful geometric constructions
• Enables use of familiar techniques…
techniques
– Discretization
– Random sampling
– Bisection
– Graph search
• … To solve high-dimensional
high dimensional motion planning

• We’ll use these ideas in Lab 6

16