function thdot = d2(t,th)
global th_r;
global th_h;
global L1;
global L2;
global tau;

% dynamics of 2link arm
% 
% th_r: desired state in angular space
% th_h: history of internal state
%
% Hiroaki Gomi, 21/Jul/2000

tf = 0.001;

l1 = L1;
l2 = L2;

z1 = 0.327;
z2 = 0.109;
z3 = 0.106;

S2 = sin(th(2));
C2 = cos(th(2));

delay_t = 0.040; % sec

delay = delay_t/tf;
% FbGain = [25,0,25,0; 0,25,0,25];
FbGain = [8,0,3,0; 0,8,0,3];
%FbGain = [5,0,1,0; 0,5,0,1];


%%%%%%%%%%%%%%%%%%%%%%%%%
% 2-link arm dynamics
%tau = tau_fb + tau_smith + tau_ff;

I = [z1+2*z2*C2, z3+z2*C2; z3+z2*C2, z3];
th_dd = inv(I)*(tau - [-z2*S2*(th(4)^2+2*th(3)*th(4)), z2*th(3)^2*S2]');

thdot = [th(3)  th(4)  th_dd(1)  th_dd(2)]';