$SPAD/src/algebra dhmatrix.spad

Richard Paul and Timothy Daly

April 4, 2022

Abstract

Contents

1 Homogeneous Transformations  3
2 Notation  3
3 Vectors  4
4 Planes  5
5 Transformations  7
6 Translation Transformation  7
7 Rotation Transformations  9
8 Coordinate Frames  12
9 Relative Transformations  13
10 Objects  14
11 Inverse Transformations  14
12 General Rotation Transformation  15
13 Equivalent Angle and Axis of Rotation  18
14 Example 1.1  21
15 Stretching and Scaling  22
16 Perspective Transformations  23
17 Transform Equations  25
18 Summary  25
19 Denavit-Hartenberg Matrices  26
20 License  28

1 Homogeneous Transformations

The study of robot manipulation is concerned with the relationship between objects, and between objects and manipulators. In this chapter we will develop the representation necessary to describe these relationships. Similar problems of representation have already been solved in the field of computer graphics, where the relationship between objects must also be described. Homogeneous transformations are used in this field and in computer vision [Duda] [Robserts63] [Roberts65]. These transformations were employed by Denavit to describe linkages [Denavit] and are now used to describe manipulators [Pieper] [Paul72] [Paul77b].

We will first establish notation for vectors and planes and then introduce transformations on them. These transformations consist primarily of translation and rotation. We will then show that these transformations can also be considered as coordinate frames in which to represent objects, including the manipulator. The inverse transformation will then be introduced. A later section describes the general rotation transformation representing a rotation about a vector. An algorithm is then described to find the equivalent axis and angle of rotations represented by any given transformation. A brief section on stretching and scaling transforms is included together with a section on the perspective transformation. The chapter concludes with a section on transformation equations.

2 Notation

In describing the relationship between objects we will make use of point vectors, planes, and coordinate frames. Point vectors are denoted by lower case, bold face characters. Planes are denoted by script characters, and coordinate frames by upper case, bold face characters. For example:

vectors  v, x1, x
planes  P, Q
coordinate frames I, A, CONV

We will use point vectors, planes, and coordinate frames as variables which have associated values. For example, a point vector has as value its three Cartesian coordinate components.

If we wish to describe a point in space, which we will call p, with respect to a coordinate frame E, we will use a vector which we will call v. We will write this as

Ev

The leading superscript describes the defining coordinate frame.

We might also wish to describe this same point, p, with respect to a different coordinate frame, for example H, using a vector w as

Hw v and w are two vectors which probably have different component values and v ≠ w even though both vectors describe the same point p. The case might also exist of a vector a describing a point 3 inches above any frame

F^{1}a ^{F^{2}}a

In this case the vectors are identical but describe different points. Frequently, the defining frame will be obvious from the text and the superscripts will be left off. In many cases the name of the vector will be the same as the name of the object described, for example, the tip of a pin might be described by a vector tip with respect to a frame BASE as

BASEtip

If it were obvious from the text that we were describing the vector with respect to BASE then we might simply write

tip

If we also wish to describe this point with respect to another coordinate frame say, HAND, then we must use another vector to describe this relationship, for example

HANDtv

HANDtv and tip both describe the same feature but have different values. In order to refer to individual components of coordinate frames, point vectors, or planes, we add subscripts to indicate the particular component. For example, the vector ^{HAND}tv has components ^{HAND}tv_{x}, ^{HAND}tv_{y}, ^{HAND}tv_{z}.

3 Vectors

The homogeneous coordinate representation of objects in n-space is an (n + 1)- space entity such that a particular perspective projection recreates the n-space. This can also be viewed as the addition of an extra coordinate to each vector, a scale factor, such that the vector has the same meaning if each component, including the scale factor, is multiplied by a constant.

A point vector

v = ai + bj + ck  (1.1)

where i, j, and k are unit vectors along the x, y, and z coordinate axes, respectively, is represented in homogeneous coordinates as a column matrix

v = [ \begin{matrix}x \\ y \\ z \\ w \end{matrix} ]  (1.2)

where

a = x/w
b = y/w  (1.3)
c = z/w

Thus the vector 3i+4j+5k can be represented as [3, 4, 5, 1]^{T} or as [6, 8, 10, 2]^{T} or again as [−30, −40, −50, −10]^{T}, etc. The superscript T indicates the transpose of the row vector into a column vector. The vector at the origin, the null vector, is represented as [0, 0, 0, n]^{T} where n is any non-zero scale factor. The vector [0, 0, 0, 0]^{T} is undefined. Vectors of the form [a, b, c, 0]^{T} represent vectors at infinity and are used to represent directions; the addition of any other finite vector does not change their value in any way.

We will also make use of the vector dot and cross products. Given two vectors

a = a_{x}i + a_{y}j + a_{z}k  (1.4)
b = b_{x}i + b_{y}j + b_{z}k

we define the vector dot product, indicated by “·” as

a · b = a_{x}b_{x} + a_{y}b_{y} + a_{z}b_{z}  (1.5)

The dot product of two vectors is a scalar. The cross product, indicated by an “×”, is another vector perpendicular to the plane formed by the vectors of the product and is defined by

a × b = (a_{y}b_{z} − a_{z}b_{y})i + (a_{z}b_{x} − a_{x}b_{z})j + (a_{x}b_{y} − a_{y}b_{x})k  (1.6)

This definition is easily remembered as the expansion of the determinant

a × b = | \begin{matrix}i & j & k \\ a_{x} & a_{y} & a_{z} \\ b_{x} & b_{y} & b_{z} \end{matrix} |  (1.7)

4 Planes

A plane is represented as a row matrix

P= [a, b, c, d]  (1.8)

such that if a point v lies in a plane P the matrix product

P v = 0  (1.9)

or in expanded form

xa + yb + zc + wd = 0  (1.10)

If we define a constant

m = +\sqrt{a^{2} + b^{2} + c^{2}}  (1.11)

and divide Equation 1.10 by wm we obtain

\frac{x}{w} \frac{a}{m} + \frac{y}{w} \frac{b}{m} + \frac{z}{w} \frac{c}{m} = −\frac{d}{m}  (1.12)

The left hand side of Equation 1.12 is the vector dot product of two vectors (x/w)i + (y/w)j + (z/w)k and (a/m)i + (b/m)j + (c/m)k and represents the directed distance of the point (x/w)i + (y/w)j + (z/w)k along the vector (a/m)i + (b/m)j + (c/m)k. The vector (a/m)i + (b/m)j + (c/m)k can be interpreted as the outward pointing normal of a plane situated a distance −d/m from the origin in the direction of the normal. Thus a plane P parallel to the x,y plane, one unit along the z axis, is represented as

P= [0, 0, 1, −1]  (1.13)
or as P= [0, 0, 2, −2]  (1.14)
or as P= [0, 0, −100, 100]  (1.15)

A point v = [10, 20, 1, 1] should lie in this plane

[0, 0, −100, 100] [ \begin{matrix}10 \\ 20 \\ 1 \\ 1 \end{matrix} ] = 0  (1.16)

or

[0, 0, 1, −1] [ \begin{matrix}−5 \\ −10 \\ −.5 \\ −.5 \end{matrix} ] = 0  (1.17)

The point v = [0, 0, 2, 1] lies above the plane

[0, 0, 2, −2] [ \begin{matrix}0 \\ 0 \\ 2 \\ 1 \end{matrix} ] = 2  (1.18)

and P v is indeed positive, indicating that the point is outside the plane in the direction of the outward pointing normal. A point v = [0, 0, 0, 1] lies below the plane

[0, 0, 1, −1] [ \begin{matrix}0 \\ 0 \\ 0 \\ 1 \end{matrix} ] = −1  (1.19)

The plane [0, 0, 0, 0] is undefined.

5 Transformations

A transformation of the space H is a 4x4 matrix and can represent translation, rotation, stretching, and perspective transformations. Given a point u, its transformation v is represented by the matrix product

v = Hu  (1.20)

The corresponding plane transformation P to Q is

Q=P H^{−1}  (1.21)

as we requre that the condition

Q v =P u  (1.22)

is invariant under all transformations. To verify this we substitute from Equations 1.20 and 1.21 into the left hand side of 1.22 and we obtain on the right hand side H^{−1}H which is the identity matrix I

P H^{−1}Hu =P u  (1.23)

6 Translation Transformation

The transformation H corresponding to a translation by a vector ai + bj + ck is

H = Trans(a, b, c) = [ \begin{matrix}1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.24)

Given a vector u = [x, y, z, w]^{T} the transformed vector v is given by

H = Trans(a, b, c) = [ \begin{matrix}1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}x \\ y \\ z \\ w \end{matrix} ]  (1.25)

v = [ \begin{matrix}x + aw \\ y + bw \\ z + cw \\ w \end{matrix} ] = [ \begin{matrix}x/w + a \\ y/w + b \\ z/w + c \\ 1 \end{matrix} ]  (1.26)

The translation may also be interpreted as the addition of the two vectors (x/w)i + (y/w)j + (z/w)k and ai + bj + ck.

Every element of a transformation matrix may be multiplied by a non-zero constant without changing the transformation, in the same manner as points and planes. Consider the vector 2i + 3j + 2k translated by, or added to 4i - 3j + 7k

[ \begin{matrix}6 \\ 0 \\ 9 \\ 1 \end{matrix} ] = [ \begin{matrix}1 & 0 & 0 & 4 \\ 0 & 1 & 0 & −3 \\ 0 & 0 & 1 & 7 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}2 \\ 3 \\ 2 \\ 1 \end{matrix} ]  (1.27)

If we multiply the transmation matrix elements by, say, -5, and the vector elements by 2, we obtain

[ \begin{matrix}−60 \\ 0 \\ −90 \\ −10 \end{matrix} ] = [ \begin{matrix}−5 & 0 & 0 & −20 \\ 0 & −5 & 0 & 15 \\ 0 & 0 & −5 & −35 \\ 0 & 0 & 0 & −5 \end{matrix} ] [ \begin{matrix}4 \\ 6 \\ 4 \\ 2 \end{matrix} ]  (1.28)

which corresponds to the vector [6, 0, 9, 1]^{T} as before. The point [2, 3, 2, 1] lies in the plane [1, 0, 0, −2]

[1, 0, 0, −2] [ \begin{matrix}2 \\ 3 \\ 2 \\ 1 \end{matrix} ] = 0  (1.29)

The transformed point is, as we have already found, [6, 0, 9, 1]^{T}. We will now compute the transformed plane. The inverse of the transform is

[ \begin{matrix}1 & 0 & 0 & −4 \\ 0 & 1 & 0 & 3 \\ 0 & 0 & 1 & −7 \\ 0 & 0 & 0 & 1 \end{matrix} ]

and the transformed plane

[1 0 0 − 6] = [1 0 0 − 2] [ \begin{matrix}1 & 0 & 0 & −4 \\ 0 & 1 & 0 & 3 \\ 0 & 0 & 1 & −7 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.30)

Once again the transformed point lies in the transformed plane

[1 0 0 − 6] [ \begin{matrix}6 \\ 0 \\ 9 \\ 1 \end{matrix} ] = 0  (1.31)

The general translation operation can be represented in Axiom as

⟨translate⟩≡

      translate(x,y,z) ==
       matrix(_
        [[1,0,0,x],_
         [0,1,0,y],_
         [0,0,1,z],_
         [0,0,0,1]])

7 Rotation Transformations

The transformations corresponding to rotations about the x, y, and z axes by an angle θ are

Rot(x, θ) = [ \begin{matrix}1 & 0 & 0 & 0 \\ 0 & cos θ & −sin θ & 0 \\ 0 & sin θ & cos θ & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.32)

Rotations can be described in Axiom as functions that return matrices. We can define a function for each of the rotation matrices that correspond to the rotations about each axis. Note that the sine and cosine functions in Axiom expect their argument to be in radians rather than degrees. This conversion is

radians = \frac{degrees ∗ π}{180}

The Axiom code for Rot(x, degree) is

⟨rotatex⟩≡

      rotatex(degree) ==
       angle := degree * pi() / 180::R
       cosAngle := cos(angle)
       sinAngle := sin(angle)
       matrix(_
        [[1,     0,           0,      0], _
         [0, cosAngle, -sinAngle, 0], _
         [0, sinAngle,  cosAngle, 0], _
         [0,     0,           0,      1]])

Rot(y, θ) = [ \begin{matrix}cos θ & 0 & sin θ & 0 \\ 0 & 1 & 0 & 0 \\ −sin θ & 0 & cos θ & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.33)

The Axiom code for Rot(y, degree) is

⟨rotatey⟩≡

rotatey(degree) ==
angle := degree * pi() / 180::R
cosAngle := cos(angle)
sinAngle := sin(angle)
matrix(_
 [[ cosAngle, 0, sinAngle, 0], _
  [    0,       1,     0,      0], _
  [-sinAngle, 0, cosAngle, 0], _
  [    0,       0,     0,      1]])

Rot(z, θ) = [ \begin{matrix}cos θ & −sin θ & 0 & 0 \\ sin θ & cos θ & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.34)

And the Axiom code for Rot(z, degree) is

⟨rotatez⟩≡

rotatez(degree) ==
angle := degree * pi() / 180::R
cosAngle := cos(angle)
sinAngle := sin(angle)
matrix(_
 [[cosAngle, -sinAngle, 0, 0], _
  [sinAngle,  cosAngle, 0, 0], _
  [   0,           0,       1, 0], _
  [   0,           0,       0, 1]])

Let us interpret these rotations by means of an example. Given a point u = 7i + 3j + 2k what is the effect of rotating it 90^{∘} about the z axis to v? The transform is obtained from Equation 1.34 with sin θ = 1 and cos θ = 0.

[ \begin{matrix}−3 \\ 7 \\ 2 \\ 1 \end{matrix} ] = [ \begin{matrix}0 & −1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}7 \\ 3 \\ 2 \\ 1 \end{matrix} ]  (1.35)

Let us now rotate v 90^{∘} about the y axis to w. The transform is obtained from Equation 1.33 and we have

[ \begin{matrix}2 \\ 7 \\ 3 \\ 1 \end{matrix} ] = [ \begin{matrix}0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 \\ −1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}−3 \\ 7 \\ 2 \\ 1 \end{matrix} ]  (1.36)

If we combine these two rotations we have

v = Rot(z, 90)u  (1.37)
and w = Rot(y, 90)v  (1.38)

Substituting for v from Equation 1.37 into Equation 1.38 we obtain

w = Rot(y, 90) Rot(z, 90) u  (1.39)

Rot(y, 90) Rot(z, 90) = [ \begin{matrix}0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 \\ −1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}0 & −1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.40)

Rot(y, 90) Rot(z, 90) = [ \begin{matrix}0 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.41)

thus

w = [ \begin{matrix}2 \\ 7 \\ 3 \\ 1 \end{matrix} ] = [ \begin{matrix}0 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}7 \\ 3 \\ 2 \\ 1 \end{matrix} ]  (1.42)

as we obtained before.

If we reverse the order of rotations and first rotate 90^{∘} about the y axis and then 90^{∘} about the z axis, we obtain a different position

Rot(z, 90)Rot(y, 90) = [ \begin{matrix}0 & −1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 \\ −1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] = [ \begin{matrix}0 & −1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ −1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]

(1.43)

and the point u transforms into w as

[ \begin{matrix}−3 \\ 2 \\ −7 \\ 1 \end{matrix} ] = [ \begin{matrix}0 & −1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ −1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}7 \\ 3 \\ 2 \\ 1 \end{matrix} ]  (1.44)

We should expect this, as matrix multiplication is noncommutative.

AB ≠ BA  (1.45)

We will now combine the original rotation with a translation 4i − 3j + 7k. We obtain the translation from Equation 1.27 and the rotation from Equation 1.41. The matrix expression is

Trans(4, −3, 7)Rot(y, 90)Rot(z, 90) = [ \begin{matrix}1 & 0 & 0 & 4 \\ 0 & 1 & 0 & −3 \\ 0 & 0 & 1 & 7 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}0 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] = [ \begin{matrix}0 & 0 & 1 & 4 \\ 1 & 0 & 0 & −3 \\ 0 & 1 & 0 & 7 \\ 0 & 0 & 0 & 1 \end{matrix} ]

(1.46)

and our point w = 7i + 3j + 2k transforms into x as

[ \begin{matrix}6 \\ 4 \\ 10 \\ 1 \end{matrix} ] = [ \begin{matrix}0 & 0 & 1 & 4 \\ 1 & 0 & 0 & −3 \\ 0 & 1 & 0 & 7 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}7 \\ 3 \\ 2 \\ 1 \end{matrix} ]  (1.47)

8 Coordinate Frames

We can interpret the elements of the homogeneous transformation as four vectors describing a second coordinate frame. The vector [0, 0, 0, 1]^{T} lies at the origin of the second coordinate frame. Its transformation corresponds to the right hand column of the transformation matrix. Consider the transform in Equation 1.47

[ \begin{matrix}4 \\ −3 \\ 7 \\ 1 \end{matrix} ] = [ \begin{matrix}0 & 0 & 1 & 4 \\ 1 & 0 & 0 & −3 \\ 0 & 1 & 0 & 7 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}0 \\ 0 \\ 0 \\ 1 \end{matrix} ]  (1.48)

The transform of the null vector is [4, −3, 7, 1]^{T}, the right hand column. If we transform vectors corresponding to unit vectors along the x, y, and z axes, we obtain [4, −2, 7, 1]^{T}, [4, −3, 8, 1]^{T}, and [5, −3, 7, 1]^{T}, respectively. Those four vectors form a coordinate frame.

The direction of these unit vectors is formed by subtracting the vector representing the origin of this coordinate frame and extending the vectors to infinity by reducing their scale factors to zero. The direction of the x, y, and z axes of this frame are [0, 1, 0, 0]^{T}, [0, 0, 1, 0]^{T}, and [1, 0, 0, 0]^{T}, respectively. These direction vectors correspond to the first three columns of the transformation matrix. The transformation matrix thus describes the three axis directions and the position of the origin of a coordinate frame rotated and translated away from the reference coordinate frame. When a vector is transformed, as in Equation 1.47, the original vector can be considered as a vector described in the coordinate frame. The transformed vector is the same vector described with respect to the reference coordinate frame.

9 Relative Transformations

The rotations and translations we have been describing have all been made with respect to the fixed reference coordinate frame. Thus, in the example given,

Trans(4, −3, 7)Rot(y, 90)Rot(z, 90) = [ \begin{matrix}0 & 0 & 1 & 4 \\ 1 & 0 & 0 & −3 \\ 0 & 1 & 0 & 7 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.49)

the frame is first rotated around the reference z axis by 90^{∘}, then rotated 90^{∘} around the reference y axis, and finally translated by 4i − 3j + 7k. We may also interpret the operation in the reverse order, from left to right, as follows: the object is first translated by 4i − 3j + 7k; it is then rotated 90^{∘} around the current frames axes, which in this case are the same as the reference axes; it is then rotated 90^{∘} about the newly rotated (current) frames axes.

In general, if we postmultiply a transform representing a frame by a second transformation describing a rotation and/or translation, we make that translation and/or rotation with respect to the frame axes described by the first transformation. If we premultiply the frame transformation by a transformation representing a translation and/or rotation, then that translation and/or rotation is made with respect to the base reference coordinate frame. Thus, given a frame C and a transformation T, corresponding to a rotation of 90^{∘} about the z axis, and a translation of 10 units in the x direction, we obtain a new position X when the change is made in the base coordinates X = TC

[ \begin{matrix}0 & 0 & 1 & 0 \\ 1 & 0 & 0 & 20 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] = [ \begin{matrix}0 & −1 & 0 & 10 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}1 & 0 & 0 & 20 \\ 0 & 0 & −1 & 10 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.50)

and a new position Y when the change is made relative to the frame axes as Y = CT

[ \begin{matrix}0 & −1 & 0 & 30 \\ 0 & 0 & −1 & 10 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] = [ \begin{matrix}1 & 0 & 0 & 20 \\ 0 & 0 & −1 & 10 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}0 & −1 & 0 & 10 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.51)

10 Objects

Transformations are used to describe the position and orientation of objects. An object is described by six points with respect to a coordinate frame fixed in the object.

If we rotate the object 90^{∘} about the z axis and then 90^{∘} about the y axis, followed by a translation of four units in the x direction, we can describe the transformation as

Trans(4, 0, 0)Rot(y, 90)Rot(z, 90) = [ \begin{matrix}0 & 0 & 1 & 4 \\ 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.52)

The transformation matrix represents the operation of rotation and translation on a coordinate frame originally aligned with the reference coordinate frame. We may transform the six points of the object as

[ \begin{matrix}4 & 4 & 6 & 6 & 4 & 4 \\ 1 & −1 & −1 & 1 & 1 & −1 \\ 0 & 0 & 0 & 0 & 4 & 4 \\ 1 & 1 & 1 & 1 & 1 & 1 \end{matrix} ] = [ \begin{matrix}0 & 0 & 1 & 4 \\ 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}1 & −1 & −1 & 1 & 1 & −1 \\ 0 & 0 & 0 & 0 & 4 & 4 \\ 0 & 0 & 2 & 2 & 0 & 0 \\ 1 & 1 & 1 & 1 & 1 & 1 \end{matrix} ]

(1.53)

It can be seen that the object described bears the same fixed relationship to its coordinate frame, whose position and orientation are described by the transformation. Given an object described by a reference coordinate frame, and a transformation representing the position and orientation of the object’s axes, the object can be simply reconstructed, without the necessity of transforming all the points, by noting the direction and orientation of key features with respect to the describing frame’s coordinate axes. By drawing the transformed coordinate frame, the object can be related to the new axis directions.

11 Inverse Transformations

We are now in a position to develop the inverse transformation as the transform which carries the transformed coordinate frame back to the original frame. This is simply the description of the reference coordinate frame with respect to the transformed frame. Suppose the direction of the reference frame x axis is [0, 0, 1, 0]^{T} with respect to the transformed frame. The y and z axes are [1, 0, 0, 0]^{T} and [0, 1, 0, 0]^{T}, respectively. The location of the origin is [0, 0, −4, 1]^{T} with respect to the transformed frame and thus the inverse transformation is

T^{−1} = [ \begin{matrix}0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 1 & 0 & 0 & −4 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.54)

That this is indeed the tranform inverse is easily verifyed by multiplying it by the transform T to obtain the identity transform

[ \begin{matrix}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] = [ \begin{matrix}0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 1 & 0 & 0 & −4 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}0 & 0 & 1 & 4 \\ 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.55)

In general, given a transform with elements

T = [ \begin{matrix}n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.56)

then the inverse is

T^{−1} = [ \begin{matrix}n_{x} & n_{y} & n_{z} & −p · n \\ o_{x} & o_{y} & o_{z} & −p · o \\ a_{x} & a_{y} & a_{z} & −p · a \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.57)

where p, n, o, and a are the four column vectors and “·” represents the vector dot product. This result is easily verified by postmultiplying Equation 1.56 by Equation 1.57.

12 General Rotation Transformation

We state the rotation transformations for rotations about the x, y, and z axes (Equations 1.32, 1.33 and 1.34). These transformations have a simple geometric interpretation. For example, in the case of a rotation about the z axis, the column representing the z axis will remain constant, while the column elements representing the x and y axes will vary. We will now develop the transformation matrix representing a rotation around an arbitrary vector k located at the origin. In order to do this we will imagine that k is the z axis unit vector of a coordinate frame C

C = [ \begin{matrix}n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.58)

k = a_{x}i + a_{y}j + a_{z}k  (1.59)

Rotating around the vector k is then equivalent to rotating around the z axis of the frame C.

Rot(k, θ) = Rot(^{C}z, θ)  (1.60)

If we are given a frame T described with respect to the reference coordinate frame, we can find a frame X which describes the same frame with respect to frame C as

T = CX  (1.61)

where X describes the position of T with respect to frame C. Solving for X we obtain

X = C^{−1}T  (1.62)

Rotation T around k is equivalent to rotating X around the z axis of frame C

Rot(k, θ)T = CRot(z, θ)X  (1.63)
Rot(k, θ)T = CRot(z, θ)C^{−1}T.  (1.64)

Thus

Rot(k, θ) = CRot(z, θ)C^{−1}  (1.65)

However, we have only k, the z axis of the frame C. By expanding equation 1.65 we will discover that CRot(z, θ)C^{−1} is a function of k only.

Multiplying Rot(z, θ) on the right by C^{−1} we obtain

Rot(z, θ)C^{−1} = [ \begin{matrix}cosθ & −sinθ & 0 & 0 \\ sinθ & cosθ & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}n_{x} & n_{y} & n_{z} & 0 \\ o_{x} & o_{x} & o_{z} & 0 \\ a_{x} & a_{y} & a_{z} & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]

= [ \begin{matrix}n_{x}cosθ − o_{x}sinθ & n_{y}cosθ − o_{y}sinθ & n_{z}cosθ − o_{z}sinθ & 0 \\ n_{x}sinθ + o_{x}cosθ & n_{y}sinθ + o_{y}cosθ & n_{z}sinθ + o_{z}cosθ & 0 \\ a_{x} & a_{y} & a_{z} & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.66)

premultiplying by

C = [ \begin{matrix}n_{x} & o_{x} & a_{x} & 0 \\ n_{y} & o_{y} & a_{y} & 0 \\ n_{z} & o_{z} & a_{z} & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.67)

we obtain CRot(z, θ)C^{−1}

[ \begin{matrix}n_{x}n_{x}cosθ − n_{x}o_{x}sinθ + n_{x}o_{x}sinθ + o_{x}o_{x}cosθ + a_{x}a_{x} \\ n_{y}n_{x}cosθ − n_{y}o_{x}sinθ + n_{x}o_{y}sinθ + o_{x}o_{y}cosθ + a_{y}a_{x} \\ n_{z}n_{x}cosθ − n_{z}o_{x}sinθ + n_{x}o_{z}sinθ + o_{x}o_{z}cosθ + a_{z}a_{x} \\ 0 \end{matrix}

\begin{matrix}n_{x}n_{y}cosθ − n_{x}o_{y}sinθ + n_{y}o_{x}sinθ + o_{y}o_{x}cosθ + a_{x}a_{y} \\ n_{y}n_{y}cosθ − n_{y}o_{y}sinθ + n_{y}o_{y}sinθ + o_{y}o_{y}cosθ + a_{y}a_{y} \\ n_{z}n_{y}cosθ − n_{z}o_{y}sinθ + n_{y}o_{z}sinθ + o_{y}o_{z}cosθ + a_{z}a_{y} \\ 0 \end{matrix}  (1.68)

\begin{matrix}n_{x}n_{z}cosθ − n_{x}o_{z}sinθ + n_{z}o_{x}sinθ + o_{z}o_{x}cosθ + a_{x}a_{x} & 0 \\ n_{y}n_{z}cosθ − n_{y}o_{z}sinθ + n_{z}o_{y}sinθ + o_{z}o_{y}cosθ + a_{y}a_{z} & 0 \\ n_{z}n_{z}cosθ − n_{z}o_{z}sinθ + n_{z}o_{z}sinθ + o_{z}o_{z}cosθ + a_{z}a_{z} & 0 \\ 0 & 1 \end{matrix} ]

Simplifying, using the following relationships: the dot product of any row or column of C with any other row or column is zero, as the vectors are orthogonal; the dot product of any row or column of C with itself is 1 as the vectors are of unit magnitude; the z unit vector is the vector cross product of the x and y vectors or

a = n × o  (1.69)

which has components

a_{x} = n_{y}o_{z} − n_{z}o_{y}
a_{y} = n_{z}o_{x} − n_{x}o_{z}
a_{z} = n_{x}o_{y} − n_{y}o_{x}

the versine, abbreviated vers θ, is defined as vers θ = (1 − cos θ), k_{x} = a_{x}, k_{y} = a_{y} and k_{z} = a_{z}. We obtain Rot(k, θ) =

[ \begin{matrix}k_{x}k_{x}versθ + cosθ & k_{y}k_{x}versθ − k_{z}sinθ & k_{z}k_{x}versθ + k_{y}sinθ & 0 \\ k_{x}k_{y}versθ + k_{z}sinθ & k_{y}k_{y}versθ + cosθ & k_{z}k_{y}versθ − k_{x}sinθ & 0 \\ k_{x}k_{z}versθ − k_{y}sinθ & k_{y}k_{z}versθ + k_{x}sinθ & k_{z}k_{z}versθ + cosθ & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] (1.70)

This is an important result and should be thoroughly understood before proceeding further.

From this general rotation transformation we can obtain each of the elementary rotation transforms. For example Rot(x, θ) is Rot(k, θ) where k_{x} = 1, k_{y} = 0, and k_{z} = 0. Substituting these values of k into Equation 1.70 we obtain

Rot(x, θ) = [ \begin{matrix}1 & 0 & 0 & 0 \\ 0 & cosθ & −sinθ & 0 \\ 0 & sinθ & cosθ & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.71)

as before.

13 Equivalent Angle and Axis of Rotation

Given any arbitrary rotational transformation, we can use Equation 1.70 to obtain an axis about which an equivalent rotation θ is made as follows. Given a rotational transformation R

R = [ \begin{matrix}n_{x} & o_{x} & a_{x} & 0 \\ n_{y} & o_{y} & a_{y} & 0 \\ n_{z} & o_{z} & a_{z} & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.72)

we may equate R to Rot(k,θ)

[ \begin{matrix}n_{x} & o_{x} & a_{x} & 0 \\ n_{y} & o_{y} & a_{y} & 0 \\ n_{z} & o_{z} & a_{z} & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] =

[ \begin{matrix}k_{x}k_{x}versθ + cosθ & k_{y}k_{x}versθ − k_{z}sinθ & k_{z}k_{x}versθ + k_{y}sinθ & 0 \\ k_{x}k_{y}versθ + k_{z}sinθ & k_{y}k_{y}versθ + cosθ & k_{z}k_{y}versθ − k_{x}sinθ & 0 \\ k_{x}k_{z}versθ − k_{y}sinθ & k_{y}k_{z}versθ + k_{x}sinθ & k_{z}k_{z}versθ + cosθ & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] (1.73)

Summing the diagonal terms of Equation 1.73 we obtain

n_{x} + o_{y} + a_{z} + 1 = k_{x}^{2}versθ + cosθ + k_{y}^{2}versθ + cosθ + k_{z}^{2}versθ + cosθ + 1 (1.74)

\begin{aligned}n_{x} + o_{y} + a_{z} & = & (k_{x}^{2} + k_{y}^{2} + k_{z}^{2})versθ + 3cosθ \\ & = & 1 + 2cosθ \end{aligned}  (1.75)

and the cosine of the angle of rotation is

cosθ = \frac{1}{2}(n_{x} + o_{y} + a_{z} − 1)  (1.76)

Differencing pairs of off-diagonal terms in Equation 1.73 we obtain

o_{z} − a_{y} = 2k_{x}sinθ  (1.77)
a_{x} − n_{z} = 2k_{y}sinθ  (1.78)
n_{y} − o_{x} = 2k_{z}sinθ  (1.79)

Squaring and adding Equations 1.77-1.79 we obtain an expression for sinθ

(o_{z} − a_{y})^{2} + (a_{x} − n_{z})^{2} + (n_{y} − o_{x})^{2} = 4sin^{2}θ  (1.80)

and the sine of the angle of rotation is

sin θ = ±\frac{1}{2}\sqrt{(o_{z} − a_{y})^{2} + (a_{x} − n_{z})^{2} + (n_{y} − o_{x})^{2}}  (1.81)

We may define the rotation to be positive about the vector k such that 0 ≤ θ ≤ 180^{∘}. In this case the + sign is appropriate in Equation 1.81 and thus the angle of rotation θ is uniquely defined as

tan θ = \frac{\sqrt{(o_{z} − a_{y})^{2} + (a_{x} − n_{z})^{2} + (n_{y} − o_{x})^{2}}}{(n_{x} + o_{y} + a_{z} − 1)}  (1.82)

The components of k may be obtained from Equations 1.77-1.79 as

k_{x} = \frac{o_{z} − a_{y}}{2sinθ}  (1.83)
k_{y} = \frac{a_{x} − n_{z}}{2sinθ}  (1.84)
k_{z} = \frac{n_{y} − o_{x}}{2sinθ}  (1.85)

When the angle of rotation is very small, the axis of rotation is physically not well defined due to the small magnitude of both numerator and denominator in Equations 1.83-1.85. If the resulting angle is small, the vector k should be renormalized to ensure that |k| = 1. When the angle of rotation approaches 180^{∘} the vector k is once again poorly defined by Equation 1.83-1.85 as the magnitude of the sine is again decreasing. The axis of rotation is, however, physically well defined in this case. When θ < 150^{∘}, the denominator of Equations 1.83-1.85 is less than 1. As the angle increases to 180^{∘} the rapidly decreasing magnitude of both numerator and denominator leads to considerable inaccuracies in the determination of k. At θ = 180^{∘}, Equations 1.83-1.85 are of the form 0/0, yielding no information at all about a physically well defined vector k. If the angle of rotation is greater than 90^{∘}, then we must follow a different approach in determining k. Equating the diagonal elements of Equation 1.73 we obtain

k_{x}^{2}versθ + cosθ = n_{x}  (1.86)
k_{y}^{2}versθ + cosθ = o_{y}  (1.87)
k_{z}^{2}versθ + cosθ = a_{z}  (1.88)

Substituting for cosθ and versθ from Equation 1.76 and solving for the elements of k we obtain further

k_{x} = ±\sqrt{\frac{n_{x} − cosθ}{1 − cosθ}}  (1.89)

k_{y} = ±\sqrt{\frac{o_{y} − cosθ}{1 − cosθ}}  (1.90)

k_{z} = ±\sqrt{\frac{a_{z} − cosθ}{1 − cosθ}}  (1.91)

The largest component of k defined by Equations 1.89-1.91 corresponds to the most positive component of n_{x}, o_{y}, and a_{z}. For this largest element, the sign of the radical can be obtained from Equations 1.77-1.79. As the sine of the angle of rotation θ must be positive, then the sign of the component of k defined by Equations 1.77-1.79 must be the same as the sign of the left hand side of these equations. Thus we may combine Equations 1.89-1.91 with the information contained in Equations 1.77-1.79 as follows

k_{x} = sgn(o_{z} − a_{y})\sqrt{\frac{(n_{x} − cosθ)}{1 − cosθ}}  (1.92)

k_{y} = sgn(a_{x} − n_{z})\sqrt{\frac{(o_{y} − cosθ)}{1 − cosθ}}  (1.93)

k_{z} = sgn(n_{y} − o_{x})\sqrt{\frac{(a_{z} − cosθ)}{1 − cosθ}}  (1.94)

where sgn(e) = +1 if e ≥ 0 and sgn(e) = −1 if e ≤ 0.

Only the largest element of k is determined from Equations 1.92-1.94, corresponding to the most positive element of n_{x}, o_{y}, and a_{z}. The remaining elements are more accurately determined by the following equations formed by summing pairs of off-diagonal elements of Equation 1.73

n_{y} + o_{x} = 2k_{x}k_{y}versθ  (1.95)
o_{z} + a_{y} = 2k_{y}k_{z}versθ  (1.96)
n_{z} + a_{x} = 2k_{z}k_{x}versθ  (1.97)

If k_{x} is largest then

k_{y} = \frac{n_{y} + o_{x}}{2k_{x}versθ} from Equation 1.95  (1.98)

k_{z} = \frac{a_{x} + n_{z}}{2k_{x}versθ} from Equation 1.97  (1.99)

If k_{y} is largest then

k_{x} = \frac{n_{y} + o_{x}}{2k_{y}versθ} from Equation 1.95  (1.100)

k_{z} = \frac{o_{z} + a_{y}}{2k_{y}versθ} from Equation 1.96  (1.101)

If k_{z} is largest then

k_{x} = \frac{a_{x} + n_{z}}{2k_{z}versθ} from Equation 1.97  (1.102)

k_{y} = \frac{o_{z} + a_{y}}{2k_{z}versθ} from Equation 1.96  (1.103)

14 Example 1.1

Determine the equivalent axis and angle of rotation for the matrix given in Equations 1.41

Rot(y, 90)Rot(z, 90) = [ \begin{matrix}0 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.104)

We first determine cos θ from Equation 1.76

cosθ = \frac{1}{2}(0 + 0 + 0 − 1) = −\frac{1}{2}  (1.105)

and sin θ from Equation 1.81

sinθ = \frac{1}{2}\sqrt{(1 − 0)^{2} + (1 − 0)^{2} + (1 − 0)^{2}} = \frac{\sqrt{3}}{2}  (1.106)

Thus

θ = tan^{−1} (\frac{\sqrt{3}}{2}/\frac{−1}{2}) = 120^{∘}  (1.107)

As θ > 90, we determine the largest component of k corresponding to the largest element on the diagonal. As all diagonal elements are equal in this example we may pick any one. We will pick k_{x} given by Equation 1.92

k_{x} = +\sqrt{(0 + \frac{1}{2})/(1 + \frac{1}{2})} = \frac{1}{\sqrt{3}}  (1.108)

As we have determined k_{x} we may now determine k_{y} and k_{z} from Equations 1.98 and 1.99, respectively

k_{y} = \frac{1 + 0}{\sqrt{3}} = \frac{1}{\sqrt{3}}  (1.109)

k_{z} = \frac{1 + 0}{\sqrt{3}} = \frac{1}{\sqrt{3}}  (1.110)

In summary, then

Rot(y, 90)Rot(z, 90) = Rot(k, 120)  (1.111)

where

k = \frac{1}{\sqrt{3}}i + \frac{1}{\sqrt{3}}j + \frac{1}{\sqrt{3}}k  (1.112)

Any combination of rotations is always equivalent to a single rotation about some axis k by an angle θ, an important result that we will make use of later.

15 Stretching and Scaling

A transform T

T = [ \begin{matrix}a & 0 & 0 & 0 \\ 0 & b & 0 & 0 \\ 0 & 0 & c & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.113)

will stretch objects uniformly along the x axis by a factor a, along the y axis by a factor b, and along the z axis by a factor c. Consider any point on an object xi + yj + zk; its tranform is

[ \begin{matrix}ax \\ by \\ cz \\ 1 \end{matrix} ] = [ \begin{matrix}a & 0 & 0 & 0 \\ 0 & b & 0 & 0 \\ 0 & 0 & c & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ] [ \begin{matrix}x \\ y \\ z \\ 1 \end{matrix} ]  (1.114)

indicating stretching as stated. Thus a cube could be transformed into a rectangular parallelepiped by such a transform.

The Axiom code to perform this scale change is:

⟨scale⟩≡

      scale(scalex, scaley, scalez) ==
       matrix(_
        [[scalex, 0      ,0     , 0], _
         [0     , scaley ,0     , 0], _
         [0     , 0,      scalez, 0], _
         [0     , 0,      0     , 1]])

The transform S where

S = [ \begin{matrix}s & 0 & 0 & 0 \\ 0 & s & 0 & 0 \\ 0 & 0 & s & 0 \\ 0 & 0 & 0 & 1 \end{matrix} ]  (1.115)

will scale any object by the factor s.

16 Perspective Transformations

Consider the image formed of an object by a simple lens.

The axis of the lens is along the y axis for convenience. An object point x,y,z is imaged at x′,y′,z′ if the lens has a focal length f (f is considered positive). y′ represents the image distance and varies with object distance y. If we plot points on a plane perpendicular to the y axis located at y′ (the film plane in a camera), then a perspective image is formed.

We will first obtain values of x′, y′, and z′, then introduce a perspective transformation and show that the same values are obtained.

Based on the fact that a ray passing through the center of the lens is undeviated we may write

\frac{z}{y} = \frac{z′}{y′}  (1.116)

and \frac{x}{y} = \frac{x′}{y′}  (1.117)

Based on the additional fact that a ray parallel to the lens axis passes through the focal point f, we may write

\frac{z}{f} = \frac{z′}{y′ + f}  (1.118)

and \frac{x}{f} = \frac{x′}{y′ + f}  (1.119)

Notice that x′, y′, and z′ are negative and that f is positive. Eliminating y′ between Equations 1.116 and 1.118 we obtain

\frac{z}{f} = \frac{z′}{(\frac{z′y}{z} + f)}  (1.120)

and solving for z′ we obtain the result

z′ = \frac{z}{(1 − \frac{y}{f})}  (1.121)

Working with Equations 1.117 and 1.119 we can similarly obtain

x′ = \frac{x}{(1 − \frac{y}{f})}  (1.122)

In order to obtain the image distance y′ we rewrite Equations 1.116 and 1.118 as

\frac{z}{z′} = \frac{y}{y′}  (1.123)

and

\frac{z}{z′} = \frac{f}{y′ + f}  (1.124)

thus

\frac{y}{y′} = \frac{f}{y′ + f}  (1.125)

and solving for y′ we obtain the result

y′ = \frac{y}{(1 − \frac{y}{f})}  (1.126)

The homogeneous transformation P which produces the same result is

P = [ \begin{matrix}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & −\frac{1}{f} & 0 & 1 \end{matrix} ]  (1.127)

as any point xi + yj + zk transforms as

[ \begin{matrix}x \\ y \\ z \\ 1 − \frac{y}{f} \end{matrix} ] = [ \begin{matrix}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & −\frac{1}{f} & 0 & 1 \end{matrix} ] [ \begin{matrix}x \\ y \\ z \\ 1 \end{matrix} ]  (1.128)

The image point x′, y′,, z′, obtained by dividing through by the weight factor (1 − \frac{y}{f}), is

\frac{x}{(1 − \frac{y}{f})}i + \frac{y}{(1 − \frac{y}{f})}j + \frac{z}{(1 − \frac{y}{f})}k  (1.129)

This is the same result that we obtained above.

A transform similar to P but with −\frac{1}{f} at the bottom of the first column produces a perspective transformation along the x axis. If the −\frac{1}{f} term is in the third column then the projection is along the z axis.

17 Transform Equations

We will frequently be required to deal with transform equations in which a coordinate frame is described in two or more ways. A manipulator is positioned with respect to base coordinates by a transform Z. The end of the manipulator is described by a transform ^{Z}T_{6}, and the end effector is described by ^{T_{6}}E. An object is positioned with respect to base coordinates by a transform B, and finally the manipulator end effector is positioned with respect to the object by ^{B}G. We have two descriptions of the position of the end effector, one with respect to the object and one with respect to the manipulator. As both positions are the same, we may equate the two descriptions

Z^{Z}T_{6}^{T_{6}}E = B^{B}G  (1.130)

If we wish to solve Equation 1.130 for the manipulator transform T_{6} we must premultiply Equation 1.130 by Z^{−1} and postmultiply by E^{−1} to obtain

T_{6} = Z^{−1}BGE^{−1}  (1.131)

As a further example, consider that the position of the object B is unknown, but that the manipulator is moved such that the end effector is positioned over the object correctly. We may then solve for B from Equation 1.130 by postmultiplying by G^{−1}.

B = ZT_{6}EG^{−1}  (1.133)

18 Summary

Homogeneous transformations may be readily used to describe the positions and orientations of coordinate frames in space. If a coordinate frame is embedded in an object then the position and orientation of the object are also readily described.

The description of object A in terms of object B by means of a homogeneous transformation may be inverted to obtain the description of object B in terms of object A. This is not a property of a simple vector description of the relative displacement of one object with respect to another.

Transformations may be interpreted as a product of rotation and translation transformations. If they are intrepreted from left to right, then the rotations and translations are in terms of the currently defined coordinate frame. If they are interpreted from right to left, then the rotations and translations are described with respect to the reference coordinate frame.

Homogeneous transformations describe coordinate frames in terms of rectangular components, which are the sines and cosines of angles. This description may be related to rotations in which case the description is in terms of a vector and angle of rotation.

19 Denavit-Hartenberg Matrices

⟨domain DHMATRIX DenavitHartenbergMatrix⟩≡

--Copyright The Numerical Algorithms Group Limited 1991.
++ 4x4 Matrices for coordinate transformations
++ Author: Timothy Daly
++ Date Created: June 26, 1991
++ Date Last Updated: 26 June 1991
++ Description:
++   This package contains functions to create 4x4 matrices
++   useful for rotating and transforming coordinate systems.
++   These matrices are useful for graphics and robotics.
++   (Reference: Robot Manipulators Richard Paul MIT Press 1981)

)abbrev domain DHMATRIX DenavitHartenbergMatrix
--% DHMatrix
DenavitHartenbergMatrix(R): Exports == Implementation where
++ A Denavit-Hartenberg Matrix is a 4x4 Matrix of the form:
++  \spad{nx ox ax px}
++  \spad{ny oy ay py}
++  \spad{nz oz az pz}
++   \spad{0  0  0  1}
++ (n, o, and a are the direction cosines)
R : Join(Field,  TranscendentalFunctionCategory)
-- for the implementation of dhmatrix
minrow ==> 1
mincolumn ==> 1
--
nx ==> x(1,1)::R
ny ==> x(2,1)::R
nz ==> x(3,1)::R
ox ==> x(1,2)::R
oy ==> x(2,2)::R
oz ==> x(3,2)::R
ax ==> x(1,3)::R
ay ==> x(2,3)::R
az ==> x(3,3)::R
px ==> x(1,4)::R
py ==> x(2,4)::R
pz ==> x(3,4)::R
row ==> Vector(R)

  col ==> Vector(R)
  radians ==> pi()/180
  Exports ==> MatrixCategory(R,row,col) with
   "*": (%, Point R) -> Point R
     ++ t*p applies the dhmatrix t to point p
   identity: () -> %
     ++ identity() create the identity dhmatrix
   rotatex: R -> %
     ++ rotatex(r) returns a dhmatrix for rotation about axis X for r degrees
   rotatey: R -> %
     ++ rotatey(r) returns a dhmatrix for rotation about axis Y for r degrees
   rotatez: R -> %
     ++ rotatez(r) returns a dhmatrix for rotation about axis Z for r degrees
   scale: (R,R,R) -> %
     ++ scale(sx,sy,sz) returns a dhmatrix for scaling in the X, Y and Z
     ++ directions
   translate: (R,R,R) -> %
     ++ translate(X,Y,Z) returns a dhmatrix for translation by X, Y, and Z
  Implementation ==> Matrix(R) add
    identity() == matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
--    inverse(x) == (inverse(x pretend (Matrix R))$Matrix(R)) pretend %
--    dhinverse(x) == matrix( _
--        [[nx,ny,nz,-(px*nx+py*ny+pz*nz)],_
--         [ox,oy,oz,-(px*ox+py*oy+pz*oz)],_
--         [ax,ay,az,-(px*ax+py*ay+pz*az)],_
--         [ 0, 0, 0, 1]])
    d * p ==
       v := p pretend Vector R
       v := concat(v, 1$R)
       v := d * v
       point ([v.1, v.2, v.3]$List(R))

⟨rotatex⟩

⟨rotatey⟩

⟨rotatez⟩

⟨scale⟩

⟨translate⟩

20 License

⟨license⟩≡

--Portions Copyright (c) Richard Paul
--Copyright (c) 1991-2002, The Numerical ALgorithms Group Ltd.
--All rights reserved.
--
--Redistribution and use in source and binary forms, with or without
--modification, are permitted provided that the following conditions are
--met:
--
--    - Redistributions of source code must retain the above copyright
--      notice, this list of conditions and the following disclaimer.
--
--    - Redistributions in binary form must reproduce the above copyright
--      notice, this list of conditions and the following disclaimer in
--      the documentation and/or other materials provided with the
--      distribution.
--
--    - Neither the name of The Numerical ALgorithms Group Ltd. nor the
--      names of its contributors may be used to endorse or promote products
--      derived from this software without specific prior written permission.
--
--THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
--IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
--TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
--PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
--OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
--EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
--PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
--PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
--LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
--NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
--SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

⟨* ⟩≡ ⟨license⟩

⟨domain DHMATRIX DenavitHartenbergMatrix⟩

References

[1] Paul, Richard, Robot Manipulators, MIT Press, Cambridge, Mass., (1981)