Fundamentals Of Robotics

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

Submitted To: Dr. Nasir Rehman

Submitted On: 12 March, 2021.

To Compute and Plan Trajectory of RHINO XR-3 in

tool and joint space with via points.

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 the last lab, we used cubic polynomials to obtain the expression for the path followed
by each robot. In this lab, we modify those expressions to include the via points as well.
Usually, we wish to be able to pass through a via point without stopping, and so we need
to generalize the way in which we fit cubes to the path constraints. As in the case of a single
goal point, each via point is usually specified in terms of a desired position and orientation
of the tool frame relative to the station frame. Each of these via points is "converted" into
a set of desired joint angles by application of the inverse kinematics. We then consider the
problem of computing cubes that connect the via-point values for each joint together in a
smooth way.
So, the basic task is then to find the values of the constants in the cubic polynomial. To
find these we apply the following four constraints:

The four equations describing this general cubic are

Applying these constraints into above equations gives the values of these 4 constants:
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 = [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);
v = zeros(L, 2);
traj = zeros(L, 3);
for i = 1 : L
%calculating constants using solved cubic equation +
derivatives
v(i, 1) = 0;
v(i, 2) = 2;
 a0(i) = th_i(i);
a1(i) = v(i, 1);
a2(i) = (3/tf^2)*(th_f(i) - th_i(i)) - (2/tf)*v(i, 1) -
(1/tf)*v(i, 2);
a3(i) = (-2/tf^3)*(th_f(i) - th_i(i)) + (1/tf^2)*(v(i, 1) +
v(i, 2));

%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) + a1(i)*t + (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)];

%plotting trajectory using the equations

% figure (i + 2)
% fplot (@(t)pos(t), [0 3]);
% title ('Figure 1: Position of Object');
% xlabel ('Time (t)');
% ylabel ('Ang. Position (degrees)');
%
% figure (2i)
% fplot (@(t)vel(t), [0 3]);
% title ('Figure 2: Velocity of Object');
% xlabel ('Time (t)');
% ylabel ('Ang. Velocity (degrees/sec)');
%
% figure (3i)
% fplot (@(t)acc(t), [0 3]);
% title ('Figure 3: Acceleration of Object');
% xlabel ('Time (t)');
% ylabel ('Ang. Acceleration (degrees/sec_square)');

figure(i)

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

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

subplot(3, 1, 3)
fplot(@(t)acc(t), [0 tf])
label3 = ['q[', num2str(i), '] Acceleration Trajectory'];
xlabel('Time'); ylabel ('Ang. 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 with a via point.
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 through a
specified via point.
We started by specifying the tool configuration vectors at initial and final points and
defining the via point to be followed and then finding the corresponding vectors for joint
angles by applying Inverse Kinematics. 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. From this path in figure 4, it can be clearly seen that the robot
moves from the same initial point to the final point as in last lab but this time the path is
such that the it first moves from the initial point to the via point, and then from the via point
to the final point.
The motion of the robot with via point(s) is of great importance practically. Many a times,
we are required to move the robot along a obstacle to reach the desired final position. To
avoid such obstacles, it is necessary to define via points for the path to achieve the same
goal while avoiding the obstacles.
In this lab, we also studied the IC LM 628 and LM 629 which are used as precision motion
controllers having 32 bit registers to keep exact track for the values of Position, Velocity
and Acceleration. It has an internal Trapezoidal Velocity Profile Generator which can be
tuned for the desired velocity and target position. It also comes with a programmable digital
PID filter with 16-bit coefficients for the filter parameters. The LM 629 is available in a
28-pin Dual In-line package or a SOIC-24 Package.