Prog1

% Program for inverse kinematics of 3-link arm

% Non-zero constant DH parameters.
a1 = 2; a2 = 2; a3 = 1;

% Input
phi = pi/3;
px = 2.5 + sqrt(3);
py = 1 + sqrt(3)/2;

% Intermediate Calculations
wx = px - a3*cos(phi);
wy = py - a3*sin(phi);
del = wx*wx + wy*wy;

% Calculations for theta_2

c2 = (del - a1*a1 - a2*a2)/(2*a1*a2);
s2 = sqrt(1 - c2*c2);
th21 = atan2(s2,c2);
th22 = atan2(-s2,c2);

% Calculation for finding theta_1

s11 = ((a1 + a2*cos(th21))*wy - a2*s2*wx)/del;
c11 = ((a1 + a2*cos(th21))*wx - a2*s2*wy)/del;
s12 = ((a1 + a2*cos(th22))*wy + a2*s2*wx)/del;
c12 = ((a1 + a2*cos(th22))*wx + a2*s2*wy)/del;
th11 = atan2(s11,c11);
th12 = atan2(s12,c12);

% Calculation for theta_33

th31 = phi - th11 - th21;
th32 = phi - th12 - th22;

% Angles in degree
r2d = 180/pi;
th11d = th11*r2d;
th12d = th12*r2d;
th21d = th21*r2d;
th22d = th22*r2d;
th31d = th31*r2d;
th32d = th32*r2d;
% Program for inverse kinematics of 3-link arm
% Non-zero constant DH parameters.
a1 = 2; a2 = 2; a3 = 1;

% Input
phi = pi/3;
px = 2.5 + sqrt(3);
py = 1 + sqrt(3)/2;

% Intermediate Calculations
wx = px - a3*cos(phi);
wy = py - a3*sin(phi);
del = wx*wx + wy*wy;

% Calculations for theta_2

c2 = (del - a1*a1 - a2*a2)/(2*a1*a2);
s2 = sqrt(1 - c2*c2);
th21 = atan2(s2,c2);
th22 = atan2(-s2,c2);

% Calculation for finding theta_1

s11 = ((a1 + a2*cos(th21))*wy - a2*s2*wx)/del;
c11 = ((a1 + a2*cos(th21))*wx - a2*s2*wy)/del;
s12 = ((a1 + a2*cos(th22))*wy + a2*s2*wx)/del;
c12 = ((a1 + a2*cos(th22))*wx + a2*s2*wy)/del;
th11 = atan2(s11,c11);
th12 = atan2(s12,c12);

% Calculation for theta_33

th31 = phi - th11 - th21;
th32 = phi - th12 - th22;

% Angles in degree
r2d = 180/pi;
th11d = th11*r2d;
th12d = th12*r2d;
th21d = th21*r2d;
th22d = th22*r2d;
th31d = th31*r2d;
th32d = th32*r2d;

Prog2
% Inverse dynamics for Two-Link Manipulator
% Input for trajectory and link parameters
T = 10;
th1T = pi;
th10 = 0;
th2T = pi/2;
th20 = 0;
m1 = 1;
a1 = 1;
m2 = 1;
a1 = 1;
a2 = 1;
g = 9.81;

con = 2*pi/T;
delth1 = th1T - th10;
delth2 = th2T - th20;
iner21 = m2*a1*a2;

for i = 1:51
ti(i) = (i - 1)*T/50;
ang = con*ti(i);

% Joint trajectory
th1(i) = th10 + (delth1/T)*(ti(i) - sin(ang)/con);
th1d(i) = delth1*(1 - cos(ang))/T;
th1dd(i) = delth1*con*sin(ang)/T;
th2(i) = th20 + (delth2/T)*(ti(i) - sin(ang)/con);
 th2d(i) = delth2*(1 - cos(ang))/T;
th2dd(i) = delth2*con*sin(ang)/T;
thdd = [th1dd(i), th2dd(i)];

% Inertia matrix
sth2 = sin(th2(i));
cth2 = cos(th2(i));
i22 = m2*a2*a2/3;
i21 = i22 + iner21*cth2/2;
i12 = i21;
i11 = i22 + m1*a1*a1/3 + m2*a1*a1 + iner21*cth2;

% Inverse dynamics
g1 = (m1*a1 + m2*a1)*g*cos(th1(i)) + m2*a2*g*cos(th1(i) + th2(i));
g2 = m2*a2*g*cos(th1(i) + th2(i));
f1 = i11*th1dd(i) + i12*th2dd(i) + g1;
f2 = i21*th1dd(i) + i22*th2dd(i) + g2;
tau1(i) = f1;
tau2(i) = f1 + f2;
end

% Plotting joint torques

figure
subplot(2,1,1)
plot(ti, tau1)
title('Joint Torque 1')
xlabel('Time (s)')
ylabel('Torque (Nm)')
subplot(2,1,2)
plot(ti, tau2)
title('Joint Torque 2')
xlabel('Time (s)')
ylabel('Torque (Nm)')