Added FP and TP cameras

Beginning of physics engine state machine
function name changes
TODO: Update PID parameters for height

Author: nb-programmer

TODO.md

@@ -5,6 +5,7 @@ Pretty long at the moment
 
 - [x] Complete movement swap (camera to player)
   - [x] Implement UAV movement by user
+- [ ] Show skybox texture
 - [ ] Support for Ardupilot SITL interface
   - [ ] Using Mavlink
   - [ ] Using integrated SITL as optional dependency

dronesim/interface/control.py

@@ -31,5 +31,7 @@ def rotate_counterclockwise(self, x : float):
         pass
     def freeze(self):
         pass
-    def directAction(self, action : DroneAction, args : dict = None):
+    def direct_action(self, action : DroneAction, args : dict = None):
         pass
+    def get_debug_data(self) -> dict:
+        return {}

dronesim/interface/default.py

@@ -20,7 +20,9 @@ def __init__(self,
         self._tick_rate = tick_rate
         self._update_enable = update_enable
         self._tps_update_period = tps_update_period
-        self.__state = dict(tps=0)
+
+        self.__debug_data = dict(tps=0)
+        self.__state = None
         self.__cmd_queue : Queue[StepActionType] = Queue()
 
         self._th = threading.Thread(target=self.droneTick, daemon=True)
@@ -46,18 +48,18 @@ def droneTick(self):
 
             #Perform step
             if self._update_enable:
-                droneState = self.drone.step(cmd)
+                self.__state = self.drone.step(cmd)
 
             #Update TPS
             if time.time() - last_tick_check >= self._tps_update_period:
                 tickDiff = (self.drone.metrics['ticks'] - last_ticks) / self._tps_update_period
                 last_ticks = self.drone.metrics['ticks']
-                self.__state['tps'] = int(tickDiff)
+                self.__debug_data['tps'] = int(tickDiff)
                 last_tick_check = time.time()
 
-            #Update state info from the simulation step
-            observation, reward, done, info = droneState
-            self.__state.update({
+            #Update debug state info from the simulation step
+            observation, reward, done, info = self.__state
+            self.__debug_data.update({
                 'state': info['state'],
                 'observation': observation,
                 'reward': reward,
@@ -72,6 +74,9 @@ def droneTick(self):
     def get_current_state(self):
         return self.__state
 
+    def get_debug_data(self) -> dict:
+        return self.__debug_data
+
     def rc_control(self, vector : StepRC):
         self.__cmd_queue.put_nowait(vector)
 
@@ -90,7 +95,7 @@ def freeze(self):
             'action': DroneAction.STOPINPLACE
         })
 
-    def directAction(self, action : DroneAction, args : dict = None):
+    def direct_action(self, action : DroneAction, args : dict = None):
         if args is None: args = {}
         self.__cmd_queue.put_nowait({
             'action': action,

dronesim/physics/simple.py

@@ -12,28 +12,40 @@ class SimpleDronePhysics(DronePhysicsEngine):
     Assume units are in centimeters when translating to irl unit
     '''
 
-    GRAVITY = glm.vec3(0, 0, -1e-3)
-    AIR_RESISTANCE = glm.vec3((0.95,)*3)
+    GRAVITY = glm.vec3(0, 0, -2e-1)
+    AIR_RESISTANCE = glm.vec3((0.98,)*3)
     #Angle is counter-clockwise from 3 o'clock position, so negate the magnitude to turn properly
-    RC_SCALE = glm.vec4(0.09, 0.09, 0.09, -0.05)
+    RC_SCALE = glm.vec4(0.2, 0.2, 0.1, -0.05)
+
+    TAKEOFF_COMPLETE_ERROR_MAX = 1e-1
+    ARMED_MIN_THRUST = 1e-3
+    TAKEOFF_COMPLETE_STABLE_TICKS = 30
 
     #PID controller parameters
-    STRAFE_CONTROL_PARAM = {'Kp': 0.45, 'Ki': 0.01, 'Kd': 0.0}
-    LIFT_CONTROL_PARAM = {'Kp': 0.002, 'Ki': 0.002, 'Kd': 0.0}
-    TURN_CONTROL_PARAM = {'Kp': 0.5, 'Ki': 0.0, 'Kd': 0.0}
+    STRAFE_CONTROL_PARAM = {'Kp': 0.02, 'Ki': 0.0, 'Kd': 0.0}
+    LIFT_CONTROL_PARAM = {'Kp': 0.019, 'Ki': 0.012, 'Kd': 0.0}
+    TURN_CONTROL_PARAM = {'Kp': 0.2, 'Ki': 0.0, 'Kd': 0.0}
 
     def reset(self, state : PhysicsStateType = None) -> PhysicsStateType:
+        #TODO: put all of this in a dict-inheriting class and use that as state
+
         #Position of the drone in space
         self.pos = glm.vec3((0,0,0))
         self.angle = glm.vec3((0,0,0))
         #Instantaneous velocity
         self.pvel = glm.vec4(0,0,0,1)
         self.fvel = glm.vec3()
         self.avel = glm.vec3()
+        self.ticks = 0
+        self._lastRC = None
+        self._tickLogs : dict = {}
 
         #Thrust to apply
         self.thrust_vec = glm.vec3()
 
+        #Landed, stationary
+        self._operation = DroneState.LANDED
+
         #Movement PID control. XY and W is for velocity, Z is for absolute position (altitude)
         self.control = Vec4PID(
             PID(**self.STRAFE_CONTROL_PARAM, sample_time=None),
@@ -45,9 +57,6 @@ def reset(self, state : PhysicsStateType = None) -> PhysicsStateType:
         self._state : PhysicsStateType = dict()
         self._updateState()
 
-        #Landed, stationary
-        self._operation = DroneState.LANDED
-
         return self.state
 
     def _updateState(self):
@@ -76,33 +85,59 @@ def state(self, state):
         self.reset(state)
 
     def step(self, action : StepActionType, dt : float = None) -> PhysicsStateType:
+        self.ticks += 1
+
         rcvec, op, params = self.decodeAction(action)
+
+        #State machine
         if op is not None:
             if op == DroneAction.TAKEOFF:
-                self._operation = DroneState.TAKING_OFF
-                self.control.z.setpoint = params.get("altitude", 10.0)
-
-        if rcvec is None:
-            rcvec = StepRC(0,0,0,0)
+                if self._operation == DroneState.LANDED:
+                    self._operation = DroneState.TAKING_OFF
+                    self.control.z.setpoint = params.get("altitude", 10.0)
+                    self._tickLogs[DroneState.TAKING_OFF] = 0
+
+        if self._operation == DroneState.TAKING_OFF:
+            z_error = self.control.z.setpoint - self.pos.z
+            abs_speed = glm.length(self.pvel.xyz)
+            #print(abs_speed)
+            if abs(z_error) < self.TAKEOFF_COMPLETE_ERROR_MAX:
+                self._tickLogs[DroneState.TAKING_OFF] += 1
+                if self._tickLogs[DroneState.TAKING_OFF] > self.TAKEOFF_COMPLETE_STABLE_TICKS:
+                    self._operation = DroneState.IN_AIR
+
+        #Save last RC for later
+        if rcvec is not None:
+            self._lastRC = (rcvec, self.ticks)
+        #If no RC, reuse previous RC input for some steps
+        else:
+            if self._lastRC is not None and self._lastRC[0] is not None and self.ticks < self._lastRC[1] + 30:
+                rcvec = self._lastRC[0]
+            else:
+                rcvec = StepRC(0,0,0,0)
 
         #Which direction to move using RC
         rc_vec = glm.vec4(rcvec)
         rc_vec = glm.clamp(rc_vec, -1, 1)
         rc_vec *= self.RC_SCALE
 
         is_on_surface = False
-        is_accept_rc = False
+        is_armed = self._operation != DroneState.LANDED
+        is_accept_rc = self._operation == DroneState.IN_AIR
 
         #RC thrust control
-        self.control.x.setpoint = rc_vec.x  # Target velocity
-        self.control.y.setpoint = rc_vec.y  # Target velocity
-        self.control.w.setpoint = rc_vec.w  # Target heading angle
-        self.control.z.setpoint += rc_vec.z # Absolute height
+        if is_accept_rc:
+            self.control.x.setpoint = rc_vec.x  # Target velocity
+            self.control.y.setpoint = rc_vec.y  # Target velocity
+            self.control.w.setpoint = rc_vec.w  # Target heading angle
+            self.control.z.setpoint += rc_vec.z # Absolute height
+            if self.control.z.setpoint < 0:
+                self.control.z.setpoint = 0.0
 
         #Apply thrust based on target position
         self.thrust_vec.x = self.control.x(self.pvel.x, dt)
         self.thrust_vec.y = self.control.y(self.pvel.y, dt)
-        self.thrust_vec.z = self.control.z(self.pos.z, dt)
+        self.thrust_vec.z = glm.max(self.control.z(self.pos.z, dt), self.ARMED_MIN_THRUST if is_armed else 0.0)
         self.avel.z = self.control.w(self.avel.z, dt)
 
         #3D coordinate transformation matrix