drone/drone_env.py

@@ -2,11 +2,9 @@ import numpy as np
 import gymnasium as gym
 from gymnasium.envs.classic_control import utils
 
+from rotation import *
 from drone_model import Drone
-from rotation import quat2eul
 
-deg2rad = np.pi/180
-rad2deg = 1 / deg2rad
 
 class DroneEnv(gym.Env):
 
@@ -16,57 +14,59 @@ class DroneEnv(gym.Env):
         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.state = np.zeros(13)
+        self.target = np.zeros(13)
+        self.obs = np.zeros(13)
+        
+        self.pos_threshold = np.array([1., 1., 1.])
+        self.quat_threshold = np.ones(4)
         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.pos_threshold,
+                self.quat_threshold,
                 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.action_space = gym.spaces.Box(0, 500, shape=(4,), dtype=np.float32)
+
         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])
+        d_pos = self.state[0:3] - self.target[0:3]
+        d_quat = quatmul(quatinv(self.target[3:7]), self.state[3:7])
+        d_vel = self.state[7:10] - self.target[7:10]
+        d_rot = self.state[10:13] - self.target[10:13]
+        obs = np.concatenate([d_pos, d_quat, d_vel, d_rot])
         return obs
 
+    def reward(self):
+        dist_pos = np.linalg.norm(self.obs[0:3])
+        dist_quat = quatdist(self.state[3:7], self.target[3:7])
+        terminated = (dist_pos + dist_quat) > 1
+        reward = 1 - (dist_pos + dist_quat) if not terminated else -1  
+        return reward, terminated
+
     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
+        low, high = utils.maybe_parse_reset_bounds(options, -0.1, 0.1)  
+        self.state = self.target + np.array([0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
         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.state = self.drone.step(self.state, action, self.tau)
         self.obs = self.observation()
-        terminated = (self.obs@self.obs < 1)
-        reward = 1 if terminated else 0  # Binary sparse rewards
+        reward, terminated = self.reward()
         return self.obs, reward, terminated, False, {}
+
+    def render(self, mode='human'):
+        pass

drone/drone_model.py

 import numpy as np
+import yaml
 from scipy.optimize import fsolve
 from scipy.integrate import solve_ivp
-import yaml
-from rotation import quatrot, quatmul
+from rotation import *
 
 class Drone:
 
@@ -18,27 +18,18 @@ class Drone:
         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):
+    def dyn(self, x, n):
 
         G = self.G
         MASS = self.MASS
         INERTIA = self.INERTIA
+        P_R_CG = self.P_R_CG
+        KP = self.PROP_KP
+        KM = self.PROP_KM
+
+        T = KP * n**2
+        N = KM * n**2
+        u = np.array([sum(T), P_R_CG*(T[3]-T[2]), P_R_CG*(T[0]-T[1]), N[2]+N[3]-N[0]-N[1]])
 
         # state demultiplexing
         quat = x[3:7]
@@ -46,14 +37,14 @@ class Drone:
         rot = x[10:13]
 
         # linear velocity derivative
-        dvdt = np.array([0, 0, G]) - 1/MASS * quatrot(quat, np.array([0, 0, u[0]]))
+        dvdt = quatrot(quatinv(quat), np.array([0, 0, -u[0]])) / MASS + np.array([0, 0, G])
 
         # 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]))
+        dpdt = vel
+        dqdt = 0.5 * quatmul(quat, vect2quat(rot))
 
         return np.concatenate([dpdt, dqdt, dvdt, drdt])
 
@@ -64,21 +55,20 @@ class Drone:
         x[9] = vz
 
         def func(n):
-            u = self.u_int(n)
-            dx = self.dyn(x, u, np.zeros(3))
+            dx = self.dyn(x, n)
             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)
+    def step(self, x, n, dt):
+        func = lambda t, s: self.dyn(s, n)
         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))
+    def sim(self, t_span, x0, fctrl):
+        func = lambda t, s: self.dyn(s, fctrl(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
 
@@ -20,39 +21,24 @@ def uramp(t, t0) :
 
 
 if __name__ == "__main__":
-    drone = Drone()
+    drone_env = DroneEnv()
 
     vh = 0
     vz = 0
-    # cap = 0
-    # omega = 0
-
-    n0 = drone.trim(vh, vz)
+    n0 = drone_env.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]
+    
+    drone_env.target = np.array([0, 0, -20, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+    drone_env.reset()
     x0 = drone_env.state
-    print(drone_env.state)
-    obs1 = drone_env.step(n0)[0]
-    print(drone_env.state)
+    print(x0)
 
-    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
+    def ctl(t, s):
+        dn = (upulse(t, 1, 2) - upulse(t, 2, 3))*np.linalg.norm(n0)*np.array([0.1, 0.1, 0.1, 0.1])
+        n = n0 + dn
         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)
+    sim = drone_env.drone.sim((0, 20), x0, ctl)
 
     fig, axs = plt.subplots(2, 2)
 

drone/rotation.py

 import numpy as np
+from scipy.linalg import expm, logm
 
 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
+    q = q / sum(q*q)
+    mu = 2 * np.arccos(min(1, q[0]))
+    if (q[0] >= 1) :
+        n = np.zeros(3)
+    else :
+        n = q[1:4] / np.sqrt(1 - q[0]*q[0])
+    return (mu, 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
+    mu = np.arccos((t - 1) / 2)
+    n = 0.5 * inv_hat(R.T - R) / np.sin(mu)
+    return (mu, n)
+
+
+def axangrot(ax, u):            # left hand rotation du vecteur u d'un angle mu selon l'axe n
+    mu, n = ax
+    v = (1 - np.cos(mu)) * n * np.dot(n, u) + u * np.cos(mu) - np.cross(n, u) * np.sin(mu)
+    return v
 
 
 # Quaternions
+
+def axang2quat(ax):
+    mu, n = ax
+    return np.concatenate([[np.cos(mu / 2)], n * np.sin(mu / 2)])
+    
+def eul2quat(a):
+    qx = np.array([np.cos(a[0]/2), np.sin(a[0]/2), 0, 0])
+    qy = np.array([np.cos(a[1]/2), 0, np.sin(a[1]/2), 0])
+    qz = np.array([np.cos(a[2]/2), 0, 0, np.sin(a[2]/2)])
+    return quatmul(qz, quatmul(qy, qx))
+
+def vect2quat(u):
+    return np.concatenate([[0], u])
+
+
 def quatmat(p):
     m = np.array(
         [
@@ -39,41 +65,36 @@ def quatmat(p):
 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])
+    return np.concatenate([[q[0]], -q[1:4]]) / sum(q*q)
 
 def quatrot(q, u):
-    v = quatmul(quatinv(q), quatmul(vect2quat(u), q))
+    v = quatmul(quatmul(quatinv(q), vect2quat(u)), q)
     return v[1:]
 
+def quatdist(q1, q2):
+    return 2 * np.arccos(min(1,abs(sum(q1*q2))))                 # donne une valeur d'angle entre 0 et PI
 
-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))
+def quatdist2(q1, q2):
+    return 1 - abs(sum(q1*q2))                                   # donne une valeur d'angle entre 0 et 1
+
+def quaterr(q1, q2):
+    mu, n = quat2axang(quatmul(quatinv(q2), q1))
+    return mu*n
 
 
 # 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)
+
+def axang2dcm(ax):
+    mu, n = ax
+    dcm = (1 - np.cos(mu)) * np.outer(n, n) + np.cos(mu) * np.identity(3) - np.sin(mu) * hat(n)
     return dcm
 
 def quat2dcm(q):
+    q = q / quatnorm(q)
+    q0 = q[0]
     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)
+    dcm = (2 * np.outer(w, w) + (q0**2 - w @ w) * np.identity(3) - 2 * q0 * hat(w)) 
     return dcm
 
 def eul2dcm(a):
@@ -83,7 +104,34 @@ def eul2dcm(a):
     return Rx @ Ry @ Rz
 
 
+def dcmerr(R1, R2):
+    return inv_hat(logm(R2@R1.T))
+
+def dcmdist(R1, R2):
+    return np.linalg.norm(dcmerr(R1, R2))               # donne une valeur d'angle entre 0 et PI
+
+
+def dcmerr2(R, Rd):
+    return 0.5 * inv_hat(Rd@R.T - R@Rd.T)
+
+def dcmdist2(R, Rd):
+    return np.linalg.norm(np.identity(3) - Rd@R.T)     # donne une valeur ente 0 et 2V2
+
+def dcmdist4(R, Rd):
+    return  0.5 * np.trace(np.identity(3) - Rd@R.T)
+
+
+def dcmdist3(R, Rd):
+    dist = 2 - np.sqrt(1 + np.trace(Rd@R.T))
+    return dist
+
+def dcmerr3(R, Rd):
+    err = inv_hat(Rd@R.T - R@Rd.T) / np.sqrt(1 + np.trace(Rd@R.T))
+    return err
+
+
 # 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]))
@@ -95,3 +143,10 @@ def dcm2eul(R):
     theta = -np.arcsin(R[0, 2])
     psi = np.arctan2(R[0, 1], R[0, 0])
     return np.array([phi, theta, psi])
+
+
+def euldist(e1, e2):                                    # donne une valeur d'angle entre 0 et PI
+    diff = e1 - e2
+    d = np.fmin(np.abs(diff), 2*np.pi - np.abs(diff))
+    r = np.linalg.norm(d) / np.sqrt(3)
+    return r