A.5 Determinazione delle forze agenti sulle zampe della capsula

A.5 Determinazione delle forze agenti sulle zampe della capsula

%DEFINIZIONE DEI PARAMETRI DI ANALISI UTENTE (le dimensioni sono in SI ) %Definizione della discretizzazione spaziale e temporale

N = input('Inserire il numero di segmenti longitudinali N='); for u = [0:1000]

%Controllo del dato in ingresso, N deve essere maggiore di 1 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]

%Controllo del dato in ingresso, N deve essere maggiore di 0 if M==0 M = input('Inserire un valore di M>0, M='); u = 0; else u = 1000; end end %%%%%%%%

%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]

%%%%%%%%

%CICLO DI ANALISI TEMPORALE DELLE FORZE APPLICATE ALLA CAPSULA for tempo = [1:M]

tempo %Introdotta per il controllo dell'istante di analisi %INTRODUZIONE DEI PUNTI

%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

%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)]; %Matrice delle coordinate dei vertici anteriori

%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)]; %Matrice delle coordinate dei vertici posteriori

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)];

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

%X è la matrice dei punti utilizzati per la modellazione del substrato 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

%%%%%%%%%%

%COSTRUZIONE DELLA TRIANGOLAZIONE

%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 mettono %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);

%%%%%%%%%

%INTRODUZIONE DELLE GRANDEZZE NECESSARIE ALL'ANALISI

%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 ed %associati ad un dato piano di taglio trasversale

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 = [];

%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 = [];

%Vettore di raccolta delle informazioni relative al contatto zampa-substrato contatto = [];

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

%DETERMINAZIONE DEI PUNTI RAPPRESENTANTI LA SEZIONE TRASVERSALE

%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';

%Si costruisce il vettore k in cui si ordinano tutti i triangoli cui corrispondono %intersezioni col piano

k = (da>0)+(db>0)+(dc>0); k = find((k= =1)|(k= =2));

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

INTERSEZIONE 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]; 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

%Ciclo di controllo dei dati raccolti in xyzp 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

%Esclusione dei termini che si ripetono in xyzp xyz = unique(xyzp,'rows');

dimxyz = size(xyz,1);

%Quindi si introducono una serie di matrici utilizzate da appoggio per il processo di %ordinamento xyz11 = []; xyz21 = []; xyz22 = []; xyz12 = []; xyz01 = []; xyz10= []; xyz20 = []; xyz02 = [];

%Si entra in un ciclo che scorre le righe della matrice dei punti di intersezione %mandandole 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

%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

end

%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)] 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 a partire %dall'asse 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 zamp

v_app = 0; %v_app nulla = assenza di contatto for i = [1:dimxyzord]

if xyzord(i,:)= =X(N+nzampa,:) v_app = v_app+1;

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

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

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*180)/pi];

end

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

%CALCOLO DELLE FORZE NORMALI SULLE ZAMPE

%CALCOLO DEI MODULI DELLE FORZE NORMALI if lun = = 0 %Se non c'è contatto

def = 0;

epsilon = [epsilon; def]; SIGMA = [SIGMA; 0]; FORZA = [FORZA; 0]; else %Se c'è contatto

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

%DETERMINAZIONE DELLE GRANDEZZE NECESSARIE AL CALCOLO

%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 = [];

%Definizione della matrice 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

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

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

end

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

%CALCOLO DEO VERSORI DA SOMMARE

%Introduzione delle matrici dei versori

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

%VERS_MED è la matrice dei versori mediasti 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

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

%CALCOLO DEI PESI ASSEGNATI AI VERSORI E SOMMA

%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

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;

%Quindi se ne prende modulo (che coincide con il coseno dell'angolo tra un lato e %l'altezza stessa) 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];

% 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 end %risultante %%%%%%%%%%%%%%

%INTRODUZIONE DEI COEFFICIENTI RIDUTTIVI PER EFFETTO DELL'INTERAZIONE

%Introduzione dei vettori e delle matrici necessarie all'analisi serie = [];

V_app = []; V_appord = []; V_app_ind = [];

%Selezione delle zampe effettivamente in contatto col substrato 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

%Ordinamento delle zampe in funzione della loro posizione assiale 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 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] 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

%Introduzione dei vettori e dei coefficienti 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); 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

if 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); 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 FORZACOMP = []; for i = [1:6] for j = [1:3] FORZACOMP(i,j) = FORZA(i,1)*risultante(i,j)*coeff_rid(i,1); end end %%%%%%%%%%%%%

%DETERMINAZIONE DELLE FORZE DI ATTRITO SULLE ZAMPE %DETERMINAZIONE DEI MODULI DELLE FORZE DI ATTRITO

%Si calcola il coefficiente di attrito come funzione della deformazione epsilon e quindi %si moltiplica per la forza normale

mod_fatt = []; mu = []; for i = [1:6] 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

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

%CALCOLO DELLA DIREZIONE DELLA FORZA DI ATTRITO %Definizione delle matrici e dei vettori necessari

FATT = []; vers_att = [];

%Si utilizza un doppio prodotto vettoriale for i = [1:6]

if risultante(i,:) ~= [0, 0, 0] %Indica la presenza di contatto

%DEFINIZIONE DELLA POSIZIONE DELLE ZAMPE ALLO STEP PRECEDENTE

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];

V_f_2_prec=V_f_1_prec*[1 0 0;0 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)];

%Matrice di raccolta dei vertici anteriori

V_f_prec=[V_f_1_prec;V_f_2_prec;V_f_3_prec];

%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 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_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)];

%Matrice di raccolta dei vertici posteriori

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 1/3*pi) sin(-1/3*pi);0 -sin(-1/3*pi) cos(-1/3*pi)];

%%%%%%%%%%

%DEFINIZIONE DELLA MATRICE DI RACCOLTA DELLE POSIZIONI ALLO % STEP PRECEDENTE %Introduzione di Vprec Vprec = []; %Definizione di Vprec

Vprec = [V_f_prec; V_r_prec];

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

%Il calcolo è fatto proiettando il vettore di spostamento della zampa su un piano %ortogonale alla risultante delle forze normali applicate alla stessa articolazione. La %proiezione si ottiene con un doppio prodotto vettore

%Vettore di spostamento spostamento = Vprec(i,:)-V(i,:); %Calcolo direzione 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 è il versore in cui riga per riga si raccolgono le %direzioni appena calcolate

vers_att = [vers_att; ver_secondo];

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

else %Cioè se non c'è contatto vers_att = [vers_att; 0,0,0]; FATT = [FATT; 0,0,0]; end

end

%%%%%%%%%

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

FORZARIS = FORZACOMP + FATT; FORZACOMP

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

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

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

%%%%%%%%%%

%CALCOLO RISULTANTI COMPLESSIVE %Calcolo delle risultanti

mom = sum(MOMENTO,1); forza_ris = [forza_ris; forz];

momento_ris = [momento_ris; mom]; end