The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Liang-Jun Zhang Course Project, COMP 790-072

Dec 13, 2006

C-DIST: Distance Computation for Rigid and Articulated Models in Configuration Space

2The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Distance Metric in Euclidean Space

Euclidean metric

Manhattan metric

Lp Metric

1 1( , )x y 2 2( , )x y

2 22 1 2 1( ) ( )d x x y y

2 1 2 1( ) ( )p ppd x x y y

1 1( , )x y2 2( , )x y

2 1 2 1d x x y y

3The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Distance Metric in Configuration Space (C-space)

Workspace Configuration Space

X

Y

X

Y θ

q1=<x1,y1,θ1>

A q0=<x0,y0,θ0>

?

4The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Motivation

Sampling Based Motion Planning

Only connect the nearest neighbor(s) Evaluate the dispersion of samplesMeasured by a distance metric• [Amato 00 et al], [Kuffner 04], [Plaku and Kavraki

06]

free space

milestone

local path

PRM

5The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Motivation

Penetration depthProximity queryIdentify the easiest way to separate A from BMeasured by a distance metric

B

A

B

A

6The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Challenge

Without rotational motionEuclidean metric and Lp are directly applicable.

With rotational motionChallenge to naturally combine the translational and rotational component

7The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Previous Works

Model-independentDistance metric on SE(3)• No bi-invariant metric exists• [Loncaric 85], [Park 95], [Tchon & Duleba 94]

Weighting rot. and trans. components• Left-invariant, [Park 95]

Model-dependentBased on displacement vector• DISP [Latombe 91] , [LaValle 06]• Object Norm: [Kazerounian Rastegar 92]

Based on trajectory length• Generalized Distance Metric [Zhang 06]

Based swept volume• [Xavier 97], [Choset et al. 2005]

8The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

DISP Metric

0 1 0 1( , ) max ( ) ( )x A

DISP q q x q x q

A

q0

q1

[Latobme 91] [LaValle 06]

9The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Properties

Naturally combine the translational and rotational component

Need not any scalar

Invariant w.r.t to both body and world frames

Independent from the representation of the rotation

Rotation matrix, quaternion

Easily extended for articulated body

Aq0 q1

10The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

DISP Formulation

Can be proved by screw motion

Largely simplify the computationEnough to check the vertices on the convex hull

Theorem: DISP can only be realized by the vertex on its convex hull.

V=3,024 T=1,008

V=311

q0 q1 q0 q1

11The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

DISP Computation

V=3,024 T=1,008

V=311

12The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

DISP Computation Optimization

Walking on the convex hull

Accelerating using Bounding Volume Hierarchy (BVH)

Swept sphere volume (SSV)

13The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Results (Demo)

Triangle Soup

14The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Performance

15The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Extend for Articulated Models

16The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Performance

17The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Applications

Sampling Based Motion Planning

Choose the nearest neighbours

Continuous Collision DetectionReplace the motion bound with displacement bound

Generalized Penetration Depth

18The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Conclusion

A novel algorithm to compute DISP: C-DISTConvex realization theorem

A straightforward theorem

Computation optimizationWalking on the convex hull Accelerating using BVH

Extend for articulated modelsDiscuss the potential applications

19The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Future Work

Geodesic in C-space under the DISP metricOther useful metrics and properties in C-space

Area, VolumeConvexity

20The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Appendix

21The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Outline

Previous WorkDefinitionFormulationOptimizationExtended for Articulated BodiesResultsApplications

22The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Distance for an Object at Two Configurations

d

θ

θ

d + θ ?

d + rθ ?

d

23The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Sampling Based Motion Planning

K-nearest neighborsHow to quantify the ‘near’?Use DISP metric

24The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

Continuous Collision Detection

If Separation Distance > Max Displacement, there is no collision.

AObstacle

q(t)

q(0)

q(1)

25The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL

min({ ( , ) | int( ( )) })ggPD D A B 0q q q

Generalized Penetration Depth

The minimum DISP distance over all possible collision-free configurationsA

B

PDg

Search for nearest collision-free configuration