function thdot = d2P(t,th)
global th_r;
global th_s;
global L1;
global L2;
global tau

% dynamics of 2link arm for smith predictior
% 
% 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));


%%%%%%%%%%%%%%%%%%%%%%%%%
% 2-link arm dynamics

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