Changed coord system, PID controllers exhibiting sensible behavior;

simulation.py: z coord points up in body and inertial frames, rearranging variable indices to match gazebo calculations (pos, vel, orientation, angular rate), propeller reset does not overwrite params, thrust-constant based thrust function.

physics.py: rearranged state variable indices, changed d(velocity) equations so forces act in correct direction, given pwhich direction is assumed as positive angle. Previously positive torque was causing negative x/y motion due to inverted axes.

helpers.py: bugfix in calculating control allocation matrix. x-moment is due to y-distances. Approproate signs conventions for positive moments.

control.py: New iteration of controllers (Att, Alt, Pos, Controller) which work properly when cascaded.

control.py

@@ -18,22 +18,25 @@ class PIDController:
         self.err_p = np.zeros_like(self.k_p)
         self.err_i = np.zeros_like(self.k_i)
         self.err_d = np.zeros_like(self.k_d)
+        self.err = 0.
         if self.max_err_i is None:
             self.max_err_i = np.inf
 
 
     def reset(self):
+        self.err = 0.
         self.err_p *= 0
         self.err_i *= 0
         self.err_d *= 0
 
 
     def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray:
-        self.err = err = reference - measurement
+        err = reference - measurement
         self.err_p = err
         self.err_i = np.clip(self.err_i + err, a_min=-self.max_err_i, a_max=self.max_err_i)
-        self.err_d = err - self.err_d
-        return (self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d)
+        self.err_d = err - self.err
+        self.err = err
+        return self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d
 
 
 
@@ -41,22 +44,79 @@ class PIDController:
 class PositionController(PIDController):
 
     vehicle: Multirotor
+    max_tilt: float = np.pi / 15
 
 
-    def step(self, reference, measurement):
-        ctrl = super().step(reference=reference, measurement=measurement)
+    def __post_init__(self):
+        self.k_p = np.ones(3) * np.asarray(self.k_p)
+        self.k_i = np.ones(3) * np.asarray(self.k_i)
+        self.k_d = np.ones(3) * np.asarray(self.k_d)
+        
+        self.err_p = np.zeros_like(self.k_p)
+        self.err_i = np.zeros_like(self.k_i)
+        self.err_d = np.zeros_like(self.k_d)
+        self.err = 0.
+        if self.max_err_i is None:
+            self.max_err_i = np.inf
+
+
+    def _step(self, reference, measurement):
+        roll, pitch, yaw = self.vehicle.orientation
+        err = reference - measurement
+        rot = np.asarray([
+            [np.cos(yaw),   np.sin(yaw),    0],
+            [-np.sin(yaw),  np.cos(yaw),    0],
+            [0,             0,              1]
+        ])
+        # convert reference x/y to body frame, given yaw
+        # Using rotation matrix. For a positive yaw, the target x,y will appear
+        # rotated by a negative yaw in the body x/y axes
+        err_body = rot @ err
+        ctrl = super().step(reference=ref_body, measurement=measurement)
         # ctrl[0] -> x dir -> pitch -> forward
         # ctrl[1] -> y dir -> roll -> lateral
         # ctrl[2] -> z dir -> thrust -> vertical
-        roll, pitch, yaw = self.vehicle.orientation
+        ctrl[0:2] = np.clip(ctrl[0:2], a_min=-self.max_tilt, a_max=self.max_tilt)
+        # ctrl[2] here is change in z-velocity i.e. acceleration
         ctrl[2] = self.vehicle.params.mass * (
                 ctrl[2] / (np.cos(roll) * np.cos(pitch))
             ) + \
             self.vehicle.weight
-        return ctrl
+        return ctrl # [delta pitch rate, delta roll rate, thrust force]
+
+    def step(self, reference, measurement):
+        xref = reference[0]
+        yref = reference[1]
+        psi = self.vehicle.orientation[2]
+        x = measurement[0]
+        y = measurement[1]
+        #print(x)
+        xdot = self.vehicle.velocity[0]
+        ydot = self.vehicle.velocity[1]
+        xerror = xref - x
+        yerror = yref - y
+        cosPsi = np.cos(psi)
+        sinPsi = np.sin(psi)
+        xErrorBodyFrame = xerror*cosPsi + yerror*sinPsi
+        yErrorBodyFrame = yerror*cosPsi - xerror*sinPsi
+        theta_des = (xErrorBodyFrame-xdot)*self.k_p[0] - xdot*self.k_d[0]
+        phi_des = ((yErrorBodyFrame-ydot)*self.k_p[1] - ydot*self.k_d[1])
+        theta_des = min(max(-np.pi / 15, theta_des), np.pi / 15)
+        phi_des = min(max(-np.pi / 15, phi_des), np.pi / 15)
+        thrust = self.vehicle.params.mass*(self.vehicle.simulation.g - self.k_d[2]*self.vehicle.velocity[2]-self.k_p[2]*(measurement[2]-reference[2]))
+        return theta_des, phi_des, thrust
+
 
+@dataclass
+class AttitudeController(PIDController):
+
+    vehicle: Multirotor
 
-AttitudeController = PIDController
+
+    def step(self, reference, measurement):
+        ctrl = super().step(reference, measurement)
+        # torque = moment of inertia . angular_acceleration
+        return self.vehicle.params.inertia_matrix.dot(ctrl)
 
 
 
@@ -66,6 +126,8 @@ class PosAttController:
     def __init__(self, ctrl_p: PositionController, ctrl_a: AttitudeController):
         self.ctrl_p = ctrl_p
         self.ctrl_a = ctrl_a
+        self.vehicle = self.ctrl_a.vehicle
+        assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."
 
 
     def reset(self):
@@ -74,9 +136,111 @@ class PosAttController:
 
 
     def step(self, ref_pos, pos, ref_yaw, att):
-        pitch, roll, thrust = self.ctrl_p.step(ref_pos, pos)
-        pitch, roll = np.clip((pitch, roll), a_min=-np.pi/2, a_max=np.pi/2)
-        ref_att = np.asarray([roll, pitch, ref_yaw])
-        torques = self.ctrl_a.step(ref_att, att)
+        # TODO: Att control is by angular velovity
+        pitch_rate, roll_rate, thrust = self.ctrl_p.step(ref_pos, pos)
+        ref_rate = np.asarray([roll_rate, pitch_rate, ref_yaw - att[2]])
+        torques = self.ctrl_a.step(ref_rate, self.vehicle.angular_rate)
         return thrust, torques
 
+# Altitude control
+# Position Control (x,y) -> theta, phi
+#   Attitude Control (phi, theta, psi) -> delta phi, delta theta, delta psi
+
+@dataclass
+class PosController(PIDController):
+
+    vehicle: Multirotor
+    max_tilt: float = np.pi / 15
+
+
+    def __post_init__(self):
+        self.k_p = np.ones(2) * np.asarray(self.k_p)
+        self.k_i = np.ones(2) * np.asarray(self.k_i)
+        self.k_d = np.ones(2) * np.asarray(self.k_d)
+        
+        self.err_p = np.zeros_like(self.k_p)
+        self.err_i = np.zeros_like(self.k_i)
+        self.err_d = np.zeros_like(self.k_d)
+        self.err = 0.
+        if self.max_err_i is None:
+            self.max_err_i = np.inf
+
+
+    def step(self, reference, measurement):
+        roll, pitch, yaw = self.vehicle.orientation
+        delta_x, delta_y = reference - measurement
+        rot = np.asarray([
+            [np.cos(yaw),   np.sin(yaw)],
+            [-np.sin(yaw),  np.cos(yaw)],
+        ])
+        # convert reference x/y to body frame, given yaw
+        # Using rotation matrix. For a positive yaw, the target x,y will appear
+        # desired/reference change in x/y i.e. velocity
+        ref_delta_xy = rot @ np.asarray([delta_x, delta_y])
+        # actual/measured velocity
+        mea_delta_xy = self.vehicle.velocity[:2]
+        # desired pitch, roll
+        ctrl = super().step(reference=ref_delta_xy, measurement=mea_delta_xy)
+        # ctrl[0] -> x dir -> pitch -> forward
+        # ctrl[1] -> y dir -> roll -> lateral
+        ctrl[0:2] = np.clip(ctrl[0:2], a_min=-self.max_tilt, a_max=self.max_tilt)
+        ctrl[1] *= -1 # +y motion requires negative roll
+        return ctrl # desired pitch, roll
+
+
+
+@dataclass
+class AttController(PIDController):
+
+    vehicle: Multirotor
+
+
+    def step(self, reference, measurement):
+        # desired change in orientation i.e. angular velocity
+        ref_delta = reference - measurement
+        # actual change in orientation
+        mea_delta = self.vehicle.angular_rate
+        # prescribed change in velocity i.e. angular acceleration
+        ctrl = super().step(reference=ref_delta, measurement=mea_delta)
+        # torque = moment of inertia . angular_acceleration
+        return self.vehicle.params.inertia_matrix.dot(ctrl)
+
+
+@dataclass
+class AltController(PIDController):
+
+    vehicle: Multirotor
+
+    def step(self, reference, measurement):
+            roll, pitch, yaw = self.vehicle.orientation
+            # desired change in z i.e. velocity
+            ref_delta_z = reference - measurement
+            # actual change in z
+            mea_delta_z = self.vehicle.velocity[2]
+            # change in delta_z i.e. change in velocity i.e. acceleration
+            ctrl = super().step(reference=ref_delta_z, measurement=mea_delta_z)
+            # change in z-velocity i.e. acceleration
+            ctrl = self.vehicle.params.mass * (
+                    ctrl / (np.cos(roll) * np.cos(pitch))
+                ) + \
+                self.vehicle.weight
+            return ctrl # thrust force
+
+
+class Controller:
+
+    def __init__(self, ctrl_p: PosController, ctrl_a: AttController, ctrl_z: AltController):
+        self.ctrl_p = ctrl_p
+        self.ctrl_a = ctrl_a
+        self.ctrl_z = ctrl_z
+        self.vehicle = self.ctrl_a.vehicle
+        assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."
+        assert self.ctrl_a.vehicle is self.ctrl_z.vehicle, "Vehicle instances different."
+
+    def step(self, reference, measurement=None):
+        # x,y,z,yaw
+        pitch_roll = self.ctrl_p.step(reference[:2], self.vehicle.position[:2])
+        ref_orientation = np.asarray([pitch_roll[1], pitch_roll[0], reference[3]])
+        torques = self.ctrl_a.step(ref_orientation, self.vehicle.orientation)
+        thrust = self.ctrl_z.step(reference[2], self.vehicle.position[2])
+        return np.asarray([thrust, *torques])

helpers.py

@@ -88,12 +88,12 @@ def moment_of_inertia_disk(m: float, r: float) -> float:
 
 def control_allocation_matrix(params: VehicleParams) -> Tuple[np.ndarray, np.ndarray]:
     alloc = np.zeros((4, len(params.propellers))) #[Fz, Mx, My, Mz] x n-Propellers
-    x = params.distances * np.sin(params.angles)
-    y = params.distances * np.cos(params.angles)
+    x = params.distances * np.cos(params.angles)
+    y = params.distances * np.sin(params.angles)
     for i, p in enumerate(params.propellers):
-        alloc[0, i] = -p.k_thrust                       # vertical force (negative up)
-        alloc[1, i] = p.k_thrust * x[i]                 # torque about x-axis
-        alloc[2, i] = p.k_thrust * y[i]                 # torque about y-axis
+        alloc[0, i] = p.k_thrust                       # vertical force
+        alloc[1, i] = p.k_thrust * y[i]                 # torque about x-axis
+        alloc[2, i] = p.k_thrust * (-x[i])              # torque about y-axis
         alloc[3, i] = p.k_torque * params.clockwise[i]    # torque about z-axis
     alloc_inverse = np.linalg.pinv(alloc)
     return alloc, alloc_inverse
\ No newline at end of file

physics.py

@@ -78,18 +78,18 @@ def apply_forces_torques(
     inertia_matrix: np.matrix, inertia_matrix_inverse: np.matrix
 ) -> np.ndarray:
     # Store state variables in a readable format
-    ub = x[0]       # linear velocity along body-frame-x-axis
-    vb = x[1]       # linear velocity along body-frame-y-axis
-    wb = x[2]       # linear velocity along body-frame-z-axis (down is positive)
-    p = x[3]        # body-frame-x-axis rotation rate
-    q = x[4]        # body-frame-y-axis rotation rate
-    r = x[5]        # body-frame-z-axis rotation rate
+    xI = x[0]       # Inertial frame positions
+    yI = x[1]
+    zI = x[2]
+    ub = x[3]       # linear velocity along body-frame-x-axis
+    vb = x[4]       # linear velocity along body-frame-y-axis
+    wb = x[5]       # linear velocity along body-frame-z-axis (down is positive)
     phi = x[6]      # Roll
     theta = x[7]    # Pitch
     psi = x[8]      # Yaw
-    xI = x[9]       # Inertial frame positions
-    yI = x[10]
-    zI = x[11]      # In inertial frame, down is positive z
+    p = x[9]        # body-frame-x-axis rotation rate
+    q = x[10]        # body-frame-y-axis rotation rate
+    r = x[11]        # body-frame-z-axis rotation rate
     
     # Pre-calculate trig values
     cphi = np.cos(phi);   sphi = np.sin(phi)    # roll
@@ -103,29 +103,26 @@ def apply_forces_torques(
     
     # Calculate the derivative of the state matrix using EOM
     xdot = np.zeros_like(x)
-    
-    xdot[0] = 1/mass * (fx) - g * sthe + r * vb - q * wb  # = udot
-    xdot[1] = 1/mass * (fy) + g * sphi * cthe - r * ub + p * wb # = vdot
-    xdot[2] = 1/mass * (fz) + g * cphi * cthe + q * ub - p * vb # = wdot
-
-
-    # xdot[3] = 1/I[0,0] * (tx + (I[1,1] - I[2,2]) * q * r)  # = pdot
-    # xdot[4] = 1/I[1,1] * (ty + (I[2,2] - I[0,0]) * p * r)  # = qdot
-    # xdot[5] = 1/I[2,2] * (tz + (I[0,0] - I[1,1]) * p * q)  # = rdot
-    # print(I.shape, x[3:6].shape, (I @ x[3:6]).shape)
-    gyro = np.cross(x[3:6], I @ x[3:6])
-    xdot[3:6] = I_inv @ (torques - gyro) # TODO: verify whether it is + or - gyro
-
-    xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe  # = phidot
-    xdot[7] = q * cphi - r * sphi  # = thetadot
-    xdot[8] = (q * sphi + r * cphi) / cthe  # = psidot
-    
-    xdot[9] = cthe*cpsi*ub + (-cphi * spsi + sphi*sthe*cpsi) * vb + \
+    # Position
+    xdot[0] = cthe*cpsi*ub + (-cphi * spsi + sphi*sthe*cpsi) * vb + \
         (sphi*spsi+cphi*sthe*cpsi) * wb  # = xIdot
         
-    xdot[10] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \
+    xdot[1] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \
         (-sphi*cpsi+cphi*sthe*spsi) * wb # = yIdot
         
-    xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot
+    xdot[2] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot
+    #  Velocity
+    xdot[3] = 1/mass * (fx) + g * sthe + r * vb - q * wb  # = udot
+    xdot[4] = 1/mass * (fy) - g * sphi * cthe - r * ub + p * wb # = vdot
+    xdot[5] = 1/mass * (fz) - g * cphi * cthe + q * ub - p * vb # = wdot
+
+    # Orientation
+    xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe  # = phidot
+    xdot[7] = q * cphi - r * sphi  # = thetadot
+    xdot[8] = (q * sphi + r * cphi) / cthe  # = psidot
+
+    # Angular rate
+    gyro = np.cross(x[9:12], I @ x[9:12])
+    xdot[9:12] = I_inv @ (torques - gyro) # TODO: verify whether it is + or - gyro
     
     return xdot

simulation.py

@@ -17,11 +17,20 @@ class Propeller:
     certain rate.
     """
 
-    def __init__(self, params: PropellerParams, simulation: SimulationParams) -> None:
+    def __init__(
+        self, params: PropellerParams, simulation: SimulationParams,
+        use_thrust_constant: bool=False
+    ) -> None:
         self.params: PropellerParams = params
         self.simulation: SimulationParams = simulation
         self.speed: float = 0.
         "Radians per second"
+        self.use_thrust_constant = use_thrust_constant
+
+        if use_thrust_constant:
+            self.thrust = self._thrust_constant
+        else:
+            self.thrust = self._thrust_physics
 
 
     def reset(self):
@@ -68,7 +77,11 @@ class Propeller:
         self.speed = speed
 
 
-    def thrust(self, speed, airstream_velocity: np.ndarray=np.zeros(3)) -> float:
+    def _thrust_constant(self, speed,airstream_velocity: np.ndarray=np.zeros(3)) -> float:
+        return self.params.k_thrust * speed**2
+
+
+    def _thrust_physics(self, speed, airstream_velocity: np.ndarray=np.zeros(3)) -> float:
         p = self.params
         return thrust(
             speed, airstream_velocity,
@@ -118,15 +131,16 @@ class Multirotor:
         self.propellers: List[Propeller] = None
         self.propeller_vectors: np.ndarray = None
         self.t: float = 0.
+        self.propellers = []
+        for params in self.params.propellers:
+            self.propellers.append(Propeller(params, self.simulation))
         self.reset()
 
 
     def reset(self):
         self.t = 0.
-        self.propellers = []
-        for params in self.params.propellers:
-            self.propellers.append(Propeller(params, self.simulation))
-
+        for p in self.propellers:
+            p.reset()
         x = cos(self.params.angles) * self.params.distances
         y = sin(self.params.angles) * self.params.distances
         z = np.zeros_like(y)
@@ -136,25 +150,24 @@ class Multirotor:
         self.alloc, self.alloc_inverse = control_allocation_matrix(self.params)
 
         self.state = np.zeros(12)
+        return self.state
 
 
     @property
-    def position(self) -> np.ndarray:
-        """Navigation coordinates (height = - z coordinate)"""
-        return np.asarray([self.state[9], self.state[10], -self.state[11]])
+    def position(self):
+        return self.state[0:3]
 
 
     @property
     def velocity(self) -> np.ndarray:
         """Body-frame velocity"""
-        return self.state[:3]
+        return self.state[3:6]
 
 
     @property
-    def navigation_velocity(self) -> np.ndarray:
+    def world_velocity(self) -> np.ndarray:
         dcm = direction_cosine_matrix(*self.orientation)
         v_inertial = body_to_inertial(self.velocity, dcm)
-        v_inertial[2] *= -1 # convert inertial to navigation frame (h = -z)
         return v_inertial
 
 
@@ -167,7 +180,7 @@ class Multirotor:
     @property
     def angular_rate(self) -> np.ndarray:
         """Angular rate of body frame axes (not same as rate of roll, pitch, yaw)"""
-        return self.state[3:6]
+        return self.state[9:12]
 
 
     @property
@@ -200,11 +213,8 @@ class Multirotor:
             last_speed = prop.speed
             speed = prop.apply_speed(speed)
             angular_acc = (speed - last_speed) / self.simulation.dt
-            thrust_vec[2, i] = -thrust(
-                speed, airstream_velocity_inertial[:, i], prop.params.R,
-                prop.params.A, self.simulation.rho, prop.params.a,
-                prop.params.b, prop.params.c, prop.params.eta,
-                prop.params.theta0, prop.params.theta1
+            thrust_vec[2, i] = prop.thrust(
+                speed, airstream_velocity_inertial[:, i]
             )
             torque_vec[:, i] = torque(
                 self.propeller_vectors[:,i], thrust_vec[:,i],
@@ -242,7 +252,7 @@ class Multirotor:
 
     def allocate_control(self, thrust: float, torques: np.ndarray) -> np.ndarray:
         # TODO: njit it? np.linalg.lstsq can be compiled
-        vec = np.asarray([-thrust, *torques])
+        vec = np.asarray([thrust, *torques])
         # return np.sqrt(np.linalg.lstsq(self.alloc, vec, rcond=None)[0])
         return np.sqrt(
             np.clip(self.alloc_inverse @ vec, a_min=0., a_max=None)