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 pairs3. PDt over overlapping pairs4. 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

16

The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Results-`gear’: 2T+1R

video

17

The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Results

‘Piano’ ‘World map’

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

19

The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Speedup For Star-shaped roadmap method

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)

23

The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Thanks

Any Questions?

24

The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Appendix

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