Lecture 7. Quaternions.

7. Quaternions Mechanics of Manipulation Matt Mason [email protected] http://www.cs.cmu.edu/~mason

Carnegie Mellon

Lecture 7.

Mechanics of Manipulation

–

p.1

Lecture 7. Quaternions. Chapter 1 Manipulation 1 1.1 Case 1: Manipulation by a human 1

Chapter 5

Rigid Body Statics 93

Chapter 8 Dynamics 181

5.1 5.2

Forces acting on rigid bodies 93 Polyhedral convex cones 99

8.1 8.2

Newton’s laws 181 A particle in three dimensions 181

5.3 5.4

Contact wrenches and wrench cones 102 Cones in velocity twist space 104

8.3 8.4

Moment of force; moment of momentum 183 Dynamics of a system of particles 184

5.5 5.6

The oriented plane 105 Instantaneous centers and Reuleaux’s method 109

8.5 8.6

Rigid body dynamics 186 The angular inertia matrix 189

Chapter 2 Kinematics 11

5.7 5.8

Line of force; moment labeling 110 Force dual 112

8.7 8.8

Motion of a freely rotating body 195 Planar single contact problems 197

2.1 2.2

Preliminaries 11 Planar kinematics 15

5.9 Summary 117 5.10 Bibliographic notes 117

2.3 2.4

Spherical kinematics 20 Spatial kinematics 22

2.5 2.6

Kinematic constraint 25 Kinematic mechanisms 34

Chapter 6 6.1

Coulomb’s Law 121

Chapter 9 Impact

2.7

Bibliographic notes 36

6.2 6.3

Single degree-of-freedom problems 123 Planar single contact problems 126

9.1 9.2

A particle 211 Rigid body impact 217

6.4 6.5

Graphical representation of friction cones 127 Static equilibrium problems 128

9.3

Chapter 3 Kinematic Representation 41

Bibliographic notes 223 Exercises 223

3.1 3.2

Representation of spatial rotations 41 Representation of spatial displacements 58

6.6 6.7

Planar sliding 130 Bibliographic notes 139

3.3 3.4

Kinematic constraints 68 Bibliographic notes 72

1.2 1.3

Case 2: An automated assembly system Issues in manipulation 5

3

1.4 1.5

A taxonomy of manipulation techniques 7 Bibliographic notes 8 Exercises 8

Exercises 37

Exercises 72

Exercises 118

8.9 Graphical methods for the plane 203 8.10 Planar multiple-contact problems 205 8.11 Bibliographic notes 207 Exercises 208

Friction 121

Exercises 139 Chapter 7

Quasistatic Manipulation 143

211

Chapter 10 Dynamic Manipulation 225 10.1 Quasidynamic manipulation 225 10.2 Brie y dynamic manipulation 229 10.3 Continuously dynamic manipulation 230

Chapter 4 Kinematic Manipulation 77

7.1 7.2

Grasping and fixturing 143 Pushing 147

10.4 Bibliographic notes 232 Exercises 235

4.1 4.2

Path planning 77 Path planning for nonholonomic systems 84

7.3 7.4

Stable pushing 153 Parts orienting 162

Appendix A

4.3 4.4

Kinematic models of contact 86 Bibliographic notes 88

7.5 7.6

Assembly 168 Bibliographic notes 173

Exercises 88

Infinity

237

Exercises 175

Lecture 7.


–

p.2

Outline. • What is a quaternion

• Representing rotation • Geometric view

• Transformations to other representations • Topological and metric properties

Lecture 7.


–

p.3

Why can’t we invert vectors in R3 ? We can invert reals. x ×

1 x

= 1.

We can invert elements of R2 using complex numbers. z × z ∗ /|z|2 = 1, where ∗ is complex conjugate. Can we invert v ∈ R3 ?

Lecture 7.


–

p.4


1 x

= 1.

We can invert elements of R2 using complex numbers. z × z ∗ /|z|2 = 1, where ∗ is complex conjugate. Can we invert v ∈ R3 ? No.

Lecture 7.


–

p.4


1 x

= 1.

We can invert elements of R2 using complex numbers. z × z ∗ /|z|2 = 1, where ∗ is complex conjugate. Can we invert v ∈ R3 ? No. How about v ∈ R4 ?

Lecture 7.


–

p.4


1 x

= 1.

We can invert elements of R2 using complex numbers. z × z ∗ /|z|2 = 1, where ∗ is complex conjugate. Can we invert v ∈ R3 ? No. How about v ∈ R4 ? Yes!

Hamilton’s quaternions are to R4 what complex numbers are to R.

Lecture 7.


–

p.4

Complex numbers versus quaternions To define complex numbers: Basis elements 1 and i; Vector space over reals: elements have the form x + iy ; One more axiom required: i2 = -1.

Lecture 7.


–

p.5

Complex numbers versus quaternions To define complex numbers: Basis elements 1 and i; Vector space over reals: elements have the form x + iy ; One more axiom required: i2 = -1. To define quaternions: Basis elements 1, i, j , k; Vector space over reals: elements have the form q0 + q1 i + q2 j + q3 k ; Six more axioms: i2 = j 2 = k 2 = −1 ij = k jk = i ki = j Lecture 7.


–

p.5

Quaternion notation We can write a quaternion several ways: q = q0 + q1 i + q2 j + q3 k q = (q0 , q1 , q2 , q3 ) q = q0 + q

where q0 is the scalar part and q is the vector part

Lecture 7.


–

p.6

Quaternion product We can write a quaternion product several ways: pq = (p0 + p1 i + p2 j + p3 k)(q0 + q1 i + q2 j + q3 k) = (p0 q0 − p1 q1 − p2 q2 − p3 q3 ) + . . . i + . . . j + . . . k pq = (p0 + p)(q0 + q) = (p0 q0 + p0 q + q0 p + pq)

So what is pq? Cross product? Dot product?

Lecture 7.


–

p.7


So what is pq? Cross product? Dot product?Both! Cross product minus dot product!

Lecture 7.


–

p.7


So what is pq? Cross product? Dot product?Both! Cross product minus dot product! pq = (p0 q0 − p · q + p0 q + q0 p + p × q)

Lecture 7.


–

p.7

Conjugate, length Quaternion conjugate: q ∗ = q0 − q1 i − q2 j − q3 k

Note that qq ∗ = (q0 + q)(q0 − q) = q02 + q0 q − q0 q − qq = q02 + q · q − q × q = q02 + q12 + q22 + q32

Quaternion length: √ ∗ q 2 |q| = qq = q0 + q12 + q22 + q32 Lecture 7.


–

p.8

Quaternion inverse Note that every quaternion other than the additive identity 0 has an inverse: q∗ −1 q = 2 |q| That means quaternions are a linear algebra and a field. Hamilton’s dream. Quaternions are the only extension of complex numbers that is both a linear algebra and a field. If 1D numbers are the reals, and 2D numbers are the complex numbers, then 4D numbers are quaternions, and that’s all there is. (Frobenius?)

Lecture 7.


–

p.9

Rotation using unit quaternions Let q be a unit quaternion, i.e. |q| = 1. It can be expressed as q = cos

θ θ ˆ + sin n 2 2

Let x = 0 + x be a “pure vector”. Let x′ = qxq ∗ . ˆ )x!!! Then x′ is the pure vector rot(θ, n

Lecture 7.


–

p.10

Proof that unit quaternions work Expand the product qxq ∗ ; Apply half angle formulas; Simplify; to obtain Rodrigues’s formula.

Lecture 7.


–

p.11

Why θ/2? Why qxq ∗ instead of qx? Two puzzling things. In analogy with complex numbers, why not use ˆ sin θ p = cos θ + n

x′ = px To explore that idea, define a map Lp (q) = pq with p a unit pure vector. Note that Lp (q) can be written: 

p0

  p1 Lp (q) =   p  2 p3

−p1

−p2

−p2

p1

p0 p3

−p3 p0

−p3



q0



    p 2   q1     −p1   q2   q3 p0

Note that the matrix above is orthonormal. Lp is a rotation of Euclidean 4 space! (Without even using the fact that p is a pure vector.) Lecture 7.


–

p.12

Geometrical explanation Although Lp (q) rotates the 4D space of quaternions, it is not a rotation of the 3D subspace of pure vectors. Some of the 3D subspace leaks into the fourth dimension. Consider an example using p = i. Is it a rotation about i of π/2? Li q

iq

i

Ri q i

Li i

1

1 k

k j- k plane

iqi

2

2

1- i plane

qi

2 j

Lecture 7.

Ri

q

no rotation 1

k 2 j

j


–

p.13

What do we do with a representation? Rotate a point: qxq ∗ . Compose two rotations: q(pxp∗ )q ∗ = (qp)x(qp)∗

Convert to other representations: From axis-angle to quaternion: θ θ ˆ q = cos + sin n 2 2

From quaternion to axis-angle: θ = 2 tan−1 (|q|, q0 ) ˆ = q/|q| n

assuming θ is nonzero. Lecture 7.


–

p.14

From quaternion to rotation matrix Just expand the product 

 qxq ∗ = 

q02

q12

q22

q32

+ − − 2(q1 q2 + q0 q3 )

2(q1 q3 − q0 q2 )

Lecture 7.

2(q1 q2 − q0 q3 ) q02 − q12 + q22 − q32 2(q2 q3 + q0 q1 )

2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 )

q02 − q12 − q22 + q32




 x

–

p.15

From rotation matrix to quaternion Given R = (rij ), solve expression on previous page for quaternion elements qi Linear combinations of diagonal elements seem to solve the problem: 1 = (1 + r11 + r22 + r33 ) 4 1 2 q1 = (1 + r11 − r22 − r33 ) 4 1 2 q2 = (1 − r11 + r22 − r33 ) 4 1 q32 = (1 − r11 − r22 + r33 ) 4

q02

so take four square roots and you’re done? You have to figure the signs out. There is a better way . . . Lecture 7.


–

p.16

Look at the off-diagonal elements q0 q1 q0 q2 q0 q3 q1 q2 q1 q3 q2 q3

1 = (r32 − r23 ) 4 1 = (r13 − r31 ) 4 1 = (r21 − r12 ) 4 1 = (r12 + r21 ) 4 1 = (r13 + r31 ) 4 1 = (r23 + r32 ) 4

Given any one qi , could solve the above for the other three.

Lecture 7.


–

p.17

The procedure 1. Use first four equations to find the largest qi2 . Take its square root. 2. Use the last six equations (well, three of them anyway) to solve for the other qi . That way, only have to worry about getting one sign right. Actually q and −q represent the same rotation, so no worries about signs. Taking the largest square root avoids division by small numbers.

Lecture 7.


–

p.18

Properties of unit quaternions Unit quaternions live on the unit sphere in R4 . Quaternions q and −q represent the same rotation. Inverse of rotation q is the conjugate q ∗ .

Null rotation, the identity, is the quaternion 1.

Lecture 7.


–

p.19

Metrics and topologies Quaternions have the right metric. Consider unit quaternion θ θ ˆ q = cos + sin n 2 2

Shortest path on the unit sphere joining ±q with 1 has length θ/2. What is the shortest distance on the sphere from ±p to ±q ? The same as the distance from ±pq ∗ to 1. I.e. α/2, where α is the rotation angle required from p to q . The right metric matters. Uniform distribution on the three-sphere maps to uniform distribution on SO(3). Hence the problem on the problem set. What is the topology of SO(3)? Since unit quaternion representation has the right metric, it also has the right topology. What do we call the topology of a three-sphere with antipodes identified? Lecture 7.


–

p.20

Next: Representing displacements. Chapter 1 Manipulation 1 1.1 Case 1: Manipulation by a human 1

Chapter 5

Rigid Body Statics 93

Chapter 8 Dynamics 181

5.1 5.2

Forces acting on rigid bodies 93 Polyhedral convex cones 99

8.1 8.2

Newton’s laws 181 A particle in three dimensions 181

5.3 5.4

Contact wrenches and wrench cones 102 Cones in velocity twist space 104

8.3 8.4

Moment of force; moment of momentum 183 Dynamics of a system of particles 184

5.5 5.6

The oriented plane 105 Instantaneous centers and Reuleaux’s method 109

8.5 8.6

Rigid body dynamics 186 The angular inertia matrix 189

Chapter 2 Kinematics 11

5.7 5.8

Line of force; moment labeling 110 Force dual 112

8.7 8.8

Motion of a freely rotating body 195 Planar single contact problems 197

2.1 2.2

Preliminaries 11 Planar kinematics 15

5.9 Summary 117 5.10 Bibliographic notes 117

2.3 2.4

Spherical kinematics 20 Spatial kinematics 22

2.5 2.6

Kinematic constraint 25 Kinematic mechanisms 34

Chapter 6 6.1

Coulomb’s Law 121

Chapter 9 Impact

2.7

Bibliographic notes 36

6.2 6.3

Single degree-of-freedom problems 123 Planar single contact problems 126

9.1 9.2

A particle 211 Rigid body impact 217

6.4 6.5

Graphical representation of friction cones 127 Static equilibrium problems 128

9.3

Chapter 3 Kinematic Representation 41

Bibliographic notes 223 Exercises 223

3.1 3.2

Representation of spatial rotations 41 Representation of spatial displacements 58

6.6 6.7

Planar sliding 130 Bibliographic notes 139

3.3 3.4

Kinematic constraints 68 Bibliographic notes 72

1.2 1.3

Case 2: An automated assembly system Issues in manipulation 5

3

1.4 1.5

A taxonomy of manipulation techniques 7 Bibliographic notes 8 Exercises 8

Exercises 37

Exercises 72

Exercises 118

8.9 Graphical methods for the plane 203 8.10 Planar multiple-contact problems 205 8.11 Bibliographic notes 207 Exercises 208

Friction 121

Exercises 139 Chapter 7

Quasistatic Manipulation 143

211

Chapter 10 Dynamic Manipulation 225 10.1 Quasidynamic manipulation 225 10.2 Brie y dynamic manipulation 229 10.3 Continuously dynamic manipulation 230

Chapter 4 Kinematic Manipulation 77

7.1 7.2

Grasping and fixturing 143 Pushing 147

10.4 Bibliographic notes 232 Exercises 235

4.1 4.2

Path planning 77 Path planning for nonholonomic systems 84

7.3 7.4

Stable pushing 153 Parts orienting 162

Appendix A

4.3 4.4

Kinematic models of contact 86 Bibliographic notes 88

7.5 7.6

Assembly 168 Bibliographic notes 173

Exercises 88

Infinity

237

Exercises 175

Lecture 7.


–

p.21