Matlab Code:

L1 = 1;
L2 = 0.5;
w = 0.05;
m1 = L1*w^2*7806;
m2 = L2*w^2*7806;
Iz1 = w^2+L1^2;
Iz2 = w^2+L2^2;
theta1 = 10 *pi/180;
theta2 = 90 *pi/180;
Xd = [0; 0.5];

dt = 0.01;
tf = 1;
tvec = 0: dt: tf;
ThetaD_p = [0;0];
for i = 1:length(tvec)
theta1_all(i) = theta1;
theta2_all(i) = theta2;

J = [ - L2*sin(theta1 + theta2) - L1*sin(theta1), -L2*sin(theta1 + theta2)

L2*cos(theta1 + theta2) + L1*cos(theta1), L2*cos(theta1 + theta2)];
ThetaD = inv(J)*Xd;
theta1d_all(i) = ThetaD(1);
theta2d_all(i) = ThetaD(2);

T0f3 = [ cos(theta1 + theta2), -sin(theta1 + theta2), 0, L2*cos(theta1 +

theta2) + L1*cos(theta1)
sin(theta1 + theta2), cos(theta1 + theta2), 0, L2*sin(theta1 +
theta2) + L1*sin(theta1)
0, 0, 1,
0
0, 0, 0,
1];
P3 = T0f3(1:3,4);
P2 = [L1*cos(theta1)
L1*sin(theta1)
0];
X3(i) = P3(1);
Y3(i) = P3(2);
X2(i) = P2(1);
Y2(i) = P2(2);
Theta_3(i) = theta1 + theta2;

theta1 = theta1 + dt*ThetaD(1);

theta2 = theta2 + dt*ThetaD(2);

JN = [ - L2*sin(theta1 + theta2) - L1*sin(theta1), -L2*sin(theta1 + theta2)

L2*cos(theta1 + theta2) + L1*cos(theta1), L2*cos(theta1 +
theta2)];
ThetaDN = inv(JN)*Xd;
ThetaDD = (ThetaDN-ThetaD)/dt;
theta1dd_all(i) = ThetaDD(1);
theta2dd_all(i) = ThetaDD(2);
 theta1D = ThetaD(1);
theta2D = ThetaD(2);
theta1DD = ThetaDD(1);
theta2DD = ThetaDD(2);
g = 0;
tou1 = Iz1*theta1DD + L1*(m2*cos(theta2)*(cos(theta2)*(L1*theta1DD +
g*cos(theta1)) + sin(theta2)*(L1*theta1D^2 - g*sin(theta1)) + (L2*(theta1DD
+ theta2DD))/2) - m2*sin(theta2)*((L2*(theta1D + theta2D)^2)/2 -
sin(theta2)*(L1*theta1DD + g*cos(theta1)) + cos(theta2)*(L1*theta1D^2 -
g*sin(theta1)))) + Iz2*(theta1DD + theta2DD) + (L1*m1*((L1*theta1DD)/2 +
g*cos(theta1)))/2 + (L2*m2*(cos(theta2)*(L1*theta1DD + g*cos(theta1)) +
sin(theta2)*(L1*theta1D^2 - g*sin(theta1)) + (L2*(theta1DD +
theta2DD))/2))/2;
tou2 = Iz2*(theta1DD + theta2DD) + (L2*m2*(cos(theta2)*(L1*theta1DD +
g*cos(theta1)) + sin(theta2)*(L1*theta1D^2 - g*sin(theta1)) + (L2*(theta1DD
+ theta2DD))/2))/2;
g = 9.8;
tou1g = Iz1*theta1DD + L1*(m2*cos(theta2)*(cos(theta2)*(L1*theta1DD +
g*cos(theta1)) + sin(theta2)*(L1*theta1D^2 - g*sin(theta1)) + (L2*(theta1DD
+ theta2DD))/2) - m2*sin(theta2)*((L2*(theta1D + theta2D)^2)/2 -
sin(theta2)*(L1*theta1DD + g*cos(theta1)) + cos(theta2)*(L1*theta1D^2 -
g*sin(theta1)))) + Iz2*(theta1DD + theta2DD) + (L1*m1*((L1*theta1DD)/2 +
g*cos(theta1)))/2 + (L2*m2*(cos(theta2)*(L1*theta1DD + g*cos(theta1)) +
sin(theta2)*(L1*theta1D^2 - g*sin(theta1)) + (L2*(theta1DD +
theta2DD))/2))/2;
tou2g = Iz2*(theta1DD + theta2DD) + (L2*m2*(cos(theta2)*(L1*theta1DD +
g*cos(theta1)) + sin(theta2)*(L1*theta1D^2 - g*sin(theta1)) + (L2*(theta1DD
+ theta2DD))/2))/2;
tou1_all(i) = tou1;
tou2_all(i) = tou2;
tou1g_all(i) = tou1g;
tou2g_all(i) = tou2g;
end

figure(1)
X = [0 X2(1) X3(1)];
Y = [0 Y2(1) Y3(1)];
plot(X,Y,'bO-','linewidth',2)
axis equal
axis([0 1.25 0 1.25])
grid on
pause(1)
for i = 1:length(tvec)
X = [0 X2(i) X3(i)];
Y = [0 Y2(i) Y3(i)];
plot(X3(1:i),Y3(1:i),'r:','linewidth',2); hold on
plot(X,Y,'bO-','linewidth',2); hold off
axis equal
axis([0 1.25 0 1.25])
grid on
pause(0.1)
end
pause(2)

theta1_all = theta1_all *180/pi;

theta2_all = theta2_all *180/pi;
figure(2)
plot(tvec,theta1_all, tvec,theta2_all,'linewidth',2)
legend('Theta1','Theta2')
xlabel('Time (s)');
ylabel('Angle (deg)');
title('Joint Angles')
grid on

figure(3)
plot(tvec,theta1d_all, tvec,theta2d_all,'linewidth',2)
legend('Theta1d','Theta2d')
xlabel('Time (s)');
ylabel('Rate (rad/s)');
title('Joint Rates')
grid on

figure(4)
plot(tvec,theta1dd_all, tvec,theta2dd_all,'linewidth',2)
legend('Theta1dd','Theta2dd')
xlabel('Time (s)');
ylabel('Angular Acc. (rad/s^2)');
title('Joint Accelerations')
grid on

figure(5)
plot(tvec,X3, tvec,Y3, tvec,Theta_3,'linewidth',2)
legend('X','Y','Phi')
xlabel('Time (s)');
ylabel('Pose (m & rad)');
title('Cartesian Pose')
grid on

figure(6)
plot(tvec,tou1_all,tvec,tou2_all,'linewidth',2)
legend('Tou1','Tou2')
xlabel('Time (s)');
ylabel('Torque (Nm)');
title('Joint Torques (w/o g)')
grid on

figure(7)
plot(tvec,tou1g_all,tvec,tou2g_all,'linewidth',2)
legend('Tou1','Tou2')
xlabel('Time (s)');
ylabel('Torque (Nm)');
title('Joint Torques (w/ g)')
grid on
Rate (rad/s)
Angle (deg)
Result Plots:
Pose (m & rad) Angular Acc. (rad/s 2 )
Torque (Nm)
Torque (Nm)
 t=0 (s) t=0.33 (s)

1.2

0.8

0.6

0.4

0.2

0
0 0.2 0.4 0.6 0.8 1 1.2

t=0.66 (s) t=1 (s)

Matlab Code:
syms L1 L2 L3 theta1 theta2 theta3 real;
syms theta1D theta2D theta3D real
syms theta1DD theta2DD theta3DD real
syms g m1 m2 m3 real
syms Ix1 Ix2 Ix3 real
syms Iy1 Iy2 Iy3 real
syms Iz1 Iz2 Iz3 real
syms d1D d2D d3D real
% If assign:==================
L1 = 4;
L2 = 3;
L3 = 2;
m1 = 20;
m2 = 15;
m3 = 10;
theta1 = 10 *sym(pi)/180;
theta2 = 20 *sym(pi)/180;
 theta3 = 30 *sym(pi)/180;
theta1D = 1 ;
theta2D = 2 ;
theta3D = 3 ;
theta1DD = 0.5 ;
theta2DD = 1 ;
theta3DD = 1.5 ;
Iz1 = 0.5;
Iz2 = 0.2;
Iz3 = 0.1;
%=============================

ThetaD = [theta1D theta2D theta3D];

ThetaDD = [theta1DD theta2DD theta3DD];
m = [m1 m2 m3];
Ic(:,:,1) = [Ix1 0 0
0 Iy1 0
0 0 Iz1];
Ic(:,:,2) = [Ix2 0 0
0 Iy2 0
0 0 Iz2];
Ic(:,:,3) = [Ix3 0 0
0 Iy3 0
0 0 Iz3];
dD = [d1D d2D d3D];
joints = 3;
Theta_enable = [1 1 1];
D_enable = [0 0 0];

%========================================================================
==
%Frame 0 to 1
a = 0;
alpha = 0;
d = 0;
theta = theta1;
T0f1 = HomoTrans(a,alpha,d,theta);

%Frame 1 to 2
a = L1;
alpha = 0;
d = 0;
theta = theta2;
T1f2 = HomoTrans(a,alpha,d,theta);

%Frame 2 to 3
a = L2;
alpha = 0;
d = 0;
theta = theta3;
T2f3 = HomoTrans(a,alpha,d,theta);
%Frame 3 to 4
a = L3;
alpha = 0;
d = 0;
theta = 0;
T3f4 = HomoTrans(a,alpha,d,theta);
%========================================================================
==

R(:,:,1) = (T0f1(1:3,1:3));
R(:,:,2) = (T1f2(1:3,1:3));
R(:,:,3) = (T2f3(1:3,1:3));
R(:,:,4) = (T3f4(1:3,1:3));

P(:,1) = T0f1(1:3,4);
P(:,2) = T1f2(1:3,4);
P(:,3) = T2f3(1:3,4);
P(:,4) = T3f4(1:3,4);

Om_0 = [0; 0; 0];

Omd_0 = [0; 0; 0];
V_0 = [0; 0; 0];
g=9.81;
Vd_0 = [0; g; 0];
Pc = [L1/2 L2/2 L3/2 0
0 0 0 0
0 0 0 0];
for i=1:joints
Z = [0; 0; 1];
if i == 1
Om(:,i) = R(:,:,i)'*Om_0 + ThetaD(i)*Theta_enable(i)*Z;
Omd(:,i) = R(:,:,i)'*Omd_0 + cross(R(:,:,i)*Om_0,
ThetaD(i)*Theta_enable(i)*Z) + ThetaDD(i)*Theta_enable(i)*Z;
else
Om(:,i) = R(:,:,i)'*Om(:,i-1) + ThetaD(i)*Theta_enable(i)*Z;
Omd(:,i) = R(:,:,i)'*Omd(:,i-1) + cross(R(:,:,i)*Om(:,i-1),
ThetaD(i)*Theta_enable(i)*Z) + ThetaDD(i)*Theta_enable(i)*Z;
end
if i == 1
V(:,i) = R(:,:,i)'*(V_0 + cross(Om_0,P(:,i))) + dD(i)*D_enable(i)*Z;
Vd(:,i) = R(:,:,i)'*(cross(Omd_0,P(:,i)) +
cross(Om_0,cross(Om_0,P(:,i))) + Vd_0);
else
V(:,i) = R(:,:,i)'*(V(:,i-1) + cross(Om(:,i-1),P(:,i))) +
dD(i)*D_enable(i)*Z;
Vd(:,i) = R(:,:,i)'*(cross(Omd(:,i-1),P(:,i)) +
cross(Om(:,i-1),cross(Om(:,i-1),P(:,i))) + Vd(:,i-1));
end
Vdc(:,i) = cross(Omd(:,i),Pc(:,i)) +
cross(Om(:,i),cross(Om(:,i),Pc(:,i))) + Vd(:,i);
F(:,i) = m(i)*Vdc(:,i);
N(:,i) = Ic(:,:,i)*Omd(:,i) + cross(Om(:,i),Ic(:,:,i)*Om(:,i));
 end
f(:,4) = [0;0;0];
n(:,4) = [0;0;0];
f = sym(f);
n = sym(n);
for i = joints:-1:1
f(:,i) = R(:,:,i+1)*f(:,i+1) + F(:,i);
n(:,i) = N(:,i) + R(:,:,i+1)*n(:,i+1) + cross(Pc(:,i),F(:,i)) +
cross(P(:,i+1),R(:,:,i+1)*f(:,i+1));
end
tou1 = double(n(3,1))
tou2 = double(n(3,2))
tou3 = double(n(3,3))

function T = HomoTrans(a,alpha,d,theta)
T = [ cos(theta) -sin(theta) 0 a
sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha)
-sin(alpha)*d
sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha)
cos(alpha)*d
0 0 0 1 ];
end

Execution Result:
>>
tou1 =

853.6032

tou2 =

637.2026

tou3 =

296.8187