Rigid-Body Transformations

Any of the techniques from Section 3.1 can be used to define both the obstacle region and the robot. Let O refer to the obstacle region, which is a subset of W. Let A refer to the robot, which is a subset of R2 or R3, matching the dimension of W. Although O remains fixed in the world, W, motion planning problems will require “moving” the robot, A.

• • 1. General Concepts

Before giving specific transformations, it will be helpful to define them in general to

avoid confusion in later parts when intuitive notions might fall apart. Suppose that a rigid robot, A, is defined as a subset of R2 or R3. A rigid-body transformation is a function, h : A → W, that maps every point of A into W with two requirements:

1) The distance between any pair of points of A must be preserved, and 2) the orientation of A must be preserved (no “mirror images”).

Using standard function notation, h(a) for some a ∈ A refers to the point in

W that is “occupied” by a. Let

h(A) = {h(a) ∈ W | a ∈ A}, (3.21)

which is the image of h and indicates all points in occupied by the transformed robot.

W

Transforming the robot model Consider transforming a robot model. If

A

is expressed by naming specific points in R2, as in a boundary representation of a polygon, then each point is simply transformed from a to h(a) . In this case, it is straightforward to transform the entire model using h. However, there is a slight complication if the robot model is expressed using primitives, such as

∈ W

Hi = {a ∈ R | fi(a) ≤ 0}. (3.22)

2

This differs slightly from (3.2) because the robot is defined in R2 (which is not necessarily ), and also a is used to denote a point (x, y) . Under a transfor- mation h, the primitive is transformed as

W ∈ A

h(Hi) = {h(a) ∈ W | fi(a) ≤ 0}. (3.23)

To transform the primitive completely, however, it is better to directly name points in w ∈ W, as opposed to h(a) ∈ W. Using the fact that a = h−1(w), this becomes

h(Hi) = {w ∈ W | fi(h−1(w)) ≤ 0}, (3.24) in which the inverse of h appears in the right side because the original point a ∈ A

needs to be recovered to evaluate fi. Therefore, it is important to be careful

because either h or h−1 may be required to transform the model. This will be observed in more specific contexts in some coming examples.

A parameterized family of transformations It will become important to study families of transformations, in which some parameters are used to select the particular transformation. Therefore, it makes sense to generalize h to accept two variables: a parameter vector, q Rn, along with a . The resulting transformed point a is denoted by h(q, a), and the entire robot is transformed to h(q, ) .

∈ ∈ A

A ⊂ W

The coming material will use the following shorthand notation, which requires the specific h to be inferred from the context. Let h(q, a) be shortened to a(q), and let h(q, ) be shortened to (q). This notation makes it appear that by adjusting the parameter q, the robot travels around in as different transformations are selected from the predetermined family. This is slightly abusive notation, but it is convenient. The expression (q) can be considered as a set-valued function that yields the set of points in that are occupied by when it is transformed by

A W

A A

W A

A

q. Most of the time the notation does not cause trouble, but when it does, it is helpful to remember the definitions from this section, especially when trying to determine whether h or h−1 is needed.

Defining frames It was assumed so far that is defined in R2 or R3, but before it is transformed, it is not considered to be a subset of . The transformation h places the robot in . In the coming material, it will be convenient to indicate this distinction using coordinate frames. The origin and coordinate basis vectors

W

W

A

of will be referred to as the world frame. Thus, any point w is expressed in terms of the world frame.

3

W ∈ W

The coordinates used to define are initially expressed in the body frame, which represents the origin and coordinate basis vectors of R2 or R3. In the case of R2, it can be imagined that the body frame is painted on the robot. Transforming the robot is equivalent to converting its model from the body frame

A

A ⊂

to the world frame. This has the effect of placing4 into at some position and orientation. When multiple bodies are covered in Section 3.3, each body will have its own body frame, and transformations require expressing all bodies with respect to the world frame.

A W

• • 1. 2D Transformations

Translation A rigid robot R2 is translated by using two parameters, xt, yt

A ⊂ ∈

1. Using definitions from Section 3.2.1, q = (xt, yt), and h is defined as

h(x, y) = (x + xt, y + yt). (3.25) A boundary representation of A can be translated by transforming each vertex in the sequence of polygon vertices using (3.25). Each point, (xi, yi), in the sequence

is replaced by (xi + xt, yi + yt).

Now consider a solid representation of , defined in terms of primitives. Each primitive of the form

A

is transformed to

Hi = {(x, y) ∈ R | f(x, y) ≤ 0} (3.26)

h(Hi) = {(x, y) ∈ W | f(x − xt, y − yt) ≤ 0}. (3.27)

2

Example 3.2 (Translating a Disc) For example, suppose the robot is a disc of unit radius, centered at the origin. It is modeled by a single primitive,

Hi = {(x, y) ∈ R2 | x2 + y2 − 1 ≤ 0}. (3.28)

Suppose = Hi is translated xt units in the x direction and yt units in the y

A

direction. The transformed primitive is

h(Hi) = {(x, y) ∈ W | (x − xt)2 + (y − yt)2 − 1 ≤ 0}, (3.29)

3The world frame serves the same purpose as an inertial frame in Newtonian mechanics. Intuitively, it is a frame that remains fixed and from which all measurements are taken. See Section 13.3.1.

4Technically, this placement is a function called an orientation-preserving isometric embed- ding.

• 1. Translation of the robot (b) Translation of the frame

Figure 3.7: Every transformation has two interpretations.

which is the familiar equation for a disc centered at (xt, yt). In this example, the inverse, h−1 is used, as described in Section 3.2.1. ．

The translated robot is denoted as (xt, yt). Translation by (0, 0) is the iden- tity transformation, which results in (0, 0) = , if it is assumed that

A A A ⊂ W

A

(recall that does not necessarily have to be initially embedded in ). It will be convenient to use the term degrees of freedom to refer to the maximum number of independent parameters that are needed to completely characterize the transfor-

A W

mation applied to the robot. If the set of allowable values for xt and yt forms a two-dimensional subset of R2, then the degrees of freedom is two.

Suppose that is defined directly in with translation. As shown in Figure 3.7, there are two interpretations of a rigid-body transformation applied to : 1) The world frame remains fixed and the robot is transformed; 2) the robot remains fixed and the world frame is translated. The first one characterizes the effect of the transformation from a fixed world frame, and the second one indicates how the transformation appears from the robot’s perspective. Unless stated otherwise, the first interpretation will be used when we refer to motion planning problems because it often models a robot moving in a physical world. Numerous books cover coordinate transformations under the second interpretation. This has been known to cause confusion because the transformations may sometimes appear “backward” from what is desired in motion planning.

A

A W

Rotation The robot, A, can be rotated counterclockwise by some angle θ ∈

[0, 2π) by mapping every (x, y) ∈ A as

(x, y) 1→ (x cos θ − y sin θ, x sin θ + y cos θ). (3.30)

Using a 2 × 2 rotation matrix,

（ − ＼

R(θ) = cos θ sin θ , (3.31) sin θ cos θ

the transformation can be written as

（x cos θ − y sin θ＼ = R(θ) （x＼ . (3.32)

x sin θ + y cos θ

y

Using the notation of Section 3.2.1, R(θ) becomes h(q), for which q = θ. For linear transformations, such as the one defined by (3.32), recall that the column vectors represent the basis vectors of the new coordinate frame. The column vectors of R(θ) are unit vectors, and their inner product (or dot product) is zero, indicating that they are orthogonal. Suppose that the x and y coordinate axes, which represent the body frame, are “painted” on . The columns of R(θ) can be derived by considering the resulting directions of the x- and y-axes, respectively, after performing a counterclockwise rotation by the angle θ. This interpretation generalizes nicely for higher dimensional rotation matrices.

A

Note that the rotation is performed about the origin. Thus, when defining the model of , the origin should be placed at the intended axis of rotation. Using the semi-algebraic model, the entire robot model can be rotated by transforming each primitive, yielding (θ). The inverse rotation, R( θ), must be applied to each primitive.

A

A −

Combining translation and rotation Suppose a rotation by θ is performed, followed by a translation by xt, yt. This can be used to place the robot in any desired position and orientation. Note that translations and rotations do not commute! If the operations are applied successively, each (x, y) is transformed to

∈ A

（ ＼

x cos θ − y sin θ + xt . (3.33)

x sin θ + y cos θ + yt

The following matrix multiplication yields the same result for the first two vector components:

cos θ − sin θ xt x x cos θ − y sin θ + xt

y

=







sin θ cos θ y

t   

x sin θ + y cos θ + yt

. (3.34)

0 0 1 1 1

This implies that the 3 × 3 matrix,

 − 

cos θ sin θ xt

T = sin θ cos θ y , (3.35)





t

0 0 1

represents a rotation followed by a translation. The matrix T will be referred to as a homogeneous transformation matrix. It is important to remember that T

Figure 3.8: Any three-dimensional rotation can be described as a sequence of yaw, pitch, and roll rotations.

represents a rotation followed by a translation (not the other way around). Each primitive can be transformed using the inverse of T , resulting in a transformed solid model of the robot. The transformed robot is denoted by (xt, yt, θ), and in this case there are three degrees of freedom. The homogeneous transformation matrix is a convenient representation of the combined transformations; therefore, it is frequently used in robotics, mechanics, computer graphics, and elsewhere. It is called homogeneous because over R3 it is just a linear transformation with- out any translation. The trick of increasing the dimension by one to absorb the translational part is common in projective geometry [804].

A

• • 1. 3D Transformations

Rigid-body transformations for the 3D case are conceptually similar to the 2D case; however, the 3D case appears more difficult because rotations are significantly more complicated.

3D translation The robot, A, is translated by some xt, yt, zt ∈ R using

(x, y, z) 1→ (x + xt, y + yt, z + zt). (3.36)

A primitive of the form

Hi = {(x, y, z) ∈ W | fi(x, y, z) ≤ 0} (3.37)

is transformed to

{(x, y, z) ∈ W | fi(x − xt, y − yt, z − zt) ≤ 0}. (3.38)

The translated robot is denoted as A(xt, yt, zt).

Yaw, pitch, and roll rotations A 3D body can be rotated about three orthog- onal axes, as shown in Figure 3.8. Borrowing aviation terminology, these rotations will be referred to as yaw, pitch, and roll:

1. A yaw is a counterclockwise rotation of α about the z-axis. The rotation matrix is given by

cos α sin α 0

 − 

R (α) = sin α cos α 0





. (3.39)

z

0 0 1

Note that the upper left entries of Rz (α) form a 2D rotation applied to the

x and y coordinates, whereas the z coordinate remains constant.

1. A pitch is a counterclockwise rotation of β about the y-axis. The rotation matrix is given by

cos β 0 sin β

 

R (β) = 0 1 0





. (3.40)

y

− sin β 0 cos β

1. A roll is a counterclockwise rotation of γ about the x-axis. The rotation matrix is given by

Rx(γ) =

1 0 0 

0 sin γ cos γ 

0 cos γ − sin γ

. (3.41)

Each rotation matrix is a simple extension of the 2D rotation matrix, (3.31). For example, the yaw matrix, Rz (α), essentially performs a 2D rotation with respect to the x and y coordinates while leaving the z coordinate unchanged. Thus, the third row and third column of Rz (α) look like part of the identity matrix, while the upper right portion of Rz (α) looks like the 2D rotation matrix.

The yaw, pitch, and roll rotations can be used to place a 3D body in any orientation. A single rotation matrix can be formed by multiplying the yaw, pitch, and roll rotation matrices to obtain

R(α,β, γ) = Rz (α) Ry (β) Rx(γ) =

cos α cos β cos α sin β sin γ − sin α cos γ cos α sin β cos γ + sin α sin γ

sin α cos β sin α sin β sin γ + cos α cos γ sin α sin β cos γ − cos α sin γ .





− sin β cos β sin γ cos β cos γ

(3.42)

(3.42)

It is important to note that R(α, β, γ) performs the roll first, then the pitch, and finally the yaw. If the order of these operations is changed, a different rotation

matrix would result. Be careful when interpreting the rotations. Consider the final rotation, a yaw by α. Imagine sitting inside of a robot that looks like an aircraft. If β = γ = 0, then the yaw turns the plane in a way that feels like turning a car to the left. However, for arbitrary values of β and γ, the final rotation axis will not be vertically aligned with the aircraft because the aircraft is left in an unusual orientation before α is applied. The yaw rotation occurs about the z-axis of the world frame, not the body frame of . Each time a new rotation matrix is introduced from the left, it has no concern for original body frame of

A

A

3

1. It simply rotates every point in R in terms of the world frame. Note that 3D

rotations depend on three parameters, α, β, and γ, whereas 2D rotations depend

only on a single parameter, θ. The primitives of the model can be transformed using R(α, β, γ), resulting in A(α, β, γ).

Determining yaw, pitch, and roll from a rotation matrix It is often con- venient to determine the α, β, and γ parameters directly from a given rotation matrix. Suppose an arbitrary rotation matrix

r11 r12 r13

r

r

 21

r22

23

(3.43)

r31 r32 r33

is given. By setting each entry equal to its corresponding entry in (3.42), equations are obtained that must be solved for α, β, and γ. Note that r21/r11 = tan α and

r32/r33 = tan γ. Also, r31 = − sin β and /r2 2 = cos β. Solving for each

32

• r

33

angle yields

α = tan−1(r21/r11), (3.44)

and

β = tan−1 ( − r31//r2

• r2 \, (3.45)

γ = tan−1(r32/r33). (3.46)

32

33

There is a choice of four quadrants for the inverse tangent functions. How can the correct quadrant be determined? Each quadrant should be chosen by using the signs of the numerator and denominator of the argument. The numerator sign selects whether the direction will be above or below the x-axis, and the denomi- nator selects whether the direction will be to the left or right of the y-axis. This is the same as the atan2 function in the C programming language, which nicely expands the range of the arctangent to [0, 2π). This can be applied to express (3.44), (3.45), and (3.46) as

α = atan2(r21, r11), (3.47)

and

32

33

β = atan2 ( − r31, /r2

• r2 \, (3.48)

γ = atan2(r32, r33). (3.49)

Note that this method assumes r11 /= 0 and r33 /= 0.

The homogeneous transformation matrix for 3D bodies As in the 2D

case, a homogeneous transformation matrix can be defined. For the 3D case, a 4 × 4 matrix is obtained that performs the rotation given by R(α, β, γ), followed

by a translation given by xt, yt, zt. The result is

cos α cos β cos α sin β sin γ − sin α cos γ cos α sin β cos γ + sin α sin γ xt

 

sin α cos β sin α sin β sin γ + cos α cos γ sin α sin β cos γ − cos α sin γ yt 

T =  .

− sin β cos β sin γ cos β cos γ zt





0 0 0 1

(3.50)

Once again, the order of operations is critical. The matrix T in (3.50) represents the following sequence of transformations:

• 1. Roll by γ 3. Yaw by α
  2. Pitch by β 4. Translate by (xt, yt, zt).

The robot primitives can be transformed to yield (xt, yt, zt, α, β, γ). A 3D rigid body that is capable of translation and rotation therefore has six degrees of free- dom.

A