ElectroYou

Ciao a tutti, ho trovato un programma in rete, me lo sono studiato e l'ho capito abbastanza, si basa sul robotics toolbox di matlab di Peter Corke (che trovate qui: http://www.petercorke.com/RTB/signin.php basta inserire alcuni semplici dati) ...Ecco il codice:

Codice: Seleziona tutto

% Simulazione di un task di Pick and Place
clear;
close all;

% Parametri
a2=0.6;
d4=0.6;
d6=0.2;

% Creazione Link
%link([ alpha A theta D sigma]), con sigma un flag che indica se il giunto
%è revoluto (sigma=0) o prismatico (sigma=1)
L1=link([ pi/2 0  0 0  0]);
L2=link([ 0    a2 0 0  0]);
L3=link([ pi/2 0  0 0  0]);
L4=link([-pi/2 0  0 d4 0]);
L5=link([pi/2  0  0 0  0]);
L6=link([ 0    0  0 d6 0]);

% Creazione robot
RA=robot({L1,L2,L3,L4,L5,L6});

% T_0^b
RA.base=transl([0 0 1]);  % 1m da terra

% T_e^6
RA.tool=eye(4); % SR#6 coincide con terna utensile

% Configurazione zero
qz=zeros(1,6);


% Pick
p_pick=[0.2 0.3 0]' ; % Posizione (notare che è trasposto)
R_pick=roty(pi);       % Orientamento (effettua una rotazione di pi attorno a y) 
                       % Non è possibile inserire in roty() un valore = 2k*pi con 
                       % k appartente a Z(numeri interi)
T_pick=[R_pick(1:3,1:3) p_pick; 0 0 0 1];

% Place
p_place=[-0.2 0.3 0]'; % Posizione
R_place=roty(pi)*rotz(-pi/2);   % Orientamento: rotazione intorno a Z di -pi/2 (matrice omogenea)
                                 % Non è possibile inserire in roty() un valore = 2k*pi con 
                                 % k appartente a Z(numeri interi)
T_place=[R_place(1:3,1:3) p_place; 0 0 0 1];

% Configurazione del manipolatore in fase di pick (dalla cinematica
% inversa)
q_pick=ikine(RA,T_pick);

% Configurazione del manipolatore in fase di place (dalla cinematica
% inversa)
q_place=ikine(RA,T_place);


% Punti intermedi del cammino

% Sollevamento
p_s = [0.01 0.2 0.5]';
R_s=R_pick;
T_s=[R_s(1:3,1:3) p_s; 0 0 0 1];
q_s=ikine(RA,T_s);

% Abbassamento
p_a = [0.01 -0.3 0.5]';
R_a=R_place;
T_a=[R_a(1:3,1:3) p_a; 0 0 0 1];
q_a=ikine(RA,T_a);



% Generazione della traiettoria: tempi
Tc=0.01; % Tempo di campionamento 10 ms
t_s=0.5; % Tempo in cui essere in p_s
t_a=1;% Tempo in cui essere in p_a
t_place=1.5; % Tempo in cui essere in p_place (t finale)


% Primo tratto della traiettoria
t1=[0:Tc:t_s]; 
T_1=ctraj(T_pick,T_s, length(t1)); % Traiettoria in termini di matrici omogenee

% Secondo tratto della traiettoria
t2=[t_s+Tc:Tc:t_a]; 
T_2=ctraj(T_s,T_a, length(t2)); % Traiettoria in termini di matrici omogenee

% Terzo tratto della traiettoria
t3=[t_a+Tc:Tc:t_place]; 
T_3=ctraj(T_a,T_place, length(t3)); % Traiettoria in termini di matrici omogenee

% Orizzonte temporale
t=[t1 t2 t3];

% Traiettoria desiderata nello spazio operativo
T_d=cat(3,T_1,T_2,T_3); % Concatenazione della matrici T lungo la terza dimensione

% Traiettoria desiderata nello spazio dei giunti
Q_d=ikine(RA,T_d);

figure
plot(RA,Q_d); %Figura tridimensionale

% Calcolo Jacobiano lungo la traiettoria
for k=1:length(Q_d);
  J(:,:,k)=jacob0(RA,Q_d(k,:));
  DJ(k)= det(J(:,:,k)); % Determinante 
end

% Andamento del det(J) (quanto lontani dalle sigolarita')
figure
plot(t,DJ)
title('det(J)')
grid

La parte di cui mi interessa è questa che ho postato, anche se il programma include anche i grafici delle variabili di giunto.

La mia domanda è questa:

come faccio a mettere una saturazione sugli attuatori? Cioè come faccio a impedire ai giunti di muoversi in modo rapidissimo in alcuni punti della traiettoria del manipolatore?

Spero che qualcuno mi aiuti!!!

GRAZIE!!!

ElectroYou

[MATLAB]:Robotics Toolbox

[MATLAB]:Robotics Toolbox