Merge pull request #1 from AOS55/control

Control

.gitignore

@@ -26,6 +26,9 @@ wandb
 *.zip
 logs
 .runs
+old_logs
+models
+outputs
 
 # Docs
 docs/_build

flyer_env/__init__.py

@@ -11,7 +11,7 @@ def register_flyer_envs():
     # flyer_env.py
     register(
         id="flyer-v1",
-        entry_point="flyer_env.envs:FlyerEnv",
+        entry_point="flyer_env.envs:FlyerEnv"
     )
 
     # trajectory_env.py
@@ -29,5 +29,11 @@ def register_flyer_envs():
     # forced_landing_env.py
     register(
         id="forced_landing-v1",
-        entry_point="flyer_env.envs:ForcedLandingEnv",
+        entry_point="flyer_env.envs:ForcedLandingEnv"
+    )
+
+    # control_env.py
+    register(
+        id="control-v1",
+        entry_point="flyer_env.envs:ControlEnv"
     )

flyer_env/envs/__init__.py

@@ -2,3 +2,4 @@
 from flyer_env.envs.trajectory_env import *
 from flyer_env.envs.runway_env import *
 from flyer_env.envs.forced_landing_env import *
+from flyer_env.envs.control_env import *

flyer_env/envs/common/action.py

@@ -45,10 +45,10 @@ def controlled_vehicle(self, vehicle):
 
 class ContinuousAction(ActionType):
 
-    ELEVATOR_RANGE = (-5.0 * (-np.pi/180.0), 30.0 * (np.pi/180.0))
-    AILERON_RANGE = (-5.0 * (-np.pi/180.0), 5.0 * (-np.pi/180.0))
+    ELEVATOR_RANGE = (-5.0 * (np.pi/180.0), 30.0 * (np.pi/180.0))
+    AILERON_RANGE = (-5.0 * (np.pi/180.0), 5.0 * (-np.pi/180.0))
     TLA_RANGE = (0.0, 1.0)
-    RUDDER_RANGE = (-30.0 * (-np.pi/180.0), 30.0 * (np.pi/180.0))
+    RUDDER_RANGE = (-30.0 * (np.pi/180.0), 30.0 * (np.pi/180.0))  # removed rudder from action-space for now
 
     """
     A continuous action space for thrust-lever-angle and control surface deflections.
@@ -60,7 +60,6 @@ def __init__(self,
                  elevator_range: Optional[Tuple[float, float]] = None,
                  aileron_range: Optional[Tuple[float, float]] = None,
                  tla_range: Optional[Tuple[float, float]] = None,
-                 rudder_range: Optional[Tuple[float, float]] = None,
                  powered: bool = True,
                  clip: bool = True,
                  **kwargs) -> None:
@@ -73,11 +72,10 @@ def __init__(self,
         self.elevator_range = elevator_range if elevator_range else self.ELEVATOR_RANGE
         self.aileron_range = aileron_range if aileron_range else self.AILERON_RANGE
         self.tla_range = tla_range if tla_range else self.TLA_RANGE 
-        self.rudder_range = rudder_range if rudder_range else self.RUDDER_RANGE
 
         self.powered = powered
         self.clip = clip
-        self.size = 4 if self.powered else 3
+        self.size = 3 if self.powered else 2
 
         self.last_action = np.zeros(self.size)
 
@@ -110,6 +108,66 @@ def act(self, action: np.ndarray) -> None:
         self.last_action = action
 
 
+class LongitudinalAction(ActionType):
+
+    ELEVATOR_RANGE = (-5.0 * (np.pi/180.0), 30.0 * (np.pi/180.0))
+    TLA_RANGE = (0.0, 1.0)
+
+    """
+    A continuous action space for thrust-lever-angle and elevator control.
+    Controls are set in order [elevator, tla].
+    """ 
+
+    def __init__(self,
+                 env: 'AbstractEnv',
+                 elevator_range: Optional[Tuple[float, float]] = None,
+                 tla_range: Optional[Tuple[float, float]] = None,
+                 powered: bool = True,
+                 clip: bool = True,
+                 **kwargs) -> None:
+
+        """
+        Create a continuous longitudinally constrained action space
+        """
+        super().__init__(env)
+
+        # Setup control limit ranges
+        self.elevator_range = elevator_range if elevator_range else self.ELEVATOR_RANGE
+        self.tla_range = tla_range if tla_range else self.TLA_RANGE
+
+        self.powered = powered
+        self.clip = clip
+        self.size = 2 if self.powered else 1
+
+        self.last_action = np.zeros(self.size)
+
+    def space(self) -> spaces.Box:
+        return spaces.Box(-1.0, 1.0, shape=(self.size,), dtype=np.float32)
+
+    @property
+    def vehicle_class(self) -> Callable:
+        return Aircraft
+
+    def act(self, action: np.ndarray) -> None:
+        if self.clip:
+            action = np.clip(action, -1.0, 1.0)
+        if self.powered:
+            self.controlled_vehicle.act({
+                'aileron': 0.0,
+                'elevator': utils.lmap(action[0], [-1.0, 1.0], self.elevator_range),
+                'tla': utils.lmap(action[1], [-1, 1], self.tla_range),
+                'rudder': 0.0
+            })
+        else:
+            self.controlled_vehicle.act({
+                'aileron': 0.0,
+                'elevator': utils.lmap(action[0], [-1.0, 1.0], self.elevator_range),
+                'tla': 0.0,
+                'rudder': 0.0
+            })
+        self.last_action = action 
+
+
 class ControlledAction(ActionType):
     """
     An action that controls the aircraft using a PID controller to track towards the target.
@@ -268,6 +326,8 @@ def act(self, action: Dict[str, Vector]) -> None:
 def action_factory(env: 'AbstractEnv', config: dict) -> ActionType:
     if config["type"] == "ContinuousAction":
         return ContinuousAction(env, **config)
+    elif config["type"] == "LongitudinalAction":
+        return LongitudinalAction(env, **config)
     elif config["type"] == "ControlledAction":
         return ControlledAction(env, **config)
     elif config["type"] == "PursuitAction":

flyer_env/envs/common/observation.py

@@ -68,8 +68,8 @@ def observe(self) -> np.ndarray:
 class TrajectoryObservation(ObservationType):
 
     """
-        Observe dynamics of vehicle relative to goal position
-        ONLY FOR USE WITH TRAJECTORY ENV
+    Observe dynamics of vehicle relative to goal position
+    ONLY FOR USE WITH TRAJECTORY ENV
     """
 
     FEATURES: List[str] = ['x', 'y', 'z', 'roll', 'pitch', 'yaw', 'u', 'v', 'w', 'p', 'q', 'r']
@@ -85,8 +85,73 @@ def __init__(self,
         self.features = features or self.FEATURES
         self.vehicles_count = vehicles_count
         self.features_range = features_range
-        if hasattr(env, "t_pos"):
-            self.t_pos = env.t_pos
+        if hasattr(env, "goal"):
+            self.goal = env.goal
+
+    def space(self) -> spaces.Space:
+        return spaces.Box(shape=(self.vehicles_count, len(self.features)), low=-np.inf, high=np.inf, dtype=np.float32)
+
+    def observe(self) -> np.ndarray:
+
+        df = pd.DataFrame.from_records([self.observer_vehicle.dict])[self.features]
+        df = df[self.features]
+        obs = df.values.copy()
+        obs[0, 0] = self.goal[0] - obs[0, 0]
+        obs[0, 1] = self.goal[1] - obs[0, 1]
+        obs[0, 2] = self.goal[2] - obs[0, 2]
+        return obs.astype(self.space().dtype)
+
+
+class ControlObservation(ObservationType):
+
+    """
+    Observe the aircaft without the position information
+    """
+
+    FEATURES: List[str] = ['roll', 'pitch', 'yaw', 'u', 'v', 'w', 'p', 'q', 'r']
+
+    def __init__(self,
+                 env: "AbstractEnv",
+                 features: List[str] = None,
+                 vehicles_count: int = 1,
+                 features_range: Dict[str, List[float]] = None,
+                 **kwargs: dict) -> None:
+
+        super().__init__(env)
+        self.features = features or self.FEATURES
+        self.vehicles_count = vehicles_count
+        self.features_range = features_range
+
+    def space(self) -> spaces.Space:
+        return spaces.Box(shape=(self.vehicles_count, len(self.features)), low=-np.inf, high=np.inf, dtype=np.float32)
+
+    def observe(self) -> np.ndarray:
+
+        df = pd.DataFrame.from_records([self.observer_vehicle.dict])[self.features]
+        df = df[self.features]
+        obs = df.values.copy()
+        return obs.astype(self.space().dtype)
+
+
+class LongitudinalObservation(ObservationType):
+
+    """
+    Observe the aircraft only given longitudinal data
+    """
+
+    FEATURES: List[str] = ['pitch', 'u', 'w', 'q']
+
+    def __init__(self,
+                 env: "AbstractEnv",
+                 features: List[str] = None,
+                 vehicles_count: int = 1,
+                 features_range: Dict[str, List[float]] = None,
+                 **kwargs: dict) -> None:
+
+        super().__init__(env)
+        self.features = features or self.FEATURES
+        self.vehicles_count = vehicles_count
+        self.features_range = features_range
 
     def space(self) -> spaces.Space:
         return spaces.Box(shape=(self.vehicles_count, len(self.features)), low=-np.inf, high=np.inf, dtype=np.float32)
@@ -96,9 +161,6 @@ def observe(self) -> np.ndarray:
         df = pd.DataFrame.from_records([self.observer_vehicle.dict])[self.features]
         df = df[self.features]
         obs = df.values.copy()
-        obs[0, 0] = self.t_pos[0] - obs[0, 0]
-        obs[0, 1] = self.t_pos[1] - obs[0, 1]
-        obs[0, 2] = self.t_pos[2] - obs[0, 2]
         return obs.astype(self.space().dtype)
 
 
@@ -107,5 +169,9 @@ def observation_factory(env: "AbstractEnv", config: dict) -> ObservationType:
         return DynamicObservation(env, **config)
     elif config["type"] == "Trajectory" or config["type"] == "trajectory":
         return TrajectoryObservation(env, **config)
+    elif config["type"] == "Control" or config["type"] == "control":
+        return ControlObservation(env, **config)
+    elif config["type"] == "Longitudinal" or config["type"] == "longitudinal":
+        return LongitudinalObservation(env, **config)
     else:
         raise ValueError("Unknown observation type")