3d kinematics consists of two parts – 3d rotation – 3d translation the same as 2d 3d rotation is...

3D Kinematics3D Kinematics• Consists of two parts

– 3D rotation– 3D translation

• The same as 2D

• 3D rotation is more complicated than 2D rotation (restricted to z-axis)

• Next, we will discuss the treatment for spatial (3D) rotation

3D Rotation Representations

• Euler angles• Axis-angle• 3X3 rotation matrix• Unit quaternion

• Learning Objectives– Representation

(uniqueness)– Perform rotation– Composition– Interpolation– Conversion among

representations– …

Euler Angles and GIMBAL LOCKGIMBAL LOCK

• Roll, pitch, yaw • Gimbal lock: reduced DOF due to overlapping axes

Ref: http://www.fho-emden.de/~hoffmann/gimbal09082002.pdf

Axis-Angle Axis-Angle RepresentationRepresentation

Axis-AngleAxis-Angle Representation

• Rot(n,)– n: rotation axis (global)– : rotation angle (rad. or deg.)– follow right-handed ruleright-handed rule

• Rot(n,)=Rot (-n,-)

• Problem with null rotation: rot(n,0), any n

• Perform rotation– Rodrigues formula

• Interpolation/Composition: poorpoor– Rot(n2,2)Rot(n1,1) =?= Rot(n3,3)

We create matrix R for rotation

QuaternionsQuaternions

Quaternion - Brief History

• Invented in 1843 by Irish mathematician Sir William Rowan Hamilton

• Founded when attempting to extend complex numbers to the 3rd dimension

• Discovered on October 16 in the form of the equation:

1222 ijkkji7From: http://en.wikipedia.org/wiki/Quaternion

Quaternion – Brief History

William Rowan Hamilton

Quaternion

• Definition

jikki

ikjjk

kjiij

kji

qqkqjqiqqq

1222

03210

i

j k

Applications of Quaternions

• Used to represent rotations and orientations of objects in three-dimensional space in:– Computer graphics– Control theory– Signal processing– Attitude controls– Physics– Orbital mechanics– Quantum Computing, quantum circuit design

10From: http://en.wikipedia.org/wiki/Quaternion

Advantages of Quaternions

• Avoids Gimbal Lock• Faster multiplication algorithms to combine

successive rotations than using rotation matrices

• Easier to normalize than rotation matrices• Interpolation• Mathematically stable – suitable for statistics

Operators on Quaternions

• OperatorsOperators– Addition

– Multiplication

– Conjugate

– Length

kqpjqpiqpqpqp

kqjqiqqq

kpjpippp

33221100

3210

3210

qpqppqqpqppq 0000

qqq 0

* *** pqpq

23

22

21

20

* qqqqqqq

Unit Quaternion

• Define unit quaternion as follows to represent rotation

• Example– Rot(z,90°)

• q and –q represent the same rotation

nqn ˆsincos),ˆ(Rot 22 1q

22

22 00q

Why “unit”?

DOF point of view!

Quaternion – scalarscalar and vectorvector parts

q = w + xi + yj + zk

• w, x, y, z are real numbers• w is scalar part• x, y, z are vector parts• Thus it can also be represented as:

q = (w, v(x,y,z)) or

q = w + v

Quaternion – Dimension and TransformationTransformation

• Scalar & Vector• 4 dimensions of a quaternion:

– 3-dimensional space (vector)– Angle of rotation (scalar)

• Quaternion can be transformed to other geometricgeometric algorithm:– Rotation matrix ↔ quaternion– Rotation axis and angle ↔ quaternion– Spherical rotation angles ↔ quaternion– Euler rotation angles ↔ quaternion

What are relations of quaternions to other topics in kinematics?

Details of Details of Quaternion Quaternion OperationsOperations

Quaternion Operations

• Addition/subtraction• Multiplication• Division• Conjugate• Magnitude• Normalization• Transformations• Concatenation


• Addition:– Given two quaternions:

• q1 = q1w + q1xi + q1yj + q1zk

• q2 = q2w + q2xi + q2yj + q2zk

– The result quaternion q3 is:

q3 = q1 + q2

q3 = (q1w + q2w) + (q1x + q2x)i + (q1y + q2y)j +

(q1z + q2z)k


• Subtraction:– Given two quaternions:

• q1 = q1w + q1xi + q1yj + q1zk

• q2 = q2w + q2xi + q2yj + q2zk


q3 = q1 - q2

q3 = (q1w – q2w) + (q1x – q2x)i + (q1y – q2y)j + (q1z – q2z)k


• Multiplication– Distributive– Associative– Not commutative because of the i2 =j2=k2=-1

i

jk

i

jk (-k) (-j)

(-i)


• Multiplication– Given two quaternions:

• q1 = q1w + q1xi + q1yj + q1zk

• q2 = q2w + q2xi + q2yj + q2zk


q3 = q1 * q2

q3 = q1w*q2w + q1w*q2xi + q1w*q2yj +

q1w*q2zk + q1xi*q2w + q1xi*q2xi– q1x*q2x + etc…


• Multiplication– Resulting quaternion q3 is:

q3= (q1wq2w + q1xq2x + q1yq2y + q1zq2z) +(q1wq2x + q1xq2w + q1yq2z – q1zq2y)i +(q1wq2y + q1yq2w + q1zq2x – q1xq2z)j +(q1wq2z + q1zq2w + q1xq2y – q1yq2x)k


• Multiplication– Or, in scalar-vector format:

q3= q1q2 = (q1w, v1)(q2w, v2)

= (q1wq2w - v1.v2, q1wv2 + q2v1 + v1 x v2)

or q3 = q1wq2w - v1.v2 + q1wv2 + q2v1 + v1 x v2

Dot product Cross product

scalar vector


• Magnitude– Also called modulus– Is the length of the quaternion from the origin– Given a quaternion:

• q = w + xi+ yj + zk

– The magnitude of quaternion q is |q|, where:

2222*|| zyxwqqq


• Normalization– Normalization results in a unit quaternion where:

w2 + x2 + y2 + z2 = 1

– Given a quaternion:• q = w + xi+ yj + zk

– To normalize quaternion q, divide it by its magnitude (|q|):

– Also referred to as quaternion sign: sgn(q)

q

||ˆ

q

qq


• Conjugate:– Given a quaternion:

• q = w + xi+ yj + zk

– The conjugate of quaternion q is q*, where:• q* = w – xi – yj – zk


• Inverse– Can be used for division– Given a quaternion:

• q = w + xi+ yj + zk

– The inverse of quaternion q is q-1, where:

21

||

*

q

qq

Quaternion Quaternion RotationsRotations

Matrix RotationMatrix Rotation• Matrix Rotation is based

on 3 rotations:– On axes: x, y, z– Or yaw, pitch, roll (which

one corresponds to which axis, depends on the orientation to the axes)

– Sequence matters (x-y may not equal y-x)

x

y

z

Matrix Rotation

cossin

sincos

1

cossin0

sincos0

001

z

y

x

cossin0

sincos0

001

:rotationaxisx

cos0sin

010

sin0cos

:rotationaxisy

100

0cossin

0sincos

:

rotationaxisz

x

y

z

coscoscossinsinsincossinsincossincos

cossincoscossinsinsinsincoscossinsin

sinsincoscoscos

:matrixrotationFinal

Quaternion RotationQuaternion Rotation

• w = cos(θ/2)

• v = sin(θ/2)û

• Where û is a unit/normalized vector u (i, j, k)

• Quaternion can be represented asQuaternion can be represented as:q=cos(θ/2)+sin(θ/2)(xi+yj+zk)

or

q=cos(θ/2)+sin(θ/2) û

v(x,y,z)

w

Parts of quaternion

Matrix rotation versus quaternion rotation

Let’s do rotation!

x

y

z


x

y

z

b

ac

b

a

c

b

a

c

Let’s do another one!

x

y

z

Quaternion?

v(x,y,z)w Can create rotation

by using arbitrary axis (v(x,y,z)) and rotate the object by w amount.

Rotation of a Quaternion

• Calculation is still done in matrix form

• Given a quaternion:q = w + xi+ yj + zk

• The matrix form of quaternion q is:

v(x,y,z)

w

2222

2222

2222

2222

2222

2222

zyxwyzwxwyxz

wxyzzyxwxywz

xzwywzxyzyxw

Matrix entries are taken all from quaternion

Quaternion Rotation• When it is a unit quaternion:

v(x,y,z)

w

222

22

22

2212222

2222122

2222221

yxyzwxwyxz

wxyzzxxywz

xzwywzxyzy

Quaternion matrix for unit quaternion: w=1w=1

Example of unit quaternion

• Rotation of vector v is v’– Where v is: v=ai+bj+ck

• By quaternion q=(w,u)– Where w = 1

• Vector (axis): u = i + j + k

• Rotation angle: 120° = (2π)/3 radian (θ)

• Length of u =√3

• If we rotate a vector, the result should be a vector.

2

13

)(.

2

3

2

1

ˆ2

3

2

1

ˆ60sin60cos

ˆ3

sin3

cos

ˆ2

sin2

cos

ˆ2

sin2

cos

32

32

kjiq

kjiq

uq

uq

uq

uq

uq

Our notation:quaternion

Example of quaternion rotation (cont’d)

• So to rotate v:

v’ = q v q*

• Where q* is conjugate of q:

2

)1(*

kjiq

So now we can substitute q v q*

to matrix form of quaternion

Example (cont’d)

vqvq

vqvq

v

zyxwyzwxwyxz

wxyzzyxwxywz

xzwywzxyzyxw

qvq

040

004

400

*

11112222

22111122

22221111

*

2222

2222

2222

*2222

2222

2222

Example of quaternion rotation (cont’d)

Quaternion matrix

jikki

ikjjk

kjiij

kji

qqkqjqiqqq

1222

03210

Gimbal LockGimbal LockIt happens when you turn this axis far enough…

…until thisaxis …

…aligns withthis axis.

Gimbal Lock

• Gimbal lock occurs when rotated -90° or 90° on y-axisrotated -90° or 90° on y-axis• Remember that: Remember that:

– = rotation on x-axis– = rotation on y-axis– = rotation on z-axis

0cossinsincossinsincoscos

0coscossinsinsincoscossin

100

:

lockGimbal

coscoscossinsinsincossinsincossincos

cossincoscossinsinsinsincoscossinsin

sinsincoscoscos

:matrixrotationFinal

Example 1 Example 1 of using quaternions in robotics: quaternion representing a rotationquaternion representing a rotation

x

y

z

x

y

z

22

22 00q

100

001

010

Rot (90, 0,0,1) OROR Rot (-90,0,0,-1)

Rot(z,90°)

Example of using quaternions in robotics

x

y

z

x

y

z

22

22 00q

100

001

010

Rot (90, 0,0,1) OROR Rot (-90,0,0,-1)

Rot(z,90°)

Represented as Represented as quaternionquaternion

Represented as Represented as matrixmatrix

How to represent rotation?

Operations on Unit Quaternions

• Perform Rotation

• Composition of rotations

• Interpolation– Linear– Spherical linear (more later)

)(22)(

...

020

*

xqqxqqxqqq

qxqx

****

*

)()( qpxqpqpxpqqxqx

pxpx

)(

)(,)1()( 21

tp

tpptppttp

ExampleExample of rotation using unit unit quaternionsquaternions

x

y,x’

z,z’

y’

1

2

1

1

1

2

100

001

010

100

001

010

Rpp

R

Rot(z,90°)Rot(z,90°)

p(2,1,1)

For comparison we For comparison we first use matricesfirst use matrices

ExampleExample (cont)

1

2

1

1

0

0

0

2

1

)(0

0

2

112

002

1

1

2

)(

)(22)(

22

22

22

22

21

21

020

kji

pqqpqqpqqqp

2

222 00

1120

q

p For comparison we For comparison we use quaternionsuse quaternions

Next we Next we convert to convert to matricesmatrices

We get the We get the same resultsame result

New Example: multiplication of multiplication of quaternionsquaternions

x

y,x’

z,z’

y’

)00()001(sincos

)00()100(sincos

22

22

290

290

2

22

22

290

290

1

q

q

x,x’

y

z,y’z’

)(

)00()00(

00

00

)00()00(

)00)(00(

:nCompositio

21

21

21

21

22

22

22

22

22

22

22

22

22

22

22

22

22

22

12

kji

qq

Example: Example: Conversion of quaternion Conversion of quaternion matrix to rotation matrix Rmatrix to rotation matrix R

23

22

21

2010322031

103223

22

21

203021

2031302123

22

21

20

)(2)(2

)(2)(2

)(2)(2

qqqqqqqqqqqq

qqqqqqqqqqqq

qqqqqqqqqqqq

R

100

001

010

00 22

22

3210

R

qqqqq

Matrix R represented with quaternions

We substitute values of q

And we get R

Matrix Conversion Formulas

33221123

33221122

33221121

33221120

14

1

14

1

14

1

14

1

rrrq

rrrq

rrrq

rrrq

32234

132

311341

31

211241

21

122141

30

311341

20

233241

10

rrqq

rrqq

rrqq

rrqq

rrqq

rrqq

Relations between qi and rij

Rodrigues Formula

v’=R v

r

v

v’

References:

http://mesh.caltech.edu/ee148/notes/rotations.pdf

http://www.cs.berkeley.edu/~ug/slide/pipeline/assignments/as5/rotation.html

Rotation Matrix

• Meaning of three columns

• Perform rotation: linear algebra

• Composition: trivial– orthogonalization might

be required due to floating point errors

• Interpolation: ?

Ax

uAxuAxuAx

uxuxuxx

uuuaA ij

ˆˆˆ

ˆˆˆ

ˆˆˆ

332211

332211

321

xRRxRRxRx

xRx

12122

1

Gram-Schmidt Gram-Schmidt OrthogonalizationOrthogonalization

• If 3x3 rotation matrix no longer orthonormal, metric properties might change!

321321 ˆˆˆˆˆˆ vvvuuu

222

231

11

1333

111

1222

11

ˆˆˆ

ˆˆˆ

ˆˆ

ˆˆˆˆ

ˆˆˆ

ˆˆˆˆ

ˆˆ

vvv

vuv

vv

vuuv

vvv

vuuv

uv

Verify!

Spatial Displacement

• Any displacement can be decomposed into a rotation followed by a translation

• Matrix

• QuaternionQuaternion

TxxdR

Tz

y

x

x

dRxx

10

,

1

dqxqx *

Spherical Linear Spherical Linear InterpolationInterpolation

InterpolationInterpolation

Interpolation producesan arc instead of a line

Spherical Linear InterpolationSpherical Linear Interpolation

33221100cos

sin

sin

sin

)1(sin)(

rqrqrqrq

rt

qt

ts

The computed rotation quaternion rotates about a fixed axis at constant speed

References:http://www.gamedev.net/reference/articles/article1095.asphttp://www.diku.dk/research-groups/image/teaching/Studentprojects/Quaternion/http://www.sjbrown.co.uk/quaternions.htmlhttp://www.theory.org/software/qfa/writeup/node12.html

q

r

unit sphere in R4quaternion

Spherical Linear Interpolation – Slerp for unit-length quaternions

sin

)sin())1sin((),;( 10

10

qtqtqqtslerp

- q0 and q1 are unit-length quaternions

- θ = angle between q0 and q1

- t is interval [0, 1]

- “if q0 and q1 are the same quaternion, then θ = 0, but in this case, q(t) = q0 for all t.”

Spherical Linear Interpolation - Slerp

],0[ 21t

sin

)sin())1sin((),;( 10

10

qtqtqqtslerp

- if q1 = -q0 then θ = π

- use a third quaternion p perpendicular to q0 (which could be infinite number of vectors)

- interpolation is done from q0 to p forand from p to q1 for ]1,[ 2

1t

]0,[))(sin())1(sin(

],0[)sin())(sin(),;(

21

121

21

021

10 tqtpt

tptqtqqtslerp

Spherical Linear Interpolation - Slerp1. Used in joint animation by storing starting and

ending joint position as quaternions.

2. Allows smooth rotations in keyframe animations.

3. “Spherical Linear interpolation supports the animation of joints when starting and ending joint positions are stored as quaternions that represent the joint rotations from canonical positions” – Rob Saunders, Advanced Games Design, Theory and Practice, March 2005, City University London

1. The concept of canonical (or conjugate) variables is of major importance.

2. They always occur in complementary pairs, such as spatial location x and linear momentum p, angle φ and angular momentum L, and energy E and time t.

3. They can be defined as any coordinates whose Poisson brackets give a Kronecker delta (or a Dirac delta incase of discrete variables).

Quaternion in Multi-Sensor Robot Navigation System (by S. Persa, P. Jonker, Technical University Delft, Netherlands)

Used quaternions

Dimensional Synthesis of Spatial RR Robots(A. Perez, J.M. McCarthy, University of California, Irvine)

Start*

Finish*

*assumption


x

y

z

b

ac

b

a

c

b

a

c

Visualization of Visualization of quaternionsquaternions

• Difficult to visualize• Not for the weak-on-math

Visualizing Quaternion

Rotation

Removing 720 degreetwist without moving either end(from: J.C. Hart, G.K. Francis, L.H. Kauffman, Visualizing Quaternion Rotation, ACM Transactions on Graphics, Vol. 13, No. 3 July 1994, p.267)

Quaternion Explained!

ByMathias Sunardi

forQuantum Research Group Seminar

June 15, 2006

3d kinematics consists of two parts – 3d rotation – 3d translation the same as 2d 3d rotation is...

Documents

rotation matriceseasier

2d3d rotation

null rotation

rotation axis globalq

quaternion scalar

q1 q2q3

q1 q2q3

q1w q1xi q1yj q1zkq2