Matlab Code:

L1 = 4;
L2 = 3;
L3 = 2;

theta1 = 10 *pi/180;
theta2 = 20 *pi/180;
theta3 = 30 *pi/180;
Xd = [0.2; -0.3; -0.2];
fx = 1;
fy = 2;
mz = 3;
f4_0 = [fx; fy; mz];

dt = 0.1;
tf = 5;
tvec = 0: dt: tf;

for i = 1:length(tvec)
theta1_all(i) = theta1;
theta2_all(i) = theta2;
theta3_all(i) = theta3;
J = [- L2*sin(theta1 + theta2) - L1*sin(theta1) - L3*sin(theta1 + theta2
+ theta3), - L2*sin(theta1 + theta2) - L3*sin(theta1 + theta2 + theta3),
-L3*sin(theta1 + theta2 + theta3)
 L2*cos(theta1 + theta2) + L1*cos(theta1) + L3*cos(theta1 + theta2
+ theta3), L2*cos(theta1 + theta2) + L3*cos(theta1 + theta2 + theta3),
L3*cos(theta1 + theta2 + theta3)
1,
1, 1];
ThetaD = inv(J)*Xd;
theta1d_all(i) = ThetaD(1);
theta2d_all(i) = ThetaD(2);
theta3d_all(i) = ThetaD(3);
T0f4 = [ cos(theta1 + theta2 + theta3), -sin(theta1 + theta2 + theta3),
0, L2*cos(theta1 + theta2) + L1*cos(theta1) + L3*cos(theta1 + theta2 + theta3)
sin(theta1 + theta2 + theta3), cos(theta1 + theta2 + theta3),
0, L2*sin(theta1 + theta2) + L1*sin(theta1) + L3*sin(theta1 + theta2 + theta3)
0, 0, 1,
0
0, 0, 0,
1];
P4 = T0f4(1:3,4);
P3 = [L2*(cos(theta1)*cos(theta2) - sin(theta1)*sin(theta2)) +
L1*cos(theta1)
L2*(cos(theta1)*sin(theta2) + cos(theta2)*sin(theta1)) +
L1*sin(theta1)
0];
P2 = [L1*cos(theta1)
L1*sin(theta1)
0];
X4(i) = P4(1);
Y4(i) = P4(2);
X3(i) = P3(1);
Y3(i) = P3(2);
X2(i) = P2(1);
Y2(i) = P2(2);
Theta_4(i) = theta1 + theta2 + theta3;
Det_J(i) = det(J);

Jt = [L1*sin(theta2 + theta3) + L2*sin(theta3), L3 + L1*cos(theta2 + theta3)

+ L2*cos(theta3), 1
L2*sin(theta3), L3 +
L2*cos(theta3), 1
0,
L3, 1];
R0f4 = T0f4(1:3,1:3);
f4_4 = R0f4'*f4_0
Tou = Jt*f4_4;
Tou1(i) = Tou(1);
Tou2(i) = Tou(2);
Tou3(i) = Tou(3);

theta1 = theta1 + dt*ThetaD(1);

theta2 = theta2 + dt*ThetaD(2);
theta3 = theta3 + dt*ThetaD(3);
end

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

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

theta1_all = theta1_all *180/pi;

theta2_all = theta2_all *180/pi;
theta3_all = theta3_all *180/pi;

figure(3)
plot(tvec,theta1_all, tvec,theta2_all, tvec,theta3_all,'linewidth',2)
legend('Theta1','Theta2','Theta3')
xlabel('Time (s)');
ylabel('Angle (deg)');
title('Joint Angles')
grid on

figure(4)
plot(tvec,X4, tvec,Y4, tvec,Theta_4,'linewidth',2)
legend('X','Y','Phi')
xlabel('Time (s)');
ylabel('Pose (m & rad)');
title('Cartesian Pose')
grid on

figure(5)
plot(tvec,Det_J,'linewidth',2)
legend('Det(J)')
xlabel('Time (s)');
title('Jacobian determinant')
grid on

figure(6)
plot(tvec,Tou1, tvec,Tou2, tvec,Tou3,'linewidth',2)
legend('Tou1','Tou2','Tou3')
xlabel('Time (s)');
 ylabel('Moment (Nm)');
title('Joint Torques')
grid on

Result Plots:

t=0

t=1

t=2

t=3
 t=4

t=5

Joint Rates
0.3
Theta1d
Theta2d
0.2 Theta3d

0.1

-0.1

-0.2

-0.3

-0.4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)
 Joint Angles
40
Theta1
Theta2
30 Theta3

20

10

-10

-20

-30
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)

Cartesian Pose
9
X
8 Y
Phi
7

0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)
 Jacobian determinant
7.5
Det(J)
7

6.5

5.5

4.5

3.5
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)

Joint Torques
18
Tou1
16 Tou2
Tou3

14

12

10

2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)