1er commit sur phil

drone/drone_env.py

@@ -64,7 +64,7 @@ class DroneEnv(gym.Env):
         return self.obs, {}
 
     def step(self, action):
-        new_state = self.drone.step(self.state, action, np.zeros(3), self.tau)
+        new_state = self.drone.step(self.state, action, self.tau)
         self.state = new_state
         self.obs = self.observation()
         terminated = (self.obs@self.obs < 1)

drone/drone_model.py

@@ -31,10 +31,13 @@ class Drone:
         # 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]])
+        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, u):
 
         G = self.G
         MASS = self.MASS
@@ -52,7 +55,7 @@ class Drone:
         drdt = np.linalg.inv(INERTIA) @ (u[1:4] - np.cross(rot, INERTIA@rot))
 
         # kinematic equations
-        dpdt = vel + w
+        dpdt = vel
         dqdt = 1/2 * quatmul(quat, np.concatenate([[0], rot]))
 
         return np.concatenate([dpdt, dqdt, dvdt, drdt])
@@ -64,21 +67,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, self.u_int(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, self.u_int(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, self.u_int(fctrl(t, s)))
         sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
         return sol
 

drone/main.py

@@ -24,9 +24,6 @@ if __name__ == "__main__":
 
     vh = 0
     vz = 0
-    # cap = 0
-    # omega = 0
-
     n0 = drone.trim(vh, vz)
     print(n0)
 
@@ -35,24 +32,17 @@ if __name__ == "__main__":
     obs0 = drone_env.reset()[0]
     x0 = drone_env.state
     print(drone_env.state)
-    obs1 = drone_env.step(n0)[0]
+    obs1 = drone_env.step(n0)
+    print(obs0)
     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])
+    def ctl(t, s):
+        dn = (upulse(t, 1, 2) - upulse(t, 2, 3))*np.linalg.norm(n0)*np.array([0.01, 0.01, 0.01, 0.01])
+        n = n0 + dn
         return n
 
-    def wnd(t, s):
-        w = np.zeros(3)
-        return w 
-
-    sim = drone.sim((0, 20), x0, pid, wnd)
+    sim = drone.sim((0, 20), x0, ctl)
 
     fig, axs = plt.subplots(2, 2)
 

mavion/mavion_env.py

@@ -67,4 +67,3 @@ class MavionEnv(gym.Env):
         reward = 1 if terminated else 0  # Binary sparse rewards
         self.obs = self.state - self.target
         return self.obs, reward, terminated, False, {}
-