From d2ecff231612ef9eb6c718ab24980ac9e68ae3e4 Mon Sep 17 00:00:00 2001
From: Philippe Pastor <philippe.pastor@isae.fr>
Date: Tue, 1 Oct 2024 16:50:58 +0200
Subject: [PATCH] 1er commit sur phil
---
drone/__pycache__/drone_env.cpython-311.pyc | Bin 4741 -> 4678 bytes
drone/__pycache__/drone_model.cpython-311.pyc | Bin 5833 -> 5607 bytes
drone/drone_env.py | 2 +-
drone/drone_model.py | 20 +++++++++-------
drone/main.py | 22 +++++-------------
mavion/mavion_env.py | 1 -
6 files changed, 18 insertions(+), 27 deletions(-)
diff --git a/drone/__pycache__/drone_env.cpython-311.pyc b/drone/__pycache__/drone_env.cpython-311.pyc
index f8a960b9f5f4a46e6d430e0a89bdd889752e2fb5..4fbfcf59f7b588a04139d6fc6c1f88b32cbc0a54 100644
GIT binary patch
delta 235
zcmZowJ*L9DoR^o20SL~s{z;46$ScXm#R}vx1M%k@lMVPxg=!eGKs+#524t;fo;;UN
znvrGlPCnIokXkSRs=~^yVqsuNVax`rsb#KVE@Dby%wtSps%1gZ!BWGV!ko#F#v06^
z$>LYU4pd*n1SH%v*@{Gf^evX+lGK7C4Uo7fh;RfEnv-+*BjrJcH!$1~k(lB&-G7q*
a0+%IT8`yTR?BTd5;&?^Gaq~O==}Z9WL@s9l
delta 290
zcmX@6(yGe4oR^o20SE-2{7y^T$ScXm#SY{#1M%m7lMVPxLrOq=D9B=gu~Hb9F)}c$
z24V=PVaQ^Iv6lf=t%j?tWvXGSVTfmf$<{C|V4EDmqgoF)7pNC2y9#JV3R5;%V=Z$H
za}iStQyyaqb1e&s4wf3`6qZbeG}d4SO;*1mUZDCSka<NQE8R3XibR0)EtcYv)Pf=n
wkhmC#P@k;9pTSr<c_P1v$X=#B90vq<a_`|j5OO5qqKMxW5x>pr`KL1h06U8@e*gdg
diff --git a/drone/__pycache__/drone_model.cpython-311.pyc b/drone/__pycache__/drone_model.cpython-311.pyc
index e0b739228ad83fa0adf9aeadf660a82692989f3f..8ed2baa184362bf487378913b8d226fb07a0ec34 100644
GIT binary patch
delta 1552
zcmaKsO>7fK6vt;~ckT7Z+TO--4PK=Vgb>$_NkW^Vri#;wA_O8;n$VUM1dbP@A`>^W
zt`u3jiVq;1E7K~biiBIC)QDVAxp8R^J#eY8<U`~`uN6I1Mo<Y=Rn<4^j}Vn~wEN$A
zZ{EJ0`OTYsIQ&~&`Cd`n2-<-3cjnkV<%|=5$1XVsR?4{Czb=-SD(ZUvrl*ps;@f&H
z{Cah!y0kf03y*Gv`zrmF_ihhV)#~VG@Y{GToOro!&;KWCI)%{TCrD7<e8#1On=%Th
zD<zrkgK4D&KDl=Dz?0igMS{>8GSTJE2(kpQaL2Snupp+@EXl+=*2y)pr{hAF1AIgm
zcKDp&6LgXLY$iO~lG(3v<e+d#*nsqRyPwb<rdWUp<@u!A&2&KQhuFmuu3;>(v#z(_
zl1<5!3nD@rFrB;IDHR|;_gZe#X?b)x-_yx~5M=qq6~Qw5-L)Tkna};I(_?x~m+5A2
zxJy2TY7pDN%k&UD+0X7HqQX1Gv(GsM9pfLw;+~&=XnL|Z=l};#{qzRN*JKT?k_uZ=
zf)ZL4Rtftt5MtMq0Q<}3Td^>wv`vWf*<lGgEOFb)h_fgrP1B?B4SE>lF=z7F3v!3h
z_qlfjBqq{9cr=K<Y%hVn1C{|0P&8UBGRt@1;0N531bJRWTWarG_(r(gU!J=)|IK_A
z-#oW|uBOK8YMgEP652@!qg{dId_oPWaFON=7WMDNC)k943@6#5|9Bs9jW*FP@~6z|
z{t*_Dy{DXz!Y;4{=v+tF0a%HYP=@IEs%(k*?(V}bi8?Wb3~80jrnDi!{=(gAP;wGW
z1tvTngKcvcZ0_tqAkJy;pG9}@G(8TZF|mR34Q$wOY%pymY*0Pqn1%@BHTIdh*HP2P
z>)JT8)ME!6+W<RE^YjFSHUuM=HTbSyh_XM_h<1v<ZxiK6{o{{bxLE9!u@%n0)4I!_
z1P?4<faMFjE#Jf@;U{QQ{O{83dPsMx9kRLLwhNIT*)|a9A@(Jd--AHJN1!xKWa8F2
zVSOaTInB=^ItO8GQuH+MbEXX`lg`uZi@b_O8s)>EfD|G}X>Rdi!ub43;@xa6oyr=M
z&`Dic&{LDM5MadF!yfJQy@~r1wV`BvC|T2{>e^Hh*?%4SDNl5U%RCnjDb|ACaOBS$
zU!)crWO;<9`5@;`&x5qNN7>n6So?@O#z3Ci;HMt>w=ecpa3Nryiy8jz1uhxp=$#ps
yCAEpV66}?ZG*MR)UOw6tja}_6;mb@3g>jAzhV<k##yw5+a(!a^pY1YguliqAFHTJW
delta 1758
zcmaJ>TTC2P7(Qn%dzoeS#zihG85(LJ<yuHB(x@o4G$6WJD3of;mRX>%g)+MogxSD@
z%I;Gyr<IK{5e-F<M$x`dB@q)JXyU_6vKrj@-t<XKH6i*?|Fa7NB&~mDzVlzsf6jlt
z|I9o*@}OM&LKFoAa`C%=dmFzI&zkWv=`$BkUH8ZFEjjLuk9^X0V{oov&OhI%=aei~
zFM99R>N%AwIjhyH-nH8GqV<NKitpF!)u%RcG+&$6ekS4T(7MiP@4TDnI<NcA>!&Yh
z{)<|VobU&<{!7}xV4^>$`-9r>6>UUG42N`oNV__wjlY+;I-&a~wCqVeV@geAs8PJ>
za7Hc4A*ip1qgC#R4MrB;#{z@UEK<?HZUjX*u<))K;lYAAX^EIrEMuAZfXUl+p@<oL
zjLaVJS=4qcvod$UX$_l=EHq%0Bl9w^a((OqY`Sc0ESpq*0?G{2Qg=i(lV2=vVSyaB
zmSYp~SzqL>s!6qyYu17Sn`%+n378qlkS*cN-5i7<>j!HAYwcsm6N|(|MDoP?oH;`k
zRYA3p=Y^=tu6SU25|1mDsd0?l6OQwC&?WMRaL9ZZv=D<yuI)$ex;Hj<?>Acz3-mB_
zo5yRU$t{tX=$MM&7&~Vi0b+*4I2DI^!$bp={glY~;K3IlvTQ^njLOLS*anN;;~p$-
zpA}$ae$VXXe5lmas#L%>%1H_}0FNl2k1_yCIi-qvU#CFRlKezK84i{5$_vz$;)0RK
zQ{*Twfh$B0Adw<_>9N<S*$S{djW(TGv%*awUVTHnB}N-IUD>m_H*@1faqk^w-m>WW
z%r<Y+U1bSZ85yxx<hMYUVgOV@_f}2=knipJxS9NFKZ;KftD~uqwN`JTXTaYA8Fo}*
zp3FJ2%6cJt2F)-t>@12hQ50o+SsA}?iSXfl?NE7{Q91IvBabzyCbI3QH+4edZI-<0
zt|gb9&R1jHLHd1Y3AZjWX^tpcrF;^HlQ@*7h+Z3|9w5z(G)=y8R+iASg^B=xn|R}=
zO&>SSc^A2_g)fDb>Lu~6xH6)9>Jpwh!n)o#V&0=Sy(V2Q$;e@)nJR2(I2fWgty}6Q
zL#_gedPB5j+uM|SN1b-?A$MFy)1;Kp9sw!r3<spJQ7(#}P6~0l^GbjaxGsEakMXfX
zwg_+Pihl&kZQwS>5wANBb7at6=wVQdRgsDpAZ6J|OA|=0xpTVzZvrfs5+739KvCKO
z;O3p7dIvy~@9haIK^Vp^4aq5zc1fO|RwO#DlRR`wCNHR=Q$)=4Os$Ttj;?z*@cqfP
zNxh~yQPZrKv?NMeHUb*~t*u?_=zK8v^PukON_e`aQR=U#oTV#uQ8-6|{*_A8JxQ9v
z7eZJ3LrG@5N|9+WK!G+u=?2(oi`3mMNvVR!hnY_4Z5lZSu+yI|&2iu%-)3HPran1Z
zxZ<bKPl~g=<vj1&LI&8g`Quy2fC`aD4Jag^XGQaaH2Dw(3E&^giOa+CXfW`yGC~@&
W<yTrT&fG#z$3Ij4zn&3IE&3nW{f&bF
diff --git a/drone/drone_env.py b/drone/drone_env.py
index 87d0fb9..fc33374 100644
--- a/drone/drone_env.py
+++ b/drone/drone_env.py
@@ -64,7 +64,7 @@ class DroneEnv(gym.Env):
return self.obs, {}
def step(self, action):
- new_state = self.drone.step(self.state, action, np.zeros(3), self.tau)
+ new_state = self.drone.step(self.state, action, self.tau)
self.state = new_state
self.obs = self.observation()
terminated = (self.obs@self.obs < 1)
diff --git a/drone/drone_model.py b/drone/drone_model.py
index 2fd00d6..aeb66a1 100644
--- a/drone/drone_model.py
+++ b/drone/drone_model.py
@@ -31,10 +31,13 @@ class Drone:
# position of rotor wrt center of gravity
P_R_CG = self.P_R_CG
T, N = self.thrust(n)
- u = np.array([T[0] + T[1] + T[2] + T[3], P_R_CG * (T[3] - T[2]), P_R_CG * (T[0] - T[1]), N[2] + N[3] - N[0] - N[1]])
+ u = np.array([ T[0] + T[1] + T[2] + T[3],
+ P_R_CG * (T[3] - T[2]),
+ P_R_CG * (T[0] - T[1]),
+ N[2] + N[3] - N[0] - N[1]])
return u
- def dyn(self, x, u, w):
+ def dyn(self, x, u):
G = self.G
MASS = self.MASS
@@ -52,7 +55,7 @@ class Drone:
drdt = np.linalg.inv(INERTIA) @ (u[1:4] - np.cross(rot, INERTIA@rot))
# kinematic equations
- dpdt = vel + w
+ dpdt = vel
dqdt = 1/2 * quatmul(quat, np.concatenate([[0], rot]))
return np.concatenate([dpdt, dqdt, dvdt, drdt])
@@ -64,21 +67,20 @@ class Drone:
x[9] = vz
def func(n):
- u = self.u_int(n)
- dx = self.dyn(x, u, np.zeros(3))
+ dx = self.dyn(x, self.u_int(n))
return dx[9], dx[10], dx[11], dx[12]
y0 = np.array([100, 100, 100, 100])
y = fsolve(func, y0)
return y
- def step(self, x, n, w, dt):
- func = lambda t, s: self.dyn(s, self.u_int(n), w)
+ def step(self, x, n, dt):
+ func = lambda t, s: self.dyn(s, self.u_int(n))
sol = solve_ivp(func, (0, dt), x, method='RK45', max_step=0.01)
return sol.y.T[-1]
- def sim(self, t_span, x0, fctrl, fwind):
- func = lambda t, s: self.dyn(s, self.u_int(fctrl(t, s, fwind(t, s))), fwind(t, s))
+ def sim(self, t_span, x0, fctrl):
+ func = lambda t, s: self.dyn(s, self.u_int(fctrl(t, s)))
sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
return sol
diff --git a/drone/main.py b/drone/main.py
index d3e397a..666c370 100644
--- a/drone/main.py
+++ b/drone/main.py
@@ -24,9 +24,6 @@ if __name__ == "__main__":
vh = 0
vz = 0
- # cap = 0
- # omega = 0
-
n0 = drone.trim(vh, vz)
print(n0)
@@ -35,24 +32,17 @@ if __name__ == "__main__":
obs0 = drone_env.reset()[0]
x0 = drone_env.state
print(drone_env.state)
- obs1 = drone_env.step(n0)[0]
+ obs1 = drone_env.step(n0)
+ print(obs0)
print(drone_env.state)
- def ctl(t, s, w):
- # d = (upulse(t, 5, 6))*dx*0.1
- d = (upulse(t, 1, 1.1)-upulse(t, 1.1, 1.2))*np.linalg.norm(n0)*np.array([0.01, -0.01, 0.0, 0.0])
- n = n0 + d*0.0
- return n
- def pid(t, s, w):
- n = (10 * s[2])*np.array([1, 1, 1, 1])
+ def ctl(t, s):
+ dn = (upulse(t, 1, 2) - upulse(t, 2, 3))*np.linalg.norm(n0)*np.array([0.01, 0.01, 0.01, 0.01])
+ n = n0 + dn
return n
- def wnd(t, s):
- w = np.zeros(3)
- return w
-
- sim = drone.sim((0, 20), x0, pid, wnd)
+ sim = drone.sim((0, 20), x0, ctl)
fig, axs = plt.subplots(2, 2)
diff --git a/mavion/mavion_env.py b/mavion/mavion_env.py
index 45996ce..9675959 100644
--- a/mavion/mavion_env.py
+++ b/mavion/mavion_env.py
@@ -67,4 +67,3 @@ class MavionEnv(gym.Env):
reward = 1 if terminated else 0 # Binary sparse rewards
self.obs = self.state - self.target
return self.obs, reward, terminated, False, {}
-
--
GitLab