Lab # 08
Trajectory planning of a robot using Corke’s Robotics MATLAB Toolbox
Objectives:
In this Lab session the concept of path and trajectory will be discussed in task-space and jointspace. MATLAB will be used in this session in order to provide a practical example of trajectory
generation.
Path and Trajectory:
A path is defined as the positional description of robot configurations in a particular order. The
path has no timing restrictions other than the order of the configurations. For example, a robot
arm may be desired to go through a path described by the endpoint positions A, B and C.
A trajectory is a path with all necessary timing specifications to calculate the required position,
and velocity of the robot configuration. For example, the set of endpoint positions A, B, and C in
the previous example together with the specification of velocity at each of these points forms a
well-defined trajectory, since the time to reach each of the points A, B, C and also to the
intermediate points between them can be interpolated from this information.
Exercise-01: Consider a simple 2-DOF robot. We desire to move the robot from point A to point
B. The configuration of the robot at point A is 𝛼 = 20° and 𝛽 = 30° in order for the robot to be at
point B, the configuration calculated to be as 𝛼 = 40° and 𝛽 = 80°. Also suppose that both joints
of the robot can move at the maximum rate of 10 degrees/sec. Plan this motion in Joint-Space.
One way to move the robot from point A to B is to run both joints at their maximum angular
velocities.
MATLAB Code:
clear all; clc;clf; close all;format compact;
r.b1=0.4; r.b2=0.5; % these are robot link lengths.
n=10;
q0 = [20/57.2952 30/57.2952]
qn = [40/57.2952 80/57.2952]
dq= 10/57.2952;
qq1=q0(1);
qq2=q0(2);
q=[qq1 qq2];
for i=2:n
qq1(i)=qq1(i-1) + dq;
if (qq1(i)> qn(1))
qq1(i)=qq1(i-1);
end
qq2(i)= qq2(i-1) + dq;
if (qq2(i)>= qn(2))
qq2(i)=qq2(i-1);
end
q(i,:) =[qq1(i) qq2(i)];
r = setjoint(r, q(i,1), q(i,2));
q(i,:)=[r.q1(1) r.q2(1)];
p(i,:) = [r.p(1) r.p(2)];
end
% Planar Plot of the Motion
x=[0 0 0]; y=[0 0 0];
for i=1:length(q);
xo=x; yo=y;
x(1)=0; y(1)=0; % origin
x(2)= r.b1*cos(q(i,1)); y(2)=r.b1*sin(q(i,1));
x(3)= x(2)+r.b2*cos(q(i,1)+q(i,2));
y(3)= y(2)+r.b2*sin(q(i,1)+q(i,2));
plot(xo,yo,'k-'); hold on;
plot(x,y,'k-');
plot(x,y,'r.');
axis equal;
pause(0.01);
end
function rr = setjoint(r,q1,q2);
rr=r;
rr.q1=q1 ; rr.q2=q2 ; rr.q=[q1 q2];
rr.S1=sin(q1); rr.C1=cos(q1);
rr.S2=sin(q2); rr.C2=cos(q2);
rr.S12=sin(q1+q2); rr.C12=cos(q1+q2);
rr.x0 = rr.b2*rr.C12+rr.b1*rr.C1;
rr.y0 = rr.b2*rr.S12+rr.b1*rr.S1;
rr.p=[rr.x0 rr.y0];
end
function rr= setcoord(r,x0,y0);
rr=r; c=(x0^2+y0^2+r.b1^2-r.b2^2)/2.0/r.b1;
rr.q1 = [atan2(y0,x0)+atan2(sqrt(x0^2+y0^2-c^2), c)
atan2(y0,x0)-atan2(sqrt(x0^2+y0^2-c^2), c)];
rr.S1 = sin(rr.q1); rr.C1=cos(rr.q1);
rr.q2 =atan2( y0-r.b1*rr.S1, x0-r.b1*rr.C1)-rr.q1;
rr.S2 = sin(rr.q2); rr.C2=cos(rr.q2);
rr.S12 = sin(rr.q1+rr.q2); rr.C12=cos(rr.q1+rr.q2);
end
Exercise-02: Consider the previous example and suppose that the motions of both joints of the
robot are normalized such that the joint with smaller motion will move proportionally slower so
that both joints will start and stop their motion simultaneously.
MATLAB Code:
clear all; clc;clf; close all;format compact;
r.b1=0.5; r.b2=0.4; % these are robot link lengths.
n=5;
q0 = [20/57.2952 30/57.2952]
qn = [40/57.2952 80/57.2952]
dq= (qn-q0)/(n-1);
for i=1:n
q(i,:) = q0 + dq*(i-1)
r = setjoint(r, q(i,1), q(i,2)); % set the joint disp to soln=1
q(i,:)=[r.q1(1) r.q2(1)];
p(i,:) = [r.p(1) r.p(2)];
end
% Planar Plot of the Motion
x=[0 0 0]; y=[0 0 0];
for i=1:length(q);
xo=x; yo=y;
x(1)=0; y(1)=0; % origin
x(2)= r.b1*cos(q(i,1)); y(2)=r.b1*sin(q(i,1));
x(3)= x(2)+r.b2*cos(q(i,1)+q(i,2));
y(3)= y(2)+r.b2*sin(q(i,1)+q(i,2));
plot(xo,yo,'k-'); hold on;
plot(x,y,'k-');
plot(x,y,'r.');
axis equal;
pause(0.01);
end
Exercise-03:
Suppose we want the robot’s hand to follow a known path (straight line) between points A (0.2 ,
0) and B (0.7, 0.5). The simplest solution would be divide the line into segments, and solve for
necessary angles 𝛼 and 𝛽 at each point. This technique is called interpolation between points A
and B. The resultant trajectory is in Cartesian-space since all segments of the motion must be
calculated based on the information expressed in a Cartesian frame.
MATLAB Code:
clear all; clc;clf; close all;format compact;
r.b1=0.5; r.b2=0.4; % these are robot link lengths.
n=10;
p0 = [ 0.2 0]; pn = [ 0.7 0.5];
dp= (pn-p0)/(n-1);
for i=1:n
p(i,:) = p0 + dp*(i-1)
r = setcoord( r, p(i,1),p(i,2) ); % set the coordinates in base
frame
r = setjoint(r, r.q1(1), r.q2(1)); % set the joint disp to soln=1
q(i,:)=[ r.q1(1) r.q2(1) ];
p(i,:) = [r.p(1) r.p(2)];
end
% Planar Plot of the Motion
x=[0 0 0]; y=[0 0 0];
for i=1:length(q);
xo=x; yo=y;
x(1)=0; y(1)=0; % origin
x(2)= r.b1*cos(q(i,1)); y(2)=r.b1*sin(q(i,1));
x(3)= x(2)+r.b2*cos(q(i,1)+q(i,2));
y(3)= y(2)+r.b2*sin(q(i,1)+q(i,2));
plot(xo,yo,'k-'); hold on;
plot(x,y,'k-');
plot(x+0.001,y,'r.');
axis equal; axis([-0.05 0.75 -0.05 0.55]);
pause(0.01);
end
Conclusion:
In this lab we learnt to determine the trajectory of a two-link robot in a joint-space and to
practice the interpolation technique.