Dipartimento di Meccanica Politecnico di Torino

Stefano Pastorelli

ESERCITAZIONE LAIB
Esempio codice di calcolo

Struttura multi-body a 2 gdl

parametri di Denavit-Hartenberg

esempio LAIB_ver01.doc

link

[rad]

a [m]

d [m]

[rad]

q1

0.5

q2

Dipartimento di Meccanica Politecnico di Torino

Stefano Pastorelli

file Matlab
% esempio_01.m
%
% Esempio codice di calcolo per struttura multi-body a 2 gdl
% autore: Stefano Pastorelli
close all
clear all
% dati sistema multi-body
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
alfa1=0;
a1=0;
d1=1;

%parametri di Denavit-Hartenberg costanti link 1

alfa2=0;
a2=2;
d2=0.5;

%parametri di Denavit-Hartenberg costanti link 2

P1o=[2 0 0 1]';
EE2o=[1.5 0 0 1]';

%posizione del punto P rispetto all'origine O1 espressa nel sistema 1 [m]

%posizione del punto EE rispetto all'origine O2 espressa nel sistema 2 [m]

% dati traiettoria
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tempovet=[0 1 2 3 4 5 6 7 8 9 10];
%vettore tempo [s]
q1vet=[0 5 10 15 20 25 30 35 40 45 50];
%vettore gdl giunto 1 [deg]
q2vet=[0 -6 -12 -18 -24 -30 -36 -42 -36 -30 -24]; %vettore gdl giunto 2 [deg]
% calcolo traiettoria punto EE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:length(tempovet)
%dati istantanei di traiettoria
t=tempovet(i);
%tempo [s]
q1=q1vet(i)*pi/180;
%gdl giunto 1 [rad]
q2=q2vet(i)*pi/180;
%gdl giunto 2 [rad]
%matrici dei sistemi di riferimento e vettori caratteristici
teta1=q1;
A10o=denhar_01(alfa1,a1,d1,teta1);

%matrice 4x4

teta2=q2;
A21o=denhar_01(alfa2,a2,d2,teta2);
A20o=A10o*A21o;
pEE0o=A20o*EE2o;

%matrice 4x4
%matrice 4x4
%posizione del punto EE nel sistema 0

%memorizzazione variabili
pEE0vet(:,i)=pEE0o(1:3,1);
end
% disegno traiettoria gdl e punto EE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
subplot(2,1,1)
plot(tempovet,q1vet),grid on
xlabel('tempo [s]');ylabel('q1 [deg]')
subplot(2,1,2)
plot(tempovet,q2vet),grid on
xlabel('tempo [s]');ylabel('q2 [deg]')
figure(2)
subplot(2,2,1)
plot(tempovet,pEE0vet(1,:)),grid on
xlabel('tempo [s]');ylabel('pEE0_x [m]')
subplot(2,2,2)
plot(tempovet,pEE0vet(2,:)),grid on
xlabel('tempo [s]');ylabel('pEE0_y [m]')
subplot(2,2,3)
plot(tempovet,pEE0vet(3,:)),grid on
xlabel('tempo [s]');ylabel('pEE0_z [m]')

esempio LAIB_ver01.doc

Dipartimento di Meccanica Politecnico di Torino

Stefano Pastorelli

subplot(2,2,4)
plot3(pEE0vet(1,:),pEE0vet(2,:),pEE0vet(3,:)),grid
xlabel('x');ylabel('y');zlabel('z')
title('percorso EE')
% disegno wire-frame sistema multi-body in configurazione iniziale
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
i=1;
t=tempovet(i);
q1=q1vet(i)*pi/180;
q2=q2vet(i)*pi/180;

%tempo [s]
%gdl giunto 1 [rad]
%gdl giunto 2 [rad]

%matrici dei sistemi di riferimento e vettori caratteristici

pOo=[0 0 0 1]';

%posizione dell'origine del sistema di base nel sistema 0

teta1=q1;
A10o=denhar_01(alfa1,a1,d1,teta1);
pO10o=A10o(:,4);
pP0o=A10o*P1o;

%matrice 4x4
%posizione dell'origine O1 nel sistema 0
%posizione del punto P nel sistema 0

teta2=q2;
A21o=denhar_01(alfa2,a2,d2,teta2);
A20o=A10o*A21o;
pO20o=A20o(:,4);
pEE0o=A20o*EE2o;

%matrice 4x4
%matrice 4x4
%posizione dell'origine O2 nel sistema 0
%posizione del punto EE nel sistema

punti_om=[pOo pO10o pP0o pO20o pEE0o];

punti=punti_om(1:3,:);
figure(3)
plot3(punti(1,:),punti(2,:),punti(3,:),'b'),grid
axis equal
axis([-1 4 -1 4 0 3])
xlabel('x');ylabel('y');zlabel('z')
giunto_rot(.1,.3,20,A10o,'blu')
giunto_rot(.1,.3,20,A20o,'blu')
% disegno wire-frame sistema multi-body in configurazione finale
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
i=length(tempovet);
t=tempovet(i);
q1=q1vet(i)*pi/180;
q2=q2vet(i)*pi/180;

%tempo [s]
%gdl giunto 1 [rad]
%gdl giunto 2 [rad]

%matrici dei sistemi di riferimento e vettori caratteristici

pOo=[0 0 0 1]';

%posizione dell'origine del sistema di base nel sistema 0

teta1=q1;
A10o=denhar_01(alfa1,a1,d1,teta1);
pO10o=A10o(:,4);
pP0o=A10o*P1o;

%matrice 4x4
%posizione dell'origine O1 nel sistema 0
%posizione del punto P nel sistema 0

teta2=q2;
A21o=denhar_01(alfa2,a2,d2,teta2);
A20o=A10o*A21o;
pO20o=A20o(:,4);
pEE0o=A20o*EE2o;

%matrice 4x4
%matrice 4x4
%posizione dell'origine O2 nel sistema 0
%posizione del punto EE nel sistema

punti_om=[pOo pO10o pP0o pO20o pEE0o];

punti=punti_om(1:3,:);
hold on

%mantiene figura precedente e sovrascrive nuovo plot

plot3(punti(1,:),punti(2,:),punti(3,:),'r')
giunto_rot(.1,.3,20,A10o,'red')
giunto_rot(.1,.3,20,A20o,'red')
plot3(pEE0vet(1,:),pEE0vet(2,:),pEE0vet(3,:),'g')

esempio LAIB_ver01.doc

Dipartimento di Meccanica Politecnico di Torino

Stefano Pastorelli

Output grafico

50

q1 [deg]

40
30
20
10
0

5
tempo [s]

10

5
tempo [s]

10

q2 [deg]

-10
-20
-30
-40
-50

3.6

2.5

3.4

2
pEE0y [m]

pEE0x [m]

figura 1

3.2
3
2.8
2.6

1.5
1
0.5

5
tempo [s]

10

5
tempo [s]
percorso EE

10

2.5
4

1.5

pEE0z [m]

0
4

1
0.5

2
0

5
tempo [s]

10

0 2.5

3.5

figura 2
esempio LAIB_ver01.doc

Dipartimento di Meccanica Politecnico di Torino

Stefano Pastorelli

0
4
3

4
2

3
2

0
y

0
-1

-1

figura 3

esempio LAIB_ver01.doc