diff --git a/mavion/__pycache__/mavion_model.cpython-311.pyc b/mavion/__pycache__/mavion_model.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..36b589965e84da4a609c7f9fc9026c6e1ee8bafd Binary files /dev/null and b/mavion/__pycache__/mavion_model.cpython-311.pyc differ diff --git a/mavion/main.py b/mavion/main.py index 5942126ff75abbfd7efaed198e2e188aec7b1b6e..69944de72b5b8de5e783ac2eb63419bd0a3c96b2 100644 --- a/mavion/main.py +++ b/mavion/main.py @@ -1,7 +1,7 @@ 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 diff --git a/mavion/mavion_env.py b/mavion/mavion_env.py index 10cc9d127671c50a938d19c4e7f2043d4afc971a..45996ceb6c66d2e5972e5b53c8c022bbd7375c02 100644 --- a/mavion/mavion_env.py +++ b/mavion/mavion_env.py @@ -1,7 +1,7 @@ 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): diff --git a/mavion/mavion.py b/mavion/mavion_model.py similarity index 100% rename from mavion/mavion.py rename to mavion/mavion_model.py diff --git a/mavion/quaternion.py b/mavion/quaternion.py deleted file mode 100644 index 92121f621f47a4b8540cffc9cd84d3d9ff13be67..0000000000000000000000000000000000000000 --- a/mavion/quaternion.py +++ /dev/null @@ -1,86 +0,0 @@ - -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 -