Scribd
Upload
40 views3 pages

Rae Software

The document contains code for calculating the inverse kinematics of a 3-link robotic arm and inverse dynamics of a 2-link manipulator. For the inverse kinematics, it defines DH parameters, takes in joint angles, calculates intermediate values, and solves for the remaining joint angles. For inverse dynamics, it defines link parameters, joint trajectories, calculates the inertia matrix, and solves for joint torques given accelerations and gravity.

Uploaded by

tatenda rupanga
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
40 views3 pages

Rae Software

The document contains code for calculating the inverse kinematics of a 3-link robotic arm and inverse dynamics of a 2-link manipulator. For the inverse kinematics, it defines DH parameters, takes in joint angles, calculates intermediate values, and solves for the remaining joint angles. For inverse dynamics, it defines link parameters, joint trajectories, calculates the inertia matrix, and solves for joint torques given accelerations and gravity.

Uploaded by

tatenda rupanga
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 3

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)')

You might also like