4.2 Defining the Configuration Space

This section defines the manifolds that arise from the transformations of Chapter

3. If the robot has n degrees of freedom, the set of transformations is usually a manifold of dimension n. This manifold is called the configuration space of the robot, and its name is often shortened to C-space. In this book, the C-space may be considered as a special state space. To solve a motion planning problem, algorithms must conduct a search in the C-space. The C-space provides a powerful abstraction that converts the complicated models and transformations of Chapter 3 into the general problem of computing a path that traverses a manifold. By developing algorithms directly for this purpose, they apply to a wide variety of different kinds of robots and transformations. In Section 4.3 the problem will be complicated by bringing obstacles into the configuration space, but in Section 4.2 there will be no obstacles.

• • 1. 2D Rigid Bodies: SE(2)

Section 3.2.2 expressed how to transform a rigid body in R2 by a homogeneous transformation matrix, T , given by (3.35). The task in this chapter is to char- acterize the set of all possible rigid-body transformations. Which manifold will

this be? Here is the answer and brief explanation. Since any xt, yt R can be selected for translation, this alone yields a manifold M1 = R2. Independently, any rotation, θ [0, 2π), can be applied. Since 2π yields the same rotation as 0, they can be identified, which makes the set of 2D rotations into a manifold, M2 = S1. To obtain the manifold that corresponds to all rigid-body motions, simply take

∈

∈

2

C = M × M = R

1

1 2

of cylinder.

× S . The answer to the question is that the C-space is a kind

Now we give a more detailed technical argument. The main purpose is that such a simple, intuitive argument will not work for the 3D case. Our approach is to introduce some of the technical machinery here for the 2D case, which is easier to understand, and then extend it to the 3D case in Section 4.2.2.

Matrix groups The first step is to consider the set of transformations as a group, in addition to a topological space.8 We now derive several important groups from sets of matrices, ultimately leading to SO(n), the group of n n rotation matrices, which is very important for motion planning. The set of all nonsingular

×

n × n real-valued matrices is called the general linear group, denoted by GL(n), with respect to matrix multiplication. Each matrix A ∈ GL(n) has an inverse A−1 ∈ GL(n), which when multiplied yields the identity matrix, AA−1 = I. The

8The groups considered in this section are actually Lie groups because they are smooth manifolds [63]. We will not use that name here, however, because the notion of a smooth structure has not yet been defined. Readers familiar with Lie groups, however, will recognize most of the coming concepts. Some details on Lie groups appear later in Sections 15.4.3 and 15.5.1.

matrices must be nonsingular for the same reason that 0 was removed from Q. The analog of division by zero for matrix algebra is the inability to invert a singular matrix.

Many interesting groups can be formed from one group, G1, by removing some elements to obtain a subgroup, G2. To be a subgroup, G2 must be a subset of G1 and satisfy the group axioms. We will arrive at the set of rotation matrices by constructing subgroups. One important subgroup of GL(n) is the orthogonal group, O(n), which is the set of all matrices A GL(n) for which AA = I, in which AT denotes the matrix transpose of A. These matrices have orthogonal columns (the inner product of any pair is zero) and the determinant is always 1 or

T

∈

T

−1. Thus, note that AA takes the inner product of every pair of columns. If the

columns are different, the result must be 0; if they are the same, the result is 1 because AAT = I. The special orthogonal group, SO(n), is the subgroup of O(n) in which every matrix has determinant 1. Another name for SO(n) is the group of n-dimensional rotation matrices.

A chain of groups, SO(n) O(n) GL(n), has been described in which denotes “a subgroup of.” Each group can also be considered as a topological space. The set of all n n matrices (which is not a group with respect to multiplication) with real-valued entries is homeomorphic to Rn_2 because n2 entries in the matrix can be independently chosen. For GL(n), singular matrices are removed, but an n2-dimensional manifold is nevertheless obtained. For O(n), the expression AA_T = I corresponds to n2 algebraic equations that have to be satisfied. This should substantially drop the dimension. Note, however, that many of the equations are

×

≤ ≤ ≤

redundant (pick your favorite value for n, multiply the matrices, and see what happens). There are only (n) ways (pairwise combinations) to take the inner product of pairs of columns, and there are n equations that require the magnitude of each column to be 1. This yields a total of n(n + 1)/2 independent equations. Each independent equation drops the manifold dimension by one, and the resulting dimension of O(n) is n2 n(n + 1)/2 = n(n 1)/2, which is easily remembered as (n). To obtain SO(n), the constraint det A = 1 is added, which eliminates exactly half of the elements of O(n) but keeps the dimension the same.

2

2

− −

Example 4.12 (Matrix Subgroups) It is helpful to illustrate the concepts for

n = 2. The set of all 2 × 2 matrices is

（（a b＼ I a, b, c, d R1 , (4.10) which is homeomorphic to R4. The gIroup GL(2) is formed from the set of all

c d

II ∈

nonsingular 2 2 matrices, which introduces the constraint that ad bc = 0. The set of singular matrices forms a 3D manifold with boundary in R4, but all other elements of R4 are in GL(2); therefore, GL(2) is a 4D manifold.

× − /

Next, the constraint AAT = I is enforced to obtain O(2). This becomes

（a b＼ （a c＼ = （1 0＼ , (4.11)

c d

b d

0 1

which directly yields four algebraic equations:

Note that (4.14) is redundant. There are two kinds of equations. One equation, given by (4.13), forces the inner product of the columns to be 0. There is only one because (n) = 1 for n = 2. Two other constraints, (4.12) and (4.15), force the rows to be unit vectors. There are two because n = 2. The resulting dimension of the manifold is (n) = 1 because we started with R4 and lost three dimensions from (4.12), (4.13), and (4.15). What does this manifold look like? Imagine that there are two different two-dimensional unit vectors, (a, b) and (c, d). Any value can be chosen for (a, b) as long as a2 + b2 = 1. This looks like S1, but the inner product of (a, b) and (c, d) must also be 0. Therefore, for each value of (a, b), there are two choices for c and d: 1) c = b and d = a, or 2) c = b and d = a. It appears that there are two circles! The manifold is S1 S1, in which denotes the union of disjoint sets. Note that this manifold is not connected because no path exists from one circle to the other.

2

2

⊔ ⊔

− −

The final step is to require that det A = ad bc = 1, to obtain SO(2), the set of all 2D rotation matrices. Without this condition, there would be matrices that produce a rotated mirror image of the rigid body. The constraint simply forces the choice for c and d to be c = b and a = d. This throws away one of the circles from O(2), to obtain a single circle for SO(2). We have finally obtained what you

−

−

already knew: SO(2) is homeomorphic to S1. The circle can be parameterized

using polar coordinates to obtain the standard 2D rotation matrix, (3.31), given in Section 3.2.2. ．

Special Euclidean group Now that the group of rotations, SO(n), is charac-

terized, the next step is to allow both rotations and translations. This corresponds to the set of all (n + 1) × (n + 1) transformation matrices of the form

（（R v＼ I R SO(n) and v R_n_1 . (4.16) This should look like a generalizaI tion of (3.52) and (3.56), which were for n = 2

0 1

II ∈ ∈

and n = 3, respectively. The R part of the matrix achieves rotation of an n- dimensional body in Rn, and the v part achieves translation of the same body. The result is a group, SE(n), which is called the special Euclidean group. As a topological space, SE(n) is homeomorphic to Rn SO(n), because the rotation

×

matrix and translation vectors may be chosen independently. In the case of n = 2, this means SE(2) is homeomorphic to R2 × S1, which verifies what was stated

Figure 4.8: A planning algorithm may have to cross the identification boundary to find a solution path.

at the beginning of this section. Thus, the C-space of a 2D rigid body that can translate and rotate in the plane is

C = R × S . (4.17)

2 1

To be more precise, ∼= should be used in the place of = to indicate that could be any space homeomorphic to R2 S1; however, this notation will mostly be avoided.

×

C

Interpreting the C-space It is important to consider the topological impli- cations of . Since S1 is multiply connected, R S1 and R2 S1 are multiply connected. It is difficult to visualize because it is a 3D manifold; however,

C

C × ×

there is a nice interpretation using identification. Start with the open unit cube, (0, 1)3 R3. Include the boundary points of the form (x, y, 0) and (x, y, 1), and make the identification (x, y, 0) (x, y, 1) for all x, y (0, 1). This means that when traveling in the x and y directions, there is a “frontier” to the C-space; however, traveling in the z direction causes a wraparound.

∼ ∈

⊂

It is very important for a motion planning algorithm to understand that this wraparound exists. For example, consider R S1 because it is easier to visualize. Imagine a path planning problem for which = R S1, as depicted in Figure

C ×

×

4.8. Suppose the top and bottom are identified to make a cylinder, and there is

an obstacle across the middle. Suppose the task is to find a path from qI to qG. If the top and bottom were not identified, then it would not be possible to connect qI to qG; however, if the algorithm realizes it was given a cylinder, the task is straightforward. In general, it is very important to understand the topology of ; otherwise, potential solutions will be lost.

C

The next section addresses SE(n) for n = 3. The main difficulty is determining the topology of SO(3). At least we do not have to consider n > 3 in this book.

• • 1. 3D Rigid Bodies: SE(3)

One might expect that defining for a 3D rigid body is an obvious extension of the 2D case; however, 3D rotations are significantly more complicated. The resulting

C

C-space will be a six-dimensional manifold, = R3 RP . Three dimensions come from translation and three more come from rotation.

3

C ×

The main quest in this section is to determine the topology of SO(3). In Section 3.2.3, yaw, pitch, and roll were used to generate rotation matrices. These angles are convenient for visualization, performing transformations in software, and also for deriving the DH parameters. However, these were concerned with applying a single rotation, whereas the current problem is to characterize the set of all rotations. It is possible to use α, β, and γ to parameterize the set of rotations, but it causes serious troubles. There are some cases in which nonzero angles yield the identity rotation matrix, which is equivalent to α = β = γ = 0. There are also cases in which a continuum of values for yaw, pitch, and roll angles yield the same rotation matrix. These problems destroy the topology, which causes both theoretical and practical difficulties in motion planning.

Consider applying the matrix group concepts from Section 4.2.1. The general linear group GL(3) is homeomorphic to R9. The orthogonal group, O(3), is de- termined by imposing the constraint AAT = I. There are (3) = 3 independent

2

equations that require distinct columns to be orthogonal, and three independent equations that force the magnitude of each column to be 1. This means that O(3) has three dimensions, which matches our intuition since there were three rotation parameters in Section 3.2.3. To obtain SO(3), the last constraint, det A = 1, is added. Recall from Example 4.12 that SO(2) consists of two circles, and the

constraint det A = 1 selects one of them. In the case of O(3), there are two three-spheres, S3 S3, and det A = 1 selects one of them. However, there is one additional complication: Antipodal points on these spheres generate the same ro-

⊔

tation matrix. This will be seen shortly when quaternions are used to parameterize

SO(3).

Using complex numbers to represent SO(2) Before introducing quaternions to parameterize 3D rotations, consider using complex numbers to parameterize 2D rotations. Let the term unit complex number refer to any complex number, a + bi, for which a2 + b2 = 1.

The set of all unit complex numbers forms a group under multiplication. It will be seen that it is “the same” group as SO(2). This idea needs to be made more precise. Two groups, G and H, are considered “the same” if they are isomorphic, which means that there exists a bijective function f : G H such that for all a, b G, f(a) f(b) = f(a b). This means that we can perform some calculations in G, map the result to H, perform more calculations, and map back to G without any trouble. The sets G and H are just two alternative ways to express the same group.

∈ ◦ ◦

→

The unit complex numbers and SO(2) are isomorphic. To see this clearly, recall that complex numbers can be represented in polar form as reiθ ; a unit complex number is simply eiθ . A bijective mapping can be made between 2D rotation matrices and unit complex numbers by letting eiθ correspond to the rotation matrix (3.31).

If complex numbers are used to represent rotations, it is important that they behave algebraically in the same way. If two rotations are combined, the matrices are multiplied. The equivalent operation is multiplication of complex numbers. Suppose that a 2D robot is rotated by θ1, followed by θ2. In polar form, the complex numbers are multiplied to yield eiθ_1 e_iθ_2 = e_i(θ_1+θ_2), which clearly represents a rotation of θ1 + θ2. If the unit complex number is represented in Cartesian form, then the rotations corresponding to a1 + b1i and a2 + b2i are combined to obtain (a1a2 b1b2)+(a1b2 + a2b1)i. Note that here we have not used complex numbers to express the solution to a polynomial equation, which is their more popular use; we simply borrowed their nice algebraic properties. At any time, a complex number a + bi can be converted into the equivalent rotation matrix

−

R(a, b) = （a −b＼ . (4.18) Recall that only one independent parameter needs to be specified because a2 +

b a

b2 = 1. Hence, it appears that the set of unit complex numbers is the same

manifold as SO(2), which is the circle S1 (recall, that “same” means in the sense of homeomorphism).

Quaternions The manner in which complex numbers were used to represent 2D rotations will now be adapted to using quaternions to represent 3D rotations. Let H represent the set of quaternions, in which each quaternion, h H, is represented

∈

∈

as h = a+bi+cj +dk, and a, b, c, d R. A quaternion can be considered as a four-

dimensional vector. The symbols i, j, and k are used to denote three “imaginary”

components of the quaternion. The following relationships are defined: i2 = j2 =

k2 = ijk = −1, from which it follows that ij = k, jk = i, and ki = j. Using

these, multiplication of two quaternions, h1 = a1 + b1i + c1j + d1k and h2 =

a2 + b2i+ c2j + d2k, can be derived to obtain h1 · h2 = a3 + b3i+ c3j + d3k, in which

a3 = a1a2 − b1b2 − c1c2 − d1d2

b3 = a1b2 + a2b1 + c1d2 − c2d1 c3 = a1c2 + a2c1 + b2d1 − b1d2 d3 = a1d2 + a2d1 + b1c2 − b2c1.

(4.19)

Using this operation, it can be shown that H is a group with respect to quaternion multiplication. Note, however, that the multiplication is not commutative! This is also true of 3D rotations; there must be a good reason.

For convenience, quaternion multiplication can be expressed in terms of vector multiplications, a dot product, and a cross product. Let v = [b c d] be a three-

dimensional vector that represents the final three quaternion components. The first component of h1 · h2 is a1a2 − v1 · v2. The final three components are given

by the three-dimensional vector a1v2 + a2v1 + v1 v2.

×

In the same way that unit complex numbers were needed for SO(2), unit quater- nions are needed for SO(3), which means that H is restricted to quaternions for

Figure 4.9: Any 3D rotation can be considered as a rotation by an angle θ about the axis given by the unit direction vector v = [v1 v2 v3].

Figure 4.10: There are two ways to encode the same rotation.

which a2 + b2 + c2 + d2 = 1. Note that this forms a subgroup because the multi- plication of unit quaternions yields a unit quaternion, and the other group axioms hold.

The next step is to describe a mapping from unit quaternions to SO(3). Let the unit quaternion h = a + bi + cj + dk map to the matrix

R(h) =

2(a2 + b2) − 1 2(bc − ad) 2(bd + ac)

2(bc + ad) 2(a2 + c2) − 1 2(cd − ab)



2 2

 , (4.20)

 2(bd − ac) 2(cd + ab) 2(a + d ) − 1

which can be verified as orthogonal and det R(h) = 1. Therefore, it belongs to SO(3). It is not shown here, but it conveniently turns out that h represents the rotation shown in Figure 4.9, by making the assignment

θ

（

h = cos + v1

2

θ

sin i + v2

＼ （

2

θ

sin j + v3

＼ （

2

θ

sin k. (4.21)

＼

2

Unfortunately, this representation is not unique. It can be verified in (4.20) that R(h) = R( h). A nice geometric interpretation is given in Figure 4.10. The quaternions h and h represent the same rotation because a rotation of θ about the direction v is equivalent to a rotation of 2π θ about the direction v. Consider the quaternion representation of the second expression of rotation with respect to the first. The real part is

− −

−

−

cos （ 2π − θ ＼ = cos （π − θ ＼ = − cos （ θ ＼ = −a. (4.22)

2 2 2

The i, j, and k components are

− v sin （ 2 π − θ ＼ = −v sin （π − θ ＼ = −v sin （ θ ＼ = [−b − c − d]. (4.23)

2

2

2

The quaternion h has been constructed. Thus, h and h represent the same rotation. Luckily, this is the only problem, and the mapping given by (4.20) is two-to-one from the set of unit quaternions to SO(3).

− −

This can be fixed by the identification trick. Note that the set of unit quater- nions is homeomorphic to S3 because of the constraint a2 + b2 + c2 + d2 = 1. The algebraic properties of quaternions are not relevant at this point. Just imagine each h as an element of R4, and the constraint a2 + b2 + c2 + d2 = 1 forces the points to lie on S3. Using identification, declare h h for all unit quaternions. This means that the antipodal points of S3 are identified. Recall from the end

∼ −

of Section 4.1.2 that when antipodal points are identified, RPn ∼= Sn/ ∼. Hence,

SO(3) ∼= RP3, which can be considered as the set of all lines through the origin

of R4, but this is hard to visualize. The representation of RP2 in Figure 4.5 can be extended to RP3. Start with (0, 1)3 R3, and make three different kinds of identifications, one for each pair of opposite cube faces, and add all of the

⊂

points to the manifold. For each kind of identification a twist needs to be made (without the twist, T3 would be obtained). For example, in the z direction, let

(x, y, 0) ∼ (1 − x, 1 − y, 1) for all x, y ∈ [0, 1].

One way to force uniqueness of rotations is to require staying in the “upper half” of S3. For example, require that a 0, as long as the boundary case of a = 0 is handled properly because of antipodal points at the equator of S3. If a = 0, then require that b 0. However, if a = b = 0, then require that c 0

≥

≥ ≥

because points such as (0, 0, 1, 0) and (0, 0, 1, 0) are the same rotation. Finally, if a = b = c = 0, then only d = 1 is allowed. If such restrictions are made, it is

−

important, however, to remember the connectivity of RP3. If a path travels across the equator of S3, it must be mapped to the appropriate place in the “northern hemisphere.” At the instant it hits the equator, it must move to the antipodal

point. These concepts are much easier to visualize if you remove a dimension and imagine them for S2 ⊂ R3, as described at the end of Section 4.1.2.

Using quaternion multiplication The representation of rotations boiled down to picking points on S3 and respecting the fact that antipodal points give the same element of SO(3). In a sense, this has nothing to do with the algebraic properties

of quaternions. It merely means that SO(3) can be parameterized by picking points in S3, just like SO(2) was parameterized by picking points in S1 (ignoring the antipodal identification problem for SO(3)).

However, one important reason why the quaternion arithmetic was introduced is that the group of unit quaternions with h and h identified is also isomorphic to SO(3). This means that a sequence of rotations can be multiplied together using quaternion multiplication instead of matrix multiplication. This is important because fewer operations are required for quaternion multiplication in comparison to matrix multiplication. At any point, (4.20) can be used to convert the result

−

back into a matrix; however, this is not even necessary. It turns out that a point in the world, (x, y, z) R3, can be transformed by directly using quaternion arithmetic. An analog to the complex conjugate from complex numbers is needed.

∈

For any h = a + bi + cj + dk H, let h∗ = a bi cj dk be its conjugate. For any point (x, y, z) R3, let p H be the quaternion 0 + xi + yj + zk. It can be shown (with a lot of algebra) that the rotated point (x, y, z) is given by h p h∗.

· ·

∈ ∈

∈ − − −

The i, j, k components of the resulting quaternion are new coordinates for the transformed point. It is equivalent to having transformed (x, y, z) with the matrix R(h).

Finding quaternion parameters from a rotation matrix Recall from Sec- tion 3.2.3 that given a rotation matrix (3.43), the yaw, pitch, and roll parameters could be directly determined using the atan2 function. It turns out that the quaternion representation can also be determined directly from the matrix. This is the inverse of the function in (4.20).9

For a given rotation matrix (3.43), the quaternion parameters h = a+bi+cj+dk can be computed as follows [210]. The first component is

and if a /= 0, then

a = 1 √r + r + r + 1, (4.24)

22 33

2

11

and

r r

b = , (4.25)

32 − 23

4a

r r

13 − 31

c = , (4.26)

4a

r r

21 − 12

d = . (4.27)

4a

If a = 0, then the previously mentioned equator problem occurs. In this case,

r13r12

b = , (4.28)

12

13

12

23

13

23

/r2 r2

• r2 r2

• r2 r2

r12r23

c = , (4.29)

/r2 r2

• r2 r2

• r2 r2

and

12 13

12 23

13 23

r13r23

d = . (4.30)

12

13

12

23

13

23

/r2 r2

• r2 r2

• r2 r2

This method fails if r12 = r23 = 0 or r13 = r23 = 0 or r12 = r23 = 0. These correspond precisely to the cases in which the rotation matrix is a yaw, (3.39), pitch, (3.40), or roll, (3.41), which can be detected in advance.

9Since that function was two-to-one, it is technically not an inverse until the quaternions are

restricted to the upper hemisphere, as described previously.

Special Euclidean group Now that the complicated part of representing SO(3)

has been handled, the representation of SE(3) is straightforward. The general form of a matrix in SE(3) is given by (4.16), in which R ∈ SO(3) and v ∈ R3. Since SO(3) ∼= RP3, and translations can be chosen independently, the resulting C-space

for a rigid body that rotates and translates in R3 is

C = R × RP , (4.31)

3 3

which is a six-dimensional manifold. As expected, the dimension of is exactly the number of degrees of freedom of a free-floating body in space.

C

• • 1. Chains and Trees of Bodies

If there are multiple bodies that are allowed to move independently, then their C-spaces can be combined using Cartesian products. Let Ci denote the C-space of

2

Ai. If there are n free-floating bodies in W = R or W = R3, then

C = C1 × C2 × · · · × Cn. (4.32)

If the bodies are attached to form a kinematic chain or kinematic tree, then each C-space must be considered on a case-by-case basis. There is no general rule that simplifies the process. One thing to generally be careful about is that the full range of motion might not be possible for typical joints. For example, a revolute joint might not be able to swing all of the way around to enable any θ [0, 2π).

∈

If θ cannot wind around S1, then the C-space for this joint is homeomorphic to R instead of S1. A similar situation occurs for a spherical joint. A typical ball joint cannot achieve any orientation in SO(3) due to mechanical obstructions. In this case, the C-space is not RP3 because part of SO(3) is missing.

Another complication is that the DH parameterization of Section 3.3.2 is de-

signed to facilitate the assignment of coordinate frames and computation of trans- formations, but it neglects considerations of topology. For example, a common approach to representing a spherical robot wrist is to make three zero-length links that each behave as a revolute joint. If the range of motion is limited, this might not cause problems, but in general the problems would be similar to using yaw, pitch, and roll to represent SO(3). There may be multiple ways to express the same arm configuration.

Several examples are given below to help in determining C-spaces for chains and trees of bodies. Suppose = R2, and there is a chain of n bodies that are attached by revolute joints. Suppose that the first joint is capable of rotation only

W

about a fixed point (e.g., it spins around a nail). If each joint has the full range of motion θi ∈ [0, 2π), the C-space is

C = S1 × S1 × · · · × S1 = Tn. (4.33)

However, if each joint is restricted to θi ∈ (−π/2, π/2), then C = Rn. If any transformation in SE(2) can be applied to A1, then an additional R2 is needed.

In the case of restricted joint motions, this yields Rn+2. If the joints can achieve any orientation, then = R2 Tn. If there are prismatic joints, then each joint contributes R to the C-space.

C ×

Recall from Figure 3.12 that for = R3 there are six different kinds of joints. The cases of revolute and prismatic joints behave the same as for = R2. Each screw joint contributes R. A cylindrical joint contributes R S1, unless its ro- tational motion is restricted. A planar joint contributes R2 S1 because any transformation in SE(2) is possible. If its rotational motions are restricted, then it contributes R3. Finally, a spherical joint can theoretically contribute RP3. In practice, however, this rarely occurs. It is more likely to contribute R2 S1 or R3 after restrictions are imposed. Note that if the first joint is a free-floating body,

×

×

W

W

×

3

then it contributes R3 × RP .

Kinematic trees can be handled in the same way as kinematic chains. One issue that has not been mentioned is that there might be collisions between the links. This has been ignored up to this point, but obviously this imposes very complicated restrictions. The concepts from Section 4.3 can be applied to handle this case and the placement of additional obstacles in . Reasoning about these kinds of restrictions and the path connectivity of the resulting space is indeed the main point of motion planning.

W