Page 229 - Sensing, Intelligence, Motion : How Robots and Humans Move in an Unstructured World

204 MOTION PLANNING FOR TWO-DIMENSIONAL ARM MANIPULATORS

q 2
T T

M 3
M 1

S

M 4
M 2

q 1
T T
Figure 5.11 Flattened C-space torus of Figure 5.5.

where sign() takes values +1or −1 depending on the sign of the argument. Then,
coordinates of the endpoints S and T k of each of the M k -lines are as follows:
S
T
T
S
M 1 : S = (θ ,θ ), T 1 = (θ ,θ )
1 2 1 2
T
T
S
S
M 2 : S = (θ ,θ ), T 2 = (θ ,θ − 2π · δ 2 )
1 2 1 2
(5.2)
S
T
S
T
M 3 : S = (θ ,θ ), T 3 = (θ − 2π · δ 1 ,θ )
1 2 1 2
S
S
T
T
M 4 : S = (θ ,θ ), T 4 = (θ − 2π · δ 1 ,θ − 2π · δ 2 )
1 2 1 2
Substituting these into (5.1), coefficients p and q for each of the segments are
found:
T
S
T
T
θ − θ 2 S θ · θ − θ · θ 1 S
1
2
2
2
M 1 : p = , q =
T
T
θ − θ S θ − θ S
1 1 1 1
T S T S T S S
θ − θ − 2π · δ 2 θ · θ − θ · θ + 2π · δ 2 · θ 1
2
2
1
2
2
1
M 2 : p = ,q =
T
T
θ − θ S θ − θ S
1 1 1 1
T
S
S
T
T
θ − θ 2 S θ · θ − θ · θ − 2π · δ 1 · θ 2 S
2
1
1
2
2
M 3 : p = ,q =
T S T S
θ − θ − 2π · δ 1 θ − θ − 2π · δ 1
1 1 1 1
T S T S T S S S
θ − θ − 2π · δ 2 θ · θ − θ · θ − 2π · (δ 1 · θ − δ 2 · θ )
1
2
2
1
2
1
2
2
M 4 : p = ,q =
T S T S
θ − θ − 2π · δ 1 θ − θ − 2π · δ 1
1 1 1 1
(5.3)