Skip to content
Snippets Groups Projects
Commit 4c2afbc5 authored by PASTOR Philippe's avatar PASTOR Philippe :speech_balloon:
Browse files

Téléverser un nouveau fichier

parent 7f9e3791
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 433 additions and 0 deletions
mavion/mavion.py 0 → 100644
+ 433
− 0
View file @ 4c2afbc5
import matplotlib.pyplot as plt
import numpy as np
import gymnasium as gym
from gymnasium.envs.classic_control import utils
import yaml
from scipy.optimize import fsolve
from scipy.integrate import solve_ivp
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
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
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"])
# 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.
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)
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]
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!
refer to [1] for further information.
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
"""
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]
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
RHO = self.RHO
Swet = self.WET_SURFACE
Sdry = self.DRY_SURFACE
chord = self.CHORD
ws = self.WINGSPAN
Prop_R = self.PROP_RADIUS
Thetam = self.ELEVON_MEFFICIENCY
Thetaf = self.ELEVON_FEFFICIENCY
# derivative data
Sp = np.pi * Prop_R**2
# DCM computation
D = quat2dcm(quat)
# freestream velocity computation in body frame
vinf = D @ (vel - w)
# computation of total wing section area
S = Swet + Sdry
# computation of chord matrix
B = np.diag([ws, chord, ws])
# eta computation
eta = np.sqrt(np.linalg.norm(vinf)**2 + phi_n * np.linalg.norm(B@rot)**2)
# 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
# elevon contribution
Fb = Fb + 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)
# 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
# elevon contribution
Mb = Mb + 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)
return [Fb, Mb]
def dyn(self, x, u, w):
G = self.G
MASS = self.MASS
INERTIA = self.INERTIA
# position of propellers wrt center of gravity
P_P1_CG = self.P_P1_CG
P_P2_CG = self.P_P2_CG
# position of aerodynamic wrenches wrt center of gravity
P_A1_CG = self.P_A1_CG
P_A2_CG = self.P_A2_CG
# propeller blade inertia
INERTIA_PROP_X = self.INERTIA_PROP_X
INERTIA_PROP_N = self.INERTIA_PROP_N
# state demultiplexing
quat = x[3:7]
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)
Fb = T1 + T2 + A1 + A2
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])
Mg = tau1 + tau2 + np.cross(P_P1_CG, T1) + np.cross(P_P2_CG, T2)
Mb = Ma + Mg
drdt = np.linalg.inv(INERTIA) @ (Mb - np.cross(rot, INERTIA@rot))
# kinematic equations
dpdt = vel
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 trim(self, vh, vz):
x = np.zeros(13)
x[7] = vh
x[9] = vz
def func(y):
dx, de, theta = y
eul = np.array([0, theta, 0])
quat = eul2quat(eul)
x[3:7] = quat
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]
y0 = np.array([100, 0, 0])
y = fsolve(func, y0)
return y
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')
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))
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 = 5
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])
x = s2q(st)
u = np.array([-dx, dx, de, de])
w = np.zeros(3)
def ctl(t, s, w): return u
def wnd(t, s): return w
sim = mavion.sim((0, 20), x, ctl, wnd)
fig, axs = plt.subplots(2, 2, layout='constrained')
ax = axs[0][0]
ax.plot(sim.t, sim.y[0:3].T)
ax = axs[1][0]
ax.plot(sim.t, sim.y[3:7].T)
ax = axs[0][1]
ax.plot(sim.t, sim.y[7:10].T)
ax = axs[1][1]
ax.plot(sim.t, sim.y[10:13].T)
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)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment