Homework 5

Vin Ngc Quang ILI13161

Trn Phc Thun ILI13196

Problem 13.4.
clear all;
t0 = 0;
t1 = 1.76;
t2 = 2.68;
tf = 10;
q0 = -10; q1 = 12; q2 = 35; qf = 45;
q0d = 0; qfd = 0;
qcd = 25;
%from t0 to t1
A = [1 t0 t0.^2;
0 1 2*t0;
0 1 2*t1];
b = [q0; q0d; qcd];
a = inv(A)*b;
a0 = a(1); a1 = a(2); a2 = a(3);
%from t1 to t2
A = [1 t1;
0 1];
b = [q1; qcd];
a = inv(A)*b;
c0 = a(1); c1 = a(2);
%from t2 to tf
A = [1 t2 t2.^2 t2.^3;
0 1 2*t2 3*t2.^2;
1 tf tf.^2 tf.^3;
0 1 2*tf 3*tf.^2];
b = [q2; qcd; qf; qfd];
a = inv(A)*b;
b0 = a(1); b1 = a(2); b2 = a(3); b3 = a(4);

qt = [];
qtd = [];
for t=t0:0.01:(t1-0.01),
q= a0 + a1*t + a2*t^2;
qd= a1 + 2*a2*t;
qt=[qt q];
qtd=[qtd qd];
end
for t=t1:0.01:(t2-0.01),
q=c0 + c1*t;
qd=c1;
qt=[qt q];
qtd=[qtd qd];
end
for t=t2:0.01:(tf),
q= b0 + b1*t + b2*t^2 + b3*t^3;
qd = b1 + 2*b2*t + 3*b3*t^2;
 qt = [qt q];
qtd=[qtd qd];
end

t= t0:0.01:tf;
plot(t,qt,'r',t,qtd,'g');
grid on

Problem 13.7.
t0 = 0; t1 = 0.25; t2 = 0.5;t3 = 1;
q0 = 10;
q1 = 30;
q2 = 65;
q3 = 95;
q0d = 0;
q3d = 0;
q0dd = 0;
q3dd = 0;

A = [1 t0 t0.^2 t0.^3 t0.^4 t0.^5 t0.^6 t0.^7;

0 1 2*t0 3*t0.^2 4*t0^3 5*t0.^4 6*t0.^5 7*t0.^6;
0 0 2 6*t0 12*t0.^2 20*t0.^3 30*t0.^4 42*t0.^5;
1 t1 t1.^2 t1.^3 t1.^4 t1.^5 t1.^6 t1.^7;
1 t2 t2.^2 t2.^3 t2.^4 t2.^5 t2.^6 t2.^7;
1 t3 t3.^2 t3.^3 t3.^4 t3.^5 t3.^6 t3.^7;
0 1 2*t3 3*t3.^2 4*t3.^3 5*t3.^4 6*t3.^5 7*t3.^6;
0 0 2 6*t3 12*t3.^2 20*t3.^3 30*t3.^4 42*t3.^5];
b = [q0; q0d; q0dd; q1; q2; q3; q3d; q3dd];
a = inv(A)*b;
a0 = a(1); a1 = a(2); a2 = a(3); a3 = a(4); a4 = a(5); a5 = a(6); a6 = a(7);
a7 = a(8);
t = t0:0.01:t3;
qt = a0 + a1*t + a2*t.^2 + a3*t.^3 + a4*t.^4 + a5*t.^5 + a6*t.^6 + a7*t.^7;
qtd = a1 + 2*a2*t + 3*a3*t.^2 + 4*a4*t.^3 + 5*a5*t.^4 + 6*a6*t.^5 + 7*a7*t.^6;
qtd = qtd /2;
qtdd = 2*a2 + 6*a3*t + 12*a4*t.^2 + 20*a5*t.^3 + 30*a6*t.^4 + 42*a7*t.^5;
qtdd = qtdd/10;
qtddd = 6*a3 + 24*a4*t + 60*a5*t.^2 +120*a6*t.^3 + 210*a7*t.^4;
qtddd = qtddd/100;
plot(t, qt, 'b', t, qtd, 'r', t, qtdd, 'g', t, qtddd, 'm');
grid on;
legend('qt','qtd', 'qtdd', 'qtddd');

Problem 13.10.
clc;
clear all;
t0 = 0; t1 = 2; t2 = 4; t3 = 7.5; t4 = 10;
q0 = 5; q1 = 15; q2 = 35; q3 = 65; q4 = 100;
q0d = 0; q4d = 0;
q0dd = 0; q4dd = 0;
%from t0 to t1
A = [1 t0 t0.^2 t0.^3;
0 1 2*t0 3*t0.^2;
0 0 2 6*t0;
1 t1 t1.^2 t1.^3]
b = [q0; q0d; q0dd; q1];
a = inv(A)*b;
a0 = a(1); a1 = a(2); a2 = a(3); a3 = a(4);
%from t1 to t2
q1d = a1 + 2*a2*t1 + 3*a3*t1.^2;
A = [1 t1 t1.^2;
0 1 2*t1;
1 t2 t2^2];
b = [q1; q1d; q2];
a = inv(A)*b;
b0 = a(1); b1 = a(2); b2 = a(3);
%from t2 to t3
q2d = b1 + 2*b2*t2;
A = [1 t2 t2.^2;
0 1 2*t2;
1 t3 t3.^2];
b = [q2; q2d; q3];
c0 = a(1); c1 = a(2); c2 = a(3);
%from t3 to t4
q3d = c1 + 2*c2*t3;
A = [1 t3 t3.^2 t3.^3 t3.^4;
0 1 2*t3 3*t3.^2 4*t3.^3;
1 t4 t4.^2 t4.^3 t4.^4;
0 1 2*t4 3*t4.^2 4*t4.^3;
0 0 2 6*t4 12*t4.^2];
b = [q3; q3d; q4; q4d; q4dd];
a = inv(A)*b;
d0 = a(1); d1 = a(2); d2 = a(3); d3 = a(4); d4 = a(5);
%plotting
qt = [];
qtd = [];
for t = t0:0.01:(t1 - 0.01)
q = a0 + a1*t + a2*t.^2 + a3*t.^3;
qd = a1 + 2*a2*t + 3*a3*t.^2;
qt = [qt q];
qtd = [qtd qd];
end
for t = t1:0.01:(t2-0.01)
q = b0 + b1*t + b2*t.^2;
qd = b1 + 2*b2*t;
qt = [qt q];
qtd = [qtd qd];
end
for t = t2:0.01:(t3 - 0.01)
q = c0 + c1*t + c2*t.^2;
qd = c1 * 2*c2*t;
qt = [qt q];
qtd = [qtd qd];
end
for t = t3:0.01:t4
q = d0 + d1*t + d2*t.^2 + d3*t.^3 + d4*t.^4;
qd = d1 + 2*d2*t + 3*d3*t.^2 + 4*d4*t.^3;
qt = [qt q];
qtd = [qtd qd];
end
t = t0:0.01:t4;
plot(t, qt, 'r', t, qtd/5, 'b');
xlabel('Time(s)');
ylabel('Angle(deg)');
grid on
legend('q', 'qdot/5');
Problem 13.19.

Consider a 3 articulated manipulator with l1 = l2 = l3 = 1m.

(a) Calculate a cubic rest-to-rest path in Cartesian space to join the following points with a
straight line.
P1 = (1.5, 1, 0) P2 = (0.5, 1.5, 1)

Matlab:
clear;
clear all;
t0 = 0;
tf = 10;
P1 = [-1.5; 1; 0];
P2 = [0.5; 1.5; 1];
%a)
%solve for X
A = [1 t0 t0.^2 t0.^3;
0 1 2*t0 3*t0.^2;
1 tf tf.^2 tf.^3;
0 1 2*tf 3*tf.^2];
b = [P1(1); 0; P2(1); 0];
a = inv(A)*b;
a0 = a(1); a1 = a(2); a2 = a(3); a3 = a(4);
%solve for Y
A = [1 t0 t0.^2 t0.^3;
0 1 2*t0 3*t0.^2;
1 tf tf.^2 tf.^3;
0 1 2*tf 3*tf.^2];
b = [P1(2); 0; P2(2); 0];
a = inv(A)*b;
b0 = a(1); b1 = a(2); b2 = a(3); b3 = a(4);
%solve for Z
A = [1 t0 t0.^2 t0.^3;
0 1 2*t0 3*t0.^2;
1 tf tf.^2 tf.^3;
0 1 2*tf 3*tf.^2];
b = [P1(3); 0; P2(3); 0];
a = inv(A)*b;
c0 = a(1); c1 = a(2); c2 = a(3); c3 = a(4);
t = t0:0.1:tf;
x = a0 + a1*t + a2*t.^2 + a3*t.^3;
y = b0 + b1*t + b2*t.^2 + b3*t.^3;
z = c0 + c1*t + c2*t.^2 + c3*t.^3;
plot3(x, y, z);
grid on;

(b) Calculate and plot the joint coordinates of the manipulator that
follows the Cartesian path.

Matlab:
A = [1 t0 t0.^2 t0.^3 t0.^4 t0.^5;
0 1 2*t0 3*t0.^2 4*t0.^3 5*t0.^4;
0 0 2 6*t0 12*t0.^2 20*t0.^3;
1 tf tf.^2 tf.^3 tf.^4 tf.^5;
0 1 2*tf 3*tf.^2 4*tf.^3 5*tf.^4;
 0 0 2 6*tf 12*tf.^2 20*tf.^3;];
b = [P1(1); 0; 0; P2(1); 0; 0];
a = inv(A)*b;
a0 = a(1); a1 = a(2); a2 = a(3); a3 = a(4); a4 = a(5); a5 = a(6);
b = [P1(2); 0; 0; P2(2); 0; 0];
a = inv(A)*b;
b0 = a(1); b1 = a(2); b2 = a(3); b3 = a(4); b4 = a(5); b5 = a(6);
b = [P1(3); 0; 0; P2(3); 0; 0];
a = inv(A)*b;
c0 = a(1); c1 = a(2); c2 = a(3); c3 = a(4); c4 = a(5); c5 = a(6);
t = t0:0.1:tf;
x = a0 + a1*t + a2*t.^2 + a3*t.^3 + a4*t.^4 + a5*t.^5 ;
y = b0 + b1*t + b2*t.^2 + b3*t.^3 + b4*t.^4 + b5*t.^5 ;
z = c0 + c1*t + c2*t.^2 + c3*t.^3 + c4*t.^4 + c5*t.^5;
plot3(x, y, z);
grid on;

for i = 1:1:101;
a1 = 1;
a2 = 1;
a3 = 1;
theta1(i) = atan2(y(i), x(i));
C1(i) = a1.^2 - 2*a1*z(i) + a2.^2 + (2*a2*x(i))/(cos(theta1(i))) - a3.^2 +
(x(i).^2)/(cos(theta1(i)).^2) + z(i).^2;
C2(i) = 2*a1*a2 - 2*a2*z(i);
C3(i) = a1.^2 - 2*a1*z(i) + a2.^2 - (2*a2*x(i))/(cos(theta1(i))) - a3.^2 +
(x(i).^2)/(cos(theta1(i)).^2) + z(i).^2;
theta2(i) = 2*atan(((-C2(i) + (C2(i).^2 - C1(i)*C3(i)).^(1/2)))/C1(i));
theta3(i) = acos((a1 - z(i) + a2*sin(theta2(i)))/a3) - theta2(i);

end

figure;
hold on;
plot(t,theta1,'b',t,theta2,'g',t,theta3,'r') ;
legend('theta1','theta2','theta3');
xlabel('time(s)');
ylabel('degree');
title('Joint angle');
grid on

figure,
hold on;
i = 1:1:101;

X = a3.*sin(theta2 + theta3).*cos(theta1) + a2.*cos(theta1).*cos(theta2);

Y = a3.*sin(theta2 + theta3).*sin(theta1) + a2.*sin(theta1).*cos(theta2);
Z = a1 - a3.*cos(theta2 + theta3) + a2.*sin(theta2);
plot3(X, Y, Z, '.r');
xlabel('x');
ylabel('y');
title('Real path of end effector');
grid on;
(c)

Calculate the maximum angular velocity and acceleration of the

joint variables.
% angular velocity
for i = 1:1:100;
theta1d(i) = (theta1(i+1) - theta1(i))/(0.1);
theta2d(i) = (theta2(i+1) - theta2(i))/(0.1);
theta3d(i) = (theta3(i+1) - theta3(i))/(0.1);
end
maxtheta1d = max(abs(theta1d));
maxtheta2d = max(abs(theta2d));
maxtheta3d = max(abs(theta3d));
figure;
t = t0:0.1:(tf-0.1);
plot(t,theta1d,'r',t,theta2d,'b',t,theta3d, 'm');
legend('theta1d','theta2d', 'theta3d');
ylabel('angular velocity');
xlabel('time(s)');
title('Angular velocity of joint variables');
grid on;
% acceleration
for i = 1:1:99;
theta1dd(i) = (theta1d(i+1) - theta1d(i))/(0.1);
theta2dd(i) = (theta2d(i+1) - theta2d(i))/(0.1);
theta3dd(i) = (theta3d(i+1) - theta3d(i))/(0.1);
end
maxtheta1dd = max(abs(theta1dd));
maxtheta2dd = max(abs(theta2dd));
maxtheta3dd = max(abs(theta3dd));
t = t0:0.1:(tf-0.2);
plot(t,theta1dd,'r',t,theta2dd,'b',t,theta3dd, 'm');
legend('theta1dd','theta2dd', 'theta3dd');
ylabel('angular acceleration(rad/s^2)');
xlabel('time(s)');
title('Angular acceleration of joint variables');
grid on;
Problem 13.20.

Matlab:
% 2R manipulator drawing 2 half circles
clc;
clear all;
close all;
l1 =1;
l2 =1;
t0 = 0; t1 = 5; t2 = 10;
q1 = [];
q2 = [];
% First semi circle
for i = t0:(1/36):(t1-1/36),
%X = 0.75 + 0.75*cos(i*pi/180) ;
%Y = 0.5 + sin(i*pi/180);
X = 0.75 + 0.75*cos(i*36*pi/180) ;
Y = 0.5 - sin(i*180/t1*pi/180);

c2 = (X.^2 + Y.^2 - l1^2 -l2^2)/(2*l1*l2);

s2 = sqrt(1 - c2.^2);
theta2 = atan2(s2, c2);
theta1 = atan2(Y*(l1 + l2*c2) - X*l2*s2, X*(l1 + l2*c2) + Y*l2*s2);
q1 = [q1 theta1];
q2 = [q2 theta2];
end
x =[];
y =[];
for i = 1:length(q1),
s1 = sin(q1(i));
c1 = cos(q1(i));
s2 = sin(q2(i));
c2 = cos(q2(i));
A1 = [c1 -s1 0 l1*c1;
s1 c1 0 l1*s1;
0 0 1 0;
0 0 0 1];
A2 = [c2 -s2 0 l2*c2;
s2 c2 0 l2*s2;
0 0 1 0;
0 0 0 1];
A = A1*A2;
x = [x A(1, 4)];
y = [y A(2, 4)];
end

x =[];
y =[];
% 2nd Semi circle
for i = t1:1/36:t2,
%X = -0.75 + 0.75*cos(i*pi/180) ;
%Y = 0.5 - sin(i*pi/180);

X = -0.75 - 0.75*cos(i*36*pi/180) ;
Y = 0.5 - sin(i*36*pi/180);

c2 = (X.^2 + Y.^2 - l1^2 -l2^2)/(2*l1*l2);

s2 = sqrt(1 - c2.^2);
theta2 = atan2(s2, c2);
theta1 = atan2(Y*(l1 + l2*c2) - X*l2*s2, X*(l1 + l2*c2) + Y*l2*s2);
q1 = [q1 theta1];
 q2 = [q2 theta2];
end

for i = 1:length(q1),
s1 = sin(q1(i));
c1 = cos(q1(i));
s2 = sin(q2(i));
c2 = cos(q2(i));
A1 = [c1 -s1 0 l1*c1;
s1 c1 0 l1*s1;
0 0 1 0;
0 0 0 1];
A2 = [c2 -s2 0 l2*c2;
s2 c2 0 l2*s2;
0 0 1 0;
0 0 0 1];
A = A1*A2;
% Jacobian matrix
J = [-A(1,4) -l2*(s1*c2 + s2*c1);
A(2, 4) l2*(c1*c2 - s1*s2);
0 0;
0 0;
0 0;
1 1];
Jv = [J(1,1) J(1,2);
J(2,1) J(2,2)];
Qd = inv(Jv) *[1/sqrt(2); 1/sqrt(2)];
x = [x A(1, 4)];
y = [y A(2, 4)];
%q1d = [q1d Qd(1,1)];
%q2d = [q2d Qd(2,1)];
end

theta1 = q1*180/pi;
theta2 = q2*180/pi;
figure;
t = t0:(1/36):t2;
plot(t, q1, 'r',t, q2, 'b');
legend('theta1','theta2');
ylabel('Angle(rad)');
xlabel('Time(s)');
title('Angle against time');
grid on;

figure;
hold on;
for i = 1:length(q1),
plot(x(i), y(i), '.r');
pause(0.01);
end
grid on;
% Real path
X = l1*cos(q1) + l2*cos(q1+q2);
Y = l1*sin(q1) + l2*sin(q1+q2);
t=t0:(1/36):t2;
hold on;
figure,
plot(X,Y,'.r');
xlabel('x');
ylabel('y');
title('Real path of end effector');
grid on;

% Angular velocity
for i = 1:1:360,
q1d(i) = (q1(i+1)-q1(i))/(1/36);
q2d(i) = (q2(i+1)-q2(i))/(1/36);
end
figure;
t = t0:(1/36):(t2-1/36);
plot(t, q1d, 'r',t, q2d, 'b');
legend('theta1d','theta2d');
ylabel('Angular velocity(rad/s)');
xlabel('Time(s)');
title('Angular velocity against time');
grid on;
[max_velocity_theta1,pos1] = max(abs(q1d(:)));
%max_velocity1
position_max_v1=[];
%max_velocity1 position
position_max_v1 = [position_max_v1 X(1,pos1)];
position_max_v1 = [position_max_v1 Y(1,pos1)];
position_max_v1 = [position_max_v1 t(1, pos1)];
%max_velocity2
[max_velocity_theta2,pos2] = max(abs(q2d(:)));
%max_velocity2 position
position_max_v2=[];
position_max_v2 = [position_max_v2 X(1,pos2)];
position_max_v2 = [position_max_v2 Y(1,pos2)];
position_max_v2 = [position_max_v2 t(1, pos2)];
%Angular acceleration
for i = 1:1:359,
q1dd(i) = (q1d(i+1)-q1d(i))/(1/36);
q2dd(i) = (q2d(i+1)-q2d(i))/(1/36);
end
figure;
t = t0:(1/36):(t2-1/36-1/36);
plot(t, q1dd, 'r',t, q2dd, 'b');
legend('theta1dd','theta2dd');
ylabel('Angular acceleration(rad/s^2)');
xlabel('Time(s)');
title('Angular acceleration against time');
grid on;
%max_acceleration1
[max_acceleration_theta1,pos1] = max(abs(q1dd(:)));
%max_acceleration1 position
position_max_a1=[];
position_max_a1 = [position_max_a1 X(1,pos1)];
position_max_a1 = [position_max_a1 Y(1,pos1)];
position_max_a1 = [position_max_a1 t(1, pos1)];
%max_acceleration2
[max_acceleration_theta2,pos2] = max(abs(q2dd(:)));
%max_acceleration2 position
position_max_a2=[];
position_max_a2 = [position_max_a2 X(1,pos2)];
position_max_a2 = [position_max_a2 Y(1,pos2)];
position_max_a2 = [position_max_a2 t(1, pos2)];
%Angular jerk
for i = 1:1:358,
q1ddd(i) = (q1dd(i+1)-q1dd(i))/(1/36);
q2ddd(i) = (q2dd(i+1)-q2dd(i))/(1/36);
end
figure;
t = t0:(1/36):(t2-1/36-1/36-1/36);
plot(t, q1ddd, 'r',t, q2ddd, 'b');
legend('theta1ddd','theta2ddd');
ylabel('Angular jerk(rad/s^3)');
xlabel('Time(s)');
title('Angular jerk against time');
grid on;
%max_jerk1
[max_jerk_theta1,pos1] = max(abs(q1ddd(:)));
position_max_j1=[];
position_max_j1 = [position_max_j1 X(1,pos1)];
position_max_j1 = [position_max_j1 Y(1,pos1)];
position_max_j1 = [position_max_j1 t(1, pos1)];
%max_jerk2
[max_jerk_theta2,pos2] = max(abs(q2ddd(:)));
%max_acceleration2 position
position_max_j2=[];
position_max_j2 = [position_max_j2 X(1,pos2)];
position_max_j2 = [position_max_j2 Y(1,pos2)];
position_max_j2 = [position_max_j2 t(1, pos2)];
(a) Calculate and plot the joints path if l1 = l2 = 1m.
(b)
Calculate the value and positions of the maximum angular velocity in joint variables.
max_velocity_theta1 = 6.42282712347940 (rad/s)
max_velocity_theta2 = 0.660166817926791(rad/s)
Positions for max_velocity_theta1: A = [0.08778, 0.03053] at t = 4.22s

Positions for max_velocity_theta2: B = [-0.02905, 0.77564] at t = 5.44s

(c) Calculate the value and positions of the maximum angular acceleration in joint variables.
max_acceleration_theta1 = 30.03875 (rad/s)
max_acceleration_theta2 = 4.1077 (rad/s)
position_max_a1 = [0.1139, -0.0299] at t = 4.111s
position_max_a2 = [0.094, 0.0151] at t = 4.194s

(d) Calculate the value and positions of the maximum angular jerk in joint variables.
max_jerk_theta1 = 567.304 (rad/s^3)
max_jerk_theta2 = 23.7693 (rad/s^3)
position_max_j1 = [0.094, 0.01519] at t = 4.194s
position_max_j2 = [0.1134, -0.0299] at t = 4.111s