MATLAB

Vengono di seguito riportati alcuni estratti significativi dal codice MATLAB preparato.
Per la visione e l'analisi dell'algoritmo completo si rimanda alla presentazione del progetto.

 Parametri in ingresso
 
 X,Y,Z = le coordinate dell'origine della terna obiettivo, espresse rispetto alla terna mondo
 thetaz1DEG, thetax1DEG, thetaz2DEG = gli angoli di Eulero della terna obiettivo, espressi in gradi
 In particolare:
 thetaz1 = angolo di precessione
 thetax1 = angolo di nutazione
 thetaz2 = angolo di rotazione propria
 
 
 Parametri in uscita
 
theta1DEG, d2, d3 = parametri controllati direttamente dal primo manipolatore a 3DoF (una rotazione e due traslazioni)
 
 alphaDEG, deltaDEG, thetazDEG, thetaxDEG = parametri indirettamente controllati dalla posizione reciproca dei due manipolatori a 3 DoF
 
THETA2DEG, D3, D4 = parametri controllati direttamente dal secondo manipolatore a 3DoF (una rotazione e due traslazioni)

Ricordiamo che l'obiettivo finale del progetto è la definizione della cinematica inversa del sistema, ovvero di una funzione che calcoli i parametri di movimento da assegnare ai due Haptic Masters affinchè la piattaforma end effector assuma la posizione e l'orientazione volute.
 
 
 
 Calcolo della terna obiettivo a partire dai paramentri in ingresso
 
 thetaz1 = fromDegrees('radians', thetaz1DEG);
 thetax1 = fromDegrees('radians', thetax1DEG);
 thetaz2 = fromDegrees('radians', thetaz2DEG);
 
 Traslazione = [1 0 0 X
                0 1 0 Y
                0 0 1 Z
                0 0 0 1];
 
 Rotazione_z1 = [cos(thetaz1) -sin(thetaz1) 0 0
                 sin(thetaz1) cos(thetaz1)  0 0
                 0            0             1 0
                 0            0             0 1];
 
 Rotazione_x1 = [1 0            0             0
                 0 cos(thetax1) -sin(thetax1) 0
                 0 sin(thetax1) cos(thetax1)  0
                 0 0            0             1];
 
 Rotazione_z2 = [cos(thetaz2) -sin(thetaz2) 0 0
                 sin(thetaz2) cos(thetaz2)  0 0
                 0            0             1 0
                 0            0             0 1];
 
 T9 = Traslazione*Rotazione_z1*Rotazione_x1*Rotazione_z2;
 

 T9 è la terna obiettivo.

A questo punto viene lanciato il ciclo per la determinazione degli angoli alpha e delta. Una volta trovati questi angoli si risale tramite matrici di traslazione e premoltiplicazioni alla terna finale dei due bracci robotici. I due Haptic Masters, avendo solo 3 DoF ciascuno, hanno una cinematica inversa molto semplice che può essere scritta in forma chiusa.

Cinematica inversa dell'Haptic Master

Parametri in ingresso:
x,y,z = le coordinate dell'end effector rispetto alla terna mondo

Parametri in uscita:
theta1inv,d2,d3 = l'angolo di rotazione e le due traslazioni, ovvero l'entità dei movimenti controllati dal manipolatore a 3 DoF.

Dati:
h = 830;

Calcolo valori parametri:

theta1inv = atan(y/x);

theta1invDEG = toDegrees( 'radians', theta1inv );

d2inv = z - h;

d3inv = sqrt(x^2+y^2);

Valutazione dell'errore

Con la cinematica diretta è stata calcolata la terna a cui si perviene utilizzando i parametri individuati dall'algoritmo. Questa terna, a causa degli errori di approssimazione, non coinciderà esattamente con la terna obiettivo determinata dai parametri in ingresso.

Si stima l'errore in due modi:

1. Errore di posizione: la distanza tra le origini delle due terne.
2. Errore di orientazione: 3 meno la somma dei valori assoluti dei prodotti scalari tra le tre coppie di assi corrispondenti. Infatti se le due terne coincidessero il risultato sarebbe proprio 3.

errPos = sqrt( (T9(1,4)-TC(1,4))^2 + (T9(2,4)-TC(2,4))^2 + (T9(3,4)-TC(3,4))^2 );

errOrient = 3 - ( abs(T9(:,1)'*TC(:,1)) + abs(T9(:,2)'*TC(:,2)) + abs(T9(:,3)'*TC(:,3)) );

Stampa delle terne notevoli

E' stata utilizzata la funzione plot3 di MATLAB, che permette di stampare grafici 3D.
I parametri in ingresso sono una gnerica matrice (4,4) rappresentante una terna e s, il fattore di scala per la rappresentazione degli assi cartesiani.

function stampaternaMC(M,s)

plot3( [M(1,4) M(1,4)+s*M(1,1)] , [M(2,4) M(2,4)+s*M(2,1)] , [M(3,4) M(3,4)+s*M(3,1)] , 'm' ) %vettore x
hold on
plot3( [M(1,4) M(1,4)+s*M(1,2)] , [M(2,4) M(2,4)+s*M(2,2)] , [M(3,4) M(3,4)+s*M(3,2)] , 'g' ) %vettore y
plot3( [M(1,4) M(1,4)+s*M(1,3)] , [M(2,4) M(2,4)+s*M(2,3)] , [M(3,4) M(3,4)+s*M(3,3)] , 'c' ) %vettore z

axis ( [-1000 1000 -1000 1000 0 2000 ] )
grid on
xlabel('x')
ylabel('y')
zlabel('z')