% Inverse dynamics of 2R robot.

%

% Robot parameters are:

%  Link 1: length 0.75m, point mass 40kg at midpoint

%  Link 2: length 0.75m, point mass 20kg at midpoint 

%

% Gravity acts in negative Y direction.

%

%   Input data

%

%   TH   - VECTOR 1 X 2 POSITION       FOR EACH JOINT

%   THD  - VECTOR   "   VELOCITY        "   "     " 

%   THDD -   "      "   ACCELERATION    "   "     " 

%

%   Output data

%

%   tau  -   "      "   TORQUES/FORCES  "   "     " 



function [tau, taui, tauc, taug] = idyn2r(TH, THD, THDD)



m = 20;

l = 0.75;

g = 9.81;

c1 = cos(TH(1));

c2 = cos(TH(2));

c12 = cos(TH(1)+TH(2));

s2 = sin(TH(2));



A = m*l^2/4;

B = m*l^2*s2/2;

C = m*g*l/2;





% inertia matrix

H = [ 7+4*c2 1+2*c2 ; 

      1+2*c2 1     ];



taui = A*H*THDD;



% coriolis and centrifugal torques



tauc = B*[-2*THD(1)*THD(2)-THD(2)^2 ;

                    THD(1)^2        ];



% gravity torques



taug = C*[ 4*c1+c12 ;

             c12    ];





% total torque



tau = taui + tauc + taug;



end



