3D Kinematics - Institut Teknologi Bandunginformatika.stei.itb.ac.id/.../AljabarGeometri/... · 3D Kinematics • Consists of two parts –3D rotation –3D translation • The same
Post on 27-Jan-2020
9 Views
Preview:
Transcript
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:
1222 ijkkji
7From: 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
• Operators
– Addition
– Multiplication
– Conjugate
– Length
kqpjqpiqpqpqp
kqjqiqqq
kpjpippp
33221100
3210
3210
qpqppqqpqppq 0000
qqq
0
* ***pqpq
2
3
2
2
2
1
2
0
* qqqqqqq
Unit Quaternion
• Define unit quaternion as follows to represent rotation
• Example
– Rot(z,90°)
• q and –q represent the same rotation
nqn ˆsincos),ˆ(Rot22qqq 1q
2
2
2
2 00q
Why “unit”?
DOF point of view!
Quaternion – scalar and vectorparts
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 geometricalgorithm:– 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
jk
i
jk (-k) (-j)
(-i)
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*q2xi– q1x*q2x + 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)
or
q3 = q1wq2w - v1.v2 + q1wv2 + q2v1 + v1 x v2
Dot product Cross product
scalar vector
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:
2222*|| zyxwqqq
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|):
– Also referred to as quaternion sign: sgn(q)
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:
2
1
||
*
q
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)
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 Rotation
• 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
Parts of quaternion
Matrix rotation versus quaternion rotation
Let’s do rotation!
x
y
z
Let’s do rotation!
x
y
z
b
ac
Let’s do another one!
x
y
z
Quaternion?
v(x,y,z)
wCan 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 qis:
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:w
222
22
22
2212222
2222122
2222221
yxyzwxwyxz
wxyzzxxywz
xzwywzxyzy
Quaternion matrix for unit quaternion: w=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
1
3
)(.
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 LockIt happens when you
turn this axis far
enough…
…until this
axis …
…aligns with
this axis.
Gimbal Lock
• 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
0cossinsincossinsincoscos
0coscossinsinsincoscossin
100
:
lockGimbal
coscoscossinsinsincossinsincossincos
cossincoscossinsinsinsincoscossinsin
sinsincoscoscos
:matrixrotationFinal
Example 1 of using quaternions in robotics: quaternion representing a rotation
x
y
z
x
y
z
2
2
2
2 00q
100
001
010
Rot (90, 0,0,1) OR Rot (-90,0,0,-1)
Rot(z,90°)
Example of using quaternions in robotics
x
y
z
x
y
z
2
2
2
2 00q
100
001
010
Rot (90, 0,0,1) OR Rot (-90,0,0,-1)
Rot(z,90°)
Represented as quaternion
Represented as matrix
How to represent rotation?
Operations on Unit Quaternions
• Perform Rotation
• Composition
of rotations
• Interpolation
– Linear
– Spherical linear (more later)
)(22)(
...
0
2
0
*
xqqxqqxqqq
qxqx
****
*
)()( qpxqpqpxpqqxqx
pxpx
)(
)(,)1()( 21
tp
tpptppttp
Example of rotation using unit quaternions
x
y,x’
z,z’
y’
1
2
1
1
1
2
100
001
010
100
001
010
Rpp
R
Rot(z,90°)
p(2,1,1)
For comparison we first use matrices
Example (cont)
1
2
1
1
0
0
0
2
1
)(0
0
2
112
002
1
1
2
)(
)(22)(
2
2
2
2
2
2
2
2
21
21
0
2
0
kji
pqqpqqpqqqp
2
2
2
2 00
1120
q
p For comparison we use quaternions
Next we convert to matrices
We get the same result
New Example: multiplication of quaternions
x
y,x’
z,z’
y’
)00()001(sincos
)00()100(sincos
2
2
2
2
290
290
2
2
2
2
2
290
290
1
q
q
x,x’
y
z,y’
z’)(
)00()00(
00
00
)00()00(
)00)(00(
:nCompositio
21
21
21
21
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
212
kji
Example: Conversion of quaternion matrix to rotation matrix R
2
3
2
2
2
1
2
010322031
1032
2
3
2
2
2
1
2
03021
20313021
2
3
2
2
2
1
2
0
)(2)(2
)(2)(2
)(2)(2
qqqqqqqqqqqq
qqqqqqqqqqqq
qqqqqqqqqqqq
R
100
001
010
002
2
2
23210
R
qqqqq
Matrix R represented with quaternions
We substitute values of q
And we get R
Matrix Conversion Formulas
332211
2
3
332211
2
2
332211
2
1
332211
2
0
14
1
14
1
14
1
14
1
rrrq
rrrq
rrrq
rrrq
322341
32
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 Orthogonalization
• If 3x3 rotation matrix no longer orthonormal, metric properties might change!
321321ˆˆˆˆˆˆ vvvuuu
2
22
231
11
1333
1
11
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
• Quaternion
TxxdR
Tz
y
x
x
dRxx
10
,
1
dqxqx *
Spherical Linear Interpolation
Interpolation
Interpolation produces
an arc instead of a line
Spherical 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
q
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
q
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 for
and from p to q1 for ]1,[ 21t
]0,[))(sin())1(sin(
],0[)sin())(sin(),;(
21
121
21
021
10tqtpt
tptqtqqtslerp
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)
Start*
Finish*
*assumption
Let’s do rotation!
x
y
z
b
ac
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!
ByMathias Sunardi
forQuantum Research Group Seminar
June 15, 2006
top related