Commit 4cf5fd7e authored by Ibrahim's avatar Ibrahim
Browse files

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.
parent fd140001

control.py

+176 −12
Original line number Diff line number Diff line
@@ -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

+5 −5
Original line number Diff line number Diff line
@@ -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

+26 −29
Original line number Diff line number Diff line
@@ -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)

    # Position

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

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

        

    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[1] = cthe*spsi * ub + (cphi*cpsi+sphi*sthe*spsi) * vb + \

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

        

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



    xdot[9] = 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 + \

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

        

    xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot

    # 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

+29 −19
Original line number Diff line number Diff line
@@ -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)