Loading control.py 0 → 100644 +82 −0 Original line number Diff line number Diff line from dataclasses import dataclass import numpy as np from simulation import Multirotor @dataclass class PIDController: k_p: np.ndarray k_i: np.ndarray k_d: np.ndarray max_err_i: np.ndarray def __post_init__(self): 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) if self.max_err_i is None: self.max_err_i = np.inf def reset(self): 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 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) @dataclass class PositionController(PIDController): vehicle: Multirotor def step(self, reference, measurement): ctrl = super().step(reference=reference, 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[2] = self.vehicle.params.mass * ( ctrl[2] / (np.cos(roll) * np.cos(pitch)) ) + \ self.vehicle.weight return ctrl AttitudeController = PIDController class PosAttController: def __init__(self, ctrl_p: PositionController, ctrl_a: AttitudeController): self.ctrl_p = ctrl_p self.ctrl_a = ctrl_a def reset(self): self.ctrl_a.reset() self.ctrl_p.reset() 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) return thrust, torques helpers.py +15 −1 Original line number Diff line number Diff line Loading @@ -83,3 +83,17 @@ def learn_thrust_coefficient( def moment_of_inertia_disk(m: float, r: float) -> float: return 0.5 * m * r**2 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) 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[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 +12 −13 Original line number Diff line number Diff line Loading @@ -51,12 +51,19 @@ def thrust( @njit def torque( position_vector: np.ndarray, force: np.ndarray, moment_of_inertia: float, prop_angular_acceleration: float moment_of_inertia: float, prop_angular_acceleration: float, drag_coefficient: float, prop_angular_velocity: float, clockwise: int ) -> np.ndarray: # TODO: See here # https://andrew.gibiansky.com/downloads/pdf/Quadcopter%20Dynamics,%20Simulation,%20and%20Control.pdf # Total moments in the body frame # yaw moments # tau = I . d omega/dt tau_rot = moment_of_inertia * prop_angular_acceleration tau_rot = ( # clockwise * moment_of_inertia * prop_angular_acceleration + clockwise * drag_coefficient * prop_angular_velocity**2 ) # tau = r x F tau = np.cross(position_vector, force) # print(moment_of_inertia, prop_angular_acceleration) Loading Loading @@ -102,12 +109,11 @@ def apply_forces_torques( xdot[2] = 1/mass * (fz) + g * cphi * cthe + q * ub - p * vb # = wdot # xdot[3] = 1/Ixx * (tx + (Iyy - Izz) * q * r) # = pdot # xdot[4] = 1/Iyy * (ty + (Izz - Ixx) * p * r) # = qdot # xdot[5] = 1/Izz * (tz + (Ixx - Iyy) * p * q) # = rdot # 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]) # gyro = np.cross(x[3:6], I @ x[3:6]).squeeze() xdot[3:6] = I_inv @ (torques - gyro) # TODO: verify whether it is + or - gyro xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot Loading @@ -123,10 +129,3 @@ def apply_forces_torques( xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot return xdot def control_allocation(allocation_matrix: np.matrix, net_forces: np.array, net_torques: np.array): # [F T] = P . f # P-1 F T = f pass No newline at end of file simulation.py +41 −6 Original line number Diff line number Diff line Loading @@ -7,6 +7,7 @@ from scipy.integrate import odeint from vehicle import MotorParams, PropellerParams, SimulationParams, VehicleParams from coords import body_to_inertial, direction_cosine_matrix, rotating_frame_derivative, angular_to_euler_rate from physics import thrust, torque, apply_forces_torques from helpers import control_allocation_matrix Loading @@ -32,10 +33,38 @@ class Propeller: def apply_speed(self, speed: float) -> float: """ Calculate the actual speed of the propeller after the speed signal is given. This method is *pure* and does not change the state of the propeller. Parameters ---------- speed : float Radians per second speed command Returns ------- float The actual speed """ return speed def step(self, speed: float) -> float: """ Step through the speed command. This method changes the state of the propeller. Parameters ---------- speed : float Speed command in radians per second. Returns ------- float The actual speed achieved. """ self.speed = speed Loading Loading @@ -87,7 +116,7 @@ class Multirotor: self.simulation: SimulationParams = simulation self.state: np.ndarray = None self.propellers: List[Propeller] = None self.propeller_vectors: np.matrix = None self.propeller_vectors: np.ndarray = None self.t: float = 0. self.reset() Loading @@ -104,6 +133,7 @@ class Multirotor: self.propeller_vectors = np.vstack((x, y, z)) self.inertial_matrix_inverse = np.asmatrix(np.linalg.inv(self.params.inertia_matrix)) self.alloc, self.alloc_inverse = control_allocation_matrix(self.params) self.state = np.zeros(12) Loading Loading @@ -178,9 +208,10 @@ class Multirotor: ) torque_vec[:, i] = torque( self.propeller_vectors[:,i], thrust_vec[:,i], prop.params.moment_of_inertia, clockwise * angular_acc prop.params.moment_of_inertia, angular_acc, prop.params.k_torque, speed, clockwise ) # print(clockwise * angular_acc) forces = thrust_vec.sum(axis=1) torques = torque_vec.sum(axis=1) return forces, torques Loading @@ -199,7 +230,11 @@ class Multirotor: def step(self, u: np.ndarray): self.t += self.simulation.dt self.state = odeint(self.dxdt, self.state, (0, self.simulation.dt), args=(u,))[-1] # TODO: Explore RK45() or solve_ivp() functions from scipy.integrate? self.state = odeint( self.dxdt, self.state, (0, self.simulation.dt), args=(u,), rtol=1e-5, atol=1e-5 )[-1] for u_, prop in zip(u, self.propellers): prop.step(u_) return self.state Loading @@ -208,8 +243,8 @@ 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]) # return np.sqrt(np.linalg.lstsq(self.params.alloc, vec, rcond=None)[0]) # return np.sqrt(np.linalg.lstsq(self.alloc, vec, rcond=None)[0]) return np.sqrt( np.clip(self.params.alloc_inverse @ vec, a_min=0., a_max=None) np.clip(self.alloc_inverse @ vec, a_min=0., a_max=None) ) vehicle.py +0 −20 Original line number Diff line number Diff line Loading @@ -86,26 +86,6 @@ class VehicleParams: self.clockwise[::2] = 1 self.clockwise[1::2] = -1 # Set up control allocation matrix if thrust and motor torque constants # are provided if ( all([p.k_thrust is not None for p in self.propellers]) and \ all([p.k_torque is not None for p in self.propellers]) ): alloc = np.zeros((4, len(self.propellers))) #[Fz, Mx, My, Mz] x n-Propellers x = self.distances * np.sin(self.angles) y = self.distances * np.cos(self.angles) for i, p in enumerate(self.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[3, i] = p.k_torque * self.clockwise[i] # torque about z-axis self.alloc = alloc self.alloc_inverse = np.linalg.pinv(alloc) else: self.alloc = None self.alloc_inverse = None @dataclass Loading Loading
control.py 0 → 100644 +82 −0 Original line number Diff line number Diff line from dataclasses import dataclass import numpy as np from simulation import Multirotor @dataclass class PIDController: k_p: np.ndarray k_i: np.ndarray k_d: np.ndarray max_err_i: np.ndarray def __post_init__(self): 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) if self.max_err_i is None: self.max_err_i = np.inf def reset(self): 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 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) @dataclass class PositionController(PIDController): vehicle: Multirotor def step(self, reference, measurement): ctrl = super().step(reference=reference, 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[2] = self.vehicle.params.mass * ( ctrl[2] / (np.cos(roll) * np.cos(pitch)) ) + \ self.vehicle.weight return ctrl AttitudeController = PIDController class PosAttController: def __init__(self, ctrl_p: PositionController, ctrl_a: AttitudeController): self.ctrl_p = ctrl_p self.ctrl_a = ctrl_a def reset(self): self.ctrl_a.reset() self.ctrl_p.reset() 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) return thrust, torques
helpers.py +15 −1 Original line number Diff line number Diff line Loading @@ -83,3 +83,17 @@ def learn_thrust_coefficient( def moment_of_inertia_disk(m: float, r: float) -> float: return 0.5 * m * r**2 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) 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[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 +12 −13 Original line number Diff line number Diff line Loading @@ -51,12 +51,19 @@ def thrust( @njit def torque( position_vector: np.ndarray, force: np.ndarray, moment_of_inertia: float, prop_angular_acceleration: float moment_of_inertia: float, prop_angular_acceleration: float, drag_coefficient: float, prop_angular_velocity: float, clockwise: int ) -> np.ndarray: # TODO: See here # https://andrew.gibiansky.com/downloads/pdf/Quadcopter%20Dynamics,%20Simulation,%20and%20Control.pdf # Total moments in the body frame # yaw moments # tau = I . d omega/dt tau_rot = moment_of_inertia * prop_angular_acceleration tau_rot = ( # clockwise * moment_of_inertia * prop_angular_acceleration + clockwise * drag_coefficient * prop_angular_velocity**2 ) # tau = r x F tau = np.cross(position_vector, force) # print(moment_of_inertia, prop_angular_acceleration) Loading Loading @@ -102,12 +109,11 @@ def apply_forces_torques( xdot[2] = 1/mass * (fz) + g * cphi * cthe + q * ub - p * vb # = wdot # xdot[3] = 1/Ixx * (tx + (Iyy - Izz) * q * r) # = pdot # xdot[4] = 1/Iyy * (ty + (Izz - Ixx) * p * r) # = qdot # xdot[5] = 1/Izz * (tz + (Ixx - Iyy) * p * q) # = rdot # 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]) # gyro = np.cross(x[3:6], I @ x[3:6]).squeeze() xdot[3:6] = I_inv @ (torques - gyro) # TODO: verify whether it is + or - gyro xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot Loading @@ -123,10 +129,3 @@ def apply_forces_torques( xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot return xdot def control_allocation(allocation_matrix: np.matrix, net_forces: np.array, net_torques: np.array): # [F T] = P . f # P-1 F T = f pass No newline at end of file
simulation.py +41 −6 Original line number Diff line number Diff line Loading @@ -7,6 +7,7 @@ from scipy.integrate import odeint from vehicle import MotorParams, PropellerParams, SimulationParams, VehicleParams from coords import body_to_inertial, direction_cosine_matrix, rotating_frame_derivative, angular_to_euler_rate from physics import thrust, torque, apply_forces_torques from helpers import control_allocation_matrix Loading @@ -32,10 +33,38 @@ class Propeller: def apply_speed(self, speed: float) -> float: """ Calculate the actual speed of the propeller after the speed signal is given. This method is *pure* and does not change the state of the propeller. Parameters ---------- speed : float Radians per second speed command Returns ------- float The actual speed """ return speed def step(self, speed: float) -> float: """ Step through the speed command. This method changes the state of the propeller. Parameters ---------- speed : float Speed command in radians per second. Returns ------- float The actual speed achieved. """ self.speed = speed Loading Loading @@ -87,7 +116,7 @@ class Multirotor: self.simulation: SimulationParams = simulation self.state: np.ndarray = None self.propellers: List[Propeller] = None self.propeller_vectors: np.matrix = None self.propeller_vectors: np.ndarray = None self.t: float = 0. self.reset() Loading @@ -104,6 +133,7 @@ class Multirotor: self.propeller_vectors = np.vstack((x, y, z)) self.inertial_matrix_inverse = np.asmatrix(np.linalg.inv(self.params.inertia_matrix)) self.alloc, self.alloc_inverse = control_allocation_matrix(self.params) self.state = np.zeros(12) Loading Loading @@ -178,9 +208,10 @@ class Multirotor: ) torque_vec[:, i] = torque( self.propeller_vectors[:,i], thrust_vec[:,i], prop.params.moment_of_inertia, clockwise * angular_acc prop.params.moment_of_inertia, angular_acc, prop.params.k_torque, speed, clockwise ) # print(clockwise * angular_acc) forces = thrust_vec.sum(axis=1) torques = torque_vec.sum(axis=1) return forces, torques Loading @@ -199,7 +230,11 @@ class Multirotor: def step(self, u: np.ndarray): self.t += self.simulation.dt self.state = odeint(self.dxdt, self.state, (0, self.simulation.dt), args=(u,))[-1] # TODO: Explore RK45() or solve_ivp() functions from scipy.integrate? self.state = odeint( self.dxdt, self.state, (0, self.simulation.dt), args=(u,), rtol=1e-5, atol=1e-5 )[-1] for u_, prop in zip(u, self.propellers): prop.step(u_) return self.state Loading @@ -208,8 +243,8 @@ 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]) # return np.sqrt(np.linalg.lstsq(self.params.alloc, vec, rcond=None)[0]) # return np.sqrt(np.linalg.lstsq(self.alloc, vec, rcond=None)[0]) return np.sqrt( np.clip(self.params.alloc_inverse @ vec, a_min=0., a_max=None) np.clip(self.alloc_inverse @ vec, a_min=0., a_max=None) )
vehicle.py +0 −20 Original line number Diff line number Diff line Loading @@ -86,26 +86,6 @@ class VehicleParams: self.clockwise[::2] = 1 self.clockwise[1::2] = -1 # Set up control allocation matrix if thrust and motor torque constants # are provided if ( all([p.k_thrust is not None for p in self.propellers]) and \ all([p.k_torque is not None for p in self.propellers]) ): alloc = np.zeros((4, len(self.propellers))) #[Fz, Mx, My, Mz] x n-Propellers x = self.distances * np.sin(self.angles) y = self.distances * np.cos(self.angles) for i, p in enumerate(self.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[3, i] = p.k_torque * self.clockwise[i] # torque about z-axis self.alloc = alloc self.alloc_inverse = np.linalg.pinv(alloc) else: self.alloc = None self.alloc_inverse = None @dataclass Loading
