The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Fast C-obstacle Query Computationfor Motion Planning
Liang-Jun Zhang12/13/2005
Liang-Jun Zhang1 Young J. Kim2 Gokul Varadhan1 Dinesh Manocha1
1: University of North Carolina - Chapel Hill, USA2: Ewha Womans University, Korea,
http://gamma.cs.unc.edu/cobstacle
2
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Configuration space
Free space
C-obstacle
Do they intersect? Is its configuration in C-obstacle or free space?
• Free space and C-obstacle
Robot
Obstacle
3
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
C-obstacle query in C-space
• C-obstacle query♦ Does a primitive lie completely
inside C-obstacle?♦ The primitive usually is a cell.
• Goal♦ Design an efficient C-obstacle query algorithm
Free spaceC-obstacle
4
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Motivation- An important query for Motion Planning
• Cell Decomposition Method ♦ Label Cells as FULL and EMPTY
• Complete motion planning♦ Able to find a path or report path non-existence
EMPTY
init
goal
FULL
5
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Previous work
• Computing the boundary of C-obstacle♦ Exponential complexity [Sacks 99, Sharir
97]♦ Degeneracy and floating point error
• Contact surface constraints♦ [Latombe 91, Zhu 91]♦ Complexity of contact surface enumeration♦ To deal with non-linear contact surfaces
6
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Interpretation of C-obstacle query
C-obstacle
Free space
•Does the cell lie inside C-obstacle?
• Do the robot and obstacle intersect at all configurations?
•Can the robot ‘escape’ from the obstacle at some moment?
Obstacle
7
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Algorithm overview• Penetration Depth
♦ How much does the robot penetrate into the obstacle at a configuration q ?
• Bounding Motion ♦ How much motion the robot can undergo,
when its configuration changes from q but within the query primitive?
• Query criterion♦ If Penetration Depth > Bounding
Motion the robot can not escape
♦ the query primitive lies inside C-obstacle
q
A(q)
8
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Translational Penetration Depth: PDt
• Minimum translation to separate A, B ♦ [Dobkin 93, Agarwal 00,
Bergen 01, Kim 02]
• PDt: not applicable♦ The robot is allowed to
both translate and rotate. ♦ Undergoing rotation, A
may ‘escape’ from B easier.
B
A
A’
A
B
9
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Generalized Penetration Depth:
PDg
• Consider both translation and rotation♦ [Zhang, Kim, Varadhan, Manocha: UNC-CS
TR05]
• Difficult for non-convex objects
• Convex A, B
PDg(A,B)=PDt(A,B)
10
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Algorithm-Lower bound on
PDg
1. Convex decomposition 2. Eliminate non-overlapping pairs
3. PDt over overlapping pairs
4. LB(PDg) = Max over all PDts
A
B
A2
A1
B1
B2
A2
A1
B2
B1
11
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
C-obstacle Query Criterion
If Lower Bound (PDg(A(qa), B))> Bounding Motion,
the cell C is in C-obstacle.
A(qa): set A’s config as qa
aq
C
12
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Upper bound of Motion for line segment
•
♦ Configurations qa and qb
• [Schwarzer,Saha,Latombe 04]
Max trajectory length over points on the moving
robot
bqaq,( , )a bq qA
( )aA q
( )bA q
13
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Upper bound of Motion for cell
,( , ) max ( , )a bq qA C A
Any diagonal line segment yields maximum bounding motion.
aq
cqbqqa is the center of the cell Cqb is any point on the boundary of the cell.
C
,( , ) max ( , )a bq qA C A
,( , )a cq qA
14
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
C-obstacle Query Criterion
If Lower Bound (PDg(A(qa), B))>
the cell C is in C-obstacle.
A(qa): set A’s config as qa
aqcqbq
C
,( , )a cq qA
15
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Application
• Star-shaped roadmap: a complete motion planning approach♦ [Varadhan and Manocha 05]
• To identify cells which lie inside C-obstacle♦ No subdivisions are applied for them
18
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Effective and Performance
• Query timing: 0.04ms to 0.12 ms
Culled C-obstacle Cells• Cell Culling Ratio =
All C-obstacle Cells
20
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Conclusion
• A fast C-obstacle query algorithm for rigid robots
• Based on generalized penetration depth and bounding motion computation. ♦ Need not explicit computation of the boundary
of free space.♦ Robust and efficient
• Applied for accelerating a complete motion planning approach for 2D rigid robot.
21
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Ongoing and Future work
• A Simple Path Non-Existence Algorithm for low DOF robots ♦ [L. Zhang, Y.J. Kim, D. Manocha]
WAFR2006
• Apply for 3D rigid robots• Handle articulated robots
22
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Acknowledgements
• Army Research Office• DARPA/REDCOM• NSF• ONR• Intel Corporation• KRF, STAR program of MOST,
Ewha SMBA consortium, the ITRC program (Korea)
25
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Generalized Penetration Depth:
PDg
• Consider both translation and rotation♦ [Zhang, Kim, Varadhan, Manocha et al. 05]
• Trajectory length Separating path
Robot
Obstacle
Robot
26
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
• PDg
• Difficult for non-convex objects• Convex A, B
PDg(A,B)=PDt(A,B)
MIN over all possible separating paths MAX of the trajectory length over all on the moving robot
Generalized Penetration Depth:
PDg
27
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Upper bound of Motion for line segment
• [Schwarzer,Saha,Latombe 04] ♦ Max trajectory length
over points on the moving robot
• ♦ The weighted sum of difference for x, y,
components between qa and qb
,
, , , , , ,
( , )a bq q
a x b x a y b y a r b
A
q q q q R q q
x
ybq
aq
R: ‘radius’ of the object
,( , )a bq qA