Sie sind auf Seite 1von 69

# 3D Kinematics

## Consists of two parts 3D rotation is more

3D rotation complicated than 2D
rotation (restricted to z-axis)
3D translation
The same as 2D
Next, we will discuss the
treatment for spatial (3D)
rotation
3D Rotation Representations
Euler angles Learning Objectives
Axis-angle Representation
(uniqueness)
3X3 rotation matrix
Perform rotation
Unit quaternion Composition
Interpolation
Conversion among
representations

Euler Angles and GIMBAL LOCK
Roll, pitch, yaw Gimbal lock: reduced DOF
due to overlapping axes

Ref: http://www.fho-emden.de/~hoffmann/gimbal09082002.pdf
Axis-Angle
Representation
Axis-Angle Representation
Rot(n,q)
n: rotation axis (global)
q: rotation angle (rad. or deg.)

Rot(n,q)=Rot (-n,-q)

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

Perform rotation
Rodrigues formula

Interpolation/Composition: poor
Rot(n2,q2)Rot(n1,q1) =?= Rot(n3,q3)

We create matrix
R for rotation
Quaternions
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:

i j k ijk 1
2 2 2

From: http://en.wikipedia.org/wiki/Quaternion 7
Quaternion Brief History

## William Rowan Hamilton

Quaternion
Definition

q q0 q1i q2 j q3 k q0 q
i 2 j 2 k 2 1
i
ij ji k
jk kj i
k j
ki ik j
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

From: http://en.wikipedia.org/wiki/Quaternion 10
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

Operators
Addition p p0 p1i p2 j p3k
q q0 q1i q2 j q3k
p q p0 q0 p1 q1 i p2 q2 j p3 q3 k
Multiplication

pq p0 q0 p0 q q0 p p q p q
Conjugate

q q0 q
*
pq * q* p*
Length
q q*q q02 q12 q22 q32
Unit Quaternion
Define unit quaternion as follows to represent
rotation
Why unit?
Rot (n,q ) q cos q2 sin q2 n q 1
DOF point
Example of view!

Rot(z,90) q 2
2
0 0 2
2

q and q represent the same rotation
Quaternion scalar and vector
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
Transformation
Scalar & Vector
4 dimensions of a quaternion:
3-dimensional space (vector)
Angle of rotation (scalar)
Quaternion can be transformed to other geometric
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
Quaternion
Operations
Quaternion Operations
Multiplication
Division
Conjugate
Magnitude
Normalization
Transformations
Concatenation
Quaternion Operations
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
Quaternion Operations
Subtraction:
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
Quaternion Operations
Multiplication
Distributive
Associative
Not commutative because of the i2 =j2=k2=-1
i i
(-i)

k j k (-k) (-j) j
Quaternion Operations
Multiplication
Given two quaternions:
q1 = q1w + q1xi + q1yj + q1zk
q2 = q2w + q2xi + q2yj + q2zk
The result quaternion q3 is:
q3 = q1 * q2

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

q1xi*q
q1w*q2zk + q1xi*q2w + x*q22xxi+ etc
Quaternion Operations
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
Quaternion Operations
Multiplication
Or, in scalar-vector format:

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

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

scalar vector
Dot product Cross product
or
q3 = q1wq2w - v1.v2 + q1wv2 + q2v1 + v1 x v2
Quaternion Operations
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:

| q | qq * w x y z2 2 2 2
Quaternion Operations
Normalization
q
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|):
q
q
|q|
Also referred to as quaternion sign: sgn(q)
Quaternion Operations
Conjugate:
Given a quaternion:
q = w + xi+ yj + zk
The conjugate of quaternion q is q*, where:
q* = w xi yj zk
Quaternion Operations
Inverse
Can be used for division
Given a quaternion:
q = w + xi+ yj + zk
The inverse of quaternion q is q-1, where:

1 q*
q 2
|q|
Quaternion
Rotations
Matrix Rotation
Matrix Rotation is based
on 3 rotations: y
On axes: x, y, z
Or yaw, pitch, roll (which
one corresponds to which
axis, depends on the
orientation to the axes) x
Sequence matters (x-y may z
not equal y-x)
Matrix Rotation
x axis rotation : y axis rotation :
1 0 1

0 0
y

0
0 cos sin cos 0 sin

0 cos sin 0 sin cos

0 1 0
0 sin cos
x 1
sin
y cos sin
0 cos
z sin cos

z axis rotation :
cos sin 0

sin cos 0 x
0 1
0
z

## Final rotation matrix :

cos cos cos sin sin

sin sin cos cos sin sin sin sin cos cos sin cos
cos sin cos sin sin cos sin sin sin cos cos cos

Quaternion Rotation
Parts of quaternion

w = cos(/2) w
v = sin(/2)
Where is a unit/normalized
vector u (i, j, k)
Quaternion can be represented as:
q=cos(/2)+sin(/2)(xi+yj+zk)
or
q=cos(/2)+sin(/2)
Matrix rotation versus
quaternion rotation
Lets do rotation!
y

z
Lets do rotation!
y

a
c
x

z
Lets do another one!
y

z
Quaternion?

## w Can create rotation

v(x,y,z) 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 w
form
Given a quaternion:
q = w + xi+ yj + zk
The matrix form of quaternion q
is:
w2 x 2 y 2 z 2 2 xy 2wz 2wy 2 xz

2wz 2 xy w2 x 2 y 2 z 2 2 yz 2wx
2 xz 2wy 2wx 2 yz w2 x 2 y 2 z 2

## Matrix entries are taken all from

quaternion
Quaternion Rotation
When it is a unit quaternion:
w

1 2 y 2 2 z 2 2 xy 2wz 2wy 2 xz

2wz 2 xy 1 2 x 2 z
2 2
2 yz 2wx
2 xz 2wy 2 wx 2 yz 12
2 x 2
2 y 2

## Quaternion matrix for unit quaternion: w=1

Example of unit quaternion
Our notation:
quaternion

Rotation of vector v is v q q
q cos sin u
Where v is: v=ai+bj+ck 2 2
2 2
q cos 3
sin 3
u
By quaternion q=(w,u) 2 2

Where w = 1 q cos sin
u
3 3
Vector (axis): u = i + j + k q cos 60 sin 60u

## Rotation angle: 120 = (2)/3 q

1

2 2
3
u
radian () 1 3 (i j k )
q .
Length of u =3 2 2 3
1 i j k
q
If we rotate a vector, the result 2
should be a vector.
Example of quaternion rotation
(contd)
So to rotate v:
v = q v q*
Where q* is conjugate of q:
(1 i j k )
q*
2

## So now we can substitute q v q*

to matrix form of quaternion
Example of quaternion rotation
Example (contd)
(contd)
Quaternion matrix

w2 x 2 y 2 z 2 2 xy 2wz 2wy 2 xz

qvq* 2wz 2 xy w2 x 2 y 2 z 2 2 yz 2wx v
2 xz 2wy 2wx 2 yz w2 x 2 y 2 z 2

1 1 1 1 22 22

qvq* 2 2 1 1 1 1 2 2 v
22
2 2 1 1 1 1
q q0 q1i q2 j q3 k q0 q
0 0 4
i 2 j 2 k 2 1
qvq* 4 0 0 v ij ji k
0 4 0
jk kj i
ki ik j
Gimbal Lock
until this
It happens when you
axis
turn this axis far
enough

aligns with
this axis.
Gimbal Lock
Final rotation matrix :
cos cos cos sin sin

sin sin cos cos sin sin sin sin cos cos sin cos
cos sin cos sin sin cos sin sin sin cos cos cos

## Gimbal lock occurs when rotated -90 or 90 on y-axis

Remember that:
= rotation on x-axis
= rotation on y-axis
= rotation on z-axis

Gimbal lock :
0 0 1

sin cos cos sin sin sin cos cos 0
cos cos sin sin cos sin sin cos 0

Example 1 of using quaternions in robotics:
quaternion representing a rotation
y Rot(z,90)

x
y x
q 2
2
0 0 2
2

z
z 0 1 0
1 0 0

0 0 1

## Rot (90, 0,0,1) OR Rot (-90,0,0,-1)

Example of using quaternions in
robotics
Rot(z,90) How to represent
y
rotation?
x
y x
q 2
2
0 0 2
2
Represented as
quaternion

z
z 0 1 0 Represented as matrix
1 0 0

0 0 1

## Rot (90, 0,0,1) OR Rot (-90,0,0,-1)

Operations on Unit Quaternions

## Perform Rotation x qxq* ...

(q0 q q ) x 2q0 q x 2q (q x)
2

Composition x pxp*
of rotations
x qxq* q pxp* q* (qp) x(qp)*

Interpolation p(t )
p(t ) (1 t ) p tp , p
1 2

Linear p(t )
Spherical linear (more later)
Example of rotation using unit
quaternions
y,x
For comparison we first
y x use matrices
0 1 0
z,z p(2,1,1) R 1 0 0
0 0 1
Rot(z,90) 0 1 0 2 1
p Rp 1 0 0 1 2
0 0 1 1 1
Example (cont)
p 0 2 1 1 For comparison we use

q 2
2
0 0 2
2
quaternions

p (q0 q q ) p 2q0 q p 2q (q p)
2
Next we convert
2 i j k 0 to matrices

( 12 12 ) 1 2 22 0 0 2
2 0 ( 2 )
2 2
1 2 1 1 22
1 0 1
2 0 2
0 1 1 We get the
same result
New Example: multiplication of
quaternions
q1 cos 902 sin 90
2 (0 0 1) ( 2
2
0 0 2
2
)
y,x q2 cos 902 sin 90
2 (1 0 0) ( 2
2
2
2
0 0)

y x
Compositio n :
z,z q2 q1 ( 2
2
2
2
0 0)( 2
2
0 0 2
2
)
y 2
2
2
2
2
2
( 2
2
0 0) 2
2
(0 0 2
2
)
i j k
x,x 22 0 0 ( 2
2
0 0) (0 0 2
2
)
0 0 22
z,y
( 12 12 21 12 )
z
Example: Conversion of quaternion
matrix to rotation matrix R
Matrix R represented with quaternions
q02 q12 q22 q32 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 )

R 2(q1q2 q0 q3 ) q02 q12 q22 q32 2(q2 q3 q0 q1 )
2(q1q3 q0 q2 ) 2(q2 q3 q0 q1 ) q02 q12 q22 q32

We substitute values of q q q0 q1 q2 q3 2
2
0 0 2
2

0 1 0
And we get R R 1 0 0
0 0 1
Matrix Conversion Formulas
Relations between qi and rij

q 2
0 1 r11 r22 r33
1
q0 q1 1
4
r32 r23
4
q0 q2 14 r13 r31
1 r11 r22 r33
1
q12
4 q0 q3 14 r21 r12
q1q2 14 r12 r21
1 r11 r22 r33
1
q22
4 q1q3 14 r13 r31
q32 1 r11 r22 r33
1 q2 q3 14 r23 r32
4
Rodrigues Formula

r
v

v
v=R v
References:
http://mesh.caltech.edu/ee148/notes/rotations.pdf
http://www.cs.berkeley.edu/~ug/slide/pipeline/assignments/as5/rotation.ht
ml
Rotation Matrix
Meaning of three
columns
A aij u1 u2 u3
Perform rotation: linear
algebra x x1u1 x2u2 x3u3
Composition: trivial x1 Au1 x2 Au2 x3 Au3
orthogonalization might Ax
be required due to
floating point errors
Interpolation: ? x R1 x
x R2 x R2 R1 x R2 R1 x
Gram-Schmidt
Orthogonalization
If 3x3 rotation matrix no longer orthonormal,
metric properties might change!

v1 u1
u u u3 v1 v2 v3
1 2 u2 v1
v2 u2 v1
v1 v1
u3 v1 u3 v2
v3 u3 v1 v2
v1 v1 v2 v2
Verify!
Spatial Displacement
Any displacement can be decomposed into a
rotation followed by a translation
Matrix
x Rx d
x

y R d
x , T x Tx
z

1
Quaternion 0 1

x qxq* d
Spherical Linear
Interpolation
Interpolation

Interpolation produces
an arc instead of a line
Spherical Linear Interpolation
q
sin (1 t ) sin t
s (t ) q r
sin sin
r
cos q0 r0 q1r1 q2 r2 q3 r3

## quaternion unit sphere in R4

The computed rotation quaternion rotates about a fixed axis
at constant speed
References:
http://www.gamedev.net/reference/articles/article1095.asp
http://www.diku.dk/research-groups/image/teaching/Studentprojects/Quaternion/
http://www.sjbrown.co.uk/quaternions.html
http://www.theory.org/software/qfa/writeup/node12.html
Spherical Linear Interpolation Slerp
for unit-length quaternions
sin(( 1 t )q )q0 sin( tq )q1
slerp (t ; q0 , q1 )
sin q

## - 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

## sin(( 1 t )q )q0 sin( tq )q1

slerp (t ; q0 , q1 )
sin q

## sin( ( 1 2 t )) q0 sin( t ) p t [0, 1 2 ]

slerp (t ; q0 , q1 )
sin( (1 t )) p sin( (t 2 )) q1 t [ 1 2 ,0]
1

- 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 for t [0, 1 2 ]
and from p to q1 for t [ 1 ,1] 2
Spherical Linear Interpolation - Slerp
1. Used in joint animation by storing starting
and ending joint position as quaternions.

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)

Finish*

Start*

*assumption
Lets do rotation!
y

a
c
x

z
Visualization of
quaternions
Difficult to visualize
Not for the weak-on-math
Visualizing
Quaternion
Rotation

Removing 720
degree
twist 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!

By
Mathias Sunardi
for
Quantum Research Group Seminar
June 15, 2006