L1=9.5;
L2=9.5;

% Ganhos do controlador
K=[1;
   1;
   1]*0.05;

% Coordenadas finais a atingir
xyz_final=[0;
           0;
           L1];

% Posições das juntas actuais
theta=[ 0;
       -10;
        20]*pi/180;

xyz_list=[];

figure('doublebuffer','on');
while (1)
    % Cálculo da cinemática directa
    xyz_knee=[ L1*theta(1);
              -L1*sin(theta(2));
               L1*cos(theta(2))];
    xyz=[(L1+L2)*theta(1);
         -L1*sin(theta(2))-L2*sin(theta(2)+theta(3));
          L1*cos(theta(2))+L2*cos(theta(2)+theta(3))];
    xyz_list=[xyz_list; xyz'];
    
    plot([0 xyz_knee(2)],[0 xyz_knee(3)],'b','linewidth',4);
    hold on
    plot([xyz_knee(2) xyz(2)],[xyz_knee(3) xyz(3)],'g','linewidth',4);
    plot(xyz_list(:,2),xyz_list(:,3),'r--')
    hold off
    axis([-1.1 1.1 -0.1 1.1]*(L1+L2));
    grid on

    % Cálculo do Jacobiano
    J=[(L1+L2)*cos(theta(1))                    0                                   0             ;
                0           -L1*cos(theta(2))-L2*cos(theta(2)+theta(3)) -L2*cos(theta(2)+theta(3));
                0           -L1*sin(theta(2))-L2*sin(theta(2)+theta(3)) -L2*sin(theta(2)+theta(3))];

    Jt=inv(J);

    % Cálculo do erro das coordenadas cartesianas
    error=xyz_final-xyz

    % Actualização das posições angulares a aplicar
    theta=theta+K.*(Jt*error);
    
    pause(0.1);
end