Esercitazione

Gli applet che riguardano l'esercitazione mettono a disposizione uno schema del CLIK dove i blocchi per la pianificazione della traiettoria, la cinematica diretta e la cinematica diff. inversa sono vuoti. Affinché il CLIK funzioni lo studente deve scrivere il codice che realizza le funzioni dei blocchi (figura 5.9).

Figure 5.9: Schema del CLIK per l'esercitazione

Facciamo un esempio.

Prendiamo ancora il manipolatore planare a 3 bracci dell'esempio precedente 5.1.1e cerchiamo di riprodurre nuovamente i risultati presentati nel paragrafo 3.7.4 di [Lorenzo Sciavicco(2000)], ma questa volta in maniera esatta.

Implementiamo l'algoritmo per l'inversione cinematica che utilizza l'inversa dello Jacobiano.

Accediamo all'applet ricalcando la procedura precedente e una volta caricata l'applicazione, si noterà che il pannello laterale è cambiato; sono presenti le funzioni pian.traiettoria, cinemat.diretta,cinemat.inversa che se attivate, aprono altrettante finestre Java (figura cap:Esercitazione,-pannello-laterale ). Nelle finestre è possibile scrivere il codice del linguaggio Matlab che realizza la funzione che ad ognuna di esse è associata:

Figure 5.10: Esercitazione, funzioni del pannello laterale.

• pianificazione della traiettoria, codice che realizza la traiettoria desiderata dell'organo terminale, posizione e orientamento; $x_{d}$, $\dot{x}_{d}$;
• cinematica diretta, codice che realizza la funzione $k(q)$;
• cinematica inversa, codice che realizza la funzione $\dot{q}=\dot{q}(e)$, in questo caso $\dot{q}=J_{A}^{-1}(q)\left(\dot{x}_{d}+Ke\right).$

Il codice da inserire in ciascuna finestra è il seguente:

• pianificazione della traiettoria,

  
  

  function sys = mdlOutputsTrj(t,x,u) 

  % funzione per la pianificazione della traiettoria desiderata

  % vettori di ingresso u,t:

  %  u(1) istante finale.

  %  t istante corrente.

  % vettore (colonna) d'uscita

  %      sys=traiettoria desiderata della terna utensile

  %          pos e orien e derivate.

  % *** inserisci qui il tuo codice ***

     tf= u(1);             % istante finale

     %calcolo della traiettoria desiderata

     if (t >= 4.0)   

        xd= [0; 0.5; 0.5];

        Dxd= [0; 0; 0]; 

     else 

        xd= [0.25*(1-cos(pi*t)); 0.25*(2+sin(pi*t)); sin(pi/24*t)];

        Dxd= [0.25*pi*sin(pi*t); 0.25*pi*cos(pi*t); pi/24*cos(pi/24*t)];

     end;

     sys= [xd; Dxd];

• cinematica diretta

  
  

  function sys = mdlOutputsDirKine(t,x,u)

  % funzione per la cinematica diretta

  % vettore d'ingresso u:

  %  u(1:3) posizione dei giunti.

  % vettore (colonna) d'uscita

  %    sys=posizione e orientamento della terna utensile

  % *** inserisci qui il tuo codice ***

  % u(1)-> teta1

  % u(2)-> teta2

  % u(3)-> teta3

  a(1)=.5;

  a(2)=.5;

  a(3)=.5;

  c1=  cos(u(1));

  c12= cos(u(1)+u(2));

  c123= cos(u(1)+u(2)+u(3));

  s1=  sin(u(1));

  s12= sin(u(1)+u(2));

  s123= sin(u(1)+u(2)+u(3));

  sys= [a(1)*c1+a(2)*c12+a(3)*c123;...

        a(1)*s1+a(2)*s12+a(3)*s123;...

        u(1)+u(2)+u(3)]; 

• cinematica inversa

  
  

  function sys = mdlOutputsInvKine(t,x,u)

  % funzione per l'inversione cinematica

  % vettore d'ingresso u:

  %     u(1:3) errore;

  %     u(4:6) posizione dei giunti;

  %     u(7:9) velocità desiderata

  % vettore (colonna) d'uscita

  %     sys=velocità dei giunti

  % *** inserisci qui il tuo codice ***




   K=diag([500,500,100]);

   % algoritmo con inversa dello jacobiano

    J=J_a(u(4:6));

    % calcolo del prodotto inv(Ja(q))*u

    dq = J\(u(7:9)+K*u(1:3)); 

    sys= dq;

  % end mdlOutputs

  function Ja = J_a(u)

  % funzione per il calcolo dello Jacobiano

  % u(1) -> teta1

  % u(2) -> teta2

  % u(3) -> teta3

  a(1)=.5;

  a(2)=.5;

  a(3)=.5;

  s12= a(2)*sin(u(1)+u(2));

  s123=a(3)*sin(u(1)+u(2)+u(3));

  c12= a(2)*cos(u(1)+u(2));

  c123=a(3)*cos(u(1)+u(2)+u(3));

  Ja=[-a(1)*sin(u(1))-s12-s123, -s12-s123, -s123;...

      a(1)*cos(u(1))+c12+c123, c12+c123, c123;...

                             1,        1,    1]; 

Prima di far partire la simulazione con la funzione avvio del pannello laterale, impostiamone la durata di 5 secondi^5.2. Se non sono stati compiuti errori, al termine della simulazione il robot si animerà riproducendo il percorso pianificato. Guardando i grafici risultanti (figura cap:Esercitazione.-Manipolatore-planare), funzione PLOT , si troverà uguale corrispondenza nel confronto con quelli riportati sul libro di testo .

Figure 5.11: Esercitazione. Manipolatore planare a 3 bracci, grafici delle variabili dell'algoritmo con inversa dello Jacobiano