Chapter 1 Rigid Body Kinematics

Home , Euler angles, Matrix exponential, Motion (geometry), Screw axis

Chapter 1

1.1 Introduction

This chapter builds up the basic language and tools to describe the motion of a rigid body – this is called rigid body kinematics. This material will be the foundation for describing general mechanisms consisting of interconnected rigid bodies in various topologies that are the focus of this book. Only the geometric property of the body and its evolution in time will be studied; the re- sponse of the body to externally applied forces is covered in Chapter ?? under the topic of rigid body dynamics. Rigid body kinematics is not only useful in the analysis of robotic mechanisms, but also has many other applications, such as in the analysis of planetary motion, ground, air, and water vehicles, limbs and bodies of humans and animals, and computer graphics and virtual reality. Indeed, in 1765, motivated by the precession of the equinoxes, Leonhard Euler decomposed a rigid body motion to a translation and rotation, parameterized rotation by three principal-axis rotation (Euler angles), and proved the famous Euler’s rotation theorem [?].

A body is rigid if the distance between any two points ﬁxed with respect to the body remains constant. If the body is free to move and rotate in space, it has 6 degree-of-freedom (DOF), 3 translational and 3 rotational, if we consider both translation and rotation. If the body is constrained in a plane, then it is 3-DOF, 2 translational and 1 rotational. We will adopt a coordinate-free approach as in [?, ?] and use geometric, rather than purely algebraic, arguments as much as possible. Coordinate representation is introduced to enable computational manipulation.

We will ﬁrst present some relevant mathematical background of rudimentary linear algebra, including vector space and linear transforms, in Section 1.2. The conﬁguration of a rigid body may be characterized by the position and orientation of a frame attached to the body. Various parameterizations of orientation, the rotation, or SO(3), group, is discussed in Section 1.3. The evolution on SO(3) and the relationship to angular velocity is discussed in Section 1.4. The full consideration of both position and orientation, the Euclidean, or SE(3), group, and its evoluation in time is given in Section 1.5. In Section 1.6, various distance metrics on SO(3) and SE(3) are introduced.

1 1.2 Linear Algebra

1.2.1 Linear Vector Space

A linear vector space, V, is a set that is closed under vector addition, +, and scalar multiplication. Elements of the set are called vectors and are denoted by an overhead arrow, e.g., ~v. The scalar field may be either real or complex. We will mostly just consider the real field. Vectors in a vector space may be endowed with the concept of “magnitude”, or norm, denoted by k~vk. Such a vector space is called a normed linear space. A unit vector is defined as a vector of unit magnitude, i.e., k~vk = 1. A vector space may be further equipped with an inner product < ~v, ~w>, which leads to the concept of projection and orthogonality. Such a vector space is called an inner product space. Vectors ~v and ~w are orthogonal if < ~v, ~w>= 0. The projection of ~v onto a unit vector ~e is defined as < ~v,~e> ~e. A basis of a linear vector space is a collection of vectors {~v1, . . . ,~vn} such that every vector V may be represented as a unique linear combination of these vectors. The number of basis vectors, n, is called the dimension of V.

Figure 1.1: Vector addition, orthogonality, and projection

Example: The linear vector space that will be considered most frequently in this book is the 3- dimensional Euclidean space, E3, with inner product denoted by ~v · ~w = k~vk k~wk cos θ, where θ is the angle between ~v and ~w and k~vk is the Euclidean norm of ~v. For E3, there is also an additional operation called the cross product, ~v × ~w, which produces a vector orthogonal to both ~v and ~w according to the right-hand rule, (i.e., pointing the ﬁngers, except the thumb, along ~v then curl the ﬁngers toward ~w, then the thumb points in the direction of ~v × ~w), with magnitude k~v × ~wk = k~vk k~wk |sin θ|, where θ is the angle between ~v and ~w.

1.2.2 Linear Transformation A linear transforms maps one vector space to another and satisﬁes the rule of superposition. Let L : V1 → V2, then L is a linear transform if

L(α1~v1 + α2~v2) = α1L(~v1) + α2L(~v2), for all α1, α2 ∈ R,~v1,~v2 ∈ V.

Examples:

• Dot product: L : E3 → R deﬁned by L~v = ~w · ~v, where ~w is a given vector in E3.

• Cross product: L : E3 → E3 deﬁned as L~v = ~w × ~v, where ~w is a given vector in E3.

2 If V1 and V2 are inner product spaces, we can deﬁne the adjoint of a linear transformation, ∗ which in the matrix case is just the transpose matrix. Given L : V1 → V2, L V2 → V1 is the adjoint of L if it satisﬁes ∗ < ~v2, L~v1 >V2 =< L ~v2,~v1 >V1

for all ~v1 ∈ V1 and ~v2 ∈ V2.

1.2.3 Orthonormal Frame

The orientation of a rigid body in E3 is completely characterized by an orthonormal frame attached to the body. A right-handed orthonormal frame is deﬁne as follows:

Deﬁnition 1 (~e1,~e2,~e3) is an orthonormal frame if

1. k~eik = 1, i = 1, 2, 3 (normality)

2. ~ei · ~ej = 0 if i 6= j, i, j = 1, 2, 3 (orthogonality)

3. ~e1 × ~e2 = ~e3 (right-hand rule).

Figure 1.2: An orthonormal frame

By writing h i E = ~e1 ~e2 ~e3

we can regard E : R3 → E3 as a linear transformation. The adjoint of E, E ∗ : E3 → R3, may be readily computed:   ~e1· ∗   E =  ~e2·  . ~e3·

∗ ∗ Note that E E = I3, the 3 × 3 identity matrix. Similarly EE = I, the identity operator between E3. 3 The orthonormal vectors (~e1,~e2,~e3) is also called an orthonormal basis of E since every vector 3 in E may be represented as a unique linear combination of (~e1,~e2,~e3).

3 1.2.4 Coordinate Representation of Vectors and Linear Transforms

Since E provides an orthonormal basis for E3, every ~v ∈ E3 may be uniquely represented by a vector in R3: ~v = EvE , (1.1) 3 where the vector vE ∈ R is called the representation of ~v in E. The frame E is now considered as a coordinate frame (with coordinate axes given by (~e1,~e2,~e3)). The elements of vE , the ordered set (v1, v2, v3), is the coordinate of ~v in the coordinate frame E. To obtain the coordinate of ~v, we can simply project ~v onto the coordinate axes:

∗ vE = E ~v. (1.2) This follows directly from the orthonormality of E. Consider a linear transform L : V1 → V2 where V1 and V2 are inner product spaces. Given orthonormal frames E1 and E2 for V1 and V2 respectively, the coordinate representation of L~v is ∗ ∗ E2 L~v = E2 LE1v1. It is natural to deﬁne the coordinate representation of L by

∗ L = E2 LE1. (1.3)

Because of the orthonormality of E1 and E2, we have the reverse identity

∗ L = E2LE1 . (1.4) Example:

• Let ~ei be the ith basis vector in the orthonormal frame E, then (~ei)E is the ith unit vector in R3. 3 3 • Consider an orthonormal frame E : R → E as a linear transform, then EE = I3. • Consider the dot product as a linear transformation, L : E3 → R, L~v = ~w · ~v. Let E be an orthonormal frame for E3. Then h i T ~w · ~v = ~w ·Ev = ~w · ~e1 ~w · ~e2 ~w · ~e3 v = w v

where w is the representation of ~w in E. Therefore, the coordinate representation of L is wT .

• Consider the cross product as a linear transformation, L : E3 → E3, L~v = ~w × ~v. Let E be an orthonormal frame for E3. Then h i ~w × ~v = ~w × Ev = ~w × ~e1 ~w × ~e2 ~w × ~e3 v.

The coordinate representation of the cross product, denoted by wb, is therefore the 3 × 3 skew symmetric matrix below:  0 −w w  h i 3 2 ∗   wb = E ~w × ~e1 ~w × ~e2 ~w × ~e3 =  w3 0 −w1  . (1.5) −w2 w1 0

4 All 3 × 3 matrices of the form above form a group called so(3). We denote the inverse operation of cross product as ∨ : so(3) → R3, i.e., given A ∈ so(3),   0 −a3 a2   A =  a3 0 −a1  −a2 a1 0

A∨ is deﬁned as   a1 ∨   A =  a2  . a3

3 ∨ Clearly, for any w ∈ R , (wb) = w. A list of useful identities involving the cross product are given below:

~v × ~v = 0, vvˆ = 0 (1.6a)

T vˆ = −vˆ (vb is skew-symmetric) (1.6b) ~v × ~w = −~w × ~v, vwˆ = −wvˆ (1.6c) T T ~v × (~w×) = ~w~v · −(~v · ~w)I, vˆwˆ = wv − (v w)I3 (1.6d) (~v × ~w)× = ~w~v · −~v~w·, vwdˆ = wvT − vwT (1.6e) 2 3 2 ~v × (~v × (~v×)) = − k~vk ~v×, vb = vbvbvb = − kvk vb (1.6f)

If k~vk = 1, (~v×)4k = −(~v×)2 = −~v × (~v×), (~v×)4k−1 = −(~v×), (~v×)4k−2 = (~v×)2, (~v×)4k−3 = (~v×), 4k 2 4k−1 4k−2 2 4k−3 vb = −vb , vb = −v,b vb = vb , vb = v,b k = 1, 2,.... (1.6g)

1.2.5 Coordinate Transformation

Given a vector ~v ∈ V. Let Ea and Eb be two orthonormal frames of V. Then ~v may be represented in either coordinate: ~v = Eava = Ebvb. (1.7)

The two representations va and vb are related by

∗ va = Ea Ebvb. (1.8)

The linear transformation ∗ Rab := Ea Eb (1.9)

is a 3 × 3 matrix that converts the representation in Eb to Ea. It is easy to verify that Rab is −1 T −1 orthogonal, i.e., Rab = Rab, and Rab = Rba. Given a linear transform L : V → V, it can be similarly represented in either Ea or Eb:

∗ ∗ L = EaLaEa = EbLbEb .

5 The relationship between La and Lb is then

La = RabLbRba, (1.10) −1 T where we have use the identity Rab = Rab = Rba. Example: • Dot product: The dot product is invariant under coordinate transformation: T T ~w · ~v = wa va = wb vb. • Cross product: The tranformation of the cross product is given by (1.10):

vca = Rdabvb = RabvbbRba.

1.3 Rotation Group SO(3)

1.3.1 Interpretation of Coordinate Transformation between Orthonormal Frames

The coordinate transformation matrix Rab ∈ SO(3) has various interpretations:

• Rab changes the representation of a vector from Eb to Ea:

va = Rabvb.

• Rab changes the representation of a linear transform from Eb to Ea:

La = RabLbRba.

• Rab consists of the cosines of angles between axes in Ea and Eb (it is therefore also called the direction cosine matrix):   ~ea1 · ~eb1 ~ea1 · ~eb2 ~ea1 · ~eb3 ∗   Rab = Ea Eb =  ~ea2 · ~eb1 ~ea2 · ~eb2 ~ea2 · ~eb3  .

~ea3 · ~eb1 ~ea3 · ~eb2 ~ea3 · ~eb3

The (i, j)th entry of Rab is cos θij where θij is the angle between ~eai and ~ebj .

• Rab is the representation of Eb in Ea, i.e.,

Rab = (Eb)a.

• Rab rotates Ea to Eb, represented in either Ea or Eb. To be speciﬁc, let R be the rotation operator transforming Ea to Eb, i.e., Eb = REa. (1.11)

Then the representation of R in Ea is ∗ ∗ (R)a = Ea (EbEa )Ea = Rab.

and the representation of R in Eb is ∗ ∗ (R)b = Eb (EbEa )Eb = Rab.

6 In the last interpretation of Rab, as a rotation of frames, we introduce the concept of a rotation operator. To be precise, an operator R : E3 → E3 is a rotation if for any orthonormal frame E, RE is also an orthonormal frame. An immediate consequence of the deﬁnition is that R is orthogonal, i.e., RR∗ = R∗R = I. T The coordinate transformation matrix Rab is orthogonal (i.e., RabRab = I) and detRab = 1 (due to the right-handed coordinate system). Furthermore, products of such matrices retain the same properties. These matrices form a group called the special orthogonal group with the group action given by the matrix multiplication. Since we are dealing with the Euclidean space, the dimension is three. This group is therefore denoted by SO(3). Due to the rotation interpretation, SO(3) is also called the rotation group. In the robotic literature, e.g., [?, ?, ?], the perspective is inertial-centric, i.e., all coordinate frames eventually refer to the inertial frame. Therefore, orientation of a rigid body is usually denoted by Rob, the representation of the body frame in the inertial frame. For example, a common representation of the end effector tool frame is [n, o, a] (normal-open-approach, see Fig. 1.3). How- ever, for the spacecraft dynamics literature, e.g., [?, ?], the perspective is typically body-centric. In that case, the orientation, also called the attitude, of the spacecraft is usually denoted by Rbo, the representation of the inertial frame in the body frame. This results in slightly different forms of the kinematic equations, as we shall see later.

Figure 1.3: Orthonormal frame based on the normal-open-approach (noa) convention for parallel- jaw gripper

Example: Let Eb be Ea rotated about ea3 over an angle θ (see Fig. 1.4). Then

~eb1 = cos θ ~ea1 + sin θ ~ea2 , ~eb2 = − sin θ ~ea1 + cos θ ~ea2 .

Since Rab = (Eb)a, we have  cos θ − sin θ 0    Rab =  sin θ cos θ 0  . 0 0 1

Similarly, for rotation about ea1 ,  1 0 0    Rab =  0 c −s  0 s c

where c denotes cos θ and s denotes sin θ. For rotation about ea2 ,

 c 0 s    Rab =  0 1 0  . −s 0 c

7 1.3.2 Representation of Rotation Operator: Euler-Rodrigues Formula We now consider a special type of operator, one that rotates a frame about a given unit vector ~k over an angle θ. By construction, such an operator is clearly a rotation since the operator applied to any orthonormal frame would also result in an orthonormal frame. We ﬁrst ﬁnd an algebraic representation of this operator. Let R be a rotation about a unit vector ~k over an angle θ. Consider an arbitrary vector ~v in E3. As shown in Fig. 1.5(a), R~v may be decomposed as the sum of the projection of ~v on ~k and an orthogonal component, (R~v)⊥: ~ ~ R~v = (k · ~v)k + (R~v)⊥. (1.12) The top view of the cone in Fig. 1.5(a) is a circle formed by the tip of ~v rotating about ~k, as in Fig. 1.5(b). Then (R~v)⊥ may be decomposed as ~ ~ ~ (R~v)⊥ = sin θk × ~v − cos θk × (k × ~v). (1.13)

Substituting back into (1.12), we get

R~v = (~k~k · + sin θ~k × − cos θ~k × (~k×))~v.

Since ~v is arbitrary, we obtain the Euler-Rodrigues Formula in the operator form:

R = (~k~k · + sin θ~k × − cos θ~k × (~k×)) = I + sin θ~k × +(1 − cos θ)~k × (~k×) (1.14) where the last equality follows from (1.6d). This equation may also be represented in an orthonormal frame as: R = I + sin θkˆ + (1 − cos θ)kˆ2, (1.15) where k is the coordinate representation of ~k. Due to the body-centric perspective mentioned earlier, in the spacecraft literature, this expression is more commonly written as

RT = I − sin θkˆ + (1 − cos θ)kˆ2, where RT is considered to be the spacecraft attitude.

1.3.3 Euler’s Rotation Theorem It is easy to verify that R given by the Euler-Rodrigues formula (1.15) indeed belongs to SO(3). We now consider the converse: given R ∈ SO(3), does there exist a corresponding k and θ? If so,

Figure 1.4: Rab as (Eb)a in rotation about ~e3.

8 are they unique? In other words, is (1.15) a complete parameterization of SO(3)? To answer these questions, we will show that k and θ may be solved directly from (1.15) for any R ∈ SO(3). From (1.15), it is straightforward to show that 1 cos θ = (trR − 1). (1.16) 2 By manipulating the (i, i)th element of R, we can ﬁnd an expression for the ith element of k:

2Ri,i − trR + 1 1 k = ±( ) 2 . (1.17) i 3 − trR Note that there are two possible solutions of the axis of rotation, corresponding to the two possible directions of rotation to achieve the same end result. The above approach breaks down only when trR = 3, i.e., R = I. In this case, k is an arbitrary unit vector and θ = 0. Once k is found, we can ﬁnd sin θ from the difference between R and RT : 1 sin θ = kT (R − RT )∨. (1.18) 2 The rotational angle may then be found from 1 1 θ = atan2( kT (R − RT )∨, (trR − 1)). (1.19) 2 2 There are two solutions, corresponding to the two solutions of k. Note that to determine the quadrant of θ, the atan2 function is used (requiring both sine and cosine). Since every R ∈ SO(3) can be represented by (k, θ) (and (−k, −θ)), and every (k, θ) generates a rotation matrix through the Euler-Rodrigues formula (1.15), (k, θ) completely parameterizes SO(3). This is commonly referred to as the equivalent-axis/angle representation of the rotation group. This representation has also been called the Euler’s Theorem on Rotation which states that every rigid body rotation can be represented as a single rotation about an equivalent axis. Note that k is also the eigenvector of R corresponding to the eigenvalue 1. This makes intuitive sense since rotation of k about itself is just k. As we shall see in the next section, the non-zero eigenvalues of R are of the form e±jθ. This provides an alternate, but more computationally ex- pensive, method of computing (k, θ) from R. In the case that R = I, all three eigenvalues are 1. Therefore, k is arbitrary and θ = 0.

(a) Rotation of ~v about ~k over angle θ (b) Top view

Figure 1.5: Derivation of Euler-Rodrigues formula in representing a rotation operator R

9 1.3.4 Matrix Exponential Formula In this section, we show that rotation about a unit vector can also be represented as an exponential operator by using the Cayley-Hamilton Theorem [?, ?]. This is a convenient parameterization that we will use throughout the book. Consider the 3 × 3 matrix given by the matrix exponential exp(kθˆ ). From the Cayley-Hamilton Theorem (a square matrix satisﬁes its own characteristic equation) and the inﬁnite series expansion of the exponential function, we have the following matrix polynomial expression for exp(kθˆ ):

ˆ ˆ ˆ2 exp(kθ) = a0I3 + a1k + a2k . (1.20)

The eigenvalues of kˆ are {0, ±j}. Evaluating (1.20) along the eigenvectors, we have

1 = a0 jθ e = a0 + a1j − a2 (1.21) −jθ e = a0 − a1j − a2.

These equations can be used to solve for (a0, a1, a2):

a0 = 1, a1 = sin θ, a2 = (1 − cos θ). (1.22)

Substituting back to (1.20), we have exactly the Euler-Rodrigues Formula (1.15). This means that every R ∈ SO(3) may be written as a matrix exponential exp(kθˆ ) where k and θ are solved from (1.17) and (1.19). The rotation operator, R, may also be represented as a coordinate-free exponential operator

R = exp(θ~k×), (1.23) where the exponential operator is interpreted in the inﬁnite series sense and is equivalent to (1.14). Of course, when R is represented in a coordinate frame, we recover the matrix exponential:

E ∗RE = exp(kθb ) (1.24)

where k is ~k represented in E.

1.3.5 Representations of rotation group Rotation group consists of 3 × 3 matrix subject to 6 orthonormality constraints. From Euler’s Theorem, we can also represent it in terms (k, θ) (subject to the directions of the rotation axis and rotation), which contains 4 parameters and 1 constraint (k is a unit vector). This is sometimes called the equivalent-axis/angle representation. It is natural to ask if there are other possible representations SO(3). In this section, we will examine several common representations, including Euler parameters (also known as quaternion, consisting of sine and cosine of the half angle), Euler- Rodrigues parameters (also known as Gibb’s vector, consisting of the tangent of the half angle), and all possible types of Euler angles (consisting of angles of three principal-axis rotations).

10 1.3.5.1 Euler parameters (Quaternions)

3 Euler parameters consist of a scalar part, q0, and a vector part qv ∈ R . They may be deﬁned through equivalent axis and angle:

θ ! θ ! q = cos , q = sin k. (1.25) 0 2 v 2

From this deﬁnition, it follows that 2 2 q0 + kqvk = 1. (1.26)

Therefore, (q0, qv) is a four-parameter representation of SO(3) with one constraint. The Euler " q # parameters represented as a 4 × 1 vector, q = 0 , is also called unit quaternion. The constraint qv then becomes the unit norm constraint: kqk = 1.

Given (q0, qv), the corresponding rotation matrix R can be found from the Euler-Rodrigues Formula: 2 R(q) = I3 + 2q0qˆv + 2ˆqv . (1.27)

Conversely, given R ∈ SO(3), we can ﬁnd q0

1√ q = ± trR + 1. (1.28) 0 2

If q0 6= 0, then qv may be found similar to equivalent axis:

R − RT !∨ qv = . (1.29) 4q0

If q0 = 0, then R + I = q qT , (1.30) 2 v v

which can be used to ﬁnd qv (e.g., by factorizing (R + I)/2 or ﬁnding the eigenvector of (R + I)/2 that corresponds to the eigenvalue 1).

Vector quaternion qv itself may be used to represent SO(3). This is a three-parameter represen- 3 tation with no constraint. In this case, R, for a given qv ∈ R , is given by (1.27) with q0 replaced 2 1 by (1 − kqvk ) 2 : 2 1 2 2 R(qv) = I3 + 2(1 − kqvk ) qˆv + 2ˆqv . (1.31)

Given R ∈ SO(3), qv may be found as

R − RT !∨ qv = √ . (1.32) 2 trR + 1

If trR = −1, then the alternative formula (1.30) should be used.

11 1.3.5.2 Euler-Rodrigues Parameters (Gibb’s Vector) Another three-parameter representation is the Euler-Rodrigues parameters which may be deﬁned using equivalent axis and angle or unit quaternion:

θ ! q g = tan k = v . (1.33) 2 q0 The origin of Euler-Rodrigues parameters is from the identity

(exp(kθˆ ) − I) =g ˆ(exp(kθˆ ) + I). (1.34)

It is a straightforward exercise to show that R is related to g by gˆ gˆgˆ R = I3 + 2 + 2 , (1.35) 1 + kgk2 1 + kgk2 and g may be obtained from R using

(R − RT )∨ g = ± . (1.36) trR + 1 Note that when trR → −1, kgk → ∞.

1.3.5.3 Euler Angles By far the most common three parameter representation is Euler angles. An arbitrary rotation may be decomposed as the concatenation of three principal axis (coordinate axis) rotations. The three rotation angles, called (classic) Euler angles, together with the speciﬁed axes, then completely parameterize any rotation matrix. As an example, take the common yaw-pitch-roll (also called 321 or zyx) Euler Angles representation of a frame E3 represented in the refererence frame E0. The frame E3 may be obtained from E0 through three principal-axis rotations. First generate E1 by rotating E0 about its z-axis over ∗ T the roll angle, φ. The corresponding orientation matrix is R01 = E0E1 = exp(zφb ), z = [0, 0, 1] . Then generate E2 by rotating E1 about its y-axis over the pitch angle, θ. The corresponding rotation ∗ T matrix is R12 = E1E2 = exp(yθb ), y = [0, 1, 0] . Finally, obtain E3 by rotating E2 about its x-axis ∗ over the yaw angle, ψ. The corresponding rotation matrix is R23 = E2E3 = exp(xψb ), x = [1, 0, 0]. The overall rotation is then

R03 = R01R12R23 = exp(zφb )exp(yθb )exp(xψb ). (1.37) In the coordinate-free form, we have

∗ ∗ ∗ ∗ R03 = E3 E0 = E3 E2E2 E1E1 E0 = exp(ψ~x×)exp(θ~y×)exp(ψ~z×). (1.38) which transforms E0 to E3. 3 In general, for any R ∈ SO(3) and three unit vectors in R , (a1, a2, a3), a2 not collinear with either a1 or a3, R may be written as R = R01R12R13 (1.39)

12 where Ri−1,i = exp(abiβi) (1.40) corresponds to the rotation of Ei−1 about ai over an angle βi. The triplet (β1, β2, β3) is called the Euler-Angle representation of R with the corresponding rotation axes (a1, a2, a3). If ai is a coordinate axis, i.e., x, y, or z, then (β1, β2, β3) are called the classic Euler angles. There are 12 possible classic Euler angles, (x, y, x), (x, y, z), (x, z, x), (x, z, y), (y, x, y), (y, x, z), (y, z, x), (y, z, y), (z, x, y), (z, x, z), (z, y, x), (z, y, z), where (z, y, x) is the yaw-pitch-roll representation, and (z, x, z) is a natural representation for a gyroscope. A variation of classic Euler angles may have ai pointing in the negative direction of a coordinate axis, and the coordinate axes may be re- deﬁned (e.g., x becomes y, etc.), for example, the tool frame representation of the original Unimate controller (for PUMA robots) is a variant of the 313 Euler angles:  0 1 0    R03 = exp(zβb 1)  0 0 −1  exp(yβb 2)exp(zβb 3), −1 0 0

which means that (x, y, z) axes in E1 becomes (y, −z, −x) before the next rotation (about the x axis, or −z axis of E1). Euler angles for a given R ∈ SO(3) may be found by simple algebra. For example, the roll- pitch-yaw Euler angles are related to the orientation by multiplying the three rotation matrices in (1.37):   c1c2 −s1c3 + c1s2s3 s1s3 + c1s2c3   R = exp(zφb )exp(yθb )exp(xψb ). =  s1c2 c1c3 + s1s2s3 −c1s3 + s1s2c3  −s2 c2s3 c2c3

where the short hand notation ci denotes cos βi and si denotes sin βi, i = 1, 2, 3. Given R, the Euler angles may be readily computed:

−1 (3,1) −1 (3,1) β2 = sin R or sin R + π −1 (1,2) (1,1) −1 (1,2) (1,1) β1 = tan (R ,R ) or tan (R ,R ) + π −1 (3,2) (3,3) −1 (3,2) (3,3) β3 = tan (R ,R ) or tan (R ,R ) + π

where R(i,j) denotes the (i, j)th element of R and sin−1R(3,1) is restricted to [−π/2, π/2]. Note π that in general there are two solutions for each R, except for β2 = ± 2 where the two solutions merge. We shall see that this corresponds to the singularity of the representation (see Section 1.4 below).

1.4 Differential Kinematics on SO(3)

We have so far described a rigid body ﬁxed in space; now let us consider the motion of this rigid body. First consider the rotation of a vector ~v about a unit vector ~k over a small angle θ:

exp(θ~k×)~v = (I + sin θ~k × +(1 − cos θ)~k × (~k×))~v ≈ ~v + θ~k × ~v. (1.41)

The difference between the rotated ~v and ~v is therefore orthogonal to ~v.

13 h i Now consider E = ~e1 ~e2 ~e3 to be an orthonormal frame attached to a rigid body. Suppose h i 0 0 0 0 that over a time interval δt, E rotates to E = e~ 1 e~ 2 e~ 3 . If δt is small, the angle of rotation 0 would also be small. Then, as argued above, e~ i − ~ei would be approximately orthogonal to ~ei. Therefore, E 0 is approximately given by

0 e~ 1 = ~e1 + δt(a12~e2 + a13~e3) 0 e~ 2 = ~e2 + δt(a21~e1 + a23~e3) (1.42) 0 e~ 3 = ~e3 + δt(a31~e1 + a32~e2)

0 for some constants aij. To ensure E ∈ SO(3), up to order δt, we need to have

a21 = −a12, a32 = −a23, a13 = −a31.

Deﬁne   a23   ω =  a31  . a12 We can then write (1.43) as   0 −ω3 ω2 0   E − E = δtE  ω3 0 −ω1  = δtEω.b (1.43) −ω2 ω1 0

0 dE As δt → 0, (E − E)/δt becomes dt , and we obtain the differential kinematics equation for a rigid body as dE = Eω = ~ω × E, (1.44) dt b where ~ω is the angular velocity vector that characterizes the instantaneous evolution of E, and ω is the representation of ~ω in E. The collection of all 3 × 3 matrices of the form ωb is called so(3), which may be considered as the “tangent” of SO(3). a∗ b Using (1.44), we now derive the evolution of the rotation matrix Rab = E E . Denote by ~ωi the angular velocity of Ei, i = a, b. We then have dR ab = (~ω × E )∗E + E ∗(~ω × E ) dt a a b a b b

= −(~ωda)aRab + (~ωdb)aRab

= (~ωb −d~ωa)aRab.

Deﬁne the relative angular velocity between Ea and Eb by

~ωb/a := ~ωb − ~ωa. (1.45)

Denoting (~ωb/a)a by (ωb/a)a, the differential kinematics equation for the rotation matrix can be written as: dRab = (ωd) R , (1.46) dt b/a a ab

14 which may be also written in an equivalent form as

dRab = R (ωd) . dt ab b/a b

Given R ∈ SO(3), we may write its derivative as

R˙ =ωR. ˆ (1.47)

In this case, R may be considered as the orientation of a rigid body with respect to the inertial frame and ω is the angular velocity of the body with respect to the inertial frame, respresented in the inertial frame.

As mentioned earlier, the spacecraft literature is body-centric and usually uses Rba for orientation. In that case, the differential kinematics is usually written as

dRba = −(ωd) R = −R (ωd) , dt b/a b ba ba b/a a

where we have used the fact that ~ωb/a = −~ωa/b. ˆ Consider the exponential representation of Rab = exp(kθ). Suppose k is ﬁxed and only θ varies dRab with time, then dt can be computed directly from the Euler-Rodrigues formula:

dR ab = kˆθR˙ . (1.48) dt ab

˙ In this case, the angular velocity (ωb/a)a = kθ. We next consider the differentiation of a vector. Let ~v be a vector with representations va and vb in Ea and Eb, respectively. By direct differentiation of (1.8), we have

dva d(Rabvb) = = R˙ v + R v˙ = (ωd) v + R v˙ . (1.49) dt dt ab b ab b b/a a a ab b

If ~v is a ﬁxed vector in Eb, then v˙b = 0 and v˙a = (ωdb/a)ava. Now consider the differentiation of a linear transform. Let L : SO(3) → SO(3) be a linear transform with representations La and Lb in Ea and Eb, respectively. Differentiate (1.10), we obtain

dLa d(RabLbRba) = = (ωd) R L R − R L R (ωd) + R L˙ R dt dt b/a a ab b ba ab b ba b/a a ab b ba ˙ = (ωdb/a)aLa − La(ωdb/a)a + RabLbRba. (1.50)

We shall introduce the inertia of a rigid body, J , as a linear transform in Section ??. Let Eb be a ﬁxed orthonormal frame attached to the body. Assuming the mass property remains constant, then ˙ Jb = 0, and ˙ Ja = (ωdb/a)aJa − Ja(ωb/a)a.

15 1.4.1 Differential Kinematics for Different Representations of SO(3)

Let p be a R` representation of R ∈ SO(3). The time derivative of p is of the following general form: dp = J (p)ω, (1.51) dt p `×3 where Jp ∈ R is called the representation Jacobian. If Jp(p) loses rank, p cannot be solved instantaneously. This is called a singularity of the representation. Such singularities are mathematical in nature as a deﬁciency in the local representation of SO(3) with p. It is not a physical singularity in the sense that the rigid body cannot move in certain direction. To calculate the Jacobian, we can just differentiate the kinematics equations in Section 1.3.5 and use (1.47): ω = (RT R˙ )∨. (1.52)

For equivalent axis and angle representation, the differential kinematics is given by (you are asked to veriﬁed this as a homework):

" θ˙ # " kT # ˙ = 1 θ 2 ω. (1.53) k − 2 (kb + cot 2 kb )

The Jacobian is full rank except at θ = 0 when it becomes unbounded. For the unit quaternion, the differential kinematics is

" q˙ # 1 " −qT # 0 = v ω. (1.54) q˙v 2 q0I − qˆv

T 1 2 Note that J J = 4 (1 + q0)I in this case, which means that it is never singular! For this reason, unit quaternion is a popular representation. For the vector quaternion, the differential kinematics is

1 q q˙ = 1 − kq k2 − q ω. (1.55) v 2 v cv

The Jacobian is singular if and only if kqvk = 1. For Gibb’s vector, we have 1 β˙ = (I − βb + ββT )ω. (1.56) 2

The Jacobian is full rank except when kβk = ∞ (which is equivalent to kqvk = 1). For the yaw-pitch-roll (321) Euler Angle, we can apply (1.52) to obtain

 ˙ ˙    −s1θ2 + c1c2θ3 0 −s1 c1c2  ˙ ˙    ˙ ω =  c1θ2 + s1c2θ3  =  0 c1 s1c2  θ. (1.57) ˙ ˙ θ1 − s2θ3 1 0 −s2 which has singularities at cos β2 = 0, as indicated before.

16 1.5 Euclidean Group SE(3)

The rigid body orientation is represented by an orthonormal frame, E, attached to the body. For the position of the body, we need to additionally designate the origin of the frame, O. Together (O, E) represent the position and orientation of the body, and we call it an Euclidean frame. Math- ematically, O is a point and may be considered as a vector from some reference point. As such, if Oa and Ob are two points, then ~pab = Ob − Oa is deﬁned as vector from Ob to Oa. Given two rigid bodies with corresponding Euclidean frames (Oa, Ea) and (Ob, Eb). The relative position and ∗ orientation is ~pab and EbEa , which may be represented in Ea as (pab,Rab) where pab = (~pab)a and ∗ 3 Rab = Ea Eb. A common representation of (p, R), p ∈ R , R ∈ SO(3), is a 4 × 4 homogeneous transformation " R p # H = . (1.58) 0 1 Such matrices form a non-commutative group called the special Euclidean group of order 3, SE(3), with the group operation given by the matrix multiplication. The inverse of H is given by " RT −p # H−1 = . 0 1

Consider a vector ~rb attached to Ob. Seen from Oa, the vector becomes

~ra = ~rb + ~pab.

Representing in Ea we have ra = Rabrb + pab. This may be written as " r # " r # a = H b , (1.59) 1 ab 1 where " R p # H := ab ab . (1.60) ab 0 1

Therefore Hab ∈ SE(3) plays a similar role as Rab in SO(3): transforming a vector in body b and represented in Eb to a vector in body a and represented in Ea. The following property of Hab is straightforward to verify:

−1 HabHbc = Hac,Hab = Hba,Haa = I4.

1.5.1 Matrix Exponetial Formula

For R ∈ SO(3), we can represent it as a matrix exponential R = exp(kθb ) where k is the equivalent axis and θ the equivalent angle. Does H ∈ SE(3) have a similar matrix exponential representation and interpretation? Let " k # ξ = , k ∈ 3, kkk = 1, v ∈ 3. v R R

17 Deﬁne " # kb v ξb = . (1.61) 0 0 All 4 × 4 matrices of the form above form a group, called se(3), which as we shall see later may be considered as the “tangent” of SE(3). As in the SO(3) case, we can apply the Cayley-Hamilton Theorem to evaluate exp(ξθb ): " # exp(kθb )(I − exp(kθb ))kvb + θkkT v exp(ξθb ) = . (1.62) 0 1

Equating this expression with (1.58), we see that (k, θ) are just the equivalent axis and angle of R, i.e., R = exp(kθb ) and the vector v is related to p through

((I − R)kb + θkkT )v = p. (1.63)

It can be shown that the matrix ((I − R)kb + θkkT ) is invertible if and only if θ = 0. Therefore, if p θ 6= 0, v is determined uniquely from p. When θ = 0, R = I and k is arbitrary. We choose k = kpk and v to be collinear with k. Then as θ → 0,

((I − R)kb + θkkT )v → θv = p.

The matrix exponential formula for rotation (1.24) may be interpreted as a rotation about a unit axis k over an angle θ. The matrix exponential expression for Euclidean motion (1.62) also has a intuitive geometric interpretation. Consider the interpretation of (p, R) ∈ SE(3) as a transformation of a vector rb to ra as in (1.59). Apply (1.63), then

T T ra = Rabrb + pab = Rabrb + ((I − Rab)kb + θkk )v =v ˆ + Rab(rb − kvb ) + (k v)θk.

This may be visualized as a screw motion of the vector rb − kvb rotating and translating about k as shown in Figure xxx. The unit vector k is the screw axis, θ is the screw magnitude, and kT vθ is the pitch.

1.5.2 Differential Kinematics on SE(3) Similar to the differential kinematics on SO(3), (1.46), we can propagate in SE(3) by differenti- ating Hab in (1.60): " # R˙ p˙ H˙ = ab ab ab 0 0 " # ((ωd) R p˙ = a/b )a ab ab 0 0 " #" # ((ωd) p˙ − ((ωd) p R p = a/b )a ab a/b )a ab ab ab . (1.64) 0 0 0 1 Deﬁne " # d p˙ab − ((ωa/b))apab (Va/b)a = . (1.65) d ((ωa/b))a

18 We then have the generalization of (1.46) to SE(3): ˙ Hab = (Va/b)aHab. (1.66)

d Note that the linear velocity portion (Va/b)a is Rab dt (Rbapab), or the derivative of pab in the B frame represented in the A frame. We can also write ˙ Hab = Hab(Va/b)b (1.67)

with " # Rbap˙ab (Va/b)b = . (1.68) (ωa/b)b

The linear velocity portion (Va/b)b is now the derivative of pab in the A frame represented in the B frame. The 6 × 1 vectors (Va/b)a and (Va/b)b are called spatial velocities (in A and B frames, respectively), which are transformed according to: " # Rab pˆabRab (Va/b)a = (Va/b)b. (1.69) 0 Rab

The transformation matrix above is called the adjoint transformation and denoted by AdHab . Note that −1 −1 Ad = Ad = AdHba . Hab Hab

In summary, Hab trasnforms rigid body conﬁguration from frame A to frame B while AdHab transforms the spatial velocity from frame A to frame B. As another perspective, let A and B be two Euclidean frames attached to the same rigid body. From (1.49), ~ωb = ~ωa, ~vb = ~ωa × ~pab + ~va. (1.70)

Representing (~ωb,~vb) in Eb and (~ωa,~va) in Ea, we have

" ω # " R 0 #" ω # b = ba a . (1.71) vb −Rbapˆab Rba va

Φ T We denote the transformation matrix by ba which is the same as AdHab . We can differentiate (1.71) once more to obtain the acceleration propagation:

" ω˙ # " R 0 #" ω˙ # " 0 # b = ba a + . (1.72) v˙b −Rbapˆab Rba v˙a −Rbaωˆdapabωa In the absence of externally applied force and torque, the power is conserved within the rigid body; therefore, T T " ω # " τ # " ω # " τ # a a = b b . (1.73) va fa vb fb Using (1.71), we obtain the dual relationship: " # " # τa T τb = Φba . (1.74) fa fb

19 1.6 Distance Metrics on SO(3) and SE(3)

For planning and control, it is important to choose a distance metric on SO(3) and SE(3). First consider SO(3). Let R1 and R2 be two SO(3) matrices. We may consider R1 − R2 as the orientation error, but R1 − R2 is no longer in SO(3). We can choose

ρ(R1,R2) = kβ(R1) − β(R2)k (1.75) where β is any representation of SO(3). T Another possibility is to form the relative orientation R1 R2 and use the metric

T ρ(R1,R2) = β(I) − β(R1 R2) . (1.76)

Note that if β is a 3-parameter representation, β(I) = 0. common metrics on SO(3): The metric for the distance portion of SE(3) may be any norm on R3:

ρp(p1, p2) = kp1 − p2k .

Derivatives of metrics Kinematic control Some possibilities:

T • Some norm on R − Rd or R Rd − I.

T • Some norm on p(R) − p(Rd) or p(R Rd) − p(I), where p is any vector representation of SO(3).

T Let E = R Rd. 1. 1 1 e = tr((E − I)T (E − I)) = kE − Ik (Frobenus norm) = ... = 4 kq (E)k2 . R 2 2 F v ˙ T T e˙R = 2 sin φφ = 2 sin φk ω = qv q0ω.

2. 1 1 T 1 φ T T e = tr((E 2 − I) (E 2 − I)) = 4(1 − q (E)), e˙ = 2 sin k ω = 2q ω. R 2 0 R 2 v 3. 1 e = kp(E)k2 R 2 p(E) is any 3-parameter representation of E.

T e˙R = p(E) Jpω, Jp = representation Jacobian .