Robótica

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
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;
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)
xlabel('tempo [s]');ylabel('pEE0_y [m]')
subplot(2,2,3)
xlabel('tempo [s]');ylabel('pEE0_z [m]')
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]

pOo=[0 0 0 1]';
%posizione dell'origine del sistema di base nel sistema 0
teta1=q1;
pO10o=A10o(:,4);
pP0o=A10o*P1o;
%matrice 4x4
%posizione dell'origine O1 nel sistema 0
%posizione del punto P nel sistema 0
teta2=q2;
A20o=A10o*A21o;
pO20o=A20o(:,4);
pEE0o=A20o*EE2o;
%matrice 4x4
%matrice 4x4
%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]

pOo=[0 0 0 1]';
%posizione dell'origine del sistema di base nel sistema 0
teta1=q1;
pO10o=A10o(:,4);
pP0o=A10o*P1o;
%matrice 4x4
%posizione del punto P nel sistema 0
teta2=q2;
A20o=A10o*A21o;
pO20o=A20o(:,4);
pEE0o=A20o*EE2o;
%matrice 4x4
%matrice 4x4
%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')
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
Stefano Pastorelli
0
4
3
4
2
3
2
0
y
0
-1
-1
figura 3

Robótica

Caricato da

Copyright:

Formati disponibili

Robótica

Caricato da

Informazioni sul documento

Descrizione originale:

Copyright

Formati disponibili

Condividi questo documento

Condividi o incorpora il documento

Opzioni di condivisione

Hai trovato utile questo documento?

Questo contenuto è inappropriato?

Copyright:

Formati disponibili

Robótica

Caricato da

Copyright:

Formati disponibili

Dipartimento di Meccanica Politecnico di Torino

Struttura multi-body a 2 gdl

Dipartimento di Meccanica Politecnico di Torino

%parametri di Denavit-Hartenberg costanti link 1

%parametri di Denavit-Hartenberg costanti link 2

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

Dipartimento di Meccanica Politecnico di Torino

%matrici dei sistemi di riferimento e vettori caratteristici

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

punti_om=[pOo pO10o pP0o pO20o pEE0o];

%matrici dei sistemi di riferimento e vettori caratteristici

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

punti_om=[pOo pO10o pP0o pO20o pEE0o];

%mantiene figura precedente e sovrascrive nuovo plot

Dipartimento di Meccanica Politecnico di Torino

Dipartimento di Meccanica Politecnico di Torino

Potrebbero piacerti anche