clear all
clf
%% Cinematica inversa di un semplice robot 2D.
%L'obiettivo è realizzare un robot con tre coppie rotoidali con assi
%paralleli (robot piano) che produca con l'end-effector una traiettoria circolare.
%Per fare ciò viene definito un errore e=xdesiderato-xeffettivo, 
%che deve essere minimizzato.

%Parametri del cerchio (percorso del robot)
omega=1;
r=10;

%Parametri del robot, secondo Denavit-Hartemberg
a=[30,30,3];
alfa=[0,0,0];
d=[0,0,0];
q(:,1)=[pi/3,pi/3,pi/3];  % q è il parametro variabile, che deve essere 
                          % questo è il parametro di configurazione iniziale del robot

% Matrice che minimizza l'errore; attenzione ai valori sulla diagonale:
% valori bassi => convergenza lenta; valori elevati => instabilità(es. con 100)                          
K=1.5*eye(3);


%Parametri del loop
T_f=10;                     % tempo finale
deltat=0.1;                 % passo di iterazione
n_inc=round(T_f/deltat)     % numero di passi
i=1;                        % inizializzazione di un contatore
t=0;                        % inizializzazione di t

figure(1)                   % impostazione della grafica
hold on
axis equal
axis([-40 40 -20 60])       % limite degli assi

%% LOOP: 
%qua dentro vengono aggiornati ciclicamente la posizione xd desiderata e 
%quella P_3 effettiva, in modo da minimizzare l'errore
for i=1:n_inc

    % DEFINIZIONE DEL ROBOT: trovo i vari punti che lo compongono

    %Le matrici H-D fanno riferimento ad una funzione creata ad hoc per avere
    %la matrice direttamente a partire dai parametri di Denavit Hartenberg.
    A_0=create_DH(a(1),alfa(1),d(1),q(1,i));
    A_1=create_DH(a(2),alfa(2),d(2),q(2,i));
    A_2=create_DH(a(3),alfa(3),d(3),q(3,i));
    
    P_i=[5 6 0 0]';        % Posizione della base del robot del robot;
    P_0=[0 0 0 1]';         % coordinate delll'origine di un sistema di riferimento
    P_1=A_0*P_0+P_i;            % coordinate dei punti che costituiscono il robot
    P_2=A_0*A_1*P_0+P_i;
    P_3=A_0*A_1*A_2*P_0+P_i;

    %PUNTO EFFETTIVO
    P_3new=[P_3(1) P_3(2) q(1,i)+q(2,i)+q(3,i)]';% Punto dell'end effector 
                                                 % calcolato come (x,y,phi) 
    

    %GRAFICO: Eventuale rappresentazione del robot in una sua configurazione,
    %altrimenti si vede solo l'ultima.

    figure(1)
    hold on
    axis equal
    axis([-40 40 -20 60])       % limite degli assi
    plot3([P_i(1) P_1(1)],[P_i(2) P_1(2)],[P_i(3) P_1(3)],'k');
    plot3([P_1(1) P_2(1)],[P_1(2) P_2(2)],[P_1(3) P_2(3)],'k');
    plot3([P_2(1) P_3(1)],[P_2(2) P_3(2)],[P_2(3) P_3(3)],'k');
%     pause
%     clf


    % traiettoria desiderata dell'end-effector
    % (cerchio di raggio r percorso a velocità angolare costante omega)
    xd(:,i)=[r*cos(omega*t);r*sin(omega*t);omega*t];                        %xd:posizione desiderata
    xd_grad(:,i)=[-r*sin(omega*t)*omega;r*cos(omega*t)*omega;omega];        %gradiente di xd

    % ERRORE
    e=xd(:,i)-P_3new; %differenza tra punto desiderato ed effettivo, calcolati 
                      %come (x,y,phi), con phi=q1+q2+q3 per P_3new.
    

    % JACOBIANO ANALITICO: è la derivata dell'ultima colonna della matrice di rototraslazione A_30
    J=[-a(1)*sin(q(1,i))-a(2)*sin(q(1,i)+q(2,i))-a(3)*sin(q(1,i)+q(2,i)+q(3,i)) -a(2)*sin(q(1,i)+q(2,i))-a(3)*sin(q(1,i)+q(2,i)+q(3,i)) -a(3)*sin(q(1,i)+q(2,i)+q(3,i));
        a(1)*cos(q(1,i))+a(2)*cos(q(1,i)+q(2,i))+a(3)*cos(q(1,i)+q(2,i)+q(3,i))  a(2)*cos(q(1,i)+q(2,i))+a(3)*cos(q(1,i)+q(2,i)+q(3,i))  a(3)*cos(q(1,i)+q(2,i)+q(3,i));
        1 1 1];

    % ANGOLI Q: aggiornamento degli angoli secondo il concetto di derivata:
    %q_grad=(q(i+1)-q(i))/deltat. In questo modo ricavo q(i+1) da usare nel
    %loop successivo.
    q_grad=J^(-1)*(xd_grad(:,i)+K*e);   % q_punto
    
    % integrazione di q a partire da q_punto
    q(:,i+1)=q(:,i)+q_grad.*deltat;     % valore di q al passo successivo


    P(:,i)=[P_3new(1:2)]'; %memorizzo x e y di P_3 da plottare poi

    % ANDAMENTO ERRORE: verifico l'andamento dell'errore calcolando
    % la distanza tra xd e P_3 al variare del tempo
    e_dist(i)=sqrt((xd(1,i)-P_3(1))^2+(xd(2,i)-P_3(2))^2);
    T(i)=t;
    if e_dist(i)>0.2
        count=i; %contatore per capire dopo quante iterazioni la distanza è < di 0.2
    end
    
    i=i+1;
    t=t+deltat;
end

T_conv=count*deltat; %tempo necessario a convergere, tenere conto che in t=pi arriva a pi il cerchio
plot(xd(1,:),xd(2,:));
axis equal
hold on
plot(P(1,:),P(2,:));
plot3([P_i(1) P_1(1)],[P_i(2) P_1(2)],[P_i(3) P_1(3)]);
plot3([P_1(1) P_2(1)],[P_1(2) P_2(2)],[P_1(3) P_2(3)]);
plot3([P_2(1) P_3(1)],[P_2(2) P_3(2)],[P_2(3) P_3(3)]);


figure
plot(T,e_dist);
disp(T_conv)