version 1.0

drone/drone.yaml

+---
+NAME : "DRONE"
+G :     9.81
+RHO :   1.225   
+MASS :  0.5
+P_R_CG : 0.1
+INERTIA : [0.0036, 0.0036, 0.0072]
+PROP_KP : 7.84e-06 
+PROP_KM : 2.400e-7     

drone/drone_env.py

+import numpy as np
+import gymnasium as gym
+from gymnasium.envs.classic_control import utils
+
+from drone_model import Drone
+from rotation import quat2eul
+
+deg2rad = np.pi/180
+rad2deg = 1 / deg2rad
+
+class DroneEnv(gym.Env):
+
+    def __init__(self, drone=Drone(), render_mode=None):
+
+        self.drone = drone
+        self.tau = 0.02  # seconds between state updates
+        
+        # Angle at which to fail the episode
+        self.pos_threshold = np.array([2.4, 2.4, 2.4])
+        self.ang_threshold = np.array([15, 15, 15]) * deg2rad
+        self.vel_threshold = np.finfo(np.float32).max * np.ones(3)
+        self.rot_threshold = np.finfo(np.float32).max * np.ones(3)
+
+        high = np.concatenate(
+            [
+                self.pos_threshold * 2,
+                self.ang_threshold * 2,
+                self.vel_threshold,
+                self.rot_threshold
+            ],
+            dtype=np.float32,
+        )
+        self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
+        self.action_space = gym.spaces.Box(0, 1, shape=(4,), dtype=np.float32)
+        self.state = np.zeros(13)
+        self.obs = np.zeros(12)
+        self.target = np.zeros(12)
+        self.steps_beyond_terminated = None
+
+    def observation(self):
+        pos = self.state[0:3]
+        vel = self.state[7:10]
+        rot = self.state[10:13]
+        quat = self.state[3:7]
+        ang = quat2eul(quat)
+        d_pos = pos - self.target[0:3]
+        d_ang = ((ang - self.target[3:6]) + np.pi) % (2*np.pi) - np.pi
+        d_vel = vel - self.target[6:9]
+        d_rot = rot - self.target[9:12]
+        obs = np.concatenate([d_pos, d_ang, d_vel, d_rot])
+        return obs
+
+    def reset(self, seed=None, options=None):
+        super().reset(seed=seed)
+        # Note that if you use custom reset bounds, it may lead to out-of-bound
+        # state/observations.
+        low, high = utils.maybe_parse_reset_bounds(
+            options, -0.05, 0.05  # default low
+        )  # default high
+        # self.state = self.np_random.uniform(low=low, high=high, size=(13,))
+        self.state = np.array([0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+        self.steps_beyond_terminated = None
+        self.obs = self.observation()
+        return self.obs, {}
+
+    def step(self, action):
+        new_state = self.drone.step(self.state, action, np.zeros(3), self.tau)
+        self.state = new_state
+        self.obs = self.observation()
+        terminated = (self.obs@self.obs < 1)
+        reward = 1 if terminated else 0  # Binary sparse rewards
+        return self.obs, reward, terminated, False, {}

drone/drone_model.py

+import numpy as np
+from scipy.optimize import fsolve
+from scipy.integrate import solve_ivp
+import yaml
+from rotation import quatrot, quatmul
+
+class Drone:
+
+    def __init__(self, file="drone.yaml"):
+        stream = open(file, "r")
+        drone = yaml.load(stream, Loader=yaml.Loader)
+        self.NAME = drone['NAME']
+        self.RHO = drone['RHO']
+        self.G = drone['G']
+        self.MASS = drone['MASS']
+        self.INERTIA = np.diag(drone["INERTIA"])
+        self.P_R_CG = np.array(drone["P_R_CG"])
+        self.PROP_KP = drone['PROP_KP']
+        self.PROP_KM = drone['PROP_KM']
+
+    def thrust(self, n):
+        # prop forces computation / notice that negative thrust is not implemented
+        Kp = self.PROP_KP
+        T = Kp * n**2
+        # prop moments computation
+        Km = self.PROP_KM
+        N = np.sign(n) * Km * n**2
+        return T, N
+
+    def u_int(self, n):
+        # 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]])
+        return u
+
+    def dyn(self, x, u, w):
+
+        G = self.G
+        MASS = self.MASS
+        INERTIA = self.INERTIA
+
+        # state demultiplexing
+        quat = x[3:7]
+        vel = x[7:10]
+        rot = x[10:13]
+
+        # linear velocity derivative
+        dvdt = np.array([0, 0, G]) - 1/MASS * quatrot(quat, np.array([0, 0, u[0]]))
+
+        # angular velocity derivative
+        drdt = np.linalg.inv(INERTIA) @ (u[1:4] - np.cross(rot, INERTIA@rot))
+
+        # kinematic equations
+        dpdt = vel + w
+        dqdt = 1/2 * quatmul(quat, np.concatenate([[0], rot]))
+
+        return np.concatenate([dpdt, dqdt, dvdt, drdt])
+
+    def trim(self, vh, vz):
+        x = np.zeros(13)
+        x[3:7] = np.array([1, 0, 0, 0])
+        x[7] = vh
+        x[9] = vz
+
+        def func(n):
+            u = self.u_int(n)
+            dx = self.dyn(x, u, np.zeros(3))
+            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)
+        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))
+        sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
+        return sol
+

drone/main.py

+import numpy as np
+import matplotlib.pyplot as plt
+from drone_model import Drone
+from drone_env import DroneEnv
+
+deg2rad = np.pi/180
+rad2deg = 1 / deg2rad
+
+def upulse(t, t0, tf):
+    u = 0 if (t<t0 or t>=tf) else 1
+    return u
+
+def ustep(t, t0) :
+    u = 0 if t<t0 else 1
+    return u
+
+def uramp(t, t0) :
+    u = 0 if t<t0 else (t - t0)
+    return u
+
+
+if __name__ == "__main__":
+    drone = Drone()
+
+    vh = 0
+    vz = 0
+    # cap = 0
+    # omega = 0
+
+    n0 = drone.trim(vh, vz)
+    print(n0)
+
+    drone_env = DroneEnv(drone)
+    drone_env.target = np.array([0, 0, -20, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+    obs0 = drone_env.reset()[0]
+    x0 = drone_env.state
+    print(drone_env.state)
+    obs1 = drone_env.step(n0)[0]
+    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])
+        return n
+
+    def wnd(t, s):
+        w = np.zeros(3)
+        return w 
+
+    sim = drone.sim((0, 20), x0, pid, wnd)
+
+    fig, axs = plt.subplots(2, 2)
+
+    ax = axs[0][0]
+    ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
+    ax.legend()
+    ax.grid()
+    ax.set_title(label='positions')
+
+    ax = axs[1][0]
+    quat = sim.y[3:7].T
+    ax.plot(sim.t, quat, label=('w', 'x', 'y', 'z'))
+    ax.legend()
+    ax.grid()
+    ax.set_title(label='quaternion')
+
+    ax = axs[0][1]
+    ax.plot(sim.t, sim.y[7:10].T, label=('u','v','w'))
+    ax.legend()
+    ax.grid()
+    ax.set_title(label='vitesses')
+
+    ax = axs[1][1]
+    ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
+    ax.legend()
+    ax.grid()
+    ax.set_title(label='rotations')
+    plt.show()

drone/rotation.py

+import numpy as np
+
+deg2rad = np.pi/180
+rad2deg = 1 / deg2rad
+
+
+def hat(x):
+    return np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
+
+def inv_hat(m):
+    return np.array([m[2, 1], m[0, 2], m[1, 0]])
+
+# (a, n): a = angle, n = vecteur unitaire (axe de rotation)
+def quat2axang(q):
+    a = 2 * np.arccos(q[0])
+    n = q[1:4] / np.sqrt(1 - q[0] * q[0])
+    return a, n
+
+def dcm2axang(R):
+    t = np.trace(R)
+    n = 0.5 * inv_hat(R - R.T)
+    r = np.linalg.norm(n)
+    a = np.arctan2(r, (t - 1) / 2)
+    return a, n / r
+
+
+# Quaternions
+def quatmat(p):
+    m = np.array(
+        [
+            [p[0], -p[1], -p[2], -p[3]],
+            [p[1],  p[0], -p[3],  p[2]], 
+            [p[2],  p[3],  p[0], -p[1]], 
+            [p[3], -p[2],  p[1],  p[0]]
+        ]
+    )
+    return m
+
+def quatmul(p, q):
+    return quatmat(p)@q
+
+def quatnorm(q): 
+    return np.dot(q, q)
+
+def quatconj(q):
+    return q * np.array([1, -1, -1, -1])
+
+def quatinv(q):
+    return quatconj(q) / quatnorm(q)
+
+def vect2quat(u):
+    return np.concatenate([[0], u])
+
+def quatrot(q, u):
+    v = quatmul(quatinv(q), quatmul(vect2quat(u), q))
+    return v[1:]
+
+
+def axang2quat(a, n):
+    return np.concatenate([[np.cos(a / 2)], n * np.sin(a / 2)])
+    
+def eul2quat(a):
+    qr = np.array([np.cos(a[0]/2), np.sin(a[0]/2), 0, 0])
+    qp = np.array([np.cos(a[1]/2), 0, np.sin(a[1]/2), 0])
+    qy = np.array([np.cos(a[2]/2), 0, 0, np.sin(a[2]/2)])
+    return quatmul(qy, quatmul(qp, qr))
+
+
+# Matrices de Rotation (DCM)
+def axang2dcm(a, n):
+    dcm = (1 - np.cos(a)) * np.outer(n, n) + np.cos(a) * np.identity(3) - np.sin(a) * hat(n)
+    return dcm
+
+def quat2dcm(q):
+    w = q[1:4]
+    dcm = (2 * np.outer(w, w) + (q[0]**2 - w @ w) * np.identity(3) - 2 * q[0] * hat(w)) / quatnorm(q)
+    return dcm
+
+def eul2dcm(a):
+    Rx = np.array([[1, 0, 0], [0, np.cos(a[0]), np.sin(a[0])], [0, -np.sin(a[0]), np.cos(a[0])]])
+    Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])], [0, 1, 0], [np.sin(a[1]), 0, np.cos(a[1])]])
+    Rz = np.array([[np.cos(a[2]), np.sin(a[2]), 0], [-np.sin(a[2]), np.cos(a[2]), 0], [0, 0, 1]])               
+    return Rx @ Ry @ Rz
+
+
+# Angles d'Euler (phi, theta, psi) selon axes (x, y, z) : z en premier
+def quat2eul(q):
+    phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2)
+    theta = -np.arcsin(2 * (q[1] * q[3] - q[0] * q[2]))
+    psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2)
+    return np.array([phi, theta, psi])
+    
+def dcm2eul(R):
+    phi = np.arctan2(R[1, 2], R[2, 2])
+    theta = -np.arcsin(R[0, 2])
+    psi = np.arctan2(R[0, 1], R[0, 0])
+    return np.array([phi, theta, psi])

mavion/ctlfun.py

-
-import numpy as np
-
-def upulse(t, t0, tf):
-    u = 0 if (t<t0 or t>=tf) else 1
-    return u
-
-def ustep(t, t0) :
-    u = 0 if t<t0 else 1
-    return u
-
-def uramp(t, t0) :
-    u = 0 if t<t0 else (t - t0)
-    return u

mavion/main.py

+import numpy as np
+from rotation import *
+import matplotlib.pyplot as plt
+from mavion import Mavion
+
+def upulse(t, t0, tf):
+    u = 0 if (t<t0 or t>=tf) else 1
+    return u
+
+def ustep(t, t0) :
+    u = 0 if t<t0 else 1
+    return u
+
+def uramp(t, t0) :
+    u = 0 if t<t0 else (t - t0)
+    return u
+
+if __name__ == "__main__":
+    mavion = Mavion()
+
+    vh = 0
+    vz = 0
+    cap = 0
+    omega = 0
+
+    dx, de, theta = mavion.trim(vh, vz)
+    theta = (theta + np.pi) % (2*np.pi) - np.pi
+    print(dx, de, theta*57.3)
+
+    pos = np.array([0, 0, -10])
+    ang = np.array([0, theta, cap])
+    vel = np.array([vh * np.cos(cap), vh * np.sin(cap), vz])
+    rot = np.array([0, 0, 0])
+    quat = eul2quat(ang)
+
+    x0 = np.concatenate([pos, quat, vel, rot])
+    print(mavion.equi(x0))
+
+    def ctl(t, s, w):
+        ddx = (upulse(t, 5, 6) - upulse(t, 6, 7))*dx*0.1
+        qt = np.array([1/np.sqrt(2), 0, 1/np.sqrt(2), 0])
+        q = s[3:7]
+        dde = 1.0 * (q - qt)[2]
+        u = np.array([-dx-ddx, dx+ddx, de+dde, de+dde])
+        return u
+
+    def wnd(t, s):
+        w = np.zeros(3)
+        return w 
+
+    sim = mavion.sim((0, 60), x0, ctl, wnd)
+
+    fig, axs = plt.subplots(2, 2)
+
+    ax = axs[0][0]
+    ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
+    ax.legend()
+    ax.grid()
+    ax.set_title(label='positions')
+
+    ax = axs[1][0]
+    quat = sim.y[3:7].T
+    ax.plot(sim.t, quat, label=('q0', 'q1', 'q2', 'q3'))
+    ax.set_title(label='quaternion')
+    # eul = np.apply_along_axis(quat2eul, 1, sim.y[3:7].T)*57.3
+    # ax.plot(sim.t, eul, label=('phi', 'theta', 'psi'))
+    # ax.set_title(label='euler angles')
+    ax.legend()
+    ax.grid()
+
+    ax = axs[0][1]
+    ax.plot(sim.t, sim.y[7:10].T, label=('u','v','w'))
+    ax.legend()
+    ax.grid()
+    ax.set_title(label='vitesses')
+
+    ax = axs[1][1]
+    ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
+    ax.legend()
+    ax.grid()
+    ax.set_title(label='rotations')
+    plt.show()

mavion/mavion.py

-import matplotlib.pyplot as plt
 import numpy as np
-import gymnasium as gym
-from gymnasium.envs.classic_control import utils
-import yaml
+from rotation import *
 from scipy.optimize import fsolve
 from scipy.integrate import solve_ivp
-from ctlfun import *
-
-
-deg2rad = 2 * np.pi / 360
-
-def hat(x):
-    mat = np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
-    return mat
-
-def quat2dcm(q):
-    dcm = np.array(
-        [
-            [
-                q[0] ** 2 + q[1] ** 2 - q[2] ** 2 - q[3] ** 2,
-                2 * (q[1] * q[2] + q[0] * q[3]),
-                2 * (q[1] * q[3] - q[0] * q[2]),
-            ],
-            [
-                2 * (q[1] * q[2] - q[0] * q[3]),
-                q[0] ** 2 - q[1] ** 2 + q[2] ** 2 - q[3] ** 2,
-                2 * (q[2] * q[3] - q[0] * q[1]),
-            ],
-            [
-                2 * (q[1] * q[3] + q[0] * q[2]),
-                2 * (q[2] * q[3] - q[0] * q[1]),
-                q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2,
-            ],
-        ]
-    )
-    return dcm
-
-def eul2quat(a):
-    c = np.cos(a/2)
-    s = np.sin(a/2)
-    q = np.array(
-        [
-            c[0] * c[1] * c[2] + s[0] * s[1] * s[2],
-           -c[0] * s[1] * s[2] + c[0] * c[1] * s[2],
-            c[0] * s[1] * c[2] + s[0] * c[1] * s[2],
-            c[0] * c[1] * s[2] - s[0] * s[1] * c[2],
-        ]
-    )
-    return q
-
-def quat2eul(q):
-    phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]))
-    theta = np.arcsin(2 * (q[0] * q[2] - q[1] * q[3]))
-    psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[3] * q[3] + q[2] * q[2]))
-    return np.array([phi, theta, psi])
-
-def eul2dcm(a):
-    Rx = np.array([[1, 0, 0],
-                   [0, np.cos(a[0]), np.sin(a[0])],
-                   [0, -np.sin(a[0]), np.cos(a[0])]])
-
-    Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])],
-                   [0, 1, 0],
-                   [np.sin(a[1]), 0, np.cos(a[1])]])
-
-    Rz = np.array([[np.cos(a[2]),  np.sin(a[2]), 0],
-                   [-np.sin(a[2]), np.cos(a[2]), 0],
-                   [0, 0, 1]])
-                   
-    return Rx @ Ry @ Rz
-
-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]])
-
+import yaml
 
 class Mavion:
 
     def __init__(self, file="mavion.yaml"):
         stream = open(file, "r")
-        drone = yaml.load(stream, Loader=yaml.Loader)
-
-        self.NAME = drone['NAME']
-        self.RHO = drone['RHO']
-        self.G = drone['G']
-        self.MASS = drone['MASS']
-        self.CHORD = drone['CHORD']
-        self.WINGSPAN = drone['WINGSPAN']
-        self.WET_SURFACE = drone['WET_SURFACE']
-        self.DRY_SURFACE = drone['DRY_SURFACE']
-        self.PROP_RADIUS = drone['PROP_RADIUS']
-
-        self.Cd0 = 0.1
-        self.Cy0 = 0.1
-        self.dR = -0.1 * self.CHORD
-        self.PHI_fv = np.diag([self.Cd0, self.Cy0, (2 * np.pi + self.Cd0)])
-        self.PHI_mv = np.array(
-            [
-                [0, 0, 0],
-                [0, 0, -1 / self.CHORD * self.dR * (2 * np.pi + self.Cd0)],
-                [0, 1 / self.WINGSPAN * self.dR * self.Cy0, 0],
-            ]
-        )
-        self.PHI_mw = 1 / 2 * np.diag([0.5, 0.5, 0.5])
-        self.PHI = np.block([[self.PHI_fv, self.PHI_mv], [self.PHI_mv, self.PHI_mw]])
-        self.PHI_n = drone["PHI_n"]
-
-        #  acording to back-of-the-envelope computations
-        self.INERTIA = np.diag(drone["INERTIA"])
+        drone_data = yaml.load(stream, Loader=yaml.Loader)
+
+        self.NAME = drone_data['NAME']
+        self.G = drone_data['G']
+        self.RHO = drone_data['RHO']
+        self.MASS = drone_data['MASS']
+        self.CHORD = drone_data['CHORD']
+        self.WINGSPAN = drone_data['WINGSPAN']
+        self.WET_SURFACE = drone_data['WET_SURFACE']
+        self.DRY_SURFACE = drone_data['DRY_SURFACE']
+        self.PROP_RADIUS = drone_data['PROP_RADIUS']
+        self.INERTIA = np.diag(drone_data["INERTIA"])
+
+        self.PROP_KP = drone_data['PROP_KP']
+        self.PROP_KM = drone_data['PROP_KM']
+        self.INERTIA_PROP_X = drone_data['INERTIA_PROP_X']
+        self.INERTIA_PROP_N = drone_data['INERTIA_PROP_N']
+
+        self.ELEVON_MEFFICIENCY = np.array(drone_data['ELEVON_MEFFICIENCY'])
+        self.ELEVON_FEFFICIENCY = np.array(drone_data['ELEVON_FEFFICIENCY'])
+        self.PHI_n = drone_data["PHI_n"]
 
         # geometric parameters
-        self.P_P1_CG = np.array(drone["P_P1_CG"]) * self.CHORD
-        self.P_P2_CG = np.array(drone["P_P2_CG"]) * self.CHORD
-        self.P_A1_CG = np.array(drone["P_A1_CG"]) * self.CHORD
-        self.P_A2_CG = np.array(drone["P_A2_CG"]) * self.CHORD
-
-        self.ELEVON_MEFFICIENCY = np.array(drone['ELEVON_MEFFICIENCY'])
-        self.ELEVON_FEFFICIENCY = np.array(drone['ELEVON_FEFFICIENCY'])
-        self.PROP_KP = drone['PROP_KP']
-        self.PROP_KM = drone['PROP_KM']
-        self.INERTIA_PROP_X = drone['INERTIA_PROP_X']
-        self.INERTIA_PROP_N = drone['INERTIA_PROP_N']
-
-        self.pos = np.zeros(3)
-        self.vel = np.zeros(3)
-        self.ang = np.zeros(3)
-        self.rot = np.zeros(3)
-        self.quat = eul2quat(self.ang)
-
-    def thrust(self, dx):
-        """
-        THRUST thrust forces and moments modeling
-        this function computes the propeller thrust by means of simple
-        prop formulas commonly applied in quadrotor literature.
+        self.P_P1_CG = np.array(drone_data["P_P1_CG"]) * self.CHORD
+        self.P_P2_CG = np.array(drone_data["P_P2_CG"]) * self.CHORD
+        self.P_A1_CG = np.array(drone_data["P_A1_CG"]) * self.CHORD
+        self.P_A2_CG = np.array(drone_data["P_A2_CG"]) * self.CHORD
 
-        INPUTS:
-            motor rot. : dx                in R(1x1)
-            + required self specs:
-            |---- PROP_KP                  in R(1x1)
-            |---- PROP_KM                  in R(1x1)
 
-        OUTPUTS:
-            Prop force : T (N) (body axis)  in R(3x1)
-            Prop moment: N (Nm) (body axis) in R(3x1)
+        self.Cd0 = 0.1
+        self.Cy0 = 0.1
+        self.dR = -0.1 * self.CHORD
 
-        vl: vehicle velocity in NED axis (m/s) [3x1 Real]
-        wb: vehicle angular velocity in body axis (rad/s) [3x1 Real]
-        q:  quaternion attitude (according to MATLAB convention) [4x1 Real]
+        self.PHI_fv = np.diag([self.Cd0, self.Cy0, (2 * np.pi + self.Cd0)])
 
-        NOTE1: notice that motor rotation's sign depend on which section we are due to
-            counter-rotating propellers;
-        NOTE2: elevon sign convention is positive pictch-up deflections.
-        NOTE3: gyroscopic effects are implemented in the caller function!
+        self.PHI_mv = np.array([[0, 0, 0], 
+            [0, 0, - self.dR * (2 * np.pi + self.Cd0) / self.CHORD], 
+            [0, self.dR * self.Cy0 / self.WINGSPAN, 0]])
 
-        refer to [1] for further information.
+        self.PHI_mw = 1 / 2 * np.diag([0.5, 0.5, 0.5])
 
-        REFERENCES:
-            [1] Lustosa L.R., Defay F., Moschetta J.-M., "The Phi-theory
-            approach to flight control design of tail-sitter vehicles"
-            @ http://lustosa-leandro.github.io
+        self.PHI = np.block([[self.PHI_fv, self.PHI_mv.T], [self.PHI_mv, self.PHI_mw]])
 
-        """
 
+    
+    def thrust(self, dx):
         kp = self.PROP_KP
         km = self.PROP_KM
-
         # prop forces computation / notice that negative thrust is not implemented
         T = kp * dx**2 * np.array([1, 0, 0])
-
         # prop moments computation
         N = np.sign(dx) * km * dx**2 * np.array([1, 0, 0])
-
-        return [T, N]
+        return np.array([T, N])
 
     def aero(self, quat, vel, rot, T, de, w):
-        """
-        AERO aerodynamic forces and moments computation (in a wing section!)
-        this function computes the aerodynamic forces and moments in body axis
-        in view of Phi-theory in a wing section. it includes propwash effects
-        due to thrust.
-        NOTE2: elevon sign convention is positive pictch-up deflections.
-        """
 
         # data extraction from self struct
         PHI_fv = self.PHI_fv
         PHI_mv = self.PHI_mv
         PHI_mw = self.PHI_mw
-        phi_n  = self.PHI_n
+        PHI_n  = self.PHI_n
         RHO    = self.RHO
         Swet   = self.WET_SURFACE
         Sdry   = self.DRY_SURFACE
@@ -211,25 +94,27 @@ class Mavion:
         B = np.diag([ws, chord, ws])
 
         # eta computation
-        eta = np.sqrt(np.linalg.norm(vinf)**2 + phi_n * np.linalg.norm(B@rot)**2)
+        eta = np.sqrt(vinf@vinf + PHI_n * (B@rot)@(B@rot))
 
         # Force computation
         # airfoil contribution
-        Fb = -1/2*RHO*S*eta * PHI_fv@vinf - 1/2*RHO*S*eta * PHI_mv@B@rot - 1/2*Swet/Sp * PHI_fv@T
+        Fb_a = -1/2*RHO*S*eta * PHI_fv@vinf - 1/2*RHO*S*eta * PHI_mv@B@rot - 1/2*Swet/Sp * PHI_fv@T
         # elevon contribution
-        Fb = Fb + 1/2*RHO*S*eta * PHI_fv@np.cross(de*Thetaf, vinf) \
+        Fb_e = 1/2*RHO*S*eta * PHI_fv@np.cross(de*Thetaf, vinf) \
                 + 1/2*RHO*S*eta * PHI_mv@B@np.cross(de*Thetaf, rot) \
                 + 1/2*Swet/Sp * PHI_fv@np.cross(de*Thetaf, T)
+        Fb = Fb_a + Fb_e
 
         # Moment computation
         # airfoil contribution
-        Mb = -1/2*RHO*S*eta * B@PHI_mv@vinf - 1/2*RHO*S*eta * B@PHI_mw@B@rot - 1/2*Swet/Sp * B@PHI_mv@T
+        Mb_a = -1/2*RHO*S*eta * B@PHI_mv@vinf - 1/2*RHO*S*eta * B@PHI_mw@B@rot - 1/2*Swet/Sp * B@PHI_mv@T
         # elevon contribution
-        Mb = Mb + 1/2*RHO*S*eta * B@PHI_mv@np.cross(de*Thetam, vinf) \
+        Mb_e = 1/2*RHO*S*eta * B@PHI_mv@np.cross(de*Thetam, vinf) \
                 + 1/2*RHO*S*eta * B@PHI_mw@B@np.cross(de*Thetam, rot) \
                 + 1/2*Swet/Sp * B@PHI_mv@np.cross(de*Thetam, T)
+        Mb = Mb_a + Mb_e
 
-        return [Fb, Mb]
+        return np.array([Fb, Mb])
 
     def dyn(self, x, u, w):
 
@@ -252,33 +137,22 @@ class Mavion:
         vel = x[7:10]
         rot = x[10:13]
 
-        # input demultiplexing
-        dx1 = u[0]
-        dx2 = u[1]
-        de1 = u[2]
-        de2 = u[3]
-
         # linear velocity derivative
         D = quat2dcm(quat)
 
-        [T1, N1] = self.thrust(dx1)
-        [T2, N2] = self.thrust(dx2)
-        [A1, M1] = self.aero(quat, vel, rot, T1, de1, w)
-        [A2, M2] = self.aero(quat, vel, rot, T2, de2, w)
+        [T1, N1] = self.thrust(u[0])
+        [T2, N2] = self.thrust(u[1])
+        [F1, M1] = self.aero(quat, vel, rot, T1, u[2], w)
+        [F2, M2] = self.aero(quat, vel, rot, T2, u[3], w)
 
-        Fb = T1 + T2 + A1 + A2
+        Fb = T1 + T2 + F1 + F2
         dvdt = np.array([0, 0, G]) + 1/MASS * D.T@Fb
 
         # angular velocity derivative
-        Ma = M1 + M2 + np.cross(P_A1_CG, A1) + np.cross(P_A2_CG, A2)
-
-        P = rot[0] 
-        Q = rot[1]
-        R = rot[2]
-        Jpx = INERTIA_PROP_X
-        Jpn = INERTIA_PROP_N
-        tau1 = N1 - (P + dx1) * (Jpx - Jpn) * np.array([0, R, -Q])
-        tau2 = N2 - (P + dx2) * (Jpx - Jpn) * np.array([0, R, -Q])
+        Ma = M1 + M2 + np.cross(P_A1_CG, F1) + np.cross(P_A2_CG, F2)
+
+        tau1 = N1 - (rot[0] + u[0]) * (INERTIA_PROP_X - INERTIA_PROP_N) * np.array([0, rot[2], -rot[1]])
+        tau2 = N2 - (rot[0] + u[1]) * (INERTIA_PROP_X - INERTIA_PROP_N) * np.array([0, rot[2], -rot[1]])
         Mg = tau1 + tau2 + np.cross(P_P1_CG, T1) + np.cross(P_P2_CG, T2)
         
         Mb = Ma + Mg
@@ -286,13 +160,17 @@ class Mavion:
 
         # kinematic equations
         dpdt = vel
+        dqdt = 1/2 * quatmul(quat, np.concatenate([[0], rot]))
 
-        dqdt = np.zeros(4)
-        dqdt[0]   = -1/2 * np.dot(rot, quat[1:4])
-        dqdt[1:4] =  1/2 * (quat[0]*rot - np.cross(rot, quat[1:4]))       
-        
         return np.concatenate([dpdt, dqdt, dvdt, drdt])
 
+    def equi(self, x):
+        vel = x[7:10]
+        vh = np.linalg.norm(vel[0:2])
+        vz = vel[2]
+        cap = np.arctan2(vel[1], vel[0])
+        return np.array([vh, vz, cap])
+
     def trim(self, vh, vz):
         x = np.zeros(13)
         x[7] = vh
@@ -300,145 +178,21 @@ class Mavion:
 
         def func(y):
             dx, de, theta = y
-            eul = np.array([0, theta, 0])
-            quat = eul2quat(eul)
-            x[3:7] = quat
+            x[3:7] = np.array([np.cos(theta/2), 0, np.sin(theta/2), 0])
             u = np.array([-dx, dx, de, de])
-            dx_dt = self.dyn(x, u, np.zeros(3))
-            return dx_dt[7], dx_dt[9], dx_dt[11]
+            dq_dt = self.dyn(x, u, np.zeros(3))
+            return dq_dt[7], dq_dt[9], dq_dt[11] 
 
         y0 = np.array([100, 0, 0])
         y = fsolve(func, y0)
-        return y
+        return y  # vh, vz, tangage
 
     def step(self, x, u, w, dt):
         func = lambda t, s: self.dyn(s, u, w)
-        sol = solve_ivp(func, (0, dt), x, method='RK45')
+        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, fctrl(t, s, fwind(t, s)), fwind(t, s))
         sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
         return sol
-
-
-class MavionEnv(gym.Env):
-
-    def __init__(self, mavion=Mavion(), render_mode=None):
-
-        self.tau = 0.02  # seconds between state updates
-        
-        # Angle at which to fail the episode
-        self.pos_threshold = np.array([2.4, 2.4, 2.4])
-        self.ang_threshold = np.array([15, 15, 15]) * deg2rad
-        self.vel_threshold = np.finfo(np.float32).max * np.ones(3)
-        self.rot_threshold = np.finfo(np.float32).max * np.ones(3)
-
-        high = np.concatenate(
-            [
-                self.pos_threshold * 2,
-                self.ang_threshold * 2,
-                self.vel_threshold,
-                self.rot_threshold
-            ],
-            dtype=np.float32,
-        )
-        self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
-
-        self.action_space = gym.spaces.Box(0, 1, shape=(4,), dtype=np.float32)
-        
-        self.target = None
-        self.obs = None
-        self.steps_beyond_terminated = None
-
-        self.mavion = mavion
-
-    def reset(self, seed=None, options=None):
-        super().reset(seed=seed)
-
-        # Note that if you use custom reset bounds, it may lead to out-of-bound
-        # state/observations.
-        low, high = utils.maybe_parse_reset_bounds(
-            options, -0.05, 0.05  # default low
-        )  # default high
-        self.target = np.zeros(12)
-        self.obs = self.np_random.uniform(low=low, high=high, size=(12,))
-        self.state = self.obs + self.target
-        self.steps_beyond_terminated = None
-
-        return self.obs, {}
-
-    def step(self, action):
-        x = s2q(self.state)
-        u = action
-        w = np.zeros(3)
-        new_state = self.mavion.step(x, u, w, self.tau)
-        self.state = q2s(new_state)
-        terminated = np.array_equal(self.state, self.target)
-        reward = 1 if terminated else 0  # Binary sparse rewards
-        self.obs = self.state - self.target
-        return self.obs, reward, terminated, False, {}
-
-
-if __name__ == "__main__":
-    mavion = Mavion()
-
-    vh = 0
-    vz = 0
-    dx, de, theta = mavion.trim(vh, vz)
-    theta = (theta + np.pi) % (2*np.pi) - np.pi
-    print(dx, de, theta*57.3)
-
-    pos = np.array([0, 0, -10])
-    eul = np.array([0, theta, 0])
-    vel = np.array([vh, 0, vz])
-    rot = np.array([0, 0, 0])
-    st = np.concatenate([pos, eul, vel, rot])
-    x0 = s2q(st)
-
-    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 
-
-    sim = mavion.sim((0, 60), x0, ctl, wnd)
-
-    plt.style.use('_mpl-gallery')
-    fig, axs = plt.subplots(2, 2)
-
-    ax = axs[0][0]
-    ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
-    ax.legend()
-    ax.set_title(label='positions')
-
-    ax = axs[1][0]
-    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, label=('u','v','w'))
-    ax.legend()
-    ax.set_title(label='vitesses')
-
-    ax = axs[1][1]
-    ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
-    ax.legend()
-    ax.set_title(label='rotations')
-    plt.show()
-
-
-    # mavion_env = MavionEnv(mavion)
-
-    # st1 = mavion_env.reset()[0]
-    # print("reset :", st1)
-
-    # mavion_env.state = st
-    # st2 = mavion_env.step(u)[0]
-    # print("new state :", st2)

mavion/mavion_env.py

+import numpy as np
+import gymnasium as gym
+from gymnasium.envs.classic_control import utils
+from mavion import Mavion
+
+class MavionEnv(gym.Env):
+
+    def __init__(self, mavion=Mavion(), render_mode=None):
+
+        self.tau = 0.02  # seconds between state updates
+        
+        # Angle at which to fail the episode
+        self.pos_threshold = np.array([2.4, 2.4, 2.4])
+        self.ang_threshold = np.array([15, 15, 15]) * deg2rad
+        self.vel_threshold = np.finfo(np.float32).max * np.ones(3)
+        self.rot_threshold = np.finfo(np.float32).max * np.ones(3)
+
+        high = np.concatenate(
+            [
+                self.pos_threshold * 2,
+                self.ang_threshold * 2,
+                self.vel_threshold,
+                self.rot_threshold
+            ],
+            dtype=np.float32,
+        )
+        self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
+
+        self.action_space = gym.spaces.Box(0, 1, shape=(4,), dtype=np.float32)
+        
+        self.target = None
+        self.obs = None
+        self.steps_beyond_terminated = None
+
+        self.mavion = mavion
+
+    def obs(self):
+        Vh = np.linalg.norm(self.mavion.vel[0:2])
+        Vv = self.mavion.vel[2]
+        Cap = np.arctan2(self.mavion.vel[1], self.mavion.vel[0])
+        phi = self.mavion.ang[0]
+        Turn_rate = self.mavion.G / Vh * np.tan(phi)
+        return np.array([Vh, Vv, Cap, Turn_rate])
+
+    def reset(self, seed=None, options=None):
+        super().reset(seed=seed)
+
+        # Note that if you use custom reset bounds, it may lead to out-of-bound
+        # state/observations.
+        low, high = utils.maybe_parse_reset_bounds(
+            options, -0.05, 0.05  # default low
+        )  # default high
+        self.target = np.zeros(12)
+        self.obs = self.np_random.uniform(low=low, high=high, size=(12,))
+        self.state = self.obs + self.target
+        self.steps_beyond_terminated = None
+
+        return self.obs, {}
+
+    def step(self, action):
+        x = s2q(self.state)
+        u = action
+        w = np.zeros(3)
+        new_state = self.mavion.step(x, u, w, self.tau)
+        self.state = q2s(new_state)
+        terminated = np.array_equal(self.state, self.target)
+        reward = 1 if terminated else 0  # Binary sparse rewards
+        self.obs = self.state - self.target
+        return self.obs, reward, terminated, False, {}
+

mavion/quaternion.py

+
+import numpy as np
+
+def quatmul(p, q):
+    m = np.array(
+        [
+            [p[0], -p[1], -p[2], -p[3]],
+            [p[1],  p[0], -p[3],  p[2]], 
+            [p[2],  p[3],  p[0], -p[1]], 
+            [p[3], -p[2],  p[1],  p[0]]
+        ]
+    )
+    return m@q
+
+def quatmul(p, q):
+    r = np.zeros(4)
+    r[0] = p[0]*q[0] - np.dot(p[1:4],q[1:4])
+    r[1:4] = p[0]*q[1:4] + q[0]*p[1:4] + np.cross(p[1:4],q[1:4])
+    return r
+
+def quatnorm(q): 
+    return np.dot(q, q)
+
+def quatinv(q):
+    r = np.zeros(4)
+    r[0] = q[0]
+    r[1:4] = -q[1:4]
+    r = r / quatnorm(q)
+    return r
+
+def quat2dcm(q):
+    dcm = np.array(
+        [
+            [q[0] ** 2 + q[1] ** 2 - q[2] ** 2 - q[3] ** 2, 2 * (q[1] * q[2] + q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
+            [2 * (q[1] * q[2] - q[0] * q[3]), q[0] ** 2 - q[1] ** 2 + q[2] ** 2 - q[3] ** 2, 2 * (q[2] * q[3] - q[0] * q[1])],
+            [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] - q[0] * q[1]), q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2]
+        ]
+    )
+    return dcm
+
+def dcm2quat(d):
+    q = np.zeros(4)
+    q[0] = np.sqrt()
+
+def eul2quat(a):
+    phi, tet, psi = a
+    q_phi = np.array([np.cos(phi/2), np.sin(phi/2), 0, 0])
+    q_tet = np.array([np.cos(tet/2), 0, np.sin(tet/2), 0])
+    q_psi = np.array([np.cos(psi/2), 0, 0, np.sin(psi/2)])
+    q = quatprod(q_psi, quatprod(q_tet, q_phi))
+    return q
+
+def eul2quat2(a):
+    c = np.cos(a/2)
+    s = np.sin(a/2)
+    q = np.array(
+        [
+            c[0] * c[1] * c[2] + s[0] * s[1] * s[2],
+            s[0] * c[1] * c[2] - c[0] * s[1] * s[2],
+            c[0] * s[1] * c[2] + s[0] * c[1] * s[2],
+            c[0] * c[1] * s[2] - s[0] * s[1] * c[2],
+        ]
+    )
+    return q
+
+def quat2eul(q):
+    phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]))
+    theta = np.arcsin(2 * (q[0] * q[2] - q[1] * q[3]))
+    psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[3] * q[3] + q[2] * q[2]))
+    return np.array([phi, theta, psi])
+
+def eul2dcm(a):
+    Rx = np.array([[1, 0, 0],
+                   [0, np.cos(a[0]), np.sin(a[0])],
+                   [0, -np.sin(a[0]), np.cos(a[0])]])
+
+    Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])],
+                   [0, 1, 0],
+                   [np.sin(a[1]), 0, np.cos(a[1])]])
+
+    Rz = np.array([[np.cos(a[2]),  np.sin(a[2]), 0],
+                   [-np.sin(a[2]), np.cos(a[2]), 0],
+                   [0, 0, 1]])
+                   
+    return Rx @ Ry @ Rz
+

mavion/rotation.py

+import numpy as np
+
+def hat(x):
+    return np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
+
+def inv_hat(m):
+    return np.array([m[2, 1], m[0, 2], m[1, 0]])
+
+# (a, n): a = angle, n = vecteur unitaire (axe de rotation)
+def quat2axang(q):
+    a = 2 * np.arccos(q[0])
+    n = q[1:4] / np.sqrt(1 - q[0] * q[0])
+    return a, n
+
+def dcm2axang(R):
+    t = np.trace(R)
+    n = 0.5 * inv_hat(R - R.T)
+    r = np.linalg.norm(n)
+    a = np.arctan2(r, (t - 1) / 2)
+    return a, n / r
+
+
+# Quaternions
+def quatmat(p):
+    m = np.array(
+        [
+            [p[0], -p[1], -p[2], -p[3]],
+            [p[1],  p[0], -p[3],  p[2]], 
+            [p[2],  p[3],  p[0], -p[1]], 
+            [p[3], -p[2],  p[1],  p[0]]
+        ]
+    )
+    return m
+
+def quatmul(p, q):
+    return quatmat(p)@q
+
+def quatnorm(q): 
+    return np.dot(q, q)
+
+def quatconj(q):
+    return q * np.array([1, -1, -1, -1])
+
+def quatinv(q):
+    return quatconj(q) / quatnorm(q)
+
+def vect2quat(u):
+    return np.concatenate([[0], u])
+
+def quatrot(u, q):
+    v = quatmul(quatinv(q), quatmul(vect2quat(u), q))
+    return v[1:]
+
+
+def axang2quat(a, n):
+    return np.concatenate([[np.cos(a / 2)], n * np.sin(a / 2)])
+    
+def eul2quat(a):
+    qr = np.array([np.cos(a[0]/2), np.sin(a[0]/2), 0, 0])
+    qp = np.array([np.cos(a[1]/2), 0, np.sin(a[1]/2), 0])
+    qy = np.array([np.cos(a[2]/2), 0, 0, np.sin(a[2]/2)])
+    return quatmul(qy, quatmul(qp, qr))
+
+
+# Matrices de Rotation (DCM)
+def axang2dcm(a, n):
+    dcm = (1 - np.cos(a)) * np.outer(n, n) + np.cos(a) * np.identity(3) - np.sin(a) * hat(n)
+    return dcm
+
+def quat2dcm(q):
+    w = q[1:4]
+    dcm = (2 * np.outer(w, w) + (q[0]**2 - w @ w) * np.identity(3) - 2 * q[0] * hat(w)) / quatnorm(q)
+    return dcm
+
+def eul2dcm(a):
+    Rx = np.array([[1, 0, 0], [0, np.cos(a[0]), np.sin(a[0])], [0, -np.sin(a[0]), np.cos(a[0])]])
+    Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])], [0, 1, 0], [np.sin(a[1]), 0, np.cos(a[1])]])
+    Rz = np.array([[np.cos(a[2]), np.sin(a[2]), 0], [-np.sin(a[2]), np.cos(a[2]), 0], [0, 0, 1]])               
+    return Rx @ Ry @ Rz
+
+
+# Angles d'Euler (phi, theta, psi) selon axes (x, y, z) : z en premier
+def quat2eul(q):
+    phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2)
+    theta = -np.arcsin(2 * (q[1] * q[3] - q[0] * q[2]))
+    psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2)
+    return np.array([phi, theta, psi])
+    
+def dcm2eul(R):
+    phi = np.arctan2(R[1, 2], R[2, 2])
+    theta = -np.arcsin(R[0, 2])
+    psi = np.arctan2(R[0, 1], R[0, 0])
+    return np.array([phi, theta, psi])