% Esercitazione sulla notazione DH
% Consideriamo il seguente robot:
%
% La tabella dei paraemtri DH che otteniamo analizzando il robot (è stata riportata anche nelle slide) è la seguente
%
% Creiamo prima una funzione che generi la matrice DH a partire da dai 4 parametri a_i, alfa_i, d_i e theta_i
% che inseriamo nel file create_DH.m (vedi teoria nelle slide)
% A questo punto fissiamo 3 valori delle coordinate indipendenti

theta_1=30*pi/180;
theta_2=10*pi/180;         % valori espressi in radianti
theta_3=10*pi/180;

% Inseriamo anche i parametri geometrici

a_1=0;              % % [m]
a_2=0.7;        
a_3=0.5;  
alfa_1=pi/2;
alfa_2=0;
alfa_3=0;
d_1=0;
d_2=0;
d_3=0;

% Adesso possiamo costruire tutte e tre le matrici di rototraslazione

T_0_1=create_DH(a_1, alfa_1, d_1, theta_1)
T_1_2=create_DH(a_2, alfa_2, d_2, theta_2)
T_2_3=create_DH(a_3, alfa_3, d_3, theta_3)

% Passiamo adesso alla rappresentazione grafica del risultato
% Disegnamo per prima cosa un sistema di riferimento che ci aiuta a capire visivamente l'assonometria

hold on
plot3([0,1],[0,0],[0,0],'r')
plot3([0,0],[0,1],[0,0],'r')
plot3([0,0],[0,0],[0,1],'r')
view(3)                         % serve per vedere in 3D il risultato

%
% Ricaviamo le coordinate dei punti (in rosso in figura)

P_0=[0,0,0,1]'            % è l'origine del sistema di riferimento e coincide con P_1
P_1=P_0
P_2=T_0_1*T_1_2*[0,0,0,1]'
P_3=T_0_1*T_1_2*T_2_3*[0,0,0,1]'

% Si passa adesso alla grafica dei link del robot

plot3([P_1(1),P_2(1)],[P_1(2),P_2(2)],[P_1(3),P_2(3)],'b')
plot3([P_2(1),P_3(1)],[P_2(2),P_3(2)],[P_2(3),P_3(3)],'b')

% Arrivati a questo punto si può verificare la cinematica graficamente modificando i valori delle coordinate libere

