physics: refactored apply_physics_torques variables names to calrify forces are in body frame

        return self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d

@dataclass

class PositionController(PIDController):

    vehicle: Multirotor

    max_tilt: float = np.pi / 15

    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

        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 # [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

    def step(self, reference, measurement):

        ctrl = super().step(reference, measurement)

        # torque = moment of inertia . angular_acceleration

        return self.vehicle.params.inertia_matrix.dot(ctrl)

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):

        self.ctrl_a.reset()

        self.ctrl_p.reset()

    def step(self, ref_pos, pos, ref_yaw, 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

    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

    ub = x[3]       # linear velocity along body-frame-x-axis b1

    vb = x[4]       # linear velocity along body-frame-y-axis b2

    wb = x[5]       # linear velocity along body-frame-z-axis b3

    phi = x[6]      # Roll

    theta = x[7]    # Pitch

    psi = x[8]      # Yaw

    cthe = np.cos(theta); sthe = np.sin(theta)  # pitch

    cpsi = np.cos(psi);   spsi = np.sin(psi)    # yaw

    fx, fy, fz = forces

    tx, ty, tz = torques

    f1, f2, f3 = forces # in the body frame (b1, b2, b3)

    t1, t2, t3 = torques

    I = inertia_matrix

    I_inv = inertia_matrix_inverse

    # Calculate the derivative of the state matrix using EOM

    xdot = np.zeros_like(x)

    # Position

    # velocity = dPosition (inertial) / dt (convert body velocity to inertial)

    # Essentially = Rotation matrix (body to inertial) x body velocity

    xdot[0] = cthe*cpsi*ub + (-cphi * spsi + sphi*sthe*cpsi) * vb + \

        (sphi*spsi+cphi*sthe*cpsi) * wb  # = xIdot 

    xdot[1] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \

        (-sphi*cpsi+cphi*sthe*spsi) * wb # = yIdot 

    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

    #  Acceleration = dVelocity (body frame) / dt

    #           External forces     Gravity             Coriolis effect

    xdot[3] = 1/mass * (f1)     + g * sthe          + r * vb - q * wb  # = udot

    xdot[4] = 1/mass * (f2)     - g * sphi * cthe   - r * ub + p * wb # = vdot

    xdot[5] = 1/mass * (f3)     - g * cphi * cthe   + q * ub - p * vb # = wdot

    # Orientation

    xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe  # = phidot