load proba.dat -ascii;
pom=proba(2:numrows(proba),:);

t=[0:5e-3:(numrows(pom)-1)*5e-3]';

q1=pom(:,2);
q2=pom(:,3);
q1t=pom(:,4);
q2t=pom(:,5);
q1tt=pom(:,6);
q2tt=pom(:,7);
u1=pom(:,8);
u2=pom(:,9);
dt=5e-3;

q1d=[0];
q1dd=[0];

for ii=2:numrows(q1),
  q1d=[q1d; (q1(ii,:)-q1(ii-1,:))/dt];
  q1dd=[q1dd; (q1d(ii,:)-q1d(ii-1,:))/dt];
end

figure(1);
plot(t,q1, t,q1t,':', t, q1tt,'-.');
axis([0 5 -1 1]);

figure(2);
plot(t,q1, t,q1d,':', t, q1dd,'-.');
axis([0 5 -1 1]);

R1=1.35;
Kem1=0.1;
Kme1=Kem1;
N1=35;
l1=0.42;
m1=3.312;
J1=122e-6;

R2=8;
Kem2=48/(38*69);
Kme2=Kem2;
N2=69;
l2=0.28;
m2=0.42;
J2=2.0964e-6;

for ii=1:numrows(q1),

  D11 =1/3*m1*l1^2+1/3*m2*l2^2+m2*l2*l1*cos(q2(ii))+m2*l1^2;
  D12=1/3*m2*l2^2+1/2*m2*l2*l1*cos(q2(ii));
  D21=1/3*m2*l2^2+1/2*m2*l2*l1*cos(q2(ii));
  D22=1/3*m2*l2^2;

  h1=-q1t(ii)*q2t(ii)*m2*l2*l1*sin(q2(ii))-1/2*q2t(ii)^2*m2*l2*l1*sin(q2(ii));
  h2=1/2*m2*l2*l1*q1t(ii)^2*sin(q2(ii));

  u1nom(ii)=(D11+J1*N1^2)/(N1*Kem1/R1)*q1tt(ii)+D12/(N1*Kem1/R1)*q2tt(ii)+(h1+N1^2*Kem1*Kme1/R1*q1t(ii))/(N1*Kem1/R1);
  u2nom(ii)=D21/(N2*Kem2/R2)*q1tt(ii)+(D22+J2*N2^2)/(N2*Kem2/R2)*q2tt(ii)+(h2+N2^2*Kem2*Kme2/R2*q2t(ii))/(N2*Kem2/R2);

end

u1nom=u1nom;
u2nom=u2nom;

figure(3);
plot(t,u1, t,u2,':');

figure(4);
plot(t,u1nom, t,u2nom,':');

