function [M_01,T] = InvDyn3dof( A,q,L,R,M,go )

global CM0x CM0y CM1y 

%-- Constants and definitions ---------------------------------------------
  l0 = L(1);  l1 = L(2);  l2 = L(3);
  r0 = R(1);  r1 = R(2);  r2 = R(3);
  m0 = M(1);  m1 = M(2);  m2 = M(3);
  
  q1 = q(1);     q2 = q(2);     q3 = q(3);
  s1 = sin(q1); s2 = sin(q2); 
  c1 = cos(q1); c2 = cos(q2); 
  s23 = sin(q2+q3); c23 = cos(q2+q3);
  
 ddx0 = A(1,1); ddy0 = A(1,2); ddz0 = A(1,3);
 ddx1 = A(2,1); ddy1 = A(2,2); ddz1 = A(2,3);
 ddx2 = A(3,1); ddy2 = A(3,2); ddz2 = A(3,3);

%-- Compute forces and moments @m2 ----------------------------------------
 f_12 = m2*[ddx2 ddy2 ddz2+go]';
 r_1c2 = [r2*s23 -r2*s1*c23 r2*c1*c23]';
 M_12 = cross(r_1c2,f_12);
%-- Compute forces and moments @m1 ----------------------------------------
 f_01 = f_12+m1*[ddx1 ddy1 ddz1+go]';
 r_01 = [l1*s2 -l1*s1*c2 l1*c1*c2]';
 r_0c1 = [r1*s2 (-r1*s1*c2+CM1y*c1) (r1*c1*c2+CM1y*s1)]';
 M_01 = M_12+cross(r_01,f_12)+m1*cross(r_0c1,[ddx1 ddy1 ddz1+go]');
%-- Compute the GROUND REACTION FORCE -------------------------------------
% GRF = f_01-m0*[0 0 -go]';
%-- Compute the JOINT TORQUE VECTOR --------------------------------------- 
 T(1) = M_01(1);
 if abs(M_01(2)) < 1e-10 && abs(M_01(3)) < 1e-10,
     T(2) = 0;
 else
     [max_val,I] = max(abs(M_01(2:3))); I = I+1;
     if I == 2, T(2) = M_01(I)*c1; else T(2) = M_01(I)*s1; end     
 end
 if abs(M_12(2)) < 1e-10 && abs(M_12(3)) < 1e-10,
     T(3) = 0;
 else
     [max_val,I] = max(abs(M_12(2:3))); I = I+1;
     if I == 2, T(3) = M_12(I)*c1; else T(3) = M_12(I)*s1; end     
 end
 T = T';
%  T_ideal = go*[ (m1*(-r1*s1*c2+CM1y*c1)+m2*(-l1*s1*c2-r2*s1*c23));...
%                 -(m1*r1*c1*s2+m2*(l1*c1*s2+r2*c1*s23));...
%                 -(m2*r2*c1*s23) ]
%-- Compute the CENTRE OF PRESSURE (COP=ZMP) ------------------------------ 
% COP(1) = ( -M_01(2)+m0*go*CM0x-l0*GRF(1) )/GRF(3);
% COP(2) = (  M_01(1)+m0*go*CM0y-l0*GRF(2) )/GRF(3);
%-----------------------------------------------------------------------end             
end