From 3f72877fc26ecaeac1f8a3d082aea1c376c03e46 Mon Sep 17 00:00:00 2001
From: Philippe Pastor <philippe.pastor@isae.fr>
Date: Fri, 13 Sep 2024 13:58:35 +0200
Subject: [PATCH] change plot (euler angles)
---
mavion/__pycache__/ctlfun.cpython-311.pyc | Bin 0 -> 716 bytes
mavion/ctlfun.py | 12 ++---
mavion/mavion.py | 57 +++++++++++++---------
mavion/mavion.yaml | 2 +-
4 files changed, 38 insertions(+), 33 deletions(-)
create mode 100644 mavion/__pycache__/ctlfun.cpython-311.pyc
diff --git a/mavion/__pycache__/ctlfun.cpython-311.pyc b/mavion/__pycache__/ctlfun.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..69fa0ba76d54ea451fc7acd6bf533fec8e04f48a
GIT binary patch
literal 716
zcmZuuze~eF6u!G8wb(|bAc#2BMeLxsxQGZ92VJVzPLiecQvA`T@h%aR7CJZtr{dDO
zxk>*U$C61X2;B<ZoP6)vhEjdxzI*T9dw1Xa@?n~b1Z<!6%ipl~F_;>gIHNQ;ZNdqs
z6>>#6dnQ+mYk(Tp0d-FEBx4M)JXg|in;71J@n{R6P3ojh?^H_0z-M;`27+<wz^x6f
zuH{Kr%L!~7P=GGU=ppq&=Iy~Mht!uk9H+s4wyzJY3sJkZJ!zd>p6AZ8*3C|}_+Y`h
zsX123tCW3b*L@6QzUx<{Et1fR?J^C}B)z5O7JK0VZxuVKSG#2gG@$RB&1^4`Y@Uvw
zhLolfU=R(n4#Dg&(EzN@@<iYhA_maa!~*(p1e95#7QkI3U=IyndbTT8z{d)?1^}$g
zr9iwCf&#K>-A~vE6O#Y4Nkirj8y%&7f;StAQ-+O2RkXx1bj(u`#i}b(kU){-2EZJZ
z4(%Mj#b)VbjL>lWDhSLRSBa};QMZgALegs1pZH<5#{G(YBv#=Jf2}+P7;2Q#FOvG4
VenXv6b4VhHN1i&H3!^$E&oA_)kHi1~
literal 0
HcmV?d00001
diff --git a/mavion/ctlfun.py b/mavion/ctlfun.py
index 2f78993..80f61a7 100644
--- a/mavion/ctlfun.py
+++ b/mavion/ctlfun.py
@@ -1,20 +1,14 @@
import numpy as np
-def pulse(t, t0, tf):
+def upulse(t, t0, tf):
u = 0 if (t<t0 or t>=tf) else 1
return u
-def step(t, t0) :
+def ustep(t, t0) :
u = 0 if t<t0 else 1
return u
-def ramp(t, t0) :
+def uramp(t, t0) :
u = 0 if t<t0 else (t - t0)
return u
-
-time = np.arange(0, 20, 0.01)
-
-p = pulse(time, 1, 2)
-
-print(p)
diff --git a/mavion/mavion.py b/mavion/mavion.py
index 6532643..992b760 100644
--- a/mavion/mavion.py
+++ b/mavion/mavion.py
@@ -5,6 +5,7 @@ from gymnasium.envs.classic_control import utils
import yaml
from scipy.optimize import fsolve
from scipy.integrate import solve_ivp
+from ctlfun import *
deg2rad = 2 * np.pi / 360
@@ -69,16 +70,8 @@ def eul2dcm(a):
return Rx @ Ry @ Rz
-
-def s2q(x):
- quat = eul2quat(x[3:6])
- x = np.concatenate([x[0:3], quat, x[6:12]])
- return x
-
-def q2s(x):
- eul = quat2eul(x[3:7])
- x = np.concatenate([x[0:3], eul, x[7:13]])
- return x
+s2q = lambda x : np.concatenate([x[0:3], eul2quat(x[3:6]), x[6:12]])
+q2s = lambda x : np.concatenate([x[0:3], quat2eul(x[3:7]), x[7:13]])
class Mavion:
@@ -324,7 +317,7 @@ class Mavion:
return sol.y.T[-1]
def sim(self, t_span, x0, fctrl, fwind):
- func = lambda t, s: self.dyn(s, fctrl(t, s, w), fwind(t, s))
+ func = lambda t, s: self.dyn(s, fctrl(t, s, fwind(t, s)), fwind(t, s))
sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
return sol
@@ -390,7 +383,7 @@ class MavionEnv(gym.Env):
if __name__ == "__main__":
mavion = Mavion()
- vh = 5
+ vh = 0
vz = 0
dx, de, theta = mavion.trim(vh, vz)
theta = (theta + np.pi) % (2*np.pi) - np.pi
@@ -401,25 +394,43 @@ if __name__ == "__main__":
vel = np.array([vh, 0, vz])
rot = np.array([0, 0, 0])
st = np.concatenate([pos, eul, vel, rot])
- x = s2q(st)
+ x0 = s2q(st)
- u = np.array([-dx, dx, de, de])
- w = np.zeros(3)
+ def ctl(t, s, w):
+ d = upulse(t, 5, 6)*dx*0.0
+ u = np.array([-dx-d, dx+d, de, de])
+ return u
+
+ def wnd(t, s):
+ w = np.zeros(3)
+ return w
- def ctl(t, s, w): return u
- def wnd(t, s): return w
+ sim = mavion.sim((0, 60), x0, ctl, wnd)
- sim = mavion.sim((0, 20), x, ctl, wnd)
+ plt.style.use('_mpl-gallery')
+ fig, axs = plt.subplots(2, 2)
- fig, axs = plt.subplots(2, 2, layout='constrained')
ax = axs[0][0]
- ax.plot(sim.t, sim.y[0:3].T)
+ ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
+ ax.legend()
+ ax.set_title(label='positions')
+
ax = axs[1][0]
- ax.plot(sim.t, sim.y[3:7].T)
+ quat = sim.y[3:7].T
+ eul = np.apply_along_axis(quat2eul, 1, quat)
+ ax.plot(sim.t, eul*57.3, label=('phi','theta','psi'))
+ ax.legend()
+ ax.set_title(label='euler angles')
+
ax = axs[0][1]
- ax.plot(sim.t, sim.y[7:10].T)
+ ax.plot(sim.t, sim.y[7:10].T, label=('u','v','w'))
+ ax.legend()
+ ax.set_title(label='vitesses')
+
ax = axs[1][1]
- ax.plot(sim.t, sim.y[10:13].T)
+ ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
+ ax.legend()
+ ax.set_title(label='rotations')
plt.show()
diff --git a/mavion/mavion.yaml b/mavion/mavion.yaml
index a1abdd6..63ef25c 100644
--- a/mavion/mavion.yaml
+++ b/mavion/mavion.yaml
@@ -19,6 +19,6 @@ DRY_SURFACE : 0.0
PHI_n : 0.0
PROP_RADIUS : 0.105
ELEVON_MEFFICIENCY : [0, 0.66, 0]
-ELEVON_FEFFICIENCY : [0, 0.33,0]
+ELEVON_FEFFICIENCY : [0, 0.33, 0]
PROP_KP : 7.84e-06
PROP_KM : 2.400e-7
--
GitLab