% inverse dynamics of a 2R robot.

%

% This routine was generated using the symbolic generator NEDYN

%

% 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 2 X 1 POSITION       FOR EACH JOINT

%   THD  - VECTOR   "   VELOCITY        "   "     " 

%   THDD -   "      "   ACCELERATION    "   "     " 

%

%   Output data

%

%   tau  -   "      "   TORQUES/FORCES  "   "     " 



function tau  =  r2idyn(TH,THD,THDD)



      g = 9.81;

      a1 = 0.75000;

      C1 = cos(TH(1));

      S1 = sin(TH(1));

      QD1 = THD(1);

      QF1 = THDD(1);

      sx1 =  -0.375000;

      a2 = 0.75000;

      C2 = cos(TH(2));

      S2 = sin(TH(2));

      QD2 = THD(2);

      QF2 = THDD(2);

      sx2 =  -0.375000;

      j22 =     0.000500;

      x001 =     7.500000;

      x002 =    15.000000;

      x003 =    15.000000;

      Z0001 = -QD1*QD1*a1+S1*g;

      Z0002 = C1*g+QF1*a1;

      Z0004 = QF1*sx1+Z0002;

      Z0005 = QD1+QD2;

      Z0006 = QF1+QF2;

      Z0007 = C2*Z0001+S2*Z0002;

      Z0008 = C2*Z0002-S2*Z0001;

      Y001 = Z0005*Z0005;

      Z0009 = -Y001*a2+Z0007;

      Z0010 = Z0006*a2+Z0008;

      Z0011 = -Y001*sx2+Z0009;

      Z0012 = Z0006*sx2+Z0010;

%    *** DYNAMICS CALCULATIONS ***

      Z0013 = Z0006*j22+Z0012*x001;

      Z0018 = x002*(C2*Z0012+S2*Z0011);

      Z0019 = Z0013+Z0018;

      Z0020 = Z0004*x003+Z0019;

%

%   THE GENERALIZED JOINT FORCES ARE

%

      tau(1) = Z0020;

      tau(2) = Z0013;



end

