Call for Papers - Educational Administration: Theory and Practice, E-ISSN: 21...
Chapter_2_Representing Position and Orientation.pdf
1. Robotics
(10651682)
Department Of Mechatronics Engineering
Faculty Of Engineering And Information Technology
An-Najah National University
Dr. Mohammad Abuabiah
m.abuabiah@najah.edu
Office: 11-4-130
1
3. • Numbers are an important part of mathematics. We use numbers for
counting: there are 2 apples.
• We use denominate numbers, a number plus a unit, to specify
distance: the object is 2 m away. We also call this single number a
scalar.
• We use a vector, a denominate number plus a direction, to specify a
location: the object is 2 m due north.
• We may also want to know the orientation of the object: the object is 2
m due north and facing west.
• The combination of position and orientation we call pose.
3
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
4. 2.1 Introduction
• A point in space is a familiar concept from mathematics and can
be described by a coordinate vector.
• The vector represents the displacement of the point with respect
to some reference coordinate frame – we call this a bound vector
since it cannot be freely moved.
Dr.
Mohammad
Abuabiah
An-Najah
National
University
4
5. • A coordinate frame, or Cartesian coordinate
system, is a set of orthogonal axes which intersect
at a point known as the origin.
• A vector can be described in terms of its
components, a linear combination of unit vectors
which are parallel to the axes of the coordinate
frame.
• Note that points and vectors are different types of
mathematical objects even though each can be
described by a tuple of numbers. We can add
vectors but adding points makes no sense.
• The difference of two points is a vector, and we
can add a vector to a point to obtain another
point.
5
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
6. • A point is an interesting mathematical abstraction, but a real
object comprises infinitely many points.
• An object, unlike a point, also has an orientation. If we attach a
coordinate frame to an object, we can describe every point
within the object as a constant vector with respect to that frame.
6
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
7. • To distinguish the different frames we label them and in
this case the object coordinate frame is labeled {𝑩}
and its axes are labeled 𝒙𝑩 and 𝒚𝑩 , adopting the
frame’s label as their subscript.
• To completely describe the pose of a rigid object in a
3-dimensional world we need 6 not 3 dimensions: 3 to
describe its position and 3 to describe its orientation.
• If we increase the value of one of the position
dimensions the object will move continuously in a
straight line,
• If we increase the value of one of the orientation
dimensions the object will rotate in some way and
soon get back to its original orientation.
• We clearly need to treat the position and orientation
dimensions quite differently.
7
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
8. • The pose of the coordinate frame is denoted by the symbol ξ
(pronounced Ksi - 𝛏) / Or it can be written as 𝑷𝑩𝑶𝑹𝑮.
8
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
9. • The Figure shows two frames {𝑨} and {𝑩} and the
relative pose: 𝑨𝝃𝑩 which describes {𝑩} with respect
to {𝑨}.
• The leading superscript (A) denotes the reference
coordinate frame and the subscript denotes (𝑩) the
frame being described.
• We could also think of 𝑨
𝝃𝑩 as describing some
motion – imagine picking up {𝑨} and applying a
displacement and a rotation so that it is transformed
to {𝑩}.
• If the initial superscript is missing we assume that the
change in pose is relative to the world coordinate
frame which is generally denoted {𝑶}.
9
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
10. • The point P in Fig. can be described with
respect to either coordinate frame by the
vectors 𝐀𝒑 or 𝐁𝒑 respectively. Formally they
are related by:
𝐀
𝒑 = 𝑨
𝝃𝑩 ∙ 𝐁
𝒑
• where the right-hand side expresses the
motion from {𝑨} to {𝑩} and then to 𝑷.
• The operator ∙ transforms the vector,
resulting in a new vector that describes
the same point but with respect to a
different coordinate frame.
10
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
11. • The remainder of this chapter discusses concrete representations
of ξ for various common cases that we will encounter in robotics.
• We start by considering the two-dimensional case which is
comparatively straightforward and then extend those concepts to
three dimensions.
• In each case we consider rotation first, and then add translation
to create a description of pose.
11
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.1 Introduction
12. • A 2-dimensional world, or plane, is familiar to us
from high-school Euclidean geometry.
• We use a right-handed Cartesian coordinate
system or coordinate frame with orthogonal axes
denoted x and y and typically drawn with the x-
axis horizontal and the y-axis vertical.
• The point of intersection is called the origin. Unit-
vectors parallel to the axes are denoted ෝ
𝒙 and ෝ
𝒚.
• A point is represented by its x- and y-coordinates
(x, y) or as a bound vector
𝒑 = 𝑥ෝ
𝒙 + 𝑦ෝ
𝒚
12
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2 Working in Two Dimensions (2D)
13. • Figure below shows a frame {B} that is rotated relative to frame {A}
by 30 degree and a point 𝒑 which is given by:
𝐁𝒑 =
𝟎. 𝟎
𝟐. 𝟎
• Describe 𝐁
𝒑 relative to frame {A} ≡ 𝐀
𝒑
(Mapping)
13
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.1 2D Frame Rotation
𝜽
𝑷
14. • Since the transforms from frame {𝑨} to {𝑩} is only rotation, we can
write the unit vectors of {B} in terms of {A} as following:
• 𝑨
𝑹𝑩 =
𝐜𝐨𝐬𝜽
𝒔𝒊𝒏𝜽
−𝒔𝒊𝒏𝜽
𝒄𝒐𝒔𝜽
=
𝟎. 𝟖𝟔𝟔
𝟎. 𝟓𝟎𝟎
−𝟎. 𝟓𝟎𝟎
𝟎. 𝟖𝟔𝟔
• Then we calculate 𝐀𝒑 as follows:
• 𝐀𝒑 = 𝑨 𝑹𝑩
𝐁𝒑 =
𝟎. 𝟖𝟔𝟔
𝟎. 𝟓𝟎𝟎
−𝟎. 𝟓𝟎𝟎
𝟎. 𝟖𝟔𝟔
𝟎. 𝟎
𝟐. 𝟎
• 𝐀𝒑 =
−𝟏. 𝟎𝟎𝟎
𝟏. 𝟕𝟑𝟐
14
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.1 2D Frame Rotation
𝜽
𝑷
𝐜𝐨𝐬𝜽
𝐬𝐢𝐧𝜽
−𝐬𝐢𝐧𝜽
𝐜𝐨𝐬𝜽
X Y
15. • The MATLAB Robotics Toolbox allows easy creation of these rotation
matrices:
• where the angle is specified in radians. We can also but it in degree as
follows
• Then
15
Dr.
Mohammad
Abuabiah
An-Najah
National
University
clc
clear all
close all
%% Ex 2.1 Rotation Matrix
Bp = [0;2];
R = rot2(0.5236);
display(R)
R =
0.8660 -0.5000
0.5000 0.8660
R = rot2(30,’deg’);
display(R)
Ap = R*Bp;
display(Ap)
Ap =
-1.0000
1.7321
Example 2.1* 2D Frame Rotation
𝜽
𝑷
16. • is a 2-dimensional rotation matrix with some special properties:
– it is orthonormal (also called orthogonal) since each of its columns is a unit vector
and the columns are orthogonal.
– the columns are the unit vectors that define the axes of the rotated frame 𝒀 with
respect to 𝑿 and are by definition both unit-length and orthogonal.
– it belongs to the Special Orthogonal group of dimension 2 or 𝑹 ∈ 𝑺𝑶 𝟐 ⊂ 𝑅2×2.
This means that the product of any two matrices belongs to the group, as does its
inverse.
– its determinant is +𝟏, which means that the length of a vector is unchanged after
transformation, that is, || 𝒀𝒑|| = || 𝑿𝒑||, ∀𝜽 .
– the inverse is the same as the transpose, that is, 𝑹−𝟏 = 𝑹𝑻.
16
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.1 Orthonormal Rotation Matrix
17. • We can also rearrange do the following:
𝐀𝒑 = 𝑨 𝑹𝑩 . 𝑩𝒑
𝑨𝒙
𝑨𝒚
= 𝑨 𝑹𝑩
𝑩𝒙
𝑩𝒚
𝑩𝒙
𝑩𝒚
= 𝑨
𝑹𝑩
−𝟏 𝑨𝒙
𝑨𝒚
= 𝑨
𝑹𝑩
𝑻 𝑨𝒙
𝑨𝒚
= 𝑩
𝑹𝑨
𝑨𝒙
𝑨𝒚
𝑩𝒑 = 𝑩 𝑹𝑨 . 𝑨𝒑
• Note that inverting the matrix is the same as swapping the
superscript and subscript which leads to the identity:
𝑹(−𝜽 ) = 𝑹 𝜽 𝑻
17
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.1 Orthonormal Rotation Matrix
18. • As we saw in the example, the Toolbox allows easy creation of
these rotation matrices:
• where the angle is specified in radians. We can observe some of
the properties such as
• and the product of two rotation matrices is also a rotation matrix
18
Dr.
Mohammad
Abuabiah
An-Najah
National
University
clc
clear all
close all
%% Rotation Matrix 2.1.1.1
R = rot2(0.2);
display(R)
R =
0.9801 -0.1987
0.1987 0.9801
det(R) 1
det(R*R) 1
2.2.1 Orthonormal Rotation Matrix
19. • The Toolbox also supports symbolic mathematics for example:
• Also,
• Finally,
19
Dr.
Mohammad
Abuabiah
An-Najah
National
University
syms theta
R = rot2(theta);
display(R)
R =
[ cos(theta), -sin(theta)]
[ sin(theta), cos(theta)]
1
simplify(R*R)
[ cos(2*theta), -sin(2*theta)]
[ sin(2*theta), cos(2*theta)]
simplify(det(R))
2.2.1 Orthonormal Rotation Matrix
20. • Consider a pure rotation of 0.3 radians expressed as a rotation matrix:
• We can compute the logarithm of this matrix using the MATLAB built-in
function
• and the result is a simple matrix with two elements having a magnitude
of 0.3, which intriguingly is the original rotation angle.
• There is something deep and interesting going on here – we are on the
fringes of Lie group theory which we will encounter throughout this
course.
20
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = rot2(0.3)
0.9553 -0.2955
0.2955 0.9553
S = logm(R)
S =
0.0000 -0.3000
0.3000 0.0000
2.2.1 Orthonormal Rotation Matrix
21. • In 2 dimensions the skew-symmetric matrix is
𝜔 × =
0 −𝝎
𝝎 0
• which has clear structure and only one unique element 𝝎 ∈ 𝑹. A
simple example of Toolbox support for skew-symmetric matrices is
• and the inverse operation is performed using the Toolbox function
vex (convert skew-symmetric matrix to vector)
21
Dr.
Mohammad
Abuabiah
An-Najah
National
University
T = skew(2)
T =
0 -2
2 0
vex(T) 2
2.2.2 Matrix Exponential
22. • This matrix S has a zero diagonal and is an example of a 2 × 2
skew-symmetric matrix. To recover the rotation angle:
• The inverse of a logarithm is exponentiation and using the builtin
MATLAB matrix exponential function
• the result is, as expected, our original rotation matrix. In fact the
command
• is equivalent to
22
Dr.
Mohammad
Abuabiah
An-Najah
National
University
vex(S) 0.3000
expm(S)
0.9553 -0.2955
0.2955 0.9553
R = rot2(0.3);
R = expm(skew(0.3));
2.2.2 Matrix Exponential
S =
0.0000 -0.3000
0.3000 0.0000
23. • Formally we can write
𝑹 = 𝒆 𝜽 × ∈ 𝑺𝑶(𝟐)
• where 𝜽 is the rotation angle, and the notation
· ×: ℝ → ℝ2×2
indicates a mapping from a scalar to a skew-symmetric matrix.
23
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.2 Matrix Exponential
24. • Now we need to account for the translation between the origins of
the frames, as for example shown in figure below.
• We can see clearly that the origin of {𝑩} has been displaced by the
vector 𝒕 = (𝒙, 𝒚) and then rotated counter-clockwise by an angle θ.
24
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.3 Homogeneous Transformation Matrix (2D)
25. • In this example, 𝒕 = (𝒙, 𝒚) is the translation
of the frame and the orientation/Rotation
is 𝑨𝑹𝑩.
• The coordinate vectors for point 𝑷 can be
expressed in homogeneous form for 2D
dimension as follows:
25
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.3 Homogeneous Transformation Matrix (2D)
𝑨
𝒑 =
𝑨𝑹𝑩 𝒕
𝟎𝟏×𝟐 𝟏
𝑩𝒙
𝑩𝒚
𝟏
=
𝑨𝑹𝑩 𝒕
𝟎𝟏×𝟐 𝟏
𝑩
𝒑
= 𝑨
𝑻𝑩
𝑩
𝒑
26. 𝑨
𝒑 = 𝑨 𝑻𝑩
𝑩
𝒑
• The tilde
𝒑 indicates the vector is homogeneous.
• 𝑨
𝑻𝑩 is referred to as a homogeneous transformation.
• The matrix has a very specific structure and belongs to the
Special Euclidean group of dimension 2 or 𝑻 ∈ 𝑺𝑬(𝟐) ⊂ ℝ𝟑×𝟑.
• Now, is clear that 𝑨𝑻𝑩 represents translation and orientation or
relative pose. This is often referred to as a rigid-body motion.
𝑨𝑻𝑩 =
𝑨𝑹𝑩 𝒕
𝟎𝟏×𝟐 𝟏
26
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.3 Homogeneous Transformation Matrix (2D)
27. • A vector 𝒑 = (𝒙, 𝒚) is written in homogeneous form as
𝒑 ∈ ℙ𝟐
,
𝒑 = (𝒙𝟏, 𝒙𝟐, 𝒙𝟑)
where 𝒙 = 𝒙𝟏/𝒙𝟑, 𝒚 = 𝒙𝟐/𝒙𝟑 and 𝒙𝟑 ≠ 𝟎.
• The dimension has been increased by one and a point on a plane is
now represented by a 3-vector.
• To convert a point to homogeneous form we typically append an
element equal to one
𝒑 = (𝒙, 𝒚, 𝟏).
27
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.3 Homogeneous Transformation Matrix (2D)
28. • Homogeneous vectors have the important property that 𝒑 is
equivalent to 𝝀
𝒑 for all 𝝀 ≠ 𝟎 which we write as
𝒑 ≅ 𝝀
𝒑.
• That is
𝒑 represents the same point in the plane irrespective of the
overall scaling factor.
• Homogeneous representation is important for computer vision (more
advanced course!).
28
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.3 Homogeneous Transformation Matrix (2D)
29. • Figure shows a frame {B}, which is rotated relative to frame {A} about
by 30 degrees, translated 10 units in
𝑿𝑨, and translated 5 units in
𝒀𝑨.
Find 𝐀𝒑, were 𝐁𝒑 = 𝟑. 𝟎 𝟕. 𝟎 𝑻 .
29
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.2 2D Homogeneous Matrix
𝜽
𝑷
𝐀
𝒑𝑩𝑶𝑹𝑮
𝐁𝒑
𝐀
𝒑
31. 31
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.2* 2D Homogeneous Matrix
• We can also solve this example using MATLAB and Robotics Toolbox.
%% Example 2.2
clc
clear
close all
%% Homogeneous Matrix
TB = transl2(10, 5) * trot2(30, 'deg'); %Create Frame B
display(TB)
%% Find Ap
Bp = [3;7]; %Point P with respect to {B}
Bph = e2h(Bp); %Convert Bp to Homogeneous
Aph = TB*Bph; %Find the Homogeneous point P with respect to A
Ap = h2e(Aph); %Convert Aph to Euclidean
display(Ap)
TB =
0.8660 -0.5000 10.0000
0.5000 0.8660 5.0000
0 0 1.0000
Ap =
9.0981
12.5622
32. 32
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.2* 2D Homogeneous Matrix
• We can also Plot the Results using MATLAB and Robotics Toolbox.
%% Plot T
figure, plotvol([-3 15 -3 15]); %Create a square plot
trplot2(TA, 'frame', 'A', 'color', 'r') %Plot Frame A
trplot2(TB, 'frame', 'B', 'color', 'b') %Plot Frame B
%% Plot P
plot_point(Ap, 'label', 'P', 'solid', 'ko'); %Plot the Point P
TB =
0.8660 -0.5000 10.0000
0.5000 0.8660 5.0000
0 0 1.0000
Ap =
9.0981
12.5622
33. • To make this more tangible we will show some numerical
examples using MATLAB and the Toolbox.
• We create a homogeneous transformation which represents a
translation of (1, 2) followed by a rotation of 30°
• The function transl2 creates a relative pose with a finite translation
but zero rotation,
• while trot2 creates a relative pose with a finite rotation but zero
translation.
33
Dr.
Mohammad
Abuabiah
An-Najah
National
University
T1 = transl2(1, 2) * trot2(30, 'deg')
T1 =
0.8660 -0.5000 1.0000
0.5000 0.8660 2.0000
0 0 1.0000
2.2.3 Homogeneous Transformation Matrix (2D)
34. • We can plot this, relative to the world coordinate frame, by
• The options specify that the label for the frame is {1} and it is colored
blue
34
Dr.
Mohammad
Abuabiah
An-Najah
National
University
figure, plotvol([0 5 0 5]); %Create a square plot with dimension 5
trplot2(T1, 'frame', '1', 'color', 'b')
2.2.3 Homogeneous Transformation Matrix (2D)
T1 =
0.8660 -0.5000 1.0000
0.5000 0.8660 2.0000
0 0 1.0000
35. • We create another relative pose which is a displacement of (2, 1)
and zero rotation and we plot in red
35
Dr.
Mohammad
Abuabiah
An-Najah
National
University
T2 = transl2(2, 1);
trplot2(T2, 'frame', '2', 'color', 'r');
2.2.3 Homogeneous Transformation Matrix (2D)
36. • Now we can compose the two relative poses and plot it, in green, as
36
Dr.
Mohammad
Abuabiah
An-Najah
National
University
T3 = T1*T2;
trplot2(T3, 'frame', '3', 'color', 'g');
2.2.3 Homogeneous Transformation Matrix (2D)
37. • We see that the displacement of (2, 1) has
been applied with respect to frame {1}.
• It is important to note that our final
displacement is not (3, 3) because the
displacement is with respect to the rotated
coordinate frame.
• The noncommutativity of composition is
clearly demonstrated by:
37
Dr.
Mohammad
Abuabiah
An-Najah
National
University
T4 = T2*T1;
trplot2(T4, 'frame', '4', 'color', 'c');
2.2.3 Homogeneous Transformation Matrix (2D)
38. • Now we define a point (3, 2) relative to the world frame; which is
a column vector and add it to the plot
38
Dr.
Mohammad
Abuabiah
An-Najah
National
University
P = [3 ; 2 ];
plot_point(P, 'label', 'P', 'solid', 'ko');
2.2.3 Homogeneous Transformation Matrix (2D)
39. • To determine the coordinate of the point
with respect to {𝟏},
𝟎
𝒑 = 𝟎
𝜻𝟏 ∙ 𝟏
𝒑
and then rearrange as
𝟏
𝒑 = 𝟏
𝜻𝟎 ∙ 𝟎
𝒑
= 𝟎𝜻𝟏
−𝟏
∙ 𝟎𝒑
• Substituting numerical values
39
Dr.
Mohammad
Abuabiah
An-Najah
National
University
P1 = inv(T1) * [P; 1]
P1 =
1.7321
-1.0000
1.0000
2.2.3 Homogeneous Transformation Matrix (2D)
40. • The result is also in homogeneous form and
has a negative y-coordinate in frame {𝟏}.
• Using the Toolbox we could also have
expressed this as
• where the result is in Euclidean coordinates.
• The helper function e2h converts Euclidean
coordinates to homogeneous and h2e
performs the inverse conversion.
40
Dr.
Mohammad
Abuabiah
An-Najah
National
University
h2e( inv(T1) * e2h(P) )
P1 =
1.7321
-1.0000
2.2.3 Homogeneous Transformation Matrix (2D)
41. • Summary MATLAB Code:
41
Dr.
Mohammad
Abuabiah
An-Najah
National
University
clc
clear all
close all
%% Pose 2.1.2.1
T1 = transl2(1, 2) * trot2(30, 'deg');
display(T1)
%% Plot
figure, plotvol([0 5 0 5]); %Create a square plot with dimension 5
trplot2(T1, 'frame', '1', 'color', 'b')
T2 = transl2(2, 1);
trplot2(T2, 'frame', '2', 'color', 'r');
T3 = T1*T2;
trplot2(T3, 'frame', '3', 'color', 'g');
T4 = T2*T1;
trplot2(T4, 'frame', '4', 'color', 'c');
P = [3 ; 2 ];
plot_point(P, 'label', 'P', 'solid', 'ko');
%% Calcuation
P1 = inv(T1) * [P; 1]
h2e( inv(T1) * e2h(P) )
2.2.3 Homogeneous Transformation Matrix (2D)
42. • We will explore the noncommutativity property in more depth and
illustrate with the example of a pure rotation.
• First we create and plot a reference coordinate frame {𝟎} and a
target frame {X}
• and create a rotation of 2 radians (approximately 115°)
42
Dr.
Mohammad
Abuabiah
An-Najah
National
University
plotvol([-5 4 -1 5]);
T0 = eye(3,3); % Or transl2(0, 0);
trplot2(T0, 'frame', '0');
X = transl2(2, 3);
trplot2(X, 'frame', 'X');
R = trot2(2);
2.2.4 Centers of Rotation
43. • and plot the effect of the two
possible orders of composition
• We see that the frame
{𝑹𝑿} has been rotated about
the origin, while frame
{𝑿𝑹} has been rotated about
the origin of {𝑿}.
43
Dr.
Mohammad
Abuabiah
An-Najah
National
University
trplot2(R*X, 'framelabel', 'RX', 'color', 'r');
trplot2(X*R, 'framelabel', 'XR', 'color', 'r');
2.2.4 Centers of Rotation
44. • What if we wished to rotate a
coordinate frame about an arbitrary
point? First of all we will establish a new
point C and display it
• and then compute a transform to rotate
about point C
• and applying this
44
Dr.
Mohammad
Abuabiah
An-Najah
National
University
C = [1 2]';
plot_point(C, 'label', ' C', 'solid', 'ko')
RC = transl2(C) * R * transl2(-C);
trplot2(RC*X, 'framelabel', 'XC', 'color', 'r');
2.2.4 Centers of Rotation
45. • we see that the frame has indeed been rotated
about point C.
• Creating the required transform was somewhat
cumbersome and not immediately obvious.
• Reading from right to left:
𝑹𝑪 = 𝒕𝒓𝒂𝒏𝒔𝒍𝟐 𝑪 ∗ 𝑹 ∗ 𝒕𝒓𝒂𝒏𝒔𝒍𝟐 −𝑪
𝑿𝑪 = 𝑹𝑪 ∗ 𝑿
• 𝑹𝑪 left multiplies 𝑿 , therefore the first transform
applied to 𝑿 is 𝒕𝒓𝒂𝒏𝒔𝒍(−𝑪), then 𝑹, then 𝒕𝒓𝒂𝒏𝒔𝒍(𝑪).
• we first apply an origin shift, a translation from C to
the origin of the reference frame, apply the rotation
about that origin, and then apply the inverse origin
shift, a translation from the reference frame origin
back to C.
45
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.4 Centers of Rotation
46. • Summary: The frame {𝑿} is rotated by 2 radians about {𝟎} to give frame
{𝑹𝑿}, about {𝑿} to give {𝑿𝑹}, and about point C to give frame {𝑿𝑪}.
➢ A more descriptive way to achieve this is using twists.
46
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.2.4 Centers of Rotation
47. • The 3-dimensional case is an extension of the 2-dimensional case
discussed in the previous section.
• We add an extra coordinate axis, typically denoted by z, that is
orthogonal to both the x- and y-axes.
• The direction of the z-axis obeys the right-hand rule and forms a right-
handed coordinate frame.
51
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3 Working in Three Dimensions (3D)
48. • Unit vectors parallel to the axes are
denoted ෝ
𝒙, ෝ
𝒚 and ො
𝒛 such that:
ෝ
𝒙 = ෝ
𝒚 × ො
𝒛, ෝ
𝒚 = ො
𝒛 × ෝ
𝒙, ො
𝒛 = ෝ
𝒙 × ෝ
𝒚
• A point P is represented by its x-, y- and z-
coordinates (𝒙, 𝒚, 𝒛) or as a bound vector
𝒑 = 𝑥ෝ
𝒙 + 𝑦ෝ
𝒚 + 𝑧ො
𝒛
• The figure shows a red coordinate frame
{𝑩} that we wish to describe with respect
to the blue reference frame {𝑨}.
52
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3 Working in Three Dimensions (3D)
49. • We can see clearly that the origin of {𝑩} has
been displaced by the vector 𝐭 = (𝒙, 𝒚, 𝒛)
and then rotated in some complex fashion.
Just as for the 2-dimensional case the way
we represent orientation is very important.
• Our approach is to again consider an
arbitrary point P with respect to each of the
coordinate frames and to determine the
relationship between 𝑨𝒑 and 𝑩𝒑.
• We will again consider the problem in two
parts: rotation and then translation.
• Rotation is surprisingly complex for the 3-
dimensional case and we devote all of the
next section to it.
53
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3 Working in Three Dimensions (3D)
50. • We would like some way to describe the orientation of one with respect
to the other. We can imagine picking up frame {𝑨} in our hand and
rotating it until it looked just like frame {𝑩}.
54
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.1 Orientation in 3-Dimensions
51. • We start by considering rotation about a single coordinate axis.
55
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.1 Orientation in 3-Dimensions
52. • Example showing the noncommutativity of rotation:
56
Dr.
Mohammad
Abuabiah
An-Najah
National
University
1)
2)
2.3.1 Orientation in 3-Dimensions
53. • In 3-dimensions rotation is not commutative – the order in which
rotations are applied makes a difference to the result.
• Mathematicians have developed many ways to represent rotation:
– Orthonormal rotation matrices,
– Euler and Cardan angles,
– Rotation axis and angle,
– Exponential coordinates, and
– Unit quaternions.
• All can be represented as vectors or matrices, the natural datatypes
of MATLAB or as a Toolbox defined class.
57
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.1 Orientation in 3-Dimensions
54. • Just as for the 2-dimensional case we can represent the orientation of a
coordinate frame by its unit vectors expressed in terms of the reference
coordinate frame.
𝑨𝒙
𝑨𝒚
𝑨
𝒛
= 𝑨
𝑹𝑩
𝑩𝒙
𝑩𝒚
𝑩
𝒛
• which transforms the description of a vector defined with respect to
frame {𝑩} to a vector with respect to {𝑨}.
• it belongs to the special orthogonal group of dimension 3 or 𝑹 ∈ 𝑺𝑶 𝟑 ⊂
ℝ𝟑×𝟑
.
• This means that the product of any two matrices within the group also
belongs to the group, as does its inverse.
58
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.2 Orthonormal Rotation Matrix (3D)
55. • The orthonormal rotation matrices for rotation of 𝜽 about the x-, y- and
z-axes are
59
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.2 Orthonormal Rotation Matrix (3D)
56. • Figure below shows a frame {B} that is rotated relative to frame {A}
about ෩
𝒁 by 30 degree and a point 𝒑 which is given by:
𝐁𝒑 =
𝟎. 𝟎
𝟐. 𝟎
𝟎
• Hint: ෩
𝒁 is pointing out of the page.
• Describe 𝐁𝒑 relative to frame {A} ≡ 𝐀𝒑
60
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.3 3D Frame Rotation
𝜽
𝑷
57. • Since the transforms from frame {𝑨} to {𝑩} is only rotation about ෩
𝒁 ,
we can write the unit vectors of {B} in terms of {A} as following:
• 𝑨𝑹𝑩 =
𝐜𝐨𝐬𝜽
𝒔𝒊𝒏𝜽
𝟎
−𝒔𝒊𝒏𝜽
𝒄𝒐𝒔𝜽
𝟎
𝟎
𝟎
𝟏
=
𝟎. 𝟖𝟔𝟔
𝟎. 𝟓𝟎𝟎
𝟎
−𝟎. 𝟓𝟎𝟎
𝟎. 𝟖𝟔𝟔
𝟎
𝟎
𝟎
𝟏
• Then we calculate 𝐀
𝒑 as follows:
• 𝐀
𝒑 = 𝑨
𝑹𝑩
𝐁
𝒑 =
𝟎. 𝟖𝟔𝟔
𝟎. 𝟓𝟎𝟎
𝟎
−𝟎. 𝟓𝟎𝟎
𝟎. 𝟖𝟔𝟔
𝟎
𝟎
𝟎
𝟏
𝟎. 𝟎
𝟐. 𝟎
𝟎. 𝟎
• 𝐀𝒑 =
−𝟏. 𝟎𝟎𝟎
𝟏. 𝟕𝟑𝟐
𝟎
61
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.3 3D Frame Rotation
𝜽
𝑷
58. • The MATLAB Robotics Toolbox allows easy creation of these rotation
matrices:
• Then
62
Dr.
Mohammad
Abuabiah
An-Najah
National
University
clc
clear all
close all
%% Ex 2.1 Rotation Matrix
Bp = [0;2;0];
R = rotz(30,’deg’);
display(R)
R =
0.8660 -0.5000 0
0.5000 0.8660 0
0 0 1.0000
Ap = R*Bp;
display(Ap)
Ap =
-1.0000
1.7321
0.0000
Example 2.3* 3D Frame Rotation
59. • The Toolbox provides functions to compute these elementary
rotation matrices, for example 𝑹𝒙(𝜽) is:
63
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = rotx(pi/2)
R =
1 0 0
0 0 -1
0 1 0
2.3.2 Orthonormal Rotation Matrix (3D)
60. • Reading an orthonormal rotation matrix,
• the new frame has its x-axis in the old x-
direction (1, 0, 0), its y-axis in the old z-
direction (0, 0, 1), and the new z-axis in the
old negative y-direction (0,−1, 0).
• In this case the x-axis was unchanged since
this is the axis around which the rotation
occurred.
64
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R =
1 0 0
0 0 -1
0 1 0
2.3.2 Orthonormal Rotation Matrix (3D)
61. • If we consider that the rotation matrix represents a pose then the
corresponding coordinate frame can be displayed graphically
65
Dr.
Mohammad
Abuabiah
An-Najah
National
University
figure,view(-30,30)
trplot(R)
2.3.2 Orthonormal Rotation Matrix (3D)
62. • We can visualize a rotation more powerfully using the Toolbox
function tranimate which animates a rotation
• You can change the speed of the animation by using fps command
(default 10), such as
• Also, you can change the color the axes in the order x=red, y=green,
z=blue
66
Dr.
Mohammad
Abuabiah
An-Najah
National
University
figure,view(-30,30)
tranimate(R)
figure,view(-30,30)
tranimate(R,'fps', 5)
figure,view(-30,30)
tranimate(R,’rgb')
2.3.2 Orthonormal Rotation Matrix (3D)
64. • To illustrate compounding of rotations we will rotate the frame
again, this time around its y-axis
68
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = rotx(pi/2) * roty(pi/2)
figure,view(-30,30)
trplot(R)
Check: The noncommutativity of rotation!
2.3.2 Orthonormal Rotation Matrix (3D)
65. • We return now to representing relative pose in three dimensions the
position and orientation change between the two coordinate frames as
shown in Fig. This is often referred to as a rigid-body displacement or
rigid-body motion.
70
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.3 Homogeneous Transformation Matrix (3D)
66. • The derivation for the homogeneous
transformation matrix is similar to the 2D case
but extended to account for the z-dimension.
• 𝒕 ∈ ℝ𝟑
is a vector defining the origin of frame
{𝑩} with respect to frame {𝑨}, and 𝑹 is the 𝟑 × 𝟑
orthonormal matrix which describes the
orientation of the axes of frame {𝑩} with
respect to frame {𝑨}.
𝑨
𝒙
𝑨𝒚
𝑨𝒛
𝟏
=
𝑨
𝑹𝑩 𝒕
𝟎𝟏×𝟑 𝟏
𝑩
𝒙
𝑩𝒚
𝑩𝒛
𝟏
71
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.3 Homogeneous Transformation Matrix (3D)
67. • If points are represented by homogeneous
coordinate vectors, then
• and 𝑨𝑻𝑩 is a 4×4 homogeneous transformation
matrix.
• This matrix has a very specific structure and
belongs to the special Euclidean group of
dimension 3 or 𝑻 ∈ 𝐒𝐄(𝟑) ⊂ 𝑹𝟒×𝟒.
72
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.3 Homogeneous Transformation Matrix (3D)
68. • Figure shows a frame {B}, which is rotated relative to frame {A} about
𝒁 by 30 degrees, translated 10 units in
𝑿𝑨, and translated 5 units in
𝒀𝑨 and 0 units in
𝒁𝑨. Find 𝐀𝒑, were 𝐁𝒑 = 𝟑. 𝟎 𝟕. 𝟎 𝟎. 𝟎 𝑻 .
73
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.4 3D Homogeneous Matrix
𝜽
𝑷
𝐀
𝒑𝑩𝑶𝑹𝑮
𝐁𝒑
𝐀
𝒑
70. 75
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.4* 3D Homogeneous Matrix
• We can also solve this example using MATLAB and Robotics Toolbox.
%% Example 2.4
clc
clear
close all
%% Homogeneous Matrix
TA = transl(0, 0,0); %Create Frame A
TB = transl(10, 5,0) * trotz(30, 'deg'); %Create Frame B
display(TB)
%% Plot T
figure,plotvol([-1, 11, -1, 13, 0, 8]);
view(-30,30)
trplot(TA, 'frame', 'A', 'color', 'r') %Plot Frame A
hold on
trplot(TB, 'frame', 'B', 'color', 'b') %Plot Frame B
%% Find Ap
Bp = [3;7;0]; %Point P with respect to {B}
Bph = e2h(Bp); %Convert Bp to Homogeneous
Aph = TB*Bph; %Find the Homogeneous point P
with respect to A
Ap = h2e(Aph); %Convert Aph to Euclidean
display(Ap)
%% Plot P
plot_point(Ap, 'label', 'P', 'solid', 'ko'); %Plot the
Point P
72. • The 4 × 4 homogeneous transformation is very commonly used in
robotics, computer graphics and computer vision.
• It is supported by the Toolbox and will be used throughout this
course/book as a concrete representation of 3-dimensional pose.
• The Toolbox has many functions to create homogeneous
transformations. For example we can demonstrate composition of
transforms by
77
Dr.
Mohammad
Abuabiah
An-Najah
National
University
T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)
T =
1 0 0 1
0 0 -1 0
0 1 0 1
0 0 0 1
2.3.3 Homogeneous Transformation Matrix (3D)
73. • The function transl creates a relative pose with a finite translation but
no rotation, while trotx creates a relative pose corresponding to a
rotation of
𝝅
𝟐
about the x-axis with zero translation.
• We can think of this expression as representing a walk along the x-
axis for 1 unit, then a rotation by 90° about the x-axis and then a walk
of 1 unit along the new y-axis which was the previous z-axis.
• The result, as shown in the last column of the resulting matrix is a
translation of 1 unit along the original x-axis and 1 unit along the
original z-axis.
78
Dr.
Mohammad
Abuabiah
An-Najah
National
University
T =
1 0 0 1
0 0 -1 0
0 1 0 1
0 0 0 1
T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)
2.3.3 Homogeneous Transformation Matrix (3D)
74. • the orientation of the final pose shows the effect of the rotation about the x-
axis. We can plot the corresponding coordinate frame by
• Or as animation,
• The rotation matrix component of 𝑻 is
• and the translation component is a column vector
79
Dr.
Mohammad
Abuabiah
An-Najah
National
University
figure,view(-30,30)
trplot(T)
figure,view(-30,30)
tranimate(T,'rgb','fps',5)
t2r(T)
transl(T)'
1 0 0
0 0 -1
0 1 0
1 0 1
2.3.3 Homogeneous Transformation Matrix (3D)
75. 80
Dr.
Mohammad
Abuabiah
An-Najah
National
University
clc
clear
close all
%% Hom. 3D
To = transl(0, 0, 0);
T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)
figure,plotvol([-1 3 -1 1 0 2]);
view(-30,30)
trplot(To,'color','r','frame', 'A')
hold on
trplot(T,'frame', 'B')
hold on
tranimate(T,'rgb','fps',5)
2.3.3 Homogeneous Transformation Matrix (3D)
76. • Any rigid-body motion in 3D space is
equivalent to a screw motion – motion about
and along some line in space.
• We represent a screw a s a pair of 3-vectors
𝒔 = (𝒗, 𝝎) ∈ ℝ𝟔
• The 𝝎 component of the twist vector is the
direction of the screw axis. The 𝒗 component
is called the moment and encodes the
position of the line of the twist axis in space
and also the pitch o f the screw.
• The pitch is the ratio of the distance along the
screw axis to the rotation about the screw
axis.
81
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.4 Twists in 3D
77. • Consider the example of a rotation of 0.3 radians about the x-
axis. We first specify a unit twist; with an axis that is parallel to
the x-axis and passes through the origin
• which we convert, for the required rotation angle, to an SE(3)-
homogeneous transformation
• and has the same value we would obtain using trotx(0.3).
82
Dr.
Mohammad
Abuabiah
An-Najah
National
University
tw = Twist('R', [1 0 0], [0 0 0])
tw =
( 0 0 0; 1 0 0 )
tw.T(0.3)
1.0000 0 0 0
0 0.9553 -0.2955 0
0 0.2955 0.9553 0
0 0 0 1.0000
2.3.4 Twists in 3D
78. • For pure translation in the y-direction the unit twist would be
• which we convert, for the required translation distance, to an
SE(3)-homogeneous transformation.
• which is, as expected, an identity matrix rotational component
(no rotation) and a translational component of 2 in the y-
direction.
• and has the same value we would obtain using transl(0, 2, 0).
83
Dr.
Mohammad
Abuabiah
An-Najah
National
University
tw = Twist('T', [0 1 0])
tw =
( 0 1 0; 0 0 0 )
tw.T(2)
1 0 0 0
0 1 0 2
0 0 1 0
0 0 0 1
2.3.4 Twists in 3D
79. • A frame {B} is described as initially coincident with {A}. We then rotate {B}
about the vector 𝑨 ෩
𝑲 = 𝟎. 𝟕𝟎𝟕𝟎 𝟎. 𝟕𝟎𝟕𝟎 𝟎. 𝟎 𝑻 (passing through the
origin) by an amount 𝜽 = 𝟑𝟎 degrees. Give the frame description of {B}.
• SOLUTION:
84
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.5 Frame Rotation Passing Origin
𝑨 ෩
𝑲
P = [0.707 .707 0.0]';
tw = Twist('R',P,[0 0 0])
TR = tw.T(30,'deg’);
display(TR)
TR =
0.9330 0.0670 0.3536 0
0.0670 0.9330 -0.3536 0
-0.3536 0.3536 0.8660 0
0 0 0 1.0000
80. • Note that, this rotation has the following theory behind! (Book Page 47)
85
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.5 Frame Rotation Passing Origin
81. • A frame {B} is described as initially coincident with {A}. We then rotate {B}
about the vector 𝑨 ෩
𝑲 = 𝟎. 𝟕𝟎𝟕𝟎 𝟎. 𝟕𝟎𝟕𝟎 𝟎. 𝟎 𝑻 (passing through the
point 𝑨
𝑷 = 𝟏. 𝟎 𝟐. 𝟎 𝟑. 𝟎 𝑻
) by an amount 𝜽 = 𝟑𝟎 degrees. Give the
frame description of {B}.
• SOLUTION:
86
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.6 Frame Rotation Passing Point
P = [0.707 .707 0.0]';
tw = Twist('R',P,[1, 2, 3])
TR = tw.T(30,'deg’);
display(TR)
TR =
0.9330 0.0670 0.3536 -1.1276
0.0670 0.9330 -0.3536 1.1276
-0.3536 0.3536 0.8660 0.0484
0 0 0 1.0000
82. • Consider an x-axis rotation expressed as a rotation matrix
• As we did for the 2-dimensional case we can compute the logarithm of this matrix
using the MATLAB builtin function logm
• and the result is a sparse matrix with two elements that have a magnitude of 0.3,
which is the original rotation angle.
• This matrix has a zero diagonal and is another example of a skew-symmetric matrix,
in this case 3 × 3.
87
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = rotx(0.3)
R =
1.0000 0 0
0 0.9553 -0.2955
0 0.2955 0.9553
S = logm(R)
S =
0 0 0
0 0.0000 -0.3000
0 0.3000 0.0000
2.3.5 Matrix Exponentials (3D)
83. • Applying vex t o the skew-symmetric matrix gives
• and we find the original rotation angle is in the first element,
corresponding to the x-axis about which the rotation occurred.
• For the 3-dimensional case the Toolbox function trlog is
equivalent.
88
Dr.
Mohammad
Abuabiah
An-Najah
National
University
vex(S)' 0.3000 0 0
[th,w] = trlog(R)
th =
0.3000
w =
1.0000
0
0
2.3.5 Matrix Exponentials (3D)
84. • The inverse of a logarithm is exponentiation and applying the builtin
MATLAB matrix exponential function expm
• we have regenerated our original rotation matrix. In fact the
command
• is equivalent to
• where we have specified the rotation in terms of a rotation angle and
a rotation axis ( as a unit-vector).
89
Dr.
Mohammad
Abuabiah
An-Najah
National
University
expm(S)
1.0000 0 0
0 0.9553 -0.2955
0 0.2955 0.9553
R = rotx(0.3);
R = expm( skew([1 0 0]) * 0.3 );
2.3.5 Matrix Exponentials (3D)
85. • This generalizes to rotation about any axis and formally we can write
𝑹 = 𝒆 ෝ
𝜔 ×𝜃 ∈ 𝑺𝑶 𝟑
• where 𝜽 is the rotation angle, ෝ
𝝎 is a unit-vector parallel to the rotation
axis, and the notation ∙ ×: ℝ𝟑 → ℝ𝟑×𝟑 indicates a mapping from a
vector to a skew-symmetric matrix.
• Since 𝝎 ×𝜽 = 𝝎𝜽 × we can treat 𝝎𝜽 ∈ ℝ𝟑as a rotational parameter
called exponential coordinates.
• For the 3-dimensional case, Rodrigues’ rotation formula is a
computationally efficient means of computing the matrix exponential
for the special case where the argument is a skew-symmetric matrix,
• and this is used by the Toolbox function trexp which is equivalent to
expm.
90
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.3.5 Matrix Exponentials (3D)
86. • Euler’s rotation theorem requires successive rotation about three axes such
that no two successive rotations are about the same axis.
• There are two classes of rotation sequence: Eulerian and Cardanian, named
after Euler and Cardano respectively.
• The Eulerian type involves repetition, but not successive, of rotations about
one particular axis:
𝐗𝐘𝐗, 𝐗𝐙𝐗, 𝐘𝐗𝐘, 𝐘𝐙𝐘, 𝐙𝐗𝐙, 𝐨𝐫 𝐙𝐘𝐙
• The Cardanian type is characterize by rotations about all three axes:
𝐗𝐘𝐙, 𝐗𝐙𝐘, 𝐘𝐙𝐗, 𝐘𝐗𝐙, 𝐙𝐗𝐘, 𝐨𝐫 𝐙𝐘𝐗
91
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4 Three- Angle Representations
“Any two independent orthonormal coordinate frames can be related by a sequence
of rotations (not more than three) about coordinate axes, where no two successive
rotations may be about the same axis.”
87. • The ZYZ sequence
• is commonly used in aeronautics and mechanical dynamics and is used in the
Toolbox.
• The Euler angles are the 3-vector 𝚪 = (𝝋, 𝜽, 𝝍 ).
• For example, to compute the equivalent rotation matrix for 𝚪 = (𝟎. 𝟏, 𝟎. 𝟐, 𝟎. 𝟑)
we write
• or more conveniently
92
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = rotz(0.1) * roty(0.2) * rotz(0.3);
R =
0.9021 -0.3836 0.1977
0.3875 0.9216 0.0198
-0.1898 0.0587 0.9801
R = eul2r(0.1, 0.2, 0.3)
2.4 Three- Angle Representations
88. • The ZYZ sequence
93
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4 Three- Angle Representations
89. • The inverse problem is finding the Euler angles that correspond to a given
rotation matrix
• However if 𝜽 is negative
• the inverse function
• returns a positive value for θ and quite different values for 𝝋 and 𝝍. However
the corresponding rotation matrix is the same!
• the two different sets of Euler angles correspond to the one rotation matrix.
• The mapping from a rotation matrix to Euler angles is not unique and the
Toolbox always returns a positive angle for 𝜽 .
94
Dr.
Mohammad
Abuabiah
An-Najah
National
University
gamma = tr2eul(R) gamma =
0.1000 0.2000 0.3000
R = eul2r(0.1 , -0.2, 0.3)
R =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
tr2eul(R) -3.0416 0.2000 -2.8416
2.4 Three- Angle Representations
90. • For the case where 𝜽 = 𝟎
• the inverse function returns
• which is clearly quite different but the result is the same rotation matrix.
• The explanation is that if 𝜽 = 𝟎 then 𝑹𝒚 = 𝑰,
𝑹 = 𝑹𝒛 𝜽 × 𝑰 × 𝑹𝒛 𝝍 = 𝑹𝒛 𝜽 × 𝑹𝒛 𝝍 = 𝑹𝒛(𝜽 + 𝝍)
• which is a function of the sum 𝝋 + 𝝍.
• The case 𝜽 = 𝟎 is a singularity and will be discussed in more detail in
the next section.
95
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = eul2r(0.1, 0, 0.3)
R =
0.9211 -0.3894 0
0.3894 0.9211 0
0 0 1.0000
2.4 Three- Angle Representations
tr2eul(R) 0.0000 0.0000 0.4000
91. • Another widely used convention are the Cardan angles:
𝒓𝒐𝒍𝒍, 𝒑𝒊𝒕𝒄𝒉 𝑎𝑛𝑑 𝒚𝒂𝒘
96
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4 Three- Angle Representations
92. • Confusingly there are two different versions in common use.
Textbooks seem to define the roll-pitch-yaw sequence as ZYX or
XYZ depending on whether they have mobile robot or robot arm
focus.
• Toolbox supports both formats by means of the 'xyz' and 'zyx'
options. The ZYX order is default for Release 10, but for Release 9 the
default was XYZ.
• The roll-pitch-yaw sequence allows all angles to have arbitrary sign
and it has a singularity when 𝜽𝒑 = ±
𝜋
2
which is fortunately outside
the range of feasible attitudes for most vehicles.
97
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4 Three- Angle Representations
93. • This leads to the ZYX angle sequence
• Roll-pitch-yaw angles are also known as Tait-Bryan angles, or
nautical angles, and for aeronautical applications they can be
called bank, attitude and heading angles respectively.
• When describing the attitude of a robot gripper, the convention is
that the z-axis points forward and the x-axis is either up or down.
This leads to the XYZ angle sequence
98
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4 Three- Angle Representations
94. • The Toolbox defaults to the ZYX sequence. For example
• and equivalent to:
• and the inverse is
• ZYX sequence can be overridden using the 'xyz’ option. For example
99
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = rpy2r(0.1, 0.2, 0.3) R =
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
gamma = tr2rpy(R)
gamma =
0.1000 0.2000 0.3000
2.4 Three- Angle Representations
R = rotz(0.3)*roty(0.2)*rotx(0.1)
R = rpy2r(0.1, 0.2, 0.3,’xyz’)
R =
0.9752 -0.0978 0.1987
0.1538 0.9447 -0.2896
-0.1593 0.3130 0.9363
96. • For arm-type robots it is useful to consider a coordinate frame {𝑬}
attached to the end-effector as shown in figure below:
101
Dr.
Mohammad
Abuabiah
An-Najah
National
University
z
y
x
2.4.1 Two Vector Representation
97. • Lynxmotion AL5D 4 Degrees of Freedom:
102
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4.1 Two Vector Representation
98. • By convention the axis of the tool is
associated with the z-axis and is called the
approach vector and denoted:
ෝ
𝒂 = 𝒂𝒙, 𝒂𝒚, 𝒂𝒛
• For some applications it is more convenient
to specify the approach vector than to
specify Euler or roll-pitch-yaw angles.
• However specifying the direction of the z-
axis is insufficient to describe the
coordinate frame – we also need to
specify the direction of the x- and y-axes.
103
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4.1 Two Vector Representation
99. • An orthogonal vector that provides orientation,
perhaps between the two fingers of the robot’s
gripper is called the orientation vector:
ෝ
𝒐 = 𝒐𝒙, 𝒐𝒚, 𝒐𝒛
• These two unit vectors are sufficient to
completely define the rotation matrix
𝑹 = ෝ
𝒏 ෝ
𝒐 ෝ
𝒂
• since the remaining column, the normal vector,
can be computed as
ෝ
𝒏 = ෝ
𝒐 × ෝ
𝒂
104
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.4.1 Two Vector Representation
100. • Consider an example where the gripper’s
approach and orientation vectors are
parallel to the world x- and y-directions
respectively.
• Using the Toolbox this is implemented by
• Any two nonparallel vectors are sufficient
to define a coordinate frame.
105
Dr.
Mohammad
Abuabiah
An-Najah
National
University
a = [1 0 0]’;
o = [0 1 0]’;
R = oa2r(o, a)
R =
0 0 1
0 1 0
-1 0 0
2.4.1 Two Vector Representation
101. • A fundamental problem with all the three-angle representations just
described is singularity. This is also known as gimbal lock (movie
Apollo 13).
• This occurs when the rotational axis of the middle term in the
sequence becomes parallel to the rotation axis of the first or third
term.
• A mechanical gyroscope used for spacecraft navigation is shown in
Figure (next slide).
• The innermost assembly is the stable member which has three
orthogonal gyroscopes that hold it at a constant orientation with
respect to the universe.
• Giving a direct indication of roll-pitch-yaw angles which in this design
are a Cardanian YZX sequence.
106
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.5 Singularities and Gimbal Lock
107. • Consider the situation when the
rotation angle of the middle gimbal
(rotation about the spacecraft’s z-axis)
is 90° – the axes of the inner and outer
gimbals are aligned and they share
the same rotation axis.
• Instead of the original three rotational
axes, since two are parallel, there are
now only two effective rotational axes.
• We say that one degree of freedom
has been lost.
112
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.5 Singularities and Gimbal Lock
108. • In mathematical, rather than mechanical, terms this problem can be seen
using the definition of the Lunar module’s coordinate system where the
rotation of the spacecraft’s body-fixed frame {𝑩} with respect to the stable
platform frame {𝑺} is
𝑺
𝑹𝑩 = 𝑹𝒚 𝜽𝒑 𝑹𝒛 𝜽𝒓 𝑹𝒙(𝜽𝒚)
• For the case when 𝜽𝒓 = 𝝅/𝟐 we can apply the identity
𝑹𝒚 𝜽 𝑹𝒛
𝝅
𝟐
= 𝑹𝒛
𝝅
𝟐
𝑹𝒙(𝜽)
• Leading to:
𝑺
𝑹𝑩 = 𝑹𝒛
𝝅
𝟐
𝑹𝒙 𝜽𝒑 𝑹𝒙 𝜽𝒚 = 𝑹𝒛
𝝅
𝟐
𝑹𝒙(𝜽𝒑 + 𝜽𝒚)
➢ which is unable to represent any rotation about the y-axis.
113
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.5 Singularities and Gimbal Lock
109. • For example, the following roll-pitch-yaw angle:
• and the inverse is
• Notice that, pitch angle becomes zero, and the yaw angle
becomes the sum of 𝟎. 𝟑 + 𝟎. 𝟓 angles.
114
Dr.
Mohammad
Abuabiah
An-Najah
National
University
R = rpy2r(0.3, -pi/2, 0.5)
R =
0 -0.7174 -0.6967
0 0.6967 -0.7174
1 0 0
gamma = tr2rpy(R)
gamma =
0 -1.5708 0.8000
2.5 Singularities and Gimbal Lock
110. • All three-angle representations of attitude, whether Eulerian or
Cardanian, suffer this problem of gimbal lock when two consecutive
axes become aligned.
• For ZYZ Euler angles this occurs when 𝜽 = 𝒌𝝅, 𝒌 ∈ ℤ and for roll-
pitch-yaw angles when pitch 𝜽𝒑 = ± 𝟐𝒌 + 𝟏
𝝅
𝟐
.
• The best that can be hoped for is that the singularity occurs for an
attitude which does not occur during normal operation of the
vehicle – it requires judicious choice of angle sequence and
coordinate system.
• Singularities are an unfortunate consequence of using a minimal
representation.
• To eliminate this problem we need to adopt different
representations of orientation (is to introduce a fourth parameter!).
115
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.5 Singularities and Gimbal Lock
111. • An important characteristic of relative poses is that they can be
composed or compounded.
• Consider the case shown in Fig. If one frame can be described in
terms of another by a relative pose, then they can be applied
sequentially:
116
Dr.
Mohammad
Abuabiah
An-Najah
National
University
𝑨
𝝃𝑪 = 𝑨
𝝃𝑩 ⊕ 𝑩
𝝃𝑪
2.6 Relative Poses
112. • which says, in words, that the pose
of {𝑪} relative to {𝑨} can be
obtained by compounding the
relative poses from {𝑨} to {𝑩} and
{𝑩} to {𝑪}.
• We use the operator ⊕ to indicate
composition of relative poses.
• For this case the point 𝑷 can be
described by
𝐀𝒑 = ( 𝑨𝝃𝑩 ⊕ 𝑩 𝝃𝑪) ∙ 𝑪𝒑
117
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
113. • In relative pose composition we can check that we have our
reference frames correct by ensuring that the subscript and
superscript on each side of the ⊕ operator are matched. We can
then cancel out the intermediate subscripts and superscripts:
leaving just the end most subscript and superscript which are shown
highlighted.
118
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
114. • Later in this chapter we will convert these abstract notions of ξ , ∙ and
⊕ into standard mathematical objects and operators that we can
implement in MATLAB®.
• In the examples so far we have shown 2-dimensional coordinate
frames.
• This is appropriate for a large class of robotics problems, particularly
for mobile robots which operate in a planar world.
• For other problems we require 3-dimensional coordinate frames to
describe objects in our 3-dimensional world such as the pose of a
flying or underwater robot or the end of a tool carried by a robot arm.
119
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
115. • Figure below shows a more complex 3-dimensional example in a
graphical form where we have attached 3D coordinate frames to
the various entities and indicated some relative poses.
120
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
116. • The fixed camera observes the object from its
fixed viewpoint and estimates the object’s
pose 𝑭𝝃𝑩 relative to itself.
• The other camera is not fixed, it is attached to
the robot at some constant relative pose and
estimates the object’s pose 𝑪
𝝃𝑩 relative to
itself.
• Recalling that we can compose relative poses
using the ⊕ operator we can write some
spatial relationships
𝝃𝑭 ⊕ 𝑭 𝝃𝑩 = 𝝃𝑹 ⊕ 𝑹 𝝃𝑪 ⊕ 𝑪 𝝃𝑩
𝝃𝑭 ⊕ 𝑭
𝝃𝑩 = 𝝃𝑩
• Each side of the first equation represents a
path through the network from {𝟎} to {𝑩}, a
sequence of edges (arrows) written in order.
121
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
118. • An alternative representation of the spatial relationships is a directed
graph which is shown in Fig. below.
• Each node in the graph represents a pose and each edge represents
a relative pose. An arrow from 𝑿 to 𝒀 is denoted 𝑿𝝃𝒀 and describes the
pose of 𝒀 relative to 𝑿.
123
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
119. • There are just a few algebraic rules:
• where 𝟎 represents a zero relative pose. A pose has an inverse
• which is represented graphically by an arrow from {𝒀} to {𝑿}.
Relative poses can also be composed or compounded
124
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
120. • It is important to note that the algebraic rules for poses are
different to normal algebra and that composition is not
commutative:
• with the exception being the case where 𝝃𝟏 ⊕ 𝝃𝟐 = 𝟎.
• A relative pose can transform a point expressed as a vector
relative to one frame to a vector relative to another
125
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
121. • A very useful property of poses is the ability to
perform algebra. The second loop equation
says, in words, that the pose of the robot is
the same as composing two relative poses:
• from the world frame to the fixed camera
and from the fixed camera to the robot.
• We can subtract 𝝃𝑭 from both sides of the
equation by adding the inverse of 𝝃𝑭 which
we denote as ⊖ 𝝃𝑭 and this gives
⊖ 𝝃𝑭 ⊕ 𝝃𝑭 ⊕ 𝑭 𝝃𝑩 =⊖ 𝝃𝑭 ⊕ 𝝃𝑩
𝑭𝝃𝑩 =⊖ 𝝃𝑭 ⊕ 𝝃𝑩
• Order is important here, and we add ⊖ 𝝃𝑭 to
the left on each side of the equation.
126
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
123. • We can write these expressions quickly by inspection. To find the
pose of node X with respect to node Y:
• find a path from Y to X and write down the relative poses on the
edges in a left to right order;
• if you traverse the edge in the direction of its arrow precede it with
the ⊕ operator, otherwise use ⊖.
128
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
124. • A concrete representation of relative pose is 𝝃 ∼ 𝑻 ∈ 𝐒𝐄(𝟑) and 𝑻𝟏 ⊕
𝑻𝟐 → 𝑻𝟏𝑻𝟐 which is standard matrix multiplication.
• One of the rules of pose algebra is 𝝃 ⊕ 𝟎 = 𝝃. For matrices we know
that 𝑻𝑰 = 𝑻, where 𝑰 is the identify matrix, so for pose 𝟎 → 𝑰 the
identity matrix.
• Another rule of pose algebra was that 𝝃 ⊖ 𝝃 = 𝟎. We know for
matrices that 𝑻𝑻−𝟏 = 𝑰 which implies that ⊖ 𝑻 → 𝑻−𝟏
129
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.6 Relative Poses
125. • Assume that we know the transform
𝑩𝑻𝑻 in the Figure, which describes
the frame at the manipulator’s
fingertips {T} relative to the base of
the manipulator, {B}.
• 𝑩𝑻𝑺 is known because we have a
description of the frame {S} that is
attached to the table.
• and that we know the location of
the frame attached to the bolt lying
on the table relative to the table
frame - that is, 𝑺𝑻𝑮.
130
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.7 Manipulator reaching for a bolt
126. • Calculate the position and
orientation of the bolt relative to
the manipulator’s hand, 𝑻𝑻𝑮.
• SOLUTION: Guided by our notation,
we compute the bolt frame relative
to the hand frame as:
𝑻
𝑻𝑮 = 𝑩
𝑻𝑻
−𝟏 𝑩
𝑻𝑺
𝑺
𝑻𝑮
131
Dr.
Mohammad
Abuabiah
An-Najah
National
University
Example 2.7 Manipulator reaching for a bolt
127. • In this chapter we learned how to represent points and poses in 2-
and 3-dimensional worlds.
• Points are represented by coordinate vectors relative to a
coordinate frame.
• A set of points that belong to a rigid object can be described by
a coordinate frame, and its constituent points are described by
constant vectors in the object’s coordinate frame.
• The position and orientation of any coordinate frame can be
described relative to another coordinate frame by its relative pose
𝝃.
132
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.7 Summary
129. • We have discussed a variety of mathematical objects to tangibly
represent pose.
• We have used orthonormal rotation matrices for the 2- and 3-
dimensional case to represent orientation and shown how it can
rotate a points’ coordinate vector from one coordinate frame to
another.
• Its extension, the homogeneous transformation matrix, can be used
to represent both orientation and translation and we have shown
how it can rotate and translate a point expressed in homogeneous
coordinates from one frame to another.
134
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.7 Summary
130. • There are two important lessons from this chapter.
– The first is that there are many mathematical objects that can be used to
represent pose.
– The second lesson is that coordinate frames are your friend.
• The essential first step in many vision and robotics problems is to assign
coordinate frames to all objects of interest, indicate the relative poses
as a directed graph, and write down equations for the loops.
• The notation has been defined and illustrated, and we have started
our hands-on work with MATLAB.
135
Dr.
Mohammad
Abuabiah
An-Najah
National
University
2.7 Summary
