% test_kininv2R
% Programmino per testare kininv2R
% C. Manes

a1=3; a2=2; a3=2;

pinza

% Generazione casuale angoli tra -pi e pi
theta1=pi*2*(rand(1,1)-0.5);
theta2=pi*2*(rand(1,1)-0.5);
theta3=pi*2*(rand(1,1)-0.5);

phi=theta1+theta2+theta3;

c1=cos(theta1); c12=cos(theta1+theta2); c123=cos(phi);
s1=sin(theta1); s12=sin(theta1+theta2); s123=sin(phi);

theta=[theta1; theta2; theta3];

px=a1*c1+a2*c12+a3*c123;
py=a1*s1+a2*s12+a3*s123;
p=[px; py];

th=kininv3R(p,phi,a1,a2,a3);

thD=th(:,1); thU=th(:,2);

figure(1)
[p,phi]=kindir3R(theta,a1,a2,a3,'plot'), grid
axis([-1 1 -1 1]*(a1+a2+a3)*1.1), % axis equal
axis('square')


figure(2)
[pU,phiU]=kindir3R(thU,a1,a2,a3,'plot'), grid
axis([-1 1 -1 1]*(a1+a2+a3)), % axis equal
axis('square')
set(2,'Position',[600   250   600   470])

hold on
[pD,phiD]=kindir3R(thD,a1,a2,a3,'plot'), 


T=[ c123 -s123 px;
    s123  c123 py;
      0     0   1];
  
P3=T*P;
plot(P3(1,:),P3(2,:),'r','linewidth',2)


hold off





