A.6 Analisi della locomozione

(1)

A.6 Analisi della locomozione

%FUNZIONE DI CALCOLO DEGLI SPOSTAMENTI E DELLE ROTAZIONI

function [Sommatoria] = cic19(V_f, V_r, Vt_f, Vt_r, V_tra, V_frel, V_rrel, N, M, tempo, Vprec)

%Matrice delle coordinate delle zampe V = [V_f; V_r];

%Matrice della posizione relativa delle zampe V_relativa = [V_frel; V_rrel];

%X è la matrice dei vertici X = [Vt_f; V; Vt_r];

%Ciclo di approssimazione dei dati in ingresso dimx = size(X,1); for i=[1:dimx] for j=[1:3] X_temp = round(X(i,j)*10^6)/10^6; X(i,j) = X_temp; end end

%Si definisce quindi la matrice di triangolazione TR TR = convhulln(X);

%Si escludono i triangoli che chiudono le facce estreme del tubo. Si selezionano quelli che %connettono tre vertici della sezione di estremità e si metteno a zero le corrispondenti %righe di TR

TR(sum((TR<N+1),2)= =3,:) = []; TR(sum((TR>N+6),2)= =3,:) = []; dimTR = size(TR,1);

(2)

%Definizione dei parametri caratterizzanti il comportamento del tessuto EL_T = 2e3; %Elasticità trasversale (N/m)

l_eq = 0.0115; %Larghezza equivalente (m) lc = 0.0135; %Lunghezza critica (m)

Ri = 0.015; %Raggio iniziale del condotto (m)

%DEFINIZIONE DI VETTORI E MATRICI UTILIZZATI NELL’ANALISI %Vettore del numero dei punti di intersezione

nint = [];

%Vettore delle coordinate dei punti di intersezione pint = [];

%Vettore delle lunghezze calcolate lsezione = [];

%Vettore le cui componenti indicano il numero di punti raccolti nella matrice Mt dimMt = [];

%Vettore con le dimensioni delle sottomatrici di Mtu associate a ciascun piano di taglio dM = [];

%Versore della retta di azione della risultante delle forze normali risultante = [];

%Vettore delle deformazioni epsilon = [];

%Vettore delle tensioni SIGMA = [];

%Vettore delle risultanti FORZA = [];

(3)

%Vettore degli angoli di proiezione delle forze ALFA = [];

%Matrice dei punti utilizzati per la proiezione della forza normale sulla sezione trasversale ANG = [];

%Definizione delle matrici delle risultanti forza_ris = [];

momento_ris = []; contatto = [];

%CICLO DI CALCOLO DELLE FORZE AGENTI SULLA CAPSULA for nzampa = [1:6];

%Si utilizzano le dimensioni delle matrici dei vertici e di quelle di triangolazione n = size(X,1);

m = size(TR,1);

%Quindi si definisce il piano tramite un punto dello stesso ed un vettore normale p0 = X(N+nzampa,:);

p1 = [1, 0, 0];

%Si ordinano i vertici dei triangoli a = X(TR(:,1),:);

b = X(TR(:,2),:); c = X(TR(:,3),:);

%Si verifica la presenza di intersezioni utilizzando i valori assoluti dei prodotti scalari da = (a-repmat(p0, m, 1))*p1';

db = (b-repmat(p0, m, 1))*p1'; dc = (c-repmat(p0, m, 1))*p1';

(4)

%Si costruisce il vettore in cui si ordinano tutti i triangoli cui corrispondono intersezioni k = (da>0)+(db>0)+(dc>0);

k = find((k= =1)|(k= =2));

%xyzp è la matrice dei punti di intersezione xyzp = [];

%CICLO PER LA DETERMINAZIONE E L'ORDINAMENTO DEI PUNTI DI INTER for i = k'

%Verifica dei primi due vertici if sum((da(i)= =0)+(db(i)= =0)= =2) xyz0 = [X(TR(i,1),:);X(TR(i,2),:)]; xyzp = [xyzp;xyz0]; elseif (da(i)*db(i)) <= 0 tt = abs(da(i))/(abs(da(i))+(abs(db(i)))); xyz0 = X(TR(i,1),:)*(1-tt)+X(TR(i,2),:)*tt; xyzp = [xyzp;xyz0]; end

%Verifica dei vertici 1 e 3

if sum((da(i)= =0)+(dc(i)= =0)= =2) xyz0 = [X(TR(i,1),:);X(TR(i,3),:)]; xyzp = [xyzp;xyz0]; elseif (da(i)*dc(i)) <= 0 tt = abs(da(i))/(abs(da(i))+(abs(dc(i)))); xyz0 = X(TR(i,1),:)*(1-tt)+X(TR(i,3),:)*tt; xyzp = [xyzp;xyz0]; end

%Verifica dei vertici 2 e 3

if sum((db(i)= =0)+(dc(i)= =0)= =2) xyz0 = [X(TR(i,2),:);X(TR(i,3),:)]; xyzp = [xyzp;xyz0];

(5)

elseif (db(i)*dc(i)) <= 0 tt = abs(db(i))/(abs(db(i))+(abs(dc(i)))); xyz0 = X(TR(i,2),:)*(1-tt)+X(TR(i,3),:)*tt; xyzp = [xyzp;xyz0]; end end dimxyzp = size(xyzp,1); for i=[1:dimxyzp] for j=[1:3] xyzp_temp = round(xyzp(i,j)*10^6)/10^6; xyzp(i,j) = xyzp_temp; end end xyz = unique(xyzp,'rows'); dimxyz = size(xyz,1);

%Si introducono una serie di matrici utilizzate da appoggio per l’ordinamento xyz11 = []; xyz21 = []; xyz22 = []; xyz12 = []; xyz01 = []; xyz10= []; xyz20 = []; xyz02 = [];

%Si entra in un ciclo che scorre le righe della matrice dei punti di intersezione man_ %dandole in una serie di sottomatrici in funzione del quadrante in cui questi si trovano for i = [1:dimxyz]

%Si utilizza la tg dell'angolo che ne definisce la posizione come criterio di %ordinamento dei punti d'intersezione

(6)

%Punti sull'asse Y positivo if (xyz(i,3)= =0 & xyz(i,2)>0) xyz01 = [xyz01; xyz(i,:)];

%Primo quadrante

elseif (xyz(i,3)>0 & xyz(i,2)>0) xyz11 = [xyz11; xyz(i,:)];

%Punti sull'asse Z positivo elseif (xyz(i,3)>0 & xyz(i,2)= =0) xyz10 = [xyz10; xyz(i,:)];

%Secondo quadrante

elseif (xyz(i,3)>0 & xyz(i,2)<0) xyz21 = [xyz21; xyz(i,:)];

%Punti sull'asse Y negativo elseif (xyz(i,3)= =0 & xyz(i,2)<0) xyz20 = [xyz20; xyz(i,:)];

%Terzo quadrante

elseif (xyz(i,3)<0 & xyz(i,2)<0) xyz22 = [xyz22; xyz(i,:)];

%Punti sull'asse Z negativo elseif (xyz(i,3)<0 & xyz(i,2)= =0) xyz02 = [xyz02; xyz(i,:)];

%Quarto quadrante

elseif (xyz(i,3)<0 & xyz(i,2)>0) xyz12 = [xyz12; xyz(i,:)]; end

(7)

%Si ordinano i termini di ogni sottomatrice utilizzando due cicli for.

%Il ciclo interno calcola il vettore delle tangenti, quello esterno ne estrae l'indice della %riga cui corrisponde il minimo per definire il punto relativo

%Primo quadrante xyz11ord = []; for i = [1:size(xyz11,1)] tg=[]; for j = [1:size(xyz11,1)] tg1 = xyz11(j,3)/xyz11(j,2); tg = [tg; tg1]; end [minimo,indmin] = min(tg); xyz11ord = [xyz11ord;xyz11(indmin,:)]; xyz11(indmin,:)=[]; end %Secondo quadrante xyz21ord = []; for i = [1:size(xyz21,1)] tg=[]; for j = [1:size(xyz21,1)] tg1 = xyz21(j,3)/xyz21(j,2); tg = [tg; tg1]; end [minimo,indmin] = min(tg); xyz21ord = [xyz21ord;xyz21(indmin,:)]; xyz21(indmin,:)=[]; end %Terzo quadrante xyz22ord = []; for i = [1:size(xyz22,1)] tg=[]; for j = [1:size(xyz22,1)]

(8)

tg1 = xyz22(j,3)/xyz22(j,2); tg = [tg; tg1]; end [minimo,indmin] = min(tg); xyz22ord = [xyz22ord;xyz22(indmin,:)]; xyz22(indmin,:)=[]; end %Quarto quadrante xyz12ord = []; for i = [1:size(xyz12,1)] tg=[]; for j = [1:size(xyz12,1)] tg1 = xyz12(j,3)/xyz12(j,2); tg = [tg; tg1]; end [minimo,indmin] = min(tg); xyz12ord = [xyz12ord;xyz12(indmin,:)]; xyz12(indmin,:)=[]; end

%Alla fine si definisce la matrice dei punti ordinati in senso antiorario da Y positivo xyzord= [xyz01;xyz11ord;xyz10;xyz21ord;xyz20;xyz22ord;xyz02;xyz12ord]; dimxyzord = size(xyzord,1);

%Si definisce quindi il vettore lun_app, utilizzato per il calcolo della lunghezza della %curva di intersezione tra substrato e piano di taglio

lun_app = [];

%Detto calcolo si fa sommando le distanze tra i vari punti adiacenti in un ciclo for %quindi aggiungendo a ciò la distanza tra il primo e l'ultimo termine della serie. La %variabile di appoggio v_app serve a discriminare tra le condizioni di presenza o %assenza di contatto sulla zampa

(9)

for i = [1:dimxyzord] if xyzord(i,:)= =X(N+nzampa,:) v_app = v_app+1; end end ang = zeros(3,3);

if v_app>0 %v_app>0, presenza di contatto contatto = [contatto; nzampa];

for i = [1:dimxyzord-1] lun = sqrt((xyzord(i+1,1)-xyzord(i,1))^2+... (xyzord(i+1,2)-xyzord(i,2))^2+... (xyzord(i+1,3)-xyzord(i,3))^2); lun_app = [lun_app;lun]; end lun_app = [lun_app;sqrt((xyzord(1,1)-xyzord(dimxyzord,1))^2+... (xyzord(1,2)-xyzord(dimxyzord,2))^2+... (xyzord(1,3)-xyzord(dimxyzord,3))^2)]; lun = sum(lun_app);

%Calcolo dell'angolo di proiezione della forza normale sulla sezione trasversale if xyzord(1,:) = = X(N+nzampa,:)

ang = [xyzord(dimxyzord,:); xyzord(1,:); xyzord(2,:)]; elseif xyzord(dimxyzord,:) = = X(N+nzampa,:)

ang = [xyzord(dimxyzord-1,:); xyzord(dimxyzord,:); xyzord(1,:)]; else for j = [2:dimxyzord-1]

if xyzord(j,:) = = X(N+nzampa,:)

ang = [xyzord(j-1,:); xyzord(j,:); xyzord(j+1,:)]; end end end else lun = 0; contatto = [contatto; 0]; enD

(10)

%Si raccolgono i risultati in tre matrici già definite sopra nint = [nint; dimxyzord];

pint = [pint; xyzord]; lsezione = [lsezione; lun];

%CALCOLO DELLE FORZE

%Determinazione delle dimensioni dei triangoli e applicazione del teorema di Carnot if ang = = zeros(3,3)

alfa = pi;

ALFA = [ALFA; (alfa*360)/(2*pi)]; else lato1 = sqrt((ang(1,1)-ang(3,1))^2+... (ang(1,2)-ang(3,2))^2+... (ang(1,3)-ang(3,3))^2); lato2 = sqrt((ang(1,1)-ang(2,1))^2+... (ang(1,2)-ang(2,2))^2+... (ang(1,3)-ang(2,3))^2); lato3 = sqrt((ang(2,1)-ang(3,1))^2+... (ang(2,2)-ang(3,2))^2+... (ang(2,3)-ang(3,3))^2); alfa = acos(-(lato1^2-lato2^2-lato3^2)/(2*lato2*lato3)); ALFA = [ALFA; (alfa*360)/(2*pi)];

end

%ANG è la matrice dei vertici dei triangoli ANG = [ANG; ang];

%Calcolo dei moduli delle forze normali agenti sulle zampe if lun = = 0

def = 0;

epsilon = [epsilon; def]; SIGMA = [SIGMA; 0]; FORZA = [FORZA; 0]; else

(11)

def = (lun-pi*2*Ri)/(pi*2*Ri); epsilon = [epsilon; def]; sigma = EL_T*def^2; SIGMA = [SIGMA; sigma];

FORZA = [FORZA; 2*l_eq*sigma*cos(alfa/2)]; end

%CALCOLO DELLA DIREZIONE DI AZIONE DELLE FORZE NORMALI

%Si entra in un ciclo per la selezione dei triangoli aventi un vertice coincidente con la %posizione della zampa considerata

if v_app>0

%Introduzione delle matrici di raccolta dei dati. TRI_zampa è la matrice nelle cui %righe stanno i numeri di riga di X che individuano i punti che stanno ai vertici dei %triangoli convergenti nella zampa considerata.

TRI_zampa = [];

%Selezione dei triangoli convergenti nella zampa for i = [1:dimTR]

for j = [1:3]

if TR(i,j)= =N+nzampa

TRI_zampa = [TRI_zampa; TR(i,1), TR(i,2), TR(i,3)]; end

end end

%TRI è derivata da TRI_zampa eliminando gli indici relativi alla zampa TRI = [];

for i = [1:size(TRI_zampa,1)]

if TRI_zampa(i,1) = = N+nzampa

TRI(i,:) = [TRI_zampa(i,2), TRI_zampa(i,3)]; elseif TRI_zampa(i,2) = = N+nzampa

(12)

TRI(i,:) = [TRI_zampa(i,1), TRI_zampa(i,3)]; else

TRI(i,:) = [TRI_zampa(i,1), TRI_zampa(i,2)]; end

end

%Introduzione delle matrici dei versori

%VERS è la matrice dei versori dei lati dei triangoli VERS = [];

%VERS_MED è la matrice dei versori mediati VERS_MED = [];

%Ciclo di calcolo dei versori dei lati dei triangoli for i = [1: size(TRI,1)] for j = [1:2] vers = [(X(TRI(i,j),1)-X(N+nzampa,1))/sqrt((X(TRI(i,j),1)-X(N+nzampa,1))… ^2+(X(TRI(i,j),2)-X(N+nzampa,2))^2+(X(TRI(i,j),3)-X(N+nzampa,3))^2),... (X(TRI(i,j),2)-X(N+nzampa,2))/sqrt((X(TRI(i,j),1)-X(N+nzampa,1))^2+... (X(TRI(i,j),2)-X(N+nzampa,2))^2+(-X(TRI(i,j),3)-X(N+nzampa,3))^2),... (X(TRI(i,j),3)-X(N+nzampa,3))/sqrt((X(TRI(i,j),1)-X(N+nzampa,1))^2+... (X(TRI(i,j),2)-X(N+nzampa,2))^2+(-X(TRI(i,j),3)-X(N+nzampa,3))^2)]; VERS = [VERS; vers];

end end

%Definizione dei vettori e dei coefficienti per il calcolo dei vettori mediati altezza = []; %Vettore medio

mod_alt = 0; %Modulo del vettore medio vers_alt = []; %Versore del vettore medio prod_vettore = []; %Prodotto vettore

modulo = 0; %Modulo del prodotto vettore vers_med = []; %Singolo versore medisto

(13)

VERS_MED = []; %Matrice di raccolta dei versori mediati risult = []; %Risultante

%Calcolo dei termini per mediare i versori. Si definisce il vettore altezza del singolo %triangolo convergente nella zampa come somma tra i versori dei suoi lati diviso due altezza = sum(VERS,1)/2;

%Se ne prende modulo (che coincide con il coseno dell'angolo tra lato e altezza) mod_alt = sqrt(altezza(1,1)^2+altezza(1,2)^2+altezza(1,3)^2);

%E se ne calcola il versore

vers_alt = [altezza(1,1)/mod_alt,altezza(1,2)/mod_alt,... altezza(1,3)/mod_alt];

%Il prodotto vettore tra i versori di lato e altezza serve per trovare il seno dello stesso %angolo che ne è il modulo

prod_vettore = cross(VERS(1,:), vers_alt);

modulo = sqrt(prod_vettore(1,1)^2+prod_vettore(1,2)^2+prod_vettore(1,3)^2);

%Quindi si media il versore dell'altezza vers_med = vers_alt*modulo*mod_alt;

%Infine si raccolgono i dati mediati VERS_MED = [VERS_MED; vers_med];

%Calcolo della risultante e raccolta dei risultati risult = [sum(VERS_MED,1)];

vers_risult = [risult(1,1)/sqrt(risult(1,1)^2+risult(1,2)^2+risult(1,3)^2),... risult(1,2)/sqrt(risult(1,1)^2+risult(1,2)^2+risult(1,3)^2),... risult(1,3)/sqrt(risult(1,1)^2+risult(1,2)^2+risult(1,3)^2)]; risultante = [risultante; vers_risult];

else

risultante = [risultante; 0,0,0]; end

(14)

end

%Introduzione dei coefficienti riduttivi per effetto dell'interazione %Si ordinano le zampe in funzione della loro posizione assiale serie = []; V_app = []; V_appord = []; V_app_ind = []; for i = [1:6] if contatto(i,1) ~= 0

V_app = [V_app; V(contatto(i,1),:)]; V_app_ind = [V_app_ind; i];

end end

for i = [1:6]

[minimo, indmin] = min(V_app(:,1)); V_appord = [V_appord; V_app(indmin,:)]; V_app(indmin,:)=[]; end for i = [1:size(V_appord,1)] for j = [1:6] if V(j,:) == V_appord(i,:) serie = [serie; j]; end end end

%Quindi si calcolano le distanze e si introducono i coefficienti correttivi coeff_rid = ones(6,1); V_nuo = V_appord; for i = [1:size(V_appord,1)] DIST = []; V_nuo(i,:) = []; for j = [1:size(V_appord,1)-1]

(15)

dist = abs(V_appord(i,1)-V_nuo(j,1)); DIST = [DIST; dist];

end V_nuo = V_appord; nc = 0; for k = [1:size(DIST,1)] if DIST(k,1) = = 0 nc = nc+1; end end V_appord_prec = []; V_appord_post = []; DIST_prec = []; DIST_post = []; distmin = 0; distmin_prec = 0; distmin_post = 0; if nc ~= 0 coeff_rid(serie(i,1)) = coeff_rid(serie(i,1))/(nc+1); end DIST1 = []; nc1 = 0; for i = [1:size(DIST,1)] if DIST(i,1) ~= 0 DIST1 = [DIST1;DIST]; nc1 = nc1+1; end end if nc1 ~= 0 if i == 1 distmin = min(DIST1); if distmin <= 0 lc coeff_rid(serie(i,1)) = coeff_rid(serie(i,1))*(-916.155*distmin^2+49.44*… distmin+0.5);

(16)

end

elseif i > 1 && i < size(V_appord,1) nc_prec = 0;

nc_post = 0; for k = [1:i-1]

DIST_prec = [DIST_prec; DIST(k,1)]; if DIST(k,1) ~= 0

nc_prec = nc_prec+1; end

end

for k = [i:size(V_appord,1)-1]

DIST_post = [DIST_post; DIST(k,1)]; if DIST(k,1) ~= 0 nc_post = nc_post+1; end end if nc_prec ~= 0 DIST1_prec = []; for k = [i:size(DIST_prec,1)] if DIST_prec(k,1) = = 0

DIST1_prec = [DIST1_prec; DIST_prec(k,1)]; end end distmin_prec = min(DIST1_prec); end if nc_post ~= 0 DIST1_post = []; for k = [i:size(DIST_post,1)] if DIST_post(k,1) = = 0

DIST1_post = [DIST1_post; DIST_post(k,1)]; end

end

distmin_post = min(DIST1_post); end

(17)

if distmin_prec <= lc & distmin_post <= lc

coeff_rid(serie(1,1)) = coeff_rid(serie(1,1))*(-916.155*distmin_prec^2+49.44*… distmin_prec+0.5)*(-916.155*distmin_post^2+49.44*distmin_post+0.5);

elseif distmin_prec <= lc & distmin_post > lc

coeff_rid(serie(1,1)) = coeff_rid(serie(1,1))*(916.155*distmin_prec^2+… 49.44*distmin_prec+0.5);

elseif distmin_prec > lc & distmin_post<= lc

coeff_rid(serie(1,1)) = coeff_rid(serie(1,1))*(-916.155*distmin_post^2+… 49.44*distmin_post+0.5); end else distmin = min(DIST); if distmin <= lc coeff_rid(serie(1,1)) = coeff_rid(serie(1,1))*(-916.155*distmin^2+… 49.44*distmin+0.5); end end end end

%Scrittura in componenti della forza normale sulle zampe FORZACOMP = []; for i = [1:6] for j = [1:3] FORZACOMP(i,j) = FORZA(i)*risultante(i,j); end end

%DETERMINAZIONE DELLE FORZE DI ATTRITO SULLE ZAMPE %Si calcola il coeff. di attrito come funzione della deformazione

mod_fatt = []; mu = []; for i = [1:6]

(18)

MU = 0.4208*epsilon(i)^2-0.2413*epsilon(i)+0.0489; mu = [mu; MU];

fatt = FORZA(i)*MU; mod_fatt = [mod_fatt; fatt]; end

%L'ultima questione da affrontare è la determinazione della direzione di %azione di tali forze. Per far ciò si utilizza il versore "risultante" per %definire il piano di giacitura di tale direzione

FATT = []; vers_att = [];

%Si utilizza un doppio prodotto vettoriale for i = [1:6] if risultante(i,:) ~= 0 spostamento = Vprec(i,:)-V(i,:); primo = cross(risultante(i,:),spostamento); secondo = cross(primo,risultante(i,:)); mod_secondo = sqrt(secondo(1,1)^2+secondo(1,2)^2+secondo(1,3)^2); if mod_secondo < 0.0001 vers_att = [vers_att; 0,0,0]; FATT = [FATT; 0, 0, 0]; else ver_secondo = [secondo(1,1)/sqrt(secondo(1,1)^2+secondo(1,2)^2+secondo(1,3)^2),... secondo(1,2)/sqrt(secondo(1,1)^2+secondo(1,2)^2+secondo(1,3)^2),... secondo(1,3)/sqrt(secondo(1,1)^2+secondo(1,2)^2+secondo(1,3)^2)]; vers_att = [vers_att; ver_secondo];

FATT = [FATT; mod_fatt(i,1)*vers_att(i,:)]; end

else

vers_att = [vers_att; 0,0,0]; FATT = [FATT; 0,0,0]; end

(19)

end

%CALCOLO DELLE RISULTANTI IN COMPONENTI %AZIONI RISULTANTI SULLE SINGOLE ZAMPE %Forza risultante sulle zampe

FORZARIS = FORZACOMP + FATT;

%Determinazione delle azioni a momento sulla capsula braccio = [];

MOMENTO = []; for i = [1:6]

braccio = [braccio; V_relativa(i,1), V_relativa(i,2), V_relativa(i,3)]; momento = cross(FORZARIS(i,:), braccio(i,:));

MOMENTO = [MOMENTO; momento]; end

%AZIONI RISULTANTI COMPLESSIVE %Calcolo delle risultanti

forz = sum(FORZARIS,1); mom = sum(MOMENTO,1); Azioni = [forz'; mom'/10]; quad = Azioni.*Azioni; Sommatoria = sum (quad);

%FUNZIONE DI AGGIORNAMENTO DELLE POSIZIONI DELLE ZAMPE function [Vrel] = pun1902(N, M, tempo)

%INTRODUZIONE DEI PARAMETRI GEOMETRICI PER IL MOTO L=0.360; % Lunghezza del tubo (m)

(20)

D=0.030; % Diametro del tubo (m) IASS=0.020; % Interasse zampe (m)

L_lz=0.008; % Lunghezza dei link delle zampe D_cap=0.010; % Diametro capsula

%ZAMPE ANTERIORI

%Quelle sotto sono le coordinate dei vertici delle zampe

%Si considera un incremento della posizione per ogni frame pari a 1/M*2*pi % legge di moto dei vertici con gdl sui giunti delle zampe anteriori

alfa_1=-60*pi/180*cos((tempo-1)/M*6*pi)-10*pi/180;

alfa_2=70*pi/180*cos((tempo-1)/M*6*pi+45*pi/180)+50*pi/180; % vertice della prima zampa anteriore

V_f_1=[IASS/2-L_lz*sin(alfa_1)-L_lz*sin(alfa_1+alfa_2),... -D_cap/2-L_lz*cos(alfa_1)-L_lz*cos(alfa_1+alfa_2), 0];

% vertice della seconda zampa anteriore (rotazione di -120° attorno a x)

V_f_2=V_f_1*[1 0 0;0 cos(-2/3*pi) sin(-2/3*pi);0 -sin(-2/3*pi) cos(-2/3*pi)]; % vertice della terza zampa anteriore (rotazione di 120° attorno a x)

V_f_3=V_f_1*[1 0 0;0 cos(2/3*pi) sin(2/3*pi);0 -sin(2/3*pi) cos(2/3*pi)]; V_frel=[V_f_1;V_f_2;V_f_3];

%ZAMPE POSTERIORI

% legge di moto dei vertici con gdl sui giunti delle zampe posteriori alfa_3=-60*pi/180*cos((tempo-1)/M*6*pi+pi)-10*pi/180;

alfa_4=70*pi/180*cos((tempo-1)/M*6*pi+45*pi/180+pi)+50*pi/180; % vertice della prima zampa posteriore

V_r_1=[IASS/2-L_lz*sin(alfa_3)-L_lz*sin(alfa_3+alfa_4),... -D_cap/2-L_lz*cos(alfa_3)-L_lz*cos(alfa_3+alfa_4), 0];

% vertice della seconda zampa posteriore (rotazione di -120° attorno a x) V_r_2=V_r_1*[1 0 0;0 cos(-2/3*pi) sin(-2/3*pi);0 -sin(-2/3*pi) cos(-2/3*pi)]; % vertice della terza zampa posteriore (rotazione di 120° attorno a x)

V_r_3=V_r_1*[1 0 0;0 cos(2/3*pi) sin(2/3*pi);0 -sin(2/3*pi) cos(2/3*pi)]; V_r=[V_r_1;V_r_2;V_r_3];

(21)

V_r0=V_r;

V_r0(:,1)=V_r(:,1)-IASS; % trasla i vertici anteriori indietro su x . % ruota di - 60° i vertici traslati

V_rrel=V_r0*[1 0 0;0 cos(-1/3*pi) sin(-1/3*pi);0 -sin(-1/3*pi) cos(-1/3*pi)]; Vrel = [V_frel; V_rrel];

%CICLO DI ANALISI clear all; close all;

%DEFINIZIONE DEI PARAMETRI DI ANALISI UTENTE N = input('Inserire il numero di segmenti longitudinali N='); for u = [0:1000] if (N= =0) | (N= =1) N = input('Inserire un valore di N>1, N='); u = 0; else u = 1000; end end

M = input('Inserire il numero di frazionamenti temporali M='); for u = [0:1000] if M= =0 M = input('Inserire un valore di M>0, M='); u = 0; else u = 1000; end end

%Ampiezze dei range di verifica per spostamenti e rotazioni

Ascost= input('Inserire ampiezza campo di scostamenti in m Ascost='); Arot = input('Inserire ampiezza campo di rotazioni in gradi Arot=');

(22)

%%%%%%%%%

%INTRODUZIONE DEI PARAMETRI GEOMETRICI PER IL MOTO RELATIVO L=0.360; % Lunghezza del tubo (m)

D=0.030; % Diametro del tubo (m) IASS=0.020; % Interasse zampe (m)

L_lz=0.008; % Lunghezza dei link delle zampe (m) D_cap=0.010; % Diametro capsula (m)

%%%%%%%%%%%

%MOTO DI TRASCINAMENTO

%Introduzione dei termini che descrivono la traslazione del baricentro della capsula sx_tra = 0; sy_tra = 0; sz_tra = 0; sx_tra_prec = 0; sy_tra_prec = 0; sz_tra_prec = 0; V_tra = zeros(3,3);

%Introduzione dei termini che descrivono la rotazione della capsula attorno al baricentro Mx_tra = [1, 0, 0; 0, 1, 0; 0, 0, 1]; My_tra = [1, 0, 0; 0, 1, 0; 0, 0, 1]; Mz_tra = [1, 0, 0; 0, 1, 0; 0, 0, 1]; M2x_tra = Mx_tra; M2y_tra = My_tra;

(23)

M2z_tra = Mz_tra;

%%%%%%%%%

%CICLO DI CALCOLO DEI RISULTATI for tempo = [2:M]

%Aggiornamento delle rotazioni Mx_tra = Mx_tra*M2x_tra; My_tra = My_tra*M2y_tra; Mz_tra = Mz_tra*M2z_tra;

%Definizione dei parametri di analisi

camp = input('Inserire il numero di campionamenti camp='); A_camp_scost = Ascost/camp;

A_camp_rot = Arot/camp;

%INTRODUZIONE DEI PUNTI CHE DEFINISCONO LA POSIZIONE RELATIVA %DELLE ZAMPE ALLO STEP ATTUALE DI ANALISI

%SEZIONI DI ESTREMITA'

%La sezione anteriore del tratto di tubo simulato è inizialmente posta %in x=0.18

Vt_f=[linspace(L/2,L/2,N)' D/2*cos(linspace(0,2*pi,N))' D/2*sin(linspace(0,2*pi,N))']; %La sezione posteriore del tratto di tubo simulato è inizialmente posta

%in x=-0.18

Vt_r=[linspace(-L/2,-L/2,N)', D/2*cos(linspace(0,2*pi,N))', D/2*… sin(linspace(0,2*pi,N))'];

%ZAMPE ANTERIORI

(24)

%Legge di moto dei vertici con gdl sui giunti delle zampe anteriori alfa_1=-60*pi/180*cos((tempo-1)/M*6*pi)-10*pi/180;

alfa_2=70*pi/180*cos((tempo-1)/M*6*pi+45*pi/180)+50*pi/180; % vertice della prima zampa anteriore

V_f_1=[IASS/2-L_lz*sin(alfa_1)-L_lz*sin(alfa_1+alfa_2),... -D_cap/2-L_lz*cos(alfa_1)-L_lz*cos(alfa_1+alfa_2), 0];

% vertice della seconda zampa anteriore (rotazione di -120° attorno a x)

V_f_2=V_f_1*[1 0 0;0 cos(-2/3*pi) sin(-2/3*pi);0 -sin(-2/3*pi) cos(-2/3*pi)]; % vertice della terza zampa anteriore (rotazione di 120° attorno a x)

V_f_3=V_f_1*[1 0 0;0 cos(2/3*pi) sin(2/3*pi);0 -sin(2/3*pi) cos(2/3*pi)]; %

V_F=[V_f_1;V_f_2;V_f_3];

V_frel = ((V_F*Mx_tra)*My_tra)*Mz_tra; %ZAMPE POSTERIORI

% legge di moto dei vertici con gdl sui giunti delle zampe posteriori alfa_3=-60*pi/180*cos((tempo-1)/M*6*pi+pi)-10*pi/180;

alfa_4=70*pi/180*cos((tempo-1)/M*6*pi+45*pi/180+pi)+50*pi/180; % vertice della prima zampa posteriore

V_r_1=[IASS/2-L_lz*sin(alfa_3)-L_lz*sin(alfa_3+alfa_4),... -D_cap/2-L_lz*cos(alfa_3)-L_lz*cos(alfa_3+alfa_4), 0];

% vertice della seconda zampa posteriore (rotazione di -120° attorno a x) V_r_2=V_r_1*[1 0 0;0 cos(-2/3*pi) sin(-2/3*pi);0 -sin(-2/3*pi) cos(-2/3*pi)]; % vertice della terza zampa posteriore (rotazione di 120° attorno a x)

V_r_3=V_r_1*[1 0 0;0 cos(2/3*pi) sin(2/3*pi);0 -sin(2/3*pi) cos(2/3*pi)]; %

V_R=[V_r_1;V_r_2;V_r_3]; V_R0=V_R;

V_R0(:,1)=V_R(:,1)-IASS; % trasla i vertici anteriori indietro su x . % ruota di - 60° i vertici traslati

V_R=V_R0*[1 0 0;0 cos(-1/3*pi) sin(-1/3*pi);0 -sin(-1/3*pi) cos(-1/3*pi)]; V_rrel = ((V_R*Mx_tra)*My_tra)*Mz_tra;

(25)

%%%%%%%%%%%%

%DEFINIZIONE DELLA CONFIGURAZIONE IN CUI LA CAPSULA SI TROVA %ALLO STEP PRECEDENTE

%ZAMPE ANTERIORI

% legge di moto dei vertici con gdl sui giunti delle zampe anteriori alfa_1_prec=-60*pi/180*cos((tempo-2)/M*6*pi)-10*pi/180;

alfa_2_prec=70*pi/180*cos((tempo-2)/M*6*pi+45*pi/180)+50*pi/180; % vertice della prima zampa anteriore

V_f_1_prec=[IASS/2-L_lz*sin(alfa_1_prec)-L_lz*sin(alfa_1_prec+alfa_2_prec),... -D_cap/2-L_lz*cos(alfa_1_prec)-L_lz*cos(alfa_1_prec+alfa_2_prec), 0]; % vertice della seconda zampa anteriore (rotazione di -120° attorno a x)

V_f_2_prec=V_f_1_prec*[1 0 0;0 cos(-2/3*pi) sin(-2/3*pi);… 0 -sin(-2/3*pi) cos(-2/3*pi)];

% vertice della terza zampa anteriore (rotazione di 120° attorno a x)

V_f_3_prec=V_f_1_prec*[1 0 0;0 cos(2/3*pi) sin(2/3*pi);0 -sin(2/3*pi) cos(2/3*pi)]; V_F_prec=[V_f_1_prec; V_f_2_prec; V_f_3_prec];

V_frel_prec = ((V_F_prec*Mx_tra)*My_tra)*Mz_tra;

%ZAMPE POSTERIORI

% legge di moto dei vertici con gdl sui giunti delle zampe posteriori alfa_3_prec=-60*pi/180*cos((tempo-2)/M*6*pi+pi)-10*pi/180;

alfa_4_prec=70*pi/180*cos((tempo-2)/M*6*pi+45*pi/180+pi)+50*pi/180; % vertice della prima zampa posteriore

V_r_1_prec=[IASS/2-L_lz*sin(alfa_3_prec)-L_lz*sin(alfa_3_prec+alfa_4_prec),... -D_cap/2-L_lz*cos(alfa_3_prec)-L_lz*cos(alfa_3_prec+alfa_4_prec), 0]; % vertice della seconda zampa posteriore (rotazione di -120° attorno a x)

V_r_2_prec=V_r_1_prec*[1 0 0;0 cos(-2/3*pi) sin(-2/3*pi);… 0 -sin(-2/3*pi) cos(-2/3*pi)];

(26)

V_r_3_prec=V_r_1_prec*[1 0 0;0 cos(2/3*pi) sin(2/3*pi);0 -sin(2/3*pi) cos(2/3*pi)]; V_R_prec=[V_r_1_prec; V_r_2_prec; V_r_3_prec];

V_R0_prec=V_R_prec;

V_R0_prec(:,1)=V_R_prec(:,1)-IASS; % trasla i vertici anteriori indietro su x . % ruota di - 60° i vertici traslati

V_R_prec=V_R0_prec*[1 0 0;0 cos(-1/3*pi) sin(-1/3*pi);0 -sin(-1/3*pi) cos(-1/3*pi)]; V_rrel_prec = ((V_R_prec*Mx_tra)*My_tra)*Mz_tra;

%Composizione dei moti allo step precedente if tempo = = 1

V_tra_prec = zeros(3,3); else

V_tra_prec = [sx_tra_prec, sy_tra_prec, sz_tra_prec]; end

Vprec_app = [];

Vprec_app = [V_frel_prec; V_rrel_prec]; Vprec = []; for i = [1:6] Vprec(i,:) = Vprec_app(i,:)+V_tra_prec(1,:); end sx_tra_prec = sx_tra; sy_tra_prec = sy_tra; sz_tra_prec = sz_tra; %%%%%%

%CICLI DI CALCOLO DEI RISULTATI APPLICANDO SPOSTAMENTI E ROTA_ %ZIONI DICONTROLLO PER MINIMIZZARE LE AZIONI SULLA CAPSULA %ANALISI DEL MOTO SECONDO LA TRASLAZIONE LUNGO X

Dati = [];

(27)

%Matrice di trascinamento V_tra = [sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra]; for i = [1:3]

V_tra(i,1) = V_tra(i,1)+A_camp_scost*indice; end

%Composizione dei moti V_f = V_frel+ V_tra; V_r = V_rrel+ V_tra;

%Richiamo dei risultati della funzione di calcolo

Sommatoria = cic19(V_f, V_r, Vt_f, Vt_r, V_tra, V_frel, V_rrel, N, M, tempo, Vprec);

Dati = [Dati; Sommatoria]; end

%Estrazione del minimo [Y I] = min(Dati);

[ix jx] = ind2sub([2*camp+1 1], I);

%%%%%%%%%%%%%

%ANALISI DEL MOTO SECONDO LA TRASLAZIONE LUNGO Y

Dati = [];

for indice = [-camp:camp]

(28)

V_tra(i,2) = V_tra(i,2)+A_camp_scost*indice; V_tra(i,1) = V_tra(i,1)+A_camp_scost*(ix-camp-1); end

[iy jy] = ind2sub([2*camp+1 1], I);

%%%%%%%%

%ANALISI DEL MOTO SECONDO LA TRASLAZIONE LUNGO Z

Dati = [];

V_tra(i,3) = V_tra(i,3)+A_camp_scost*indice; V_tra(i,1) = V_tra(i,1)+A_camp_scost*(ix-camp-1); V_tra(i,2) = V_tra(i,2)+A_camp_scost*(iy-camp-1); end

(29)

[iz jz] = ind2sub([2*camp+1 1], I);

%%%%%%%%%%%%

%DEFINIZIONE DEL MOTO DI TRASCINAMENTO DI TRASLAZIONE for i = [1:3]

V_tra(i,:) = [sx_tra+A_camp_scost*(ix-camp-1), sy_tra+A_camp_scost*… (iy-camp-1), sz_tra+A_camp_scost*(iz-camp-1)];

end

%%%%%%%%%%

%ANALISI DEL MOTO SECONDO LA ROTAZIONE ATTORNO A X

Dati = [];

for indice = [-camp:camp] Mx = [1, 0, 0; 0, cos(A_camp_rot*indice*pi/180), sin(A_camp_rot*indice*pi/180); 0, -sin(A_camp_rot*indice*pi/180), cos(A_camp_rot*indice*pi/180)]; V_frelx = V_frel*Mx; V_rrelx = V_rrel*Mx;

(30)

%Composizione dei moti V_f = V_frelx+ V_tra; V_r = V_rrelx+ V_tra;

[irx jrx] = ind2sub([2*camp+1 1], I); %%%%%%%%%%%

%ANALISI DEL MOTO SECONDO LA ROTAZIONE ATTORNO A Y

Dati = [];

My = [cos(A_camp_rot*indice*pi/180), 0, -sin(A_camp_rot*indice*pi/180); 0, 1, 0; sin(A_camp_rot*indice*pi/180), 0, cos(A_camp_rot*indice*pi/180)]; Mx = [1, 0, 0; 0, cos(A_camp_rot*(irx-camp-1)*pi/180), sin(A_camp_rot*(irx-camp-1)*pi/180); 0, -sin(A_camp_rot*(irx-camp-1)*pi/180), cos(A_camp_rot*(irx-camp-1)*pi/180)]; V_frely = (V_frel*Mx)*My; V_rrely = (V_rrel*Mx)*My;

%Composizione dei moti V_f = V_frely+ V_tra; V_r = V_rrely+ V_tra;

(31)

[iry jry] = ind2sub([2*camp+1 1], I);

%%%%%%%%%%%

%ANALISI DEL MOTO SECONDO LA ROTAZIONE ATTORNO A Z

Dati = [];

Mz = [cos(A_camp_rot*indice*pi/180), sin(A_camp_rot*indice*pi/180), 0; -sin(A_camp_rot*indice*pi/180), cos(A_camp_rot*indice*pi/180), 0; 0, 0, 1]; My = [cos(A_camp_rot*(iry-camp-1)*pi/180), 0, -sin(A_camp_rot*(iry-camp-1)*pi/180); 0, 1, 0; sin(A_camp_rot*(iry-camp-1)*pi/180), 0, cos(A_camp_rot*(iry-camp-1)*pi/180)]; Mx = [1, 0, 0; 0, cos(A_camp_rot*(irx-camp-1)*pi/180), sin(A_camp_rot*(irx-camp-1)*pi/180); 0, -sin(A_camp_rot*(irx-camp-1)*pi/180), cos(A_camp_rot*(irx-camp-1)*pi/180)]; V_frelz = ((V_frel*Mx)*My)*Mz; V_rrelz = ((V_rrel*Mx)*My)*Mz; %Composizione dei moti

V_f = V_frelz+ V_tra; V_r = V_rrelz+ V_tra;

(32)

[irz jrz] = ind2sub([2*camp+1 1], I); %%%%%%

%RACCOLTA DEI RISULTATI

%Matrice di aggiornamento dei dati

moto_tra(tempo, :) = [A_camp_scost*(ix-camp-1), A_camp_scost*(iy-camp-1), A_camp_scost*(iz-camp-1), A_camp_rot*(irx-camp-1), A_camp_rot*... (iry-camp-1), A_camp_rot*(irz-camp-1)]

%Aggiornamento delle matrici che definiscono il moto di rotazione della capsula Mx_tra1 = Mx_tra*… [1, 0, 0; 0, cos(A_camp_rot*(irx-camp-1)*pi/180), sin(A_camp_rot*(irx-camp-1)*pi/180); 0, -sin(A_camp_rot*(irx-camp-1)*pi/180), cos(A_camp_rot*(irx-camp-1)*pi/180)]; My_tra1 = My_tra* [cos(A_camp_rot*(iry-camp-1)*pi/180), 0, -sin(A_camp_rot*(iry-camp-1)*pi/180); 0, 1, 0; sin(A_camp_rot*(iry-camp-1)*pi/180), 0, cos(A_camp_rot*(iry-camp-1)*pi/180)]; Mz_tra1 = Mz_tra* [cos(A_camp_rot*(irz-camp-1)*pi/180), sin(A_camp_rot*(irz-camp-1)*pi/180), 0; -sin(A_camp_rot*(irz-camp-1)*pi/180), cos(A_camp_rot*(irz-camp-1)*pi/180), 0; 0, 0, 1]; %%%%%%

(33)

%PREPARAZIONE DEI DATI PER LA VERIFICA

%Termini per la rotazione M2x_tra = Mx_tra1; M2y_tra = My_tra1; M2z_tra = Mz_tra1;

%Termini per gli indici i2x = ix;

i2y = iy; i2z = iz; %%%%%%%

%VERIFICA DEI RISULTATI OTTENUTI

%num rappresenta il numero di volte in cui si desidera sia effettuata la verifica num = input('Inserire il numero di cicli di verifica num=');

for run = [1:num]

%TRASLAZIONE LUNGO X

%Definizione delle condizioni in cui si effettua la verifica %Termini per la traslazione e la rotazione

M2x_tra_v = M2x_tra; M2y_tra_v = M2y_tra; M2z_tra_v = M2z_tra;

%Termini per gli indici i2x_v = i2x;

i2y_v = i2y; i2z_v = i2z;

(34)

%E' necessario ridefinire i punti per l'analisi Vrel = pun1902(N, M, tempo);

V_frel = (([Vrel(1,:); Vrel(2,:); Vrel(3,:)]*M2x_tra_v)*M2y_tra_v)*M2z_tra_v; V_rrel = (([Vrel(4,:); Vrel(5,:); Vrel(6,:)]*M2x_tra_v)*M2y_tra_v)*M2z_tra_v;

%Analisi del moto secondo la traslazione lungo X Dati = [];

%Matrice di trascinamento V_tra = [sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra]; for i = [1:3] V_tra(i,1) = V_tra(i,1)+A_camp_scost*indice; V_tra(i,3) = V_tra(i,3)+A_camp_scost*(i2z_v-camp-1); V_tra(i,2) = V_tra(i,2)+A_camp_scost*(i2y_v-camp-1); end

[i2x j2x] = ind2sub([2*camp+1 1], I);

(35)

%TRASLAZIONE LUNGO Y

%Analisi del moto secondo la traslazione lungo Y Dati = [];

%Matrice di trascinamento V_tra = [sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra]; for i = [1:3] V_tra(i,2) = V_tra(i,2)+A_camp_scost*indice; V_tra(i,1) = V_tra(i,1)+A_camp_scost*(i2x_v-camp-1); V_tra(i,3) = V_tra(i,3)+A_camp_scost*(i2z_v-camp-1); end

Sommatoria = cic19(V_f, V_r, Vt_f, Vt_r, V_frel, V_rrel, V_R, N, M, tempo, Vprec);

(36)

%%%%%%

%TRASLAZIONE LUNGO Z

%Analisi del moto secondo la traslazione lungo Z Dati = [];

%Matrice di trascinamento V_tra = [sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra; sx_tra, sy_tra, sz_tra]; for i = [1:3] V_tra(i,3) = V_tra(i,3)+A_camp_scost*indice; V_tra(i,1) = V_tra(i,1)+A_camp_scost*(i2x_v-camp-1); V_tra(i,2) = V_tra(i,2)+A_camp_scost*(i2y_v-camp-1); end

(37)

[Y I] = min(Dati);

[i2z j2z] = ind2sub([2*camp+1 1], I);

%%%%%%%

%DEFINIZIONE DEL MOTO DI TRASCINAMENTO PER LA VERIFICA DELLE %ROTAZIONI

V_tra = zeros(3,3); for i = [1:3]

V_tra(i,:) = [sx_tra+A_camp_scost*(i2x_v-camp-1), sy_tra+A_camp_scost*… (i2y_v-camp-1), sz_tra+A_camp_scost*(i2z_v-camp-1)]; end %%%%%%%%%% %ROTAZIONE ATTORNO AD X

%Definizione delle condizioni in cui si effettua la verifica M2x_tra_v = Mx_tra;

M2y_tra_v = M2y_tra; M2z_tra_v = M2z_tra;

V_frel = [Vrel(1,:); Vrel(2,:); Vrel(3,:)]; V_rrel = [Vrel(4,:); Vrel(5,:); Vrel(6,:)];

%Analisi del moto secondo la rotazione attorno X Dati = [];

for indice = [-camp:camp] Mx = [1, 0, 0;

0, cos(A_camp_rot*indice*pi/180), sin(A_camp_rot*indice*pi/180); 0, -sin(A_camp_rot*indice*pi/180), cos(A_camp_rot*indice*pi/180)];

(38)

V_frelx = (((V_frel*Mx_tra)*M2y_tra_v)*M2z_tra_v)*Mx; V_rrelx = (((V_rrel*Mx_tra)*M2y_tra_v)*M2z_tra_v)*Mx; %Composizione dei moti

V_f = V_frelx+ V_tra; V_r = V_rrelx+ V_tra;

[ir2x jr2x] = ind2sub([2*camp+1 1], I);

%%%%%%%%%%

%ROTAZIONE ATTORNO A Y

%Definizione delle condizioni in cui si effettua la verifica M2x_tra_v = M2x_tra;

M2y_tra_v = My_tra;

M2z_tra_v = M2z_tra;

%Analisi del moto secondo la rotazione attorno Y Dati = [];

(39)

My = [cos(A_camp_rot*indice*pi/180), 0, -sin(A_camp_rot*indice*pi/180); 0, 1, 0; sin(A_camp_rot*indice*pi/180), 0, cos(A_camp_rot*indice*pi/180)]; V_frely = (((V_frel*M2x_tra_v)*My_tra)*M2z_tra_v)*My; V_rrely = (((V_rrel*M2x_tra_v)*My_tra)*M2z_tra_v)*My;

%Composizione dei moti V_f = V_frely+ V_tra; V_r = V_rrely+ V_tra;

[ir2y jr2y] = ind2sub([2*camp+1 1], I);

%%%%%%%%%%

%ROTAZIONE ATTORNO A Z

%Definizione delle condizioni in cui si effettua la verifica M2x_tra_v = M2x_tra;

M2y_tra_v = M2y_tra;

M2z_tra_v = Mz_tra;

(40)

%Analisi del moto secondo la rotazione attorno Z Dati = [];

Mz = [cos(A_camp_rot*indice*pi/180), 0, -sin(A_camp_rot*indice*pi/180); 0, 1, 0;

sin(A_camp_rot*indice*pi/180), 0, cos(A_camp_rot*indice*pi/180)]; V_frelz = (((V_frel*M2x_tra_v)*M2y_tra_v)*Mz_tra)*Mz;

V_rrelz = (((V_rrel*M2x_tra_v)*M2y_tra_v)*Mz_tra)*Mz; %Composizione dei moti

V_f = V_frelz+ V_tra; V_r = V_rrelz+ V_tra;

[ir2z jr2z] = ind2sub([2*camp+1 1], I);

%%%%%%%%%

%RACCOLTA DEI RISULTATI

%Matrice di aggiornamento dei dati

moto_tra(tempo, :) = [A_camp_scost*(i2x-camp-1), A_camp_scost*(i2y-camp-1), A_camp_scost*(i2z-camp-1),A_camp_rot*(ir2x-camp-1), A_camp_rot*(ir2y-... camp-1), A_camp_rot*(ir2z-camp-1)]

%Aggiornamento dei vettori e delle matrici che definiscono il moto di trascinamento somma = [];

(41)

somma = sum(moto_tra,1); if run = = num sx_tra = somma(1,1); sy_tra = somma(1,2); sz_tra = somma(1,3); M2x_tra = [1, 0, 0; 0, cos(A_camp_rot*(ir2x-camp-1)*pi/180), sin(A_camp_rot*(ir2x-camp-1)*pi/180); 0, -sin(A_camp_rot*(ir2x-camp-1)*pi/180), cos(A_camp_rot*(ir2x-camp-1)*pi/180)]; M2y_tra = [cos(A_camp_rot*(ir2y-camp-1)*pi/180), 0, -sin(A_camp_rot*(ir2y-camp-1)*pi/180);

0, 1, 0;

sin(A_camp_rot*(ir2y-camp-1)*pi/180), 0, cos(A_camp_rot*(ir2y-camp-1)*pi/180)]; M2z_tra = [cos(A_camp_rot*(ir2z-camp-1)*pi/180), sin(A_camp_rot*(ir2z-camp-1)*pi/180), 0;

-sin(A_camp_rot*(ir2z-camp-1)*pi/180), cos(A_camp_rot*(ir2z-camp-1)*pi/180), 0; 0, 0, 1]; else M2x_tra = Mx_tra* [1, 0, 0; 0, cos(A_camp_rot*(ir2x-camp-1)*pi/180), sin(A_camp_rot*(ir2x-camp-1)*pi/180); 0, -sin(A_camp_rot*(ir2x-camp-1)*pi/180), cos(A_camp_rot*(ir2x-camp-1)*pi/180)]; M2y_tra = My_tra* [cos(A_camp_rot*(ir2y-camp-1)*pi/180), 0, -sin(A_camp_rot*(ir2y-camp-1)*pi/180); 0, 1, 0; sin(A_camp_rot*(ir2y-camp-1)*pi/180), 0, cos(A_camp_rot*(ir2y-camp-1)*pi/180)]; M2z_tra = Mz_tra* [cos(A_camp_rot*(ir2z-camp-1)*pi/180), sin(A_camp_rot*(ir2z-camp-1)*pi/180), 0; -sin(A_camp_rot*(ir2z-camp-1)*pi/180), cos(A_camp_rot*(ir2z-camp-1)*pi/180), 0; 0, 0, 1]; end end end