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
