1

Motion and Manipulation

Configuration Space

Outline

• Motion Planning • Configuration Space and Free Space• Free Space Structure and Complexity

Motion Planning

Given• a (2- or 3-dimensional) workspace W,• a robot A of fixed and known shape moving freely

in W,

• a collection of obstacles B={B1,…,Bn} of fixed and known shape and location,

3

Motion Planning Problem

• and an initial and a final placement for A, find a path for A connecting these two placements

along which it avoids collision with the obstacles from B, or report that no such path exists.

4

5

Modeling

• Solution of the motion planning problem in configuration space C: the space of parametric representations of all robot placements.

• Configuration q: unique characterization of robot placement by (minimum) number of parameters.

Degrees of freedom (DOF)

Subset A(q) of W covered by A in q should be uniquely determined.

6

Frames and Transformations

• Fixed world frame, moving frame attached to robot A, initially coinciding.

• Maintain transformation that maps moving frame back to fixed frame.

7

Displacements

• Any displacement in R2 or R3 is the composition of a rotation and a translation with respect to a selected point O (origin).

Configuration Space

Configuration space C=R2

2RW

1A

8

Robot translates: C=R2

Robot translates and rotates: C=R2 x [0,2π)

2RW

2A

Configuration Space

9

2RW

3A

2RC

Configuration Space

10

2RW

4A

22 )2,0[RC

Configuration Space

11

Robot translates: C=R3

Robot translates and rotates: C=R3 x [0,2π) x [0,π] x [0,2π)

3RW

5A

Configuration Space

12

3RW

6A

Configuration Space

Robot translates and rotates: C=R3 x [0,2π) x [0,π]

13

14

Configuration Space

• Manifold: space locally behaves like Rn for some fixed n

• Wrap around for rotational dimensions

• Complex numbers to encode rotational part of SO(2)

• Quaternions to encode rotational part of SO(3)

Configuration Types

• q is free • q is forbidden

AA

iB)q(A

:BB i : BB i

iB)q(A

15

Obstacles in Configuration Space

• Configuration Space Obstacle for Bi:

• Forbidden Space

• Free Space

ii B)q(A|CqCB

i

iCBCB

)B()q(A|CqCB\CC ifree

A

Bi

16

Free Space and Motion Planning

• Motion Planning problem:

goalinit q ionconfigurat goal and q ionconfigurat initial Giventhat such C]1,0[: function continuous a find free

goalinit q)1( and q)0(

qinit

qgoal

17

Configuration Space Obstacles

• Explicit computation often base step in exact methods.

• Motion planning problem not yet solved when CB is known; additional processing of Cfree=C\CB is necessary for finding path.

qinit

qgoal

18

1D Configuration Space Obstacle

W=R, translating robot A=[-1,2], obstacle Bi=[0,4]

Placement of A specified by position of reference point, so

A(q) = A+q = { a+q | a Є A }

Bi

A

19

Minkowski Sum

• Minkowski sum of sets P and Q

•

}Qq,Pp|qp{QP

)A(B

}Bb),A('a|'ab{

}Bb,Aa|)a(b{

}Bb,Aa|ab{

}Bb,Aa,bqa|q{

B)q(A|qCB

i

i

i

i

i

ii

20

1D Configuration Space Obstacle

• Boundary of CBi obtained by tracing reference point while sliding A along Bi.

]5,2[]1,2[]4,0[)A(BCB ii

CBi

A

-ABi

21

2D Configuration Space Obstacle

W=R2, translating convex polygonal robot A, convex polygonal obstacle Bi

• Boundary of CBi obtained by tracing reference point while sliding A along Bi.

A Bi

22

2D Configuration Space Obstacle

Edges of CBi correspond to

• an edge of A touching a vertex of Bi, or

• a vertex of A touching an edge of Bi.23

2D Configuration Space Obstacle

• CBi is convex.

• Edges of CBi share orientations of edges of A and Bi.

24

2D Configuration Space Obstacle

• If convex A has m edges, and convex Bi has n edges, then CBi has complexity O(m+n).

• Computable in O(m+n) time if edges are ordered.

A Bi

25

Exercises on Minkowski Sums in 2D

1. Find a convex translating polygonal robot A with four vertices and a convex polygonal obstacle Bi with four vertices such that

a. the configuration space obstacle CBi has eight vertices,

b. the configuration space obstacle CBi has seven vertices,

c. the configuration space obstacle CBi has as few vertices as possible.

2. Find a convex translating polygonal robot A and a non-convex polygonal obstacle Bi without a hole such that the configuration space obstacle CBi has a hole.

3. What is the Minkowski sum of two disks with radius 1?

4. Consider two line segments of length 1. What is the a. largest area of their Minkowski sum?b. smallest area of their Minkowski sum?

26

Minkowski Sums

• Minkowski sum of a convex and a non-convex polygon can have complexity Ω(mn)!

• Minkowski sum of two non-convex polygons can have complexity Ω(m2n2)!

27

Translational Motions

• Translating polygonal amidst polygonal obstacles in W = point amidst polygonal Minkowski sums in C.

28

Mobility in Configuration Space

Translations only

Isolated point in free space: no translations possible

29

Mobility in Configuration Space

Translations only

Small translations possible, but escape impossible

30

Contact ‘Surfaces’ in a 2D C-Space

• An edge of CBi corresponds to an edge-vertex or vertex-edge contact.

• A vertex of CBi corresponds to a vertex-vertex contact = a double edge-vertex contact.

A

Bi

Bi

A

31

Contact Surfaces in 3D C-Space

Translational motion in 3D: a surface patch corresponds to

• a vertex-facet contact• an edge-edge contact• a facet-vertex contact

Arbitrary motion in 2D: a surface patch corresponds to• a vertex-edge contact• an edge-vertex contact

32

Contact Hypersurfaces

f-Dimensional C:• single contact: (f-1)-dim. boundary feature in CB/Cfree,• double contact: (f-2)-dim. boundary feature in CB/Cfree,• …• f-fold contact: 0-dim. boundary feature in CB/Cfree.

Number of multiple contacts determines complexity of Cfree,and complexity of (exact) motion planning algorithms.

O(nf) bound for constant-complexity robot.

33

Free Space Complexity

• Lower bound constructions: 2D translational motion

• Convex robot: roughly one order of magnitude smaller

• Single cell: roughly one order of magntude smaller

Ω(n2) double contacts

34

Free Space Complexity

• Lower bound constructions: 2D translation and rotation

• Convex robot: roughly one order of magnitude smaller

• Single cell: roughly one order of magntude smaller

Ω(n3) triple contacts

35

Free Space Complexity

• Similar construction in 3D

• Worst-case constructions cage the robot in most placements

• Better bounds on free space complexity under realistic assumptions, such as fatness, low density, unclutteredness, small simple cover complexity

36

Collision Detection

• Recall that the Minkowski sum of sets P and Q is

• Recall that the configuration space obstacle CB of B with respect to translating object A is

which consists of all translations of A in which it intersects B

}Qq,Pp|qp{QP

B)tA(|tB)t(A|t)A(BCB

37

Collision Detection: Intersection

• Objects A and B intersect if and only if

• If A and B intersect then the point v closest to O on the boundary of corresponds to the shortest translation taking A and B apart. The length of v is referred to as the penetration depth. (Similar for the point w closest to O on the boundary of .)

)A(BO

38

)A(B

)B(A

Collision Detection: Non-Intersection

• Clearly, objects A and B do not intersect if and only if

• If A and B do not intersect then the point v closest to O on the boundary of corresponds to the shortest translation that makes A and B intersect. The length of v is the distance between A and B. (Similar for the point w closest to O on the boundary of .)

)A(BO

39

)A(B

)B(A