1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112
|
clear all,
close all,
%declaration des constantes
g=9.8;
Tsup=0.8;%temps de support
z=0.8;%hauteur du centre de masse considérée constante pour un modele LIP
a=10;
b=1;
Tc=sqrt(z/g);
pas=0.02;
sx=[0.0 0.3 0.3 0.3 0];%longueur des pas
sy=[0.2 0.2 0.2 0.2 0.2];%largeur des pas
%Initialisation
%etape2:
CI1=[-0.151,0.467];
pxmp=0;
pymp=0;
pxpreviousm=0;
pypreviousm=0;
n=length(sx);
px=zeros(1,n);
py=zeros(1,n);
pxm=zeros(1,n);%la position modifiée du pied selon x
pym=zeros(1,n);%la position modifiée du pied selon y
xd=zeros(1,n);
yd=zeros(1,n);
vxd=zeros(1,n);
vyd=zeros(1,n);
x1=zeros(1,n);
vx1=zeros(1,n);
y1=zeros(1,n);
vy1=zeros(1,n);
%à verifier les deux equation prochaines.
xbar=zeros(1,n);
ybar=zeros(1,n);
vxbar=zeros(1,n);
vybar=zeros(1,n);
t=0;
n=5;
%etape3:condition initiales
%integration de l'equation de la dynamique
%selon x:
x10=(CI1(1)-pxpreviousm)*cosh(t/Tc)+Tc*CI1(2)*sinh(t/Tc)+pxpreviousm;
vx10=((CI1(1)-pxpreviousm)/Tc)*sinh(t/Tc)+CI1(2)*cosh(t/Tc);
%selon y:
y10=(CI1(1)-pypreviousm)*cosh(t/Tc)+Tc*CI1(2)*sinh(t/Tc)+pypreviousm;
vy10=((CI1(1)-pypreviousm)/Tc)*sinh(t/Tc)+CI1(2)*cosh(t/Tc);
xi=x10;
yi=y10;
vxi=vx10;
vyi=vy10;
C=cosh(Tsup/Tc);
S=sinh(Tsup/Tc);
%la primitive de marche initiale
xbar0=sx(1)/2;
ybarf0=1*sy(1)/2;%(-1)^(r)ddans ce cas est egale à 1 car r=0;
for r=1:n
if r<n
%Etape 3:
%selon x:
x1(r)=(xi-pxmp)*cosh(t/Tc)+Tc*vxi*sinh(t/Tc)+pxmp;
vx1(r)=((xi-pxmp)/Tc)*sinh(t/Tc)+vxi*cosh(t/Tc);
xi=x1(r);
vxi=vx1(r);
%selon y:
y1(r)=(yi-pymp)*cosh(t/Tc)+Tc*vyi*sinh(t/Tc)+pymp;
vy1(r)=((yi-pymp)/Tc)*sinh(t/Tc)+vyi*cosh(t/Tc);
yi=y1(r);
vyi=vy1(r);
%Etape4:
for t=t:pas:t+Tsup
%Etape5:
px(1,:)=pxpreviousm+sx(r);
py(1,:)=pypreviousm-((-1)^r)*sy(r);%si on choisit le pied gauche comme point de départ de la marche on remplace -(-1)^n par +(-1)^n
pxpreviousm=px(1,:);
pypreviousm=py(1,:);
D=a*((C-1)^2)+b*(S/Tc)^2;
%Etape6:les caracteristiques de la primitive de marche suivante
%Ajouter une condition
xbar(r)=sx(r+1)/2;
ybar(r)=(-1)^(r)*sy(r+1)/2;
vxbar(r)=(cosh(t/Tc)+1)/((Tc*sinh(t/Tc))*xbar(r));
vybar(r)=(cosh(t/Tc)-1)/((Tc*sinh(t/Tc))*ybar(r));
%Etape7:calcul de l'etat cible
xd(r)=px(r)+xbar(r);
vxd(r)=vxbar(r);
yd(r)=py(r)+ybar(r);
vyd(r)=vybar(r);
%Etape8:calcul de la position modifiée du pied
pxm(r)=-((a*C-1)/D)*(xd(r)-C*xi-Tc*S*vxi)-(b*S/Tc*D)*(vxd(r)-(S/Tc)*xi-C*vxd(r));
pym(r)=-((a*C-1)/D)*(yd(r)-C*yi-Tc*S*vyi)-(b*S/Tc*D)*(vyd(r)-(S/Tc)*yi-C*vyd(r));
pxmp=pxm(r);
pymp=pym(r);
end
end
end |
Partager