min/max action MavionEnv

mavion/main.py

@@ -19,7 +19,7 @@ def uramp(t, t0) :
 if __name__ == "__main__":
     mavion_env = MavionEnv()
 
-    vh = 10
+    vh = 0
     vz = 0
     cap = 0
     omega = 0
@@ -40,14 +40,14 @@ if __name__ == "__main__":
     
     # mavion_env.reset()
     # x0 = mavion_env.state
-    u0 = np.array([-dx, dx, de, de])
+    u0 = np.array([dx, dx, de, de])
 
     def ctl(t, s):
         ddx = (upulse(t, 5, 6) - upulse(t, 6, 7))*dx*0.0
         qt = np.array([1/np.sqrt(2), 0, 1/np.sqrt(2), 0])
         q = s[3:7]
         dde = 0.0 * (q - qt)[2]
-        u = u0 + np.array([-ddx, ddx, dde, dde])
+        u = u0 + np.array([ddx, ddx, dde, dde])
         return u
 
     def wnd(t, s):

mavion/mavion_env.py

@@ -11,6 +11,7 @@ class MavionEnv(gym.Env):
 
     def __init__(self, mavion=Mavion(), render_mode=None):
 
+        self.mavion = mavion
         self.tau = 0.2  # seconds between state updates
 
         # Angle at which to fail the episode
@@ -31,14 +32,19 @@ class MavionEnv(gym.Env):
             dtype=np.float32,
         )
         self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
-        self.action_space = gym.spaces.Box(-1, 1, shape=(4,), dtype=np.float32)
+        
+        action_max = np.array([1, 1, 1, 1])
+        action_min = np.array([0, 0, -1, -1])
+        self.action_space = gym.spaces.Box(action_min, action_max, dtype=np.float32)
         self.action_range = np.array([mavion.MAX_ROTOR_SPD, mavion.MAX_ROTOR_SPD, mavion.MAX_FLAP_DEF, mavion.MAX_FLAP_DEF])
 
         self.state = np.zeros(13)
         self.target = np.array([0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])
-        self.obs = self.state - self.target
         self.steps_beyond_terminated = None
-        self.mavion = mavion
+
+    def obs(self):
+        self.obs = self.state - self.target
+        return self.obs
 
     def reset(self, seed=None, options=None):
         super().reset(seed=seed)
@@ -66,5 +72,5 @@ class MavionEnv(gym.Env):
         self.state = new_state
         terminated = self.distance(self.state, self.target) > 10
         reward = 1 if not terminated else -10
-        self.obs = self.state - self.target
-        return self.obs, reward, terminated, False, {}
+        obs = self.observation()
+        return obs, reward, terminated, False, {}

mavion/mavion_model.py

@@ -122,9 +122,9 @@ class Mavion:
         vel = x[7:10]
         rot = x[10:13]
 
-        [T1, N1] = self.thrust(u[0])
+        [T1, N1] = self.thrust(-u[0])       
         [T2, N2] = self.thrust(u[1])
-        tau1 = N1 - (rot[0] + u[0]) * (INERTIA_PROP_X - INERTIA_PROP_N) * np.array([0, rot[2], -rot[1]])
+        tau1 = N1 - (rot[0] - u[0]) * (INERTIA_PROP_X - INERTIA_PROP_N) * np.array([0, rot[2], -rot[1]])
         tau2 = N2 - (rot[0] + u[1]) * (INERTIA_PROP_X - INERTIA_PROP_N) * np.array([0, rot[2], -rot[1]])
 
         vinf = quatrot(vel - w, quat)
@@ -151,7 +151,7 @@ class Mavion:
         def func(y):
             dx, de, theta = y
             x[3:7] = np.array([np.cos(theta/2), 0, np.sin(theta/2), 0])
-            u = np.array([-dx, dx, de, de])
+            u = np.array([dx, dx, de, de])
             dq_dt = self.dyn(x, u, np.zeros(3))
             return dq_dt[7], dq_dt[9], dq_dt[11]