Ch1a (Compatibility Mode)

Chapter 1: Kinematics
Chew Chee Meng

2
What you would learn
Definitions of positions and orientations of rigid
bodies
Analysis of different representations for
orientation
Transformation of coordinates
Determination of new positions and
orientations after a sequence of rigid body
motions
Kinematics modeling of robotic manipulators
Forward and inverse kinematics of position
3
Spatial descriptions and Transformations
Need to specify spatial attributes of
various objects with which a manipulation
system deals
Cartesian coordinate frames are used
Where is the
cup?
4
Spatial descriptions and Transformations
Position & orientation of a Rigid Body
Position Orientation
Location of rigid body
5
Position: Attribute of a point
Represented
by a position
vector
With reference
to a coordinate
frame
3
9 e
(
(
(
=
z
y
x
A
p
p
p
P
x
A
z
A
y
A
{A}
A
P
6
Orientation: Attribute of a body
Represented by Matrices
(Rotation Matrices)
R
A
B
= [
A
x
B
A
y
B
A
z
B
] e9
3x3
A rigid body
body frame
Reference frame
x
A
z
A
y
A
{A}
x
B
z
B
y
B
{B}
By attaching a coordinate
frame to the body and
then give a description of
this coordinate frame
relative to the reference
coordinate frame
7
Orientation representation
R
A
B
= [
A
x
B
A
y
B
A
z
B
] e9
3x3
Orthogonal unit vectors
(orthonormal matrix)
1 ) det( = R
A
B
Multiplication of rotation matrices
generally not commutative
i.e. R
1
R
2
R
2
R
1
x
A
z
A
y
A
{A}
x
B
z
B
y
B
{B}
Note:
A
x
B
means x
B
expressed in {A}
8
R
A
B
= [
A
x
B
A
y
B
A
z
B
]
(
(
(

=
A B A B A B
A B A B A B
A B A B A B
z z z y z x
y z y y y x
x z x y x x
Remark:
Components are direction cosines
(dot product of two unit vectors yields the cosine of the angle
between them)
Choice of frame is arbitrary for the vectors, but must be the same
x
A
z
A
y
A
{A}
x
B
z
B
y
B
{B}
9
Orientation
R
A
B
= [
A
x
B
A
y
B
A
z
B
]
B T
A
T
B T B B B B T
A A A A A
B T
A
R
(
(
(
= = =
(

(

x
y x y z
z
Note: Rows of the matrix are the unit
vectors of {A} expressed in {B}
(
(
(

=
A B A B A B
A B A B A B
A B A B A B
z z z y z x
y z y y y x
x z x y x x
x
A
z
A
y
A
{A}
x
B
z
B
y
B
{B}
transpose of z
A
expressed in {B}
10
Orientation
R
B
A
T A
B
R =
| |
3
1 0 0
0 1 0
0 0 1
I R R
B
A
B
A
B
A
T
B
A
T
B
A
T
B
A
A
B
T A
B
=
(
(
(
=
(
(
(
= z y x
z
y
x
Hence, description of frame {A} relative to {B},
This suggests:
Verification:
T A
B
A
B
R R =
1
I
3
denotes the 3x3 identity matrix
Linear algrebra: inverse of a matrix with orthonormal columns is
equal to its transpose.
11
x
A
z
A
y
A
{A}
x
B
z
B
y
B
{B}
Uniqueness
3 independent
parameters
(
(
(
=
33 32 31
23 22 21
13 12 11
r r r
r r r
r r r
R
12
Elementary (Basic, Fundamental) Rotation
matrices
x
Ao
z
Ao
y
A1
z
A1
u
u
Rotation about x
Ao
by angle
|
|
|
.
|
\
|
= =
cos sin 0
sin - cos 0
0 0 1
) ( R R
X A
0
1
u
A
y
Ao
,
x
A1
13
matrices
x
Ao
z
Ao
,y
A1
z
A1
u
u
Rotation about y
Ao
by angle
y
Ao
|
|
|
.
|
\
|
= =
cos 0 sin -
0 1 0
sin 0 cos
) ( R R
Y A
0
1
u
A
x
A1
14
matrices
x
Ao
z
Ao
y
A1
,z
A1
u
u
Rotation about z
Ao
by angle
y
Ao
|
|
|
.
|
\
|
= =
1 0 0
0 cos sin
0 sin - cos
) ( R R
Z A
0
1
u
A
x
A1
15
Description of a frame
A set of four vectors giving
position (typically of the origin) and
orientation information of the frame
relative to the reference frame
x
A
z
A
y
A
{A}
x
B
z
B
y
B
{B}
A
p
BORG
{ }
BORG
A A
B
R p ,
E.g., Frame {B} relative to frame {A} is described by
Once we have attached a frame to a
rigid body, how to represent the
frame location and orientation?
16
Coordinate Transformations (Mappings)
Mapping: Changing descriptions from frame to
frame
Expressing the position vector of a point in
space in terms of various reference coordinate
systems
x
A
z
A
y
A
{A}
x
B
z
B
y
B
p
{B}
?
B
P
Given
Find
A
P
17
Mappings involving translated frames (i.e.,
frames having same orientations)
A
P =
B
P +
A
P
BORG
Only applicable when orientations of frames A and B are the same
18
Mappings involving rotated frames
Above equation assumes origins of Frames A & B are coincident.
What if the two origins are not coincident?
P R P
B A
B
A
=
Coordinates of P in B
Coordinates of P in A
P
19
Example 1-1: Figure 1-1 shows a frame {B} which is
rotated relative to frame {A} about z by 30 degrees.
Here, z is pointing out of the page. There is a point P
whose position vector expressed in {B} is
B
p = [0 2 0]
T
.
What is the position vector of the point P expressed in
{A}?
P
x
A
x
B
y
A
y
B
30
o
Figure 1-1
20
Solution:
Writing the unit vectors of {B} in terms of {A} and stacking
them as columns of the rotation matrix we obtain:
(
(
(

=
1 0 0
0 866 . 0 5 . 0
0 5 . 0 866 . 0
R
A
B
Given
B
p = [0 2 0]
T
,
(
(
(

= =
0
732 . 1
1
p p
B A
B
A
R
P
x
A
x
B
y
A
y
B
30
o
21
Solution:
(
(
(

=
1 0 0
0 866 . 0 5 . 0
0 5 . 0 866 . 0
R
A
B
P
x
A
x
B
y
A
y
B
30
o
Remark:
R
A
B
acts as a mapping which is
used to describe p relative to frame
{A} given
B
p.
- The original vector p is not
changed in space. We simply
compute the new description of the
vector relative to a new frame.
22
Mappings involving general frames
x
A
z
A
y
A
{A}
x
B
z
B
y
B
p
{B}
?
B
P
Given
Find
A
P
23
x
A
z
A
y
A
x
A
z
A
y
A
{A}
x
B
z
B
y
B
p
{B}
A A A B A A B
BORG B BORG B
P P R P P R P
'
= + = +
P R P
B A
B
A ' '
=
P P P
A
BORG
A A '
+ =
and
3 '
I R
A
A
=
{A} and {A} same
orientation:
R R
A
B
A
B
=
'
24
P R P P
B A
B BORG
A A
+ =
Translational
transformation
Rotational
transformation
x
A
z
A
y
A
x
A
z
A
y
A
{A}
x
B
z
B
y
B
p
{B}
25
Homogeneous Transformation
x
A
z
A
y
A
{A}
x
B
z
B
y
B
p
{B}
P R P P
B A
B BORG
A A
+ =
P T P
B A
B
A
=
Homogeneous transformation
26
(
(
(
(
(
=
1
z
A
y
A
x
A
A
p
p
p
P
(
(
(
(
(
=
1
z
B
y
B
x
B
B
p
p
p
P
(
=
1 0
BORG
A A
B A
B
P R
T
Homogeneous
transformation matrix
Augmented vector
4x4 matrix
27
Example 1-2
Figure 1-2 shows a frame {B} which is rotated relative to
frame {A} about z by 30 degrees, and translated 10 units in
x
A
, and 5 units in y
A
. Find
A
p where
B
p = [3 7 0]
T
.
Figure 1-2
28
Solution:
(
(
(
(

=
1 0 0 0
0 1 0 0
5 0 866 . 0 5 . 0
10 0 5 . 0 866 . 0
T
A
B
(
(
(
=
0
562 . 12
098 . 9
p
A
That is,
(
(
(
(
=
(
(
(
(
(
(
(
(

= =
1
0
562 . 12
098 . 9
1
0
7
3
1 0 0 0
0 1 0 0
5 0 866 . 0 5 . 0
10 0 5 . 0 866 . 0
p p
B A
B
A
T
29
Interpretation 1:
T represents coordinate transformations in compact
form
x
A
z
A
y
A
{A}
x
B
z
B
y
B
p
{B}
(
=
1 0
BORG
A A
B A
B
P R
T
30
Interpretation 2:
T represents position & orientation of the coordinate
frame {B} relative to {A}
x
A
z
A
y
A
{A}
x
B
z
B
y
B
{B}
(
=
1 0
BORG
A A
B A
B
P R
T
31
Interpretation 3:
T represents rotation and translation of the coordinate
frame {A} to {B}
x
A
z
A
y
A
{A}
(
=
1 0
BORG
A A
B A
B
P R
T
x
B
z
B
y
B
{B}
32
For free vectors (e.g. force, velocity, etc),
augment the vectors with 0 rather than 1:
(
(
(
(
(
=
0
z
A
y
A
x
A
A
p
p
p
P
33
Consecutive transformation:
x
0
z
0
y
0
{0}
x
1
z
1
y
1
{1}
x
n
z
n
y
n
{n}
p
n
P
Given
Find
0
P
x
n-1
z
n-1
y
n-1
{n-1}
A
i
i
1 homogeneous transformation
matrix from frame i to frame i-1
(i = 1,,n)
augmented position vector of P
expressed in frame n
expressed in frame 0
34
Consecutive transformation:
0 0 1 1 0
1 2
n n n
n n
P A A A P A P
= =
A
i
i
1
homogeneous transformation
matrix from frame i to frame i-1
(i = 1,,n)
P
n
P
o
expressed in frame n
expressed in frame 0
0 1 1 0
1 2
n
n n
A A A A
=
homogeneous transformation
from frame n to frame 0
where
35
Inverse of a Homogeneous Transformation
Matrix
T
A
B
T
B
A
1
T
A
B
Given
, find or
I T T
A
B
A
B
=
1
3 3
1 0 0 0
0
0 1 0 0
0 1 0 1
0 1
0 0 1 0
0 0 0 1
A A
B BORG
X Y I
R P

(
(
(
( (
(
= =
(
( (
(
(

(

I RX
A
B
=
R R R X
B
A
T A
B
A
B
= = =
1
0 = +
BORG
A A
B
P RY
BORG
A T A
B BORG
A A
B
P R P R Y = =
1
By definition,
=>
=>
i.e.
36
Inverse of a Homogeneous Transformation
Matrix
1
0 1
A T A T A
B B BORG
B A
A B
R R P
T T
= =
(
(

37
Example 1-4
Figure 1-5 shows a frame {B} which is rotated relative to frame {A}
about z by 30 degrees, and translated four units in x
A
, and three
units in y
A
. Find .
T
B
A
Figure 1-5
38
Solution:
(
(
(
(

=
1 0 0 0
0 1 0 0
3 0 866 . 0 5 . 0
4 0 5 . 0 866 . 0
T
A
B
(
(
(
(

=
(

= =

1 0 0 0
0 1 0 0
598 . 0 0 866 . 0 5 . 0
964 . 4 0 5 . 0 866 . 0
1
1
0
p
BORG
A T A
B
T A
B A
B
B
A
R R
T T
39
Example 1-5
Assume we know T
B
T
in Figure 1-6, which describes the frame at the
manipulators fingertips {T} relative to the base of the manipulator, {B}. Also,
we know where the tabletop is located in space relative to the manipulators
base because we have a description of the frame {S} which is attached to the
table as shown, T
B
S
. Finally, we know the location of the frame attached to the
bolt lying on the table relative to the table frame, that is, T
S
G
. Calculate the
position and orientation of the bolt relative to the manipulators hand, T
T
G
.
Figure 1-6
40
Solution:
1 T T B S B B S
G B S G T S G
T T T T T T T
= =
41
Operators: Translations, Rotations, and
Transformation
Operators :
Another interpretation of the mathematical
forms used for coordinate transformations
Only one coordinate frame is involved
42
Transformation
Translational operators: Moves a point in space a
finite distance along a given vector direction
x
A
z
A
y
A
{A}
A
P
1
A
P
2
A
Q
How a vector
A
P
1
is translated by a vector
A
Q :
Result of the operation is a new vector
A
P
2
=
A
P
1
+
A
Q
1 2
1 0 0 0
1 0 0
0 1 0
0 0 1
P
q
q
q
P
A
z
y
x
A
(
(
(
(
=
To write the translation operation as a matrix operator,
where q
x
, q
y
and q
z
are components of the translation vector Q
x
y
z
q
q
q
(
(
=
(
(

43
Transformation
Rotational operators:
- Operates on a vector
A
P
1
and changes that vector to a new
vector
A
P
2
, by means of a rotation, R (about origin)
- Same as the rotation matrix that describes a frame rotated by
R relative to the reference frame
Define a rotational operator R
K
(u):
A
P
2
= R
K
(u)
A
P
1
- K is the axis of rotation
- u (in degrees) is the amount of
rotation about K
44
Transformation
Rotational operators:
(
(
(
(

=
1 0 0 0
0 1 0 0
0 0 cos sin
0 0 sin cos
) (
u u
u u
u
Z
R
E.g. operator that rotates about the Z axis by u can be written as:
(
(
(

=
1 0 0
0 cos sin
0 sin cos
) ( u u
u u
u
Z
R
homogeneous transformation matrix 3x3 rotation matrix
45
Transformation
Example 1-6
Figure 1-3 shows a vector
A
p
1
= [0 2 0]
T
. We wish to compute the vector
obtained by rotating this vector about z by 30 degrees. Call the new vector
A
p
2
.
Figure 1-3
46
Transformation
Solution:
Note: Rotation matrix which rotates vectors by 30 degrees about z =
Rotation matrix which describes a frame rotated 30 degrees about z
relative to the reference frame.
(
(
(

=
1 0 0
0 866 . 0 5 . 0
0 5 . 0 866 . 0
) 0 . 30 (
z
R
(
(
(

=
(
(
(
(
(
(

= =
0
732 . 1
1
0
2
0
1 0 0
0 866 . 0 5 . 0
0 5 . 0 866 . 0
) 0 . 30 (
1 2
p p
z
A A
R
47
Transformation
Transformation operators
- Rotate and translate
Define operator T to be one which rotates and translates a vector
A
P
1
to a new vector
A
P
2
:
A
P
2
= T
A
P
1
(1-3)
The transform that rotates by R and translates by Q is the
same as the transform that describes a frame rotated by R
and translated by Q relative to the reference frame.
48
Transformation
Example 1-7
Figure 1-4 shows a vector
A
p
1
. We wish to rotate it about z by 30
degrees, and translate it 10 units in x
A
, and 5 units in y
A
. Find
A
p
2
where
A
p
1
= [3 7 0]
T
.
49
Transformation
Solution:
The operator T, which performs the rotation and translation, is
(
(
(
(

=
1 0 0 0
0 1 0 0
5 0 866 . 0 5 . 0
10 0 5 . 0 866 . 0
T
(
(
(
(
=
(
(
(
(
(
(
(
(

= =
1
0
562 . 12
098 . 9
1
0
7
3
1 0 0 0
0 1 0 0
5 0 866 . 0 5 . 0
10 0 5 . 0 866 . 0
1 2
p p
A A
T
Note that this example is numerically similar to Example 1-2, but the
interpretation is different.
50
Other Orientation Representation
Orientation of rigid body represented by 3x3
rotation matrix R 9 elements
Subject to:
orthogonality conditions (3 equations) and
unit length conditions (3 equations)
=> only 3 of 9 elements are independent (ie. there is
redundancy in R)
Different representations of orientation which
requires only three or four parameters
51
X-Y-Z fixed angles
Each of the three rotations takes place about an axis in the
fixed reference frame, {A}
X
Y Z
(roll) (pitch)
(yaw)
52
X-Y-Z fixed angles
X
Z
YAW
PITCH
ROLL
Y
53
X-Y-Z fixed angles
( , , ) ( ) ( ) ( )
0 0 1 0 0
0 0 1 0 0
0 0 1 0 0
A
B XYZ Z Y X
R R R R
c s c s
s c c s
s c s c
c c c s s s c c s c s s
s c s s s c c s s c c s
s c s c c
| o o |
o o | |
o o
| |
o | o | o o | o
o | o | o o | o
| | |
=
( ( (
( ( (
=
( ( (
( ( (

+
(
(
= +
(
(

where c and s denote cosine and sine functions, respectively
54
X-Y-Z fixed angles
Inverse transformation
Given
(
(
(
=
33 32 31
23 22 21
13 12 11
r r r
r r r
r r r
R
A
B
, find o, ,
Nine equations (6 dependencies) and three unknowns
11 12 13
21 22 23
31 32 33
r r r c c c s s s c c s c s s
r r r s c s s s c c s s c c s
r r r s c s c c
o | o | o o | o
o | o | o o | o
| | |
+
( (
( (
= +
( (
( (

55
X-Y-Z fixed angles
11 12 13
21 22 23
31 32 33
r r r s c s c c
o | o | o o | o
o | o | o o | o
| | |
+
( (
( (
= +
( (
( (

Remark:
-Atan2(y,x) computes tan
-1
(y/x) and uses signs of both x and y to
determine the quadrant in which the angle lies
) 1 , ( 2 tan
) , ( 2 tan
2
31 31
r r A
c s A
=
= | | |
56
X-Y-Z fixed angles
21 11
tan 2( , )
tan 2( , )
A s c
r r
A
c c
o o o
| |
=
=
11 12 13
21 22 23
31 32 33
r r r s c s c c
o | o | o o | o
o | o | o o | o
| | |
+
( (
( (
= +
( (
( (

57
X-Y-Z fixed angles
11 12 13
21 22 23
31 32 33
r r r s c s c c
o | o | o o | o
o | o | o o | o
| | |
+
( (
( (
= +
( (
( (

32 33
tan 2( , )
tan 2( , )
A s c
r r
A
c c

| |
=
=
58
X-Y-Z fixed angles
) , ( 2 tan
2
21
2
11 31
r r r A + = |
32 33 21 11
tan 2( , ) tan 2( , )
r r r r
A A
c c c c
o
| | | |
= =
Remark:
- 2 solutions in general
- If |r
31
|=1 (implies r
11
= r
21
= r
32
= r
33
= 0), the solution degenerates
(mathematical singularity) such that only the difference of o and
may be computed:
If r
31
= +1: => o+ = Atan2 (-r
23
, r
22
)
If r
31
= -1: => o- = Atan2 (r
23
, r
22
)
12 13
22 23
31
0
0
0 0
r r c c c s s s c c s c s s
r r s c s s s c c s s c c s
r s c s c c
o | o | o o | o
o | o | o o | o
| | |
+
( (
( (
= +
( (
( (

0
tan 2( 1, 0) 90 A | = =
0
tan 2(1, 0) 90 A | = =
59
Z-Y-X Euler angles
Each rotation is performed about an axis of the moving frame {B},
rather than the fixed reference frame, {A}
Z
Y X
60
Z-Y-X Euler angles
R R R R
B
B
B
B
A
B
A
B
' ' '
' ' '
=
( , , ) ( ) ( ) ( )
0 0 1 0 0
0 0 1 0 0
0 0 1 0 0
A
B Z Y X Z Y X
R R R R
c s c s
s c c s
s c s c
c c c s s s c c s c s s
s c s s s c c s s c c s
s c s c c
o | o |
o o | |
o o
| |
o | o | o o | o
o | o | o o | o
| | |
' ' '
=
( ( (
( ( (
=
( ( (
( ( (

+
(
(
= +
(
(

Note:
- Same as the result obtained for the same three rotations taken in the
opposite order about fixed axes.
Recall:
Consecutive
transformation
61
Four-parameter representations for
orientation
Equivalent angle-axis
Euler parameters or Unit Quaternion
Nonminimal representations of
orientation
62
Any rotation matrix can be
represented by choosing the
proper axis and angle (Eulers
theorem on rotation)
quadruple of ordered real
parameters consisting of one
scalar (angle of rotation) and one
unit vector (axis of rotation)
(according to right-hand rule)
A
K
u
63
) (
u
K
A
R
|
|
|
|
.
|
\
|
+ +
+ +
+ +
c v k k s k v k k s k - v k k
s k - v k k c v k k s k v k k
s k v k k s k - v k k c v k k
z z x z y y z x
x y z y y z y x
y x z z x y x x
=
where vu = (1 - cosu),
,
T
A
x y z
K k k k
(
=

, sign of u is determined by right-hand rule
Remark:
-For small angular rotations, axis of rotation becomes ill-defined.
-Non-unique representation: ) ( ) (

u u
K K
R R =
(1-4)
1 (unit vector)
A
K =
64
Example 1-8
A frame {B} is described as follow: initially
coincident with {A} we rotate {B} about the vector
A
k = [0.707 0.707 0]
T
(passing through the origin)
by an amount u = 30 degrees. Give the frame
description of {B} with reference to {A} after the
rotation.
65
Solution:
( )
x x y x z z x y
x y z y y z y x
x z y y z x z z
k k v30 c30 k k v30 - k s30 k k v30 k s30
30 k k v30 k s30 k k v30 c30 k k v30 - k s30
k k v30 - k s30 k k v30 k s30 k k v30 c30
o o o o o o
A o o o o o o o
o o o o o o
R
|
+ +
= + +
+ +
\
k
|
|
|
|
|
.
where v30
o
= 1-cos 30
0
;
Since there is no translation of the origin,
( )
0
0.933 0.067 0.354 0
30 0
0.067 0.933 0.354 0
0
0.354 0.354 0.866 0
0 0 0 1
0 0 0 1
A o
A
B
R
T
(
(
(
(
(
(
= =
(
(
(
(
(

k
k = [k
x
, k
y
, k
z
]
T
=[0.707 0.707 0]
T
66
Example 1-9
A frame {B} is described as follows: initially coincident with {A}, we rotate
{B} about the vector
T A
] 0 . 0 707 . 0 707 . 0 [ = k , passing through the point
| |
T
A
P 3 2 1 = , by
o
30 = u . Give the frame description of {B} with reference to
{A}.
Solution:
Before performing the rotation, {A} and {B} are coincident. We define
two new frames, {A} and {B} which are obtained by translating {A} and
{B}, respectively, to point P. {B} and {B} can be treated as if they are
mounted on the same rigid body, whereas {A} and {A} are fixed in the
space.
67
Solution:
(
(
(
(
=
1 0 0 0
3 1 0 0
2 0 1 0
1 0 0 1
'
T
A
A
(
(
(
(
=
'
1 0 0 0
3 1 0 0
2 0 1 0
1 0 0 1
T
B
B
68
Solution:
Note that the point P is on the axis of rotation. Now, lets
rotate {B} relative to {A}. This is a rotation about an axis
which passes through the origin, so we may use
( )
|
|
|
|
.
|
\
|
+ +
+ +
+ +
=
'
c v k k s k v k k s k - v k k
s k - v k k c v k k s k v k k
s k v k k s k - v k k c v k k
z z x z y y z x
x y z y y z y x
y x z z x y x x
u
k
R
A
to compute {B} relative to {A} after the rotation.
( )
(
(
(
(

=
(
(
(
(
=
'
'
'
1 0 0 0
0 866 . 0 354 . 0 354 . 0
0 354 . 0 933 . 0 067 . 0
0 354 . 0 067 . 0 933 . 0
1 0 0 0
0
0
0
30
o A
A
B
R
T
k
69
Solution:
(
(
(
(

= =
' '
' '
1 0 0 0
05 . 0 866 . 0 354 . 0 354 . 0
13 . 1 354 . 0 933 . 0 067 . 0
13 . 1 354 . 0 067 . 0 933 . 0
T T T T
B
B
A
B
A
A
A
B
Finally,
Note: After rotating about the axis k , {B}
becomes separated from {A} as shown in Fig.
1-9.
Figure 1-9
70
Inverse Problem
R
A
B
A
K
x x x
A
B y y y
z z z
x x y x z z x y
x y z y y z y x
x z y y z x
n o a
Let R n o a
n o a
k k v c k k v - k s k k v k s
k k v k s k k v c k k v - k s
k k v - k s k k v k s
| |
|
=
|
|
\ .
+ +
= + +
+
z z
k k v c
| |
|
|
|
+
\ .
given
Given Find
, u
where vu = (1 - cosu)
71
Inverse Problem
R
A
B
R
A
B
From the diagonal elements,
Trace (
) = 1 + 2cosu
=> cosu = ( Trace (
) 1 )
(Two solutions for u = A)
+A
-A
( )
x x y x z z x y
x x x
y y y x y z y y z y x
z z z x z y y z x z z
n o a
n o a k k v k s k k v c k k v - k s
n o a k k v - k s k k v k s k k
A
K
R u
+ +
| |
|
= = + +
|
|
+
\ .
v c
| |
|
|
|
+
\ .
where vu = (1 - cosu)
72
Inverse Problem
( )
x x y x z z x y
x x x
y y y x y z y y z y x
z z z x z y y z x z z
n o a
n o a k k v k s k k v c k k v - k s
n o a k k v - k s k k v k s k k
A
K
R u
+ +
| |
|
= = + +
|
|
+
\ .
v c
| |
|
|
|
+
\ .
sin 2
n - a
k
z x
y
=
sin 2
a - o
k
y z
x
=
sin 2
o - n
k
x y
z
=
If sinu = 0 :
From : n
z
& a
x
elements
From : o
z
& a
y
elements
From : n
y
& o
x
elements
73
Inverse Problem
A
K
u
In general:
2 solutions
(1)
A
K
u
(2)
74
Inverse Problem
|
|
|
.
|
\
|
=
1 0 0
0 1 0
0 0 1
R
A
B
Identity (null rotation)
|
|
|
|
.
|
\
|
+
+
+
=
2
z z y z x
z y
2
y y x
z x y x
2
x
A
B
2k 1 - k 2k k 2k
k 2k 2k 1 - k 2k
k 2k k 2k 2k 1 -
R
-If u = 0
-If u = 180
( symmetric )
K
= any axis (infinite solutions)
75
Inverse Problem
x
z
z
x
y
y
z
y z
x
2k
a
k
2k
n
k
2o
n n
k
=
=
=
2 solutions for
(If one solution is ,
the other is )
2
x x y x z
x x x
A 2
B y y y x y y y z
2
z z z
x z y z z
-1 2k 2k k 2k k
n o a
R n o a = 2k k -1 2k 2k k
n o a
2k k 2k k -1 2k
| |
+
| |
|
|
= + |
|
|
|
|
+
\ .
\ .
-If u = 180 (cont)
K
1
K
1
K
76
Inverse Problem
R
A
B
If all off-diagonal terms are 0, one of k
x
, k
y
or k
z
= 1 &
the rest are 0s. The component of that is 1 has a
value of 1 in the diagonal of .
E.g. k
x
= 1, k
y
= k
z
= 0
A
B
1 0 0
R 0 -1 0
0 0 -1
| |
|
=
|
|
\ .
-If u = 180 (cont)
K
77
Euler parameters (Unit Quaternion)
Able to solve the non-uniqueness problem
encountered by angle/axis representation
2
sin
1
u
c
x
k =
2
sin
2
u
c
y
k =
2
sin
3
u
c
z
k =
2
cos
4
u
c =
K
Euler parameters:
where k
x
, k
y
and k
z
are the coordinates of the equivalent axis
and u is the equivalent angle
4 parameters not
independent because:
1
2
4
2
3
2
2
2
1
= + + + c c c c
78
Based on Eq ( 1-4) (equivalent angle-axis) and the definition of unit
quaternion:
(
(
(
+ +
+ +
+ +
=
1 ) 2 ( 2 ) ( 2 ) ( 2
) ( 2 1 ) ( 2 ) ( 2
) ( 2 ) ( 2 1 ) ( 2
2
3
2
4 4 1 3 2 4 2 3 1
4 1 3 2
2
2
2
4 4 3 2 1
4 2 3 1 4 3 2 1
2
1
2
4
c c c c c c c c c c
c c c c c c c c c c
c c c c c c c c c c
c
R
(1-5)
x x y x z z x y
x y z y y z y x
x z y y z x z z
k k v k s k k v c k k v - k s
k k v - k s k k v k s k k v c
A
B
R
| | + +
|
= + +
|
|
+ +
\ .
1
sin
2
x
k
c
u
=
2
sin
2
y
k
c
u
=
3
sin
2
z
k
c
u
=
( )
2 2
2
4
cos 2cos 1 2 1
u
u c = =
( )
2
2
2sin 1 cos
u
u = , etc
79
Inverse Problem
(
(
(
=
33 32 31
23 22 21
13 12 11
r r r
r r r
r r r
R
A
B
1 ) sgn(
2
1
33 22 11 23 32 1
+ = r r r r r c
1 ) sgn(
2
1
11 33 22 31 13 2
+ = r r r r r c
1 ) sgn(
2
1
22 11 33 12 21 3
+ = r r r r r c
33 22 11 4
1
2
1
r r r + + + = c
Given
, find c
i
Note: sgn(x) = 1 for x > 0 and sgn(x) = -1 for x < 0
2 2
4 1 1 2 3 4 1 3 2 4
2 2
1 2 3 4 4 2 2 3 1 4
2 2
1 3 2 4 2 3 1 4 4 3
2( ) 1 2( ) 2( )
2( ) 2( ) 1 2( )
2( ) 2( ) 2( ) 1
R
c
c c c c c c c c c c
c c c c c c c c c c
c c c c c c c c c c
( + +
(
= + +
(
(
+ +

80
Inverse Problem
1 ) sgn(
2
1
33 22 11 23 32 1
+ = r r r r r c
1 ) sgn(
2
1
11 33 22 31 13 2
+ = r r r r r c
1 ) sgn(
2
1
22 11 33 12 21 3
+ = r r r r r c
33 22 11 4
1
2
1
r r r + + + = c
Note:
-Implicitly assumed (by considering ue[-t,t])
0
4
> c
-No singularity occurs (compared to angle/axis representation)

Ch1a (Compatibility Mode)

Transféré par

Informations du document

Description originale:

Titre original

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Ch1a (Compatibility Mode)

Transféré par

Droits d'auteur :

Formats disponibles

Chapter 1: Kinematics

Chew Chee Meng

Vous aimerez peut-être aussi