3D 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 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.)
– follow right-handed rule
• 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
ij   ji  k
jk  kj  i
ki  ik  j
i
k
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
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
• 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
Rot (nˆ,q )  q  cos q2  sin q2 nˆ
• Example
– Rot(z,90°)
q

2
2
0 0
q 1
2
2

Why “unit”?
DOF point
of view!
• 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
•
•
•
•
•
•
•
•
Addition/subtraction
Multiplication
Division
Conjugate
Magnitude
Normalization
Transformations
Concatenation
Quaternion Operations
• 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
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 +
q1w*q2zk + q1xi*q2w +
– q1xi*q
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
Dot product
vector
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  z
2
2
2
2
Quaternion Operations
• 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|):
q
qˆ 
|q|
– Also referred to as quaternion sign: sgn(q)
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:
q*
q 
2
|q|
1
Quaternion
Rotations
Matrix 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)
y
x
z
Matrix Rotation
x  axis rotation :
0
1

 0 cos 
 0  sin 

0 

sin  
cos  
0
1

 0 cos 
 0  sin 

x 1
0 

sin  
cos  
y  cos   sin 
z   sin   cos 
y  axis rotation :
 cos 

 0
 sin 

0  sin  

1
0 
0 cos  
y
z  axis rotation :
 cos 

  sin 
 0

sin 
cos 
0
0

0
1 
x
z
Final rotation matrix :
cos  cos 


 sin  sin  cos   cos  sin 
  cos  sin  cos   sin  sin 

 cos  sin 
 sin  sin  sin   cos  cos 
cos  sin  sin   sin  cos 
sin 


 sin  cos  
cos  cos  
Quaternion Rotation
Parts of quaternion
• w = cos(θ/2)
• 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) û
w
Matrix rotation versus
quaternion rotation
Let’s do rotation!
y
x
z
Let’s do rotation!
y
b
a
c
x
z
Let’s do another one!
y
x
z
Quaternion?
w
v(x,y,z)
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:
 w2  x 2  y 2  z 2

 2wz  2 xy
 2 xz  2wy

2 xy  2wz
w2  x 2  y 2  z 2
2wx  2 yz
w
2wy  2 xz 

2 yz  2wx 
w2  x 2  y 2  z 2 
Matrix entries are taken all from
quaternion
Quaternion Rotation
• When it is a unit quaternion:
1  2 y 2  2 z 2
2 xy  2 wz
2 wy  2 xz 


2
2
2 yz  2 wx 
 2 wz  2 xy 1  2 x  2 z
 2 xz  2 wy
2
2
2
2
wx

2
yz
1

2
x

2
y


Quaternion matrix for unit quaternion: w=1
w
Example of unit quaternion
Our notation:
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.
q  cos
q  cos
q  cos
q
2
2
 sin
3
2

q
2
 sin
 sin

uˆ
2
3
2
uˆ
uˆ
3
3
q  cos 60  sin 60uˆ
1
3

uˆ
2 2
1
3 (i  j  k )
q 
.
2 2
3
1 i  j  k
q
2
q
Example of quaternion rotation
(cont’d)
• 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
(cont’d)
(cont’d)
Quaternion matrix
 w2  x 2  y 2  z 2

qvq*   2 wz  2 xy
 2 xz  2 wy

2 xy  2 wz
w2  x 2  y 2  z 2
2 wx  2 yz
22
22 
1  1  1  1


qvq*   2  2
1 1 1 1
2  2 v
 22

2

2
1

1

1

1


 0 0 4


qvq*   4 0 0 v
0 4 0


2 wy  2 xz 

2 yz  2wx v
w2  x 2  y 2  z 2 

q  q0  q1i  q2 j  q3 k  q0  q
i 2  j 2  k 2  1
ij   ji  k
jk  kj  i
ki  ik  j
Gimbal Lock
It happens when you
turn this axis far
enough…
…until this
axis …
…aligns with
this axis.
Gimbal Lock
Final rotation matrix :
cos  cos 


 sin  sin  cos   cos  sin 
  cos  sin  cos   sin  sin 

 cos  sin 
 sin  sin  sin   cos  cos 
cos  sin  sin   sin  cos 
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


 sin  cos   cos  sin 
  cos  cos   sin  sin 

0
 sin  sin   cos  cos 
cos  sin   sin  cos 
1

0
0 
Example 1 of using quaternions in robotics:
quaternion representing a rotation
Rot(z,90°)
y
x
y
z
x
z
q

2
2
0 0
2
2

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°)
y
How to represent
rotation?
x
y
z
x
z
q

2
2
0 0
0  1 0 
1 0 0


0 0 1
2
2

Represented as
quaternion
Represented as matrix
Rot (90, 0,0,1) OR Rot (-90,0,0,-1)
Operations on Unit Quaternions
• Perform Rotation
• Composition
of rotations
• Interpolation
x  qxq*  ...
 

 
2
 (q0  q  q ) x  2q0 q  x  2q (q  x)
x  pxp*


x  qxq*  q pxp* q*  (qp) x(qp)*
p(t )
p(t )  (1  t ) p  tp , p 
p(t )
– Linear
– Spherical linear (more later)
1
2
Example of rotation using unit
quaternions
y,x’
x
y’
z,z’
p(2,1,1)
Rot(z,90°)
0  1
R  1 0
0 0
0
p  Rp  1
0
For comparison we first
use matrices
0
0
1
 1 0 2  1
0 0 1    2 
0 1 1   1 
Example (cont)
p  0 2 1 1
q

2
2
0 0
2
2
For comparison we use
quaternions

 

 
p  (q02  q  q ) p  2q0 q  p  2q (q  p )
i j
 2
 ( 12  12 ) 1   2 22 0 0
2 1
1 
 1 0  1
  2   0   2 
 0  1  1 
k  0
 0 ( 2 )
2
2

2 
  2
1   22 
Next we convert
to matrices
We get the
same result
New Example: multiplication of
quaternions
y,x’
x
y’
q1  cos 902  sin
90
2
(0 0 1)  (
2
2
q2  cos 902  sin
90
2
(1 0 0)  (
2
2
q2 q1  (

y
x,x’
z’
2
2
2
2
2
2
2
2
2
2

0 0)(
2
2
(
2
2
2
2
0 0
0 0) 
i j k
  22 0 0   (
 0 0 22 
 ( 12 12 21 12 )
2
2
2
2
2
2
)
(0 0
)
0 0)
Compositio n :
z,z’
z,y’
2
2
0 0
2
2
0 0)  (0 0
)
2
2
)
Example: Conversion of quaternion
matrix to rotation matrix R
Matrix R represented with quaternions
q02  q12  q22  q32

R   2(q1q2  q0 q3 )
 2(q1q3  q0 q2 )

2(q1q2  q0 q3 )
q02  q12  q22  q32
2(q2 q3  q0 q1 )
We substitute values of q
q  q0
And we get R
0  1 0 
R  1 0 0
0 0 1
q1
q2
2(q1q3  q0 q2 ) 

2(q2 q3  q0 q1 ) 
q02  q12  q22  q32 
q3  

2
2
0 0
2
2

Matrix Conversion Formulas
Relations between qi and rij
q
2
0
q12
q22
q32
1
 1  r11  r22  r33 
4
1
 1  r11  r22  r33 
4
1
 1  r11  r22  r33 
4
1
 1  r11  r22  r33 
4
r32  r23 
q0 q2  14 r13  r31 
q0 q3  14 r21  r12 
q1q2  14 r12  r21 
q1q3  14 r13  r31 
q2 q3  14 r23  r32 
q0 q1 
1
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
• Perform rotation: linear
algebra
• Composition: trivial
– orthogonalization might
be required due to
floating point errors
• Interpolation: ?

A  aij  uˆ1 uˆ2


uˆ3 

x  x1uˆ1  x2uˆ2  x3uˆ3
 
 x1 Auˆ1  x2 Auˆ2  x3 Auˆ3
 Ax
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!

uˆ uˆ
 1 2

 
uˆ3   vˆ1 vˆ2
 
Verify!

vˆ3 

vˆ1  uˆ1
uˆ2  vˆ1
vˆ2  uˆ2 
vˆ1
vˆ1  vˆ1
uˆ3  vˆ1
uˆ3  vˆ2
vˆ3  uˆ3 
vˆ1 
vˆ2
vˆ1  vˆ1
vˆ2  vˆ2
Spatial Displacement
• Any displacement can be decomposed into a
rotation followed by a translation
• Matrix
x  Rx  d
•
 x

 

y
 
x   , T  

z
 

1
Quaternion   
R
0
x  qxq*  d

d 
 x  Tx


1
Spherical Linear
Interpolation
Interpolation
Interpolation produces
an arc instead of a line
Spherical Linear Interpolation
sin  (1  t )
sin t
s (t ) 
q
r
sin 
sin 
cos   q0 r0  q1r1  q2 r2  q3 r3
quaternion
q
r
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
slerp (t ; q0 , q1 )  
1
sin(  (1  t )) p  sin(  (t  2 )) q1
-
t  [0, 1 2 ]
t  [ 1 2 ,0]
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.
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)
Finish*
Start*
*assumption
Let’s do rotation!
y
b
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