% test_kininv2R
% Programmino per testare kininv2R

% C. Manes. Esercitazione del 10 novembre 2009

close all, hold off

a1=3; a2=2;

pinza
% triangolo

% Generazione casuale angoli tra -pi e pi
theta1=pi*2*(rand(1,1)-0.5);
theta2=pi*2*(rand(1,1)-0.5);
c1=cos(theta1); c12=cos(theta1+theta2);
s1=sin(theta1); s12=sin(theta1+theta2);

theta=[theta1; theta2];

px=a1*c1+a2*c12;
py=a1*s1+a2*s12;

th=kininv2R(px,py,a1,a2);

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

figure(1)
p=kindir2R(theta,a1,a2,'plot'); grid
axis equal
set(1,'Position',[0 150 500 500])
hold on

figure(2)
pU=kindir2R(thU,a1,a2,'plot'); grid

set(2,'Position',[490 150 500 500])
hold on
pD=kindir2R(thD,a1,a2,'plot');
axis equal

T=[ c12 -s12 px;
    s12  c12 py;
    0     0   1];
  
P2=T*P;
figure(1)
plot(P2(1,:),P2(2,:),'r','linewidth',2)

figure(2)
plot(P2(1,:),P2(2,:),'r','linewidth',2)

x_lim=get(gca,'XLim'); 
y_lim=get(gca,'YLim');
DM=max([abs(x_lim) abs(y_lim)]);

figure(1), axis([-1 1 -1 1]*DM*1.2)
figure(2), axis([-1 1 -1 1]*DM*1.2)

hold off


