CENG483: Behavioural Robotics
Izmir Institute of Technology
Fall 2023
Instructor: Dr. Berat Alper Erol
Homework Set: 02
Ozan Gülbaş
This homework answers the problem set sequentially.
1. Use MATLAB Robotics Toolbox and write functions for Theta (degree) = 25, 45,
65, 82:
(a) ROTX(theta). This function will give a 4x4 homogenous transformation matrix
that will rotate “theta” about the X-axis.
function output_matrix = ROTX(theta)
output_matrix = trotx(theta, "deg");
end
(b) ROTY(theta). This function will give a 4x4 homogenous transformation matrix
that will rotate “theta” about the Y-axis
function output_matrix = ROTY(theta)
output_matrix = troty(theta, "deg");
end
(c) ROTZ(theta). This function will give a 4x4 homogenous transformation matrix
that will rotate “theta” about the Z-axis
function output_matrix = ROTZ(theta)
output_matrix = trotz(theta, "deg");
end
(d) TRANS(x,y,z). This function will give a 4x4 homogenous transformation ma-
trix that will translate “x” units over the X-axis, “y” units over the Y-axis, and
“z” units over the Z-axis
function output_matrix = TRANS(x, y, z)
output_matrix = transl(x, y, z);
end
.
The code which displays ROTX,ROTY and ROTZ for given Thetas:
CENG483 Homework Set: 02 Ozan Gülbaş
thetas = [25, 45, 65, 82];
for i = 1 : length(thetas)
theta = thetas(i);
rot_matrix_X = ROTX(theta);
rot_matrix_Y = ROTY(theta);
rot_matrix_Z = ROTZ(theta);
fprintf("\nROTX with theta : %d = \n", theta)
disp(rot_matrix_X)
fprintf("\nROTY with theta : %d = \n", theta)
disp(rot_matrix_Y)
fprintf("\nROTZ with theta : %d = \n", theta)
disp(rot_matrix_Z)
fprintf("\n=======================\n")
end
The output is:
ROTX with theta : 25 =
1.0000 0 0 0
0 0.9063 -0.4226 0
0 0.4226 0.9063 0
0 0 0 1.0000
ROTY with theta : 25 =
0.9063 0 0.4226 0
0 1.0000 0 0
-0.4226 0 0.9063 0
0 0 0 1.0000
ROTZ with theta : 25 =
0.9063 -0.4226 0 0
0.4226 0.9063 0 0
0 0 1.0000 0
0 0 0 1.0000
=======================
ROTX with theta : 45 =
1.0000 0 0 0
0 0.7071 -0.7071 0
2
CENG483 Homework Set: 02 Ozan Gülbaş
0 0.7071 0.7071 0
0 0 0 1.0000
ROTY with theta : 45 =
0.7071 0 0.7071 0
0 1.0000 0 0
-0.7071 0 0.7071 0
0 0 0 1.0000
ROTZ with theta : 45 =
0.7071 -0.7071 0 0
0.7071 0.7071 0 0
0 0 1.0000 0
0 0 0 1.0000
=======================
ROTX with theta : 65 =
1.0000 0 0 0
0 0.4226 -0.9063 0
0 0.9063 0.4226 0
0 0 0 1.0000
ROTY with theta : 65 =
0.4226 0 0.9063 0
0 1.0000 0 0
-0.9063 0 0.4226 0
0 0 0 1.0000
ROTZ with theta : 65 =
0.4226 -0.9063 0 0
0.9063 0.4226 0 0
0 0 1.0000 0
0 0 0 1.0000
=======================
ROTX with theta : 82 =
1.0000 0 0 0
0 0.1392 -0.9903 0
0 0.9903 0.1392 0
0 0 0 1.0000
3
CENG483 Homework Set: 02 Ozan Gülbaş
ROTY with theta : 82 =
0.1392 0 0.9903 0
0 1.0000 0 0
-0.9903 0 0.1392 0
0 0 0 1.0000
ROTZ with theta : 82 =
0.1392 -0.9903 0 0
0.9903 0.1392 0 0
0 0 1.0000 0
0 0 0 1.0000
=======================
2. Use the resulting functions of previous problem to do the following operations:
(a) Find a final transformation that translates a frame N 2 units in X, 3 unites
in Y and -2 units in Z, i.e. (2,3,-2), and then rotate the translated frame 60
degrees over its translated Y axis.
Code:
N = TRANS(2, 3, -2);
N_rot = N * ROTY(60);
fprintf("Resulting matrix N:\n")
disp(N_rot)
Output:
Resulting matrix N:
0.5000 0 0.8660 2.0000
0 1.0000 0 3.0000
-0.8660 0 0.5000 -2.0000
0 0 0 1.0000
(b) Find a final transformation rotates a frame O 60 degrees over its translated Y
axis and then translates the rotated frame (2,-2, 2).
Code:
O = ROTY(60);
O_trans = O * TRANS(2, -2, 2);
fprintf("Resulting matrix O:\n")
disp(O_trans)
Output:
4
CENG483 Homework Set: 02 Ozan Gülbaş
Resulting matrix O:
0.5000 0 0.8660 2.7321
0 1.0000 0 -2.0000
-0.8660 0 0.5000 -0.7321
0 0 0 1.0000
5
CENG483 Homework Set: 02 Ozan Gülbaş
3. Create a 2D rotation matrix. Visualize the rotation using trplot2. Use it to trans-
form a vector. Invert it and multiply it by the original matrix; what is the result?
Reverse the order of multiplication; what is the result? What is the determinant of
the matrix and its inverse?
Code:
% Create a 2D rotation matrix
theta = 60;
T = [
cos(theta), -sin(theta);
sin(theta), cos(theta);
];
% Visualise the rotation with trplot2
figure(1)
trplot2(T, "frame", "A")
% Use it to transform a vector
V = [1; 1];
V_trans = T * V;
fprintf("Transform of vector V with T:\n")
disp(V_trans)
% Invert it and multiply it with the original matrix
T_inv = inv(T);
result1 = T_inv * T;
fprintf("Result 1:\n")
disp(result1)
% Reverse the order of multipication
result2 = T * T_inv;
fprintf("Result 2:\n")
disp(result2)
% Determinant of the matrix and its inverse
det_T = det(T);
det_T_inv = det(T_inv);
fprintf("Determinant of T:\n")
disp(det_T)
fprintf("Determinant of T inverse:\n")
disp(det_T_inv)
Output:
Transform of vector V with T:
-0.6476
-1.2572
Result 1:
6
CENG483 Homework Set: 02 Ozan Gülbaş
1.0000 0.0000
0 1.0000
Result 2:
1.0000 0.0000
0 1.0000
Determinant of T:
1
Determinant of T inverse:
1
Figure 1: Visualization of the rotation with trplot2.
7
CENG483 Homework Set: 02 Ozan Gülbaş
4. Create a 3D rotation matrix. Visualize the rotation using trplot or tranimate.
Use it to transform a vector. Invert it and multiply it by the original matrix; what
is the result? Reverse the order of multiplication; what is the result? What is the
determinant of the matrix and its inverse?
Code:
% Create a 3D rotation matrix (wrt Y axis)
theta = 60;
R_y = [
cos(theta), 0, sin(theta);
0, 1, 0;
-sin(theta), 0, cos(theta);
];
% Visualise the rotation with trplot
figure(2)
trplot(R_y, "frame", "A")
view(10,10)
% Transform a vector
V = [1; 1; 1];
V_trans = R_y * V;
fprintf("Transform of vector V with R_y:\n")
disp(V_trans)
% Invert it and multiply it with the original matrix
R_y_inv = inv(R_y);
result1 = R_y_inv * R_y;
fprintf("Result 1:\n")
disp(result1)
% Reverse the order of multiplication
result2 = R_y * R_y_inv;
fprintf("Result 2:\n")
disp(result2)
% Determinant of R_y and R_y inverse
det_R_y = det(R_y);
det_R_y_inv = det(R_y_inv);
fprintf("Determinant of R_y:\n")
disp(det_R_y)
fprintf("Determinan of R_y inverse:\n")
disp(det_R_y_inv)
Output:
Transform of vector V with R_y:
-1.2572
8
CENG483 Homework Set: 02 Ozan Gülbaş
1.0000
-0.6476
Result 1:
1.0000 0 -0.0000
0 1.0000 0
0 0 1.0000
Result 2:
1.0000 0 -0.0000
0 1.0000 0
0 0 1.0000
Determinant of R_y:
1
Determinan of R_y inverse:
1
Figure 2: Visualization of the rotation with trplot.
9
CENG483 Homework Set: 02 Ozan Gülbaş
5. Generate the sequence of plots shown in Fig. 2.12.
Code:
base_pose = roty(0);
% First sequence
rot1 = ROTX(90);
rot2 = rot1 * ROTY(90);
% Second sequence
rot3 = ROTY(90);
rot4 = rot3 * ROTX(90);
figure(3)
subplot(2, 3, 1)
trplot(base_pose)
xaxis(-0.2, 1.2);yaxis(-0.2, 1.2); zlim([-0.2, 1.2])
view(10, 10)
subplot(2, 3, 2)
trplot(rot1)
xaxis(-0.2, 1.2);yaxis(-1.2, 0.2); zlim([-0.2, 1.2])
view(10, 10)
subplot(2, 3, 3)
trplot(rot2)
xaxis(-0.2, 1.2);yaxis(-0.2, 1.2); zlim([-0.2, 1.2])
view(10, 10)
subplot(2, 3, 4)
trplot(base_pose)
xaxis(-0.2, 1.2);yaxis(-0.2, 1.2); zlim([-0.2, 1.2])
view(10, 10)
subplot(2, 3, 5)
trplot(rot3)
xaxis(-0.2, 1.2);yaxis(-0.2, 1.2); zlim([-1.2, 0.2])
view(10, 10)
subplot(2, 3, 6)
trplot(rot4)
xaxis(-0.2, 1.2);yaxis(-1.2, 0.2); zlim([-1.2, 0.2])
view(10, 10)
10
CENG483 Homework Set: 02 Ozan Gülbaş
Figure 3: Recreation of Fig. 2.12 in the textbook.
11
CENG483 Homework Set: 02 Ozan Gülbaş
6. For the 3-dimensional rotation about the vector [2, 3, 4] by 0.5 rad compute an SO(3)
rotation matrix using: the matrix exponential functions expm and trexp, Ro-
drigues’ rotation formula (code this yourself ), and the Toolbox function angvec2tr.
Compute the equivalent unit quaternion.
First, the Rodrigues’s forluma implemented like this:
function output_matrix = Rodrigues(theta, V)
I = [
1, 0, 0;
0, 1, 0;
0, 0, 1;
];
skew_matrix = skew(V) ;
output_matrix = I + sin(theta) * skew_matrix
+ (1 - cos(theta)) * skew_matrix^2;
end
Code:
% A 3D rotation matrix (wtr Y axis)
rad = 0.5;
theta = rad2deg(rad);
R_y = [
cos(theta), 0, sin(theta);
0, 1, 0;
-sin(theta), 0, cos(theta);
];
% Define vector
V = [2; 3; 4];
V_normalized = V / norm(V);
% Rotation with expm()
rot_expm = expm(skew(V_normalized) * rad);
fprintf("Rotation matrix with expm():\n")
disp(rot_expm)
% Rotation with trexp()
rot_trexp = trexp(skew(V_normalized) * rad);
fprintf("Rotation matrix with trexp():\n")
disp(rot_trexp)
% Rotation with Rodrigues’s formula
rot_rodriguez = Rodrigues(rad, V_normalized);
fprintf("Rotation matrix with Rodrigues’s formula:\n")
disp(rot_rodriguez)
12
CENG483 Homework Set: 02 Ozan Gülbaş
% Rotation with angvec2tr()
rot_toolbox = angvec2tr(rad, V_normalized);
fprintf("Rotation matrix with angvec2tr\n")
disp(rot_toolbox)
% Equivalent unit quaternion
quaternion = UnitQuaternion(rot_expm);
fprintf("Equivalent unit quaternion:\n")
disp(quaternion)
Output:
Rotation matrix with expm():
0.8945 -0.3308 0.3009
0.3814 0.9156 -0.1274
-0.2333 0.2287 0.9451
Rotation matrix with trexp():
0.8945 -0.3308 0.3009
0.3814 0.9156 -0.1274
-0.2333 0.2287 0.9451
Rotation matrix with Rodrigues’s formula:
0.8945 -0.3308 0.3009
0.3814 0.9156 -0.1274
-0.2333 0.2287 0.9451
Rotation matrix with angvec2tr:
0.8945 -0.3308 0.3009 0
0.3814 0.9156 -0.1274 0
-0.2333 0.2287 0.9451 0
0 0 0 1.0000
Equivalent unit quaternion:
UnitQuaternion with properties:
s: 0.9689
v: [0.0919 0.1378 0.1838]
13
