version 1.0

mavion/main.py

 import numpy as np
 from rotation import *
 import matplotlib.pyplot as plt
-from mavion import Mavion
+from mavion_model import Mavion
 
 def upulse(t, t0, tf):
     u = 0 if (t<t0 or t>=tf) else 1

mavion/mavion_env.py

 import numpy as np
 import gymnasium as gym
 from gymnasium.envs.classic_control import utils
-from mavion import Mavion
+from mavion_model import Mavion
 
 class MavionEnv(gym.Env):
 

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
-