Fundamentals Of Robotics

Lab Report # 3
Name: Tayyab Khalil
Section: A (Electronics)
BSEE 2017-21

Submitted To: Dr. Nasir Rehman

Submitted On: 5 March, 2021.

To Compute and Plan Trajectory of RHINO XR-3 in

tool and joint space.

PAKISTAN INSTITUTE of ENGINEERING & APPLIED SCIENCES

Introduction:

Trajectory refers to a time history of position, velocity, and acceleration for each degree
of freedom. The basic problem is to move the manipulator from an initial position to
some desired final position.

We wish to move the tool frame from its current value to a desired final value. In general,
this motion involves both a change in orientation and a change in the position of the tool
relative to the station.

In order to complete this motion, the tool frame must pass through a set of intermediate
positions and orientations. These points are termed as via points. Each of these via points
is actually the position and orientation of the tool frame relative to the station.

In this lab, we use cubic polynomials to obtain the expression for the path followed by
each robot. The general expressions for position, velocity and acceleration of each joint is
given by:

So the basic task is then to find the values of these constants. To find these we apply the
following four constraints:

Applying these constraints into above equations gives the values of these 4 constants:
Plugging these values back into the equation or position gives the final expression:

So, if we know the initial and final desired value along with the time needed for the completion
of path, a time function for position, velocity and acceleration can be found.
Now we apply these concepts onto the Rhino XR-3.

Rhino XR-3 Trajectory:

We use the equations found above to compute and plot the trajectory for Rhino XR-3. We then plot the
trajectory followed by each joint and the tooltip in 3D space along with using the Robotics toolbox to
see the robot in motion.
Main Script Code:
%Initializing robot parameters
L = 5;
d = [26.04 0 0 0 16.83];
aL = [0 22.86 22.86 0.95 0];
p = [-pi/2 0 0 -pi/2 0];
dh_inv = [d; aL; p];
%Initiializing the time of trajectory
tf = 5;

w1 = [aL(2)+aL(3)+aL(4) 0 d(1)-d(5) 0 0 -1];

%w2 = [aL(3)+aL(4) 0 d(1)+aL(2)-d(5) 0 0 -0.607];

w2 = [8.4150 14.5752 72.7100 0.6420 1.1120 -0.0000];

qi = InverseKinematics(w1);
qf = InverseKinematics(w2);

[a, traj] = trajectory_plan (qi, qf, tf, L);

syms t
qt = sym(zeros(1, L));

for i = 1 : L
qt(i) = a(1, i) + a(2, i)*t + a(3, i)*t^2 + a(4, i)*t^3;
end

tk = 0:0.1:tf;
w = zeros(3, length(tk));
qtData = zeros(length(tk), 5);
for i = 1 : length(tk)
qtFK = vpa(subs(qt, tk(i)), 6);
qtData(i, :) = qtFK;
dh = [qtFK; p; d; aL];
arm_matrix = fkine (dh, L);
w(:, i) = arm_matrix (1:3, 4);
end

% Robot Plot
for i = 1 : L
Lk(i) = Link([0 d(i) aL(i) p(i)]);
end

bot = SerialLink(Lk); % construct and join links

bot.name = 'Link Diagram'; % name the robot
f = {'r', 'LineWidth', 2};
figure(6)
for i = 1 : length(tk)
bot.plot([qtData(i, 1) qtData(i, 2) qtData(i, 3) qtData(i, 4)
qtData(i, 5)]);
pause(0.25)
end
figure(7)
bot.teach(qf);

figure(8)
scatter3(w(1, :), w(2, :), w(3, :));
xlabel('X'); ylabel('Y'); zlabel('Z');
title('Trajectory followed by the Tool Tip in 3D space. ')

Trajectory Planning Function:

function [a, traj] = trajectory_plan (th_i, th_f, tf, L)
a0 = zeros(1, L);
a1 = zeros(1, L);
a2 = zeros(1, L);
a3 = zeros(1, L);
a = zeros(4, L);
traj = zeros(L, 3);
for i = 1 : L
%calculating constants using solved cubic equation +
derivatives
a0(i) = th_i(i);
a1(i) = 0;
a2(i) = (3/tf^2)*(th_f(i) - th_i(i));
a3(i) = (-2/tf^3)*(th_f(i) - th_i(i));

%defining limits for t in above equation, increment 0.1

seconds
t = 0:0.1:tf;

%defining equations for trajectory plotting

syms pos(t);
syms vel(t);
syms acc(t);

%calculating the equation values

pos(t) = a0(i) + (a2(i)*t^2) + (a3(i)*t^3);
vel(t) = diff (pos(t));
acc(t) = diff (vel(t));
 traj = [pos(t), vel(t), acc(t)];

figure(i)

subplot(3, 1, 1)
fplot(@(t)pos(t), [0 tf])
label1 = ['q[', num2str(i), '] Positional Trajectory'];
xlabel('Time'); ylabel ('Position (degrees)');
title(label1);

subplot(3, 1, 2)
fplot(@(t)vel(t), [0 tf])
label2 = ['q[', num2str(i), '] Velocity Trajectory'];
xlabel('Time'); ylabel ('Velocity (degrees/sec)');
title(label2);

subplot(3, 1, 3)
fplot(@(t)acc(t), [0 tf])
label3 = ['q[', num2str(i), '] Acceleration Trajectory'];
xlabel('Time'); ylabel ('Acceleration (degrees/sec_square)');
title(label3);
end
a = [a0;
a1;
a2;
a3];
end

Trajectory followed by each joint:

Here we present the trajectory followed for the end points defined above by each robot
joint.
Each of the 5 figures above describes the motion of the respective joint, all of them being
smooth as clearly seen above.
Robot Motion through Robotics toolbox:
Now we use the robotics toolbox to see the robot moving from the initial
specified position – all angles being to zero – to a desired home position. Here the
robot’s initial and final position are shown:

Figure 0:1: The link diagram of robot in its initial position.

Figure 0:2: The link diagram of robot having reached desired position.
Figure 3 Verification of the robot having reached the same position as desired through rob.teach.

Figure 4: Visual depiction of the smooth trajectory followed by the robot tool tip in 3D Space.
Results and Discussions:
In this lab, we computed and plotted the trajectories for position, velocity and
acceleration as the Rhino XR-3 is moved from a initial position to a desired final
position.
We started by specifying the tool configuration vectors at initial and final points and
finding the corresponding vectors for joint angles. We then use cubic polynomials to find
the expressions for the position as a function of time. Its derivative can be used to find the
expression for velocity and acceleration as well. These expressions are plotted against
time for each joint to obtain a smooth trajectory.
We then utilize the robotics toolbox to animate the motion of robot joints through the
trajectory defined. We also trace the position of tool tip in 3D space as the robot moves
through the desired path.