Loading README.md +13 −1 Original line number Diff line number Diff line # multirotor Simulation of multi-rotor unmanned aerial vehicles in python. Please cite this as: ``` @inproceedings{ahmed2022multirotor, title={A high-Fidelity Simulation test-Bed for fault-Tolerant octo-Rotor Control Using Reinforcement Learning}, author={Ahmed, Ibrahim and Quinones-Grueiro, Marcos and Biswas, Gautam}, booktitle={2022 IEEE/AIAA 41st Digital Avionics Systems Conference (DASC)}, year={2022}, organization={IEEE} } ``` No newline at end of file control.py +19 −1 Original line number Diff line number Diff line Loading @@ -17,6 +17,8 @@ class PIDController: err = reference - measurement u = k_p * err + k_d * d(err)/dt + k_i * int(err . dt) Can control a single or an array of signals, given float or array PID constants. """ k_p: np.ndarray Loading Loading @@ -47,6 +49,22 @@ class PIDController: def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray: """ Calculate the output, based on the current measurement and the reference signal. Parameters ---------- reference : np.ndarray The reference signal(s) to track. Can be a number or an array. measurement : np.ndarray The actual measurement(s). Returns ------- np.ndarray The action signal. """ err = reference - measurement self.err_p = err self.err_i = np.clip( Loading @@ -65,7 +83,7 @@ class PosController(PIDController): Position controller. Takes reference x/y position and outputs reference pitch and roll angles for x and y motion, respectively. Uses vector from current to reference position as an approximation of Uses vector from current-to-reference position as an approximation of reference velocity. Compares against measured velocity. The deficit is used to change pitch and roll angles to increase and decrease velocity. Loading helpers.py +97 −2 Original line number Diff line number Diff line Loading @@ -9,7 +9,25 @@ from .vehicle import PropellerParams, VehicleParams def moment_of_inertia_tensor_from_cooords( point_masses: Iterable[float], coords: Iterable[np.ndarray] ) -> np.matrix: ) -> np.ndarray: """ Calculate the inertial matrix given a distribution of point masses. Parameters ---------- point_masses : Iterable[float] A list of masses. coords : Iterable[np.ndarray] The corresponding coordinates of those masses about the center of rotation. Ideally, this would be the center of mass of the object. Returns ------- np.ndarray The 3x3 inertial matrix. """ # TODO: Conditionally calculate the center of mass and transform coordinates # about it if a boolean option is provided. coords = np.asarray(coords) masses = np.asarray(point_masses) x,y,z = coords[:,0], coords[:,1], coords[:,2] Loading @@ -30,7 +48,33 @@ def moment_of_inertia_tensor_from_cooords( def vehicle_params_factory( n: int, m_prop: float, d_prop: float, params: PropellerParams, m_body: float, body_shape: str='sphere_solid', body_size: float=0.1 ): ) -> VehicleParams: """ Create a simple multirotor vehicle parameters object. The multirotor has evenly spaced propellers and a simple core shape (shell, cube etc.) Parameters ---------- n : int The number of arms/propellers. m_prop : float The mass of each propeller. d_prop : float The distance of each propeller from the center of the multirotor. params : PropellerParams The parameters describing a propeller. m_body : float The mass of the central body. body_shape : str, optional The shape of the core, by default 'sphere_solid' body_size : float, optional The dimension of the core (m), by default 0.1 Returns ------- VehicleParams The parameters object. """ angle_spacing = 2 * np.pi / n angles = np.arange(angle_spacing / 2, 2 * np.pi, angle_spacing) masses = [m_prop] * n Loading Loading @@ -62,6 +106,21 @@ def vehicle_params_factory( def find_nominal_speed(thrust_fn: Callable[[float], float], weight: float) -> float: """ Calculate the speed a propeller must spin to balance the weight. Parameters ---------- thrust_fn : Callable[[float], float] A function taking the speed as input and outputting thrust (N). weight : float The weight to balance. Returns ------- float The speed to balance the weight. """ def balance(speed: float) -> float: thrust = thrust_fn(speed) residual = thrust - weight Loading @@ -73,6 +132,24 @@ def find_nominal_speed(thrust_fn: Callable[[float], float], weight: float) -> fl def learn_thrust_coefficient( thrust_fn: Callable[[float], float], domain: Tuple=(1, 10000) ) -> float: """ Assuming a quadratic relationship between thrust and propeller speed, estimate the coefficient of proportionality k_thrust, where thrust = k_thrust . speed^2 Parameters ---------- thrust_fn : Callable[[float], float] The function accepting speed and returning thrust. domain : Tuple, optional The range of speeds to try, by default (1, 10000) Returns ------- float The thrust coefficient. """ speeds = np.linspace(domain[0], domain[1], num=250) thrust = np.zeros_like(speeds) for i, speed in enumerate(speeds): Loading @@ -84,6 +161,24 @@ def learn_thrust_coefficient( def learn_speed_voltage_scaling( speed_fn: Callable[[float], float], domain: Tuple=(0,20) ) -> float: """ Assuming a linear relationship between voltage and motor speed, learn the scaling coefficient, k_scaling, where: Parameters ---------- speed_fn : Callable[[float], float] A function accepting voltage and returning speed. domain : Tuple, optional The range of voltages to try to learn the coefficient, by default (0,20) Returns ------- float The scaling coefficient. """ signals = np.linspace(domain[0], domain[1], num=10) speeds = np.zeros_like(signals) for i, signal in enumerate(signals): Loading simulation.py +65 −9 Original line number Diff line number Diff line Loading @@ -149,13 +149,15 @@ class Motor: float The speed of the motor (rad /s) """ voltage, current, last_acc = self.voltage, self.current, self._last_angular_acc last_speed = self.speed # This method simply calls step() but restores the state of the object # afterwards, thus making it a "pure" function. voltage, current, last_acc, last_speed = \ self.voltage, self.current, self._last_angular_acc, self.speed speed = self.step(u) self.voltage = voltage self.current = current self._last_angular_acc = last_acc self.speed = last_speed self.voltage, self.current, self._last_angular_acc, self.speed = \ voltage, current, last_acc, last_speed return speed Loading Loading @@ -196,6 +198,7 @@ class Battery: """ Models the state of charge of the battery of the Multirotor. """ # TODO def __init__(self, params: BatteryParams, simulation: SimulationParams) -> None: self.params = deepcopy(params) Loading @@ -212,8 +215,19 @@ class Battery: class Multirotor: """ The multirotor class models dynamics and control allocation of a vehicle. """ def __init__(self, params: VehicleParams, simulation: SimulationParams) -> None: """ Parameters ---------- params : VehicleParams The vehicle parameters. These completely describe the vehicle's properties. simulation : SimulationParams The simulation parameters. """ self.params: VehicleParams = deepcopy(params) self.simulation: SimulationParams = simulation self.state: np.ndarray = None Loading @@ -226,7 +240,18 @@ class Multirotor: self.reset() def reset(self): def reset(self) -> np.ndarray: """ Reset the state of the vehicle. This includes resetting each propeller and re-calculating inertia and allocation matrices. Can simulate dynamics with propellers with/out motors. Returns ------- np.ndarray The state of the vehicle. """ self.t = 0. for p in self.propellers: p.reset() Loading Loading @@ -394,7 +419,22 @@ class Multirotor: return np.around(xdot, 3) def step_dynamics(self, u: np.ndarray): def step_dynamics(self, u: np.ndarray) -> np.ndarray: """ Given the 6-vector of x,y,z-forces and roll,pitch,yaw-torques, calculate the next state of the vehicle. Parameters ---------- u : np.ndarray The 6-vector, where the first 3 elements are forces (N) and the next 3 elements are the torques (Nm) Returns ------- np.ndarray The new state of the vehicle. """ self.t += self.simulation.dt self.state = odeint( self.dxdt_dynamics, self.state, (0, self.simulation.dt), args=(u,), Loading @@ -405,7 +445,23 @@ class Multirotor: return self.state def step_speeds(self, u: np.ndarray): def step_speeds(self, u: np.ndarray) -> np.ndarray: """ Given the n-vector of propeller speed signals, calculate the next state of the vehicle. Where n is number of propellers. Parameters ---------- u : np.ndarray The speed signals to be sent to each propeller's step() method. Can be the actual speed (rad/s) or the voltage signal (V) if a motor is used and MotorParams.speed_voltage_scaling constant is set. Returns ------- np.ndarray The new state of the vehicle. """ self.t += self.simulation.dt self.state = odeint( self.dxdt_speeds, self.state, (0, self.simulation.dt), args=(u,), Loading trajectories.py +10 −0 Original line number Diff line number Diff line Loading @@ -32,6 +32,16 @@ class Trajectory: Iterate over waypoints for a multirotor. The trajectory class can segment a list of waypoints into smaller sections and feed them to the controller when the vehicle is within a radius of its current waypoint. For example: m = Multirotor(...) traj = Trajectory( points=[(0,0,0), (0,0,2), (10,0,2)], vehicle=m, proximity=0.1, resolution=0.5) for point in traj: Each point is spaced `resolution` units apart in euclidean distance. When m.position is within `proximity` of current point, the next point in the trajectory is yielded. """ Loading Loading
README.md +13 −1 Original line number Diff line number Diff line # multirotor Simulation of multi-rotor unmanned aerial vehicles in python. Please cite this as: ``` @inproceedings{ahmed2022multirotor, title={A high-Fidelity Simulation test-Bed for fault-Tolerant octo-Rotor Control Using Reinforcement Learning}, author={Ahmed, Ibrahim and Quinones-Grueiro, Marcos and Biswas, Gautam}, booktitle={2022 IEEE/AIAA 41st Digital Avionics Systems Conference (DASC)}, year={2022}, organization={IEEE} } ``` No newline at end of file
control.py +19 −1 Original line number Diff line number Diff line Loading @@ -17,6 +17,8 @@ class PIDController: err = reference - measurement u = k_p * err + k_d * d(err)/dt + k_i * int(err . dt) Can control a single or an array of signals, given float or array PID constants. """ k_p: np.ndarray Loading Loading @@ -47,6 +49,22 @@ class PIDController: def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray: """ Calculate the output, based on the current measurement and the reference signal. Parameters ---------- reference : np.ndarray The reference signal(s) to track. Can be a number or an array. measurement : np.ndarray The actual measurement(s). Returns ------- np.ndarray The action signal. """ err = reference - measurement self.err_p = err self.err_i = np.clip( Loading @@ -65,7 +83,7 @@ class PosController(PIDController): Position controller. Takes reference x/y position and outputs reference pitch and roll angles for x and y motion, respectively. Uses vector from current to reference position as an approximation of Uses vector from current-to-reference position as an approximation of reference velocity. Compares against measured velocity. The deficit is used to change pitch and roll angles to increase and decrease velocity. Loading
helpers.py +97 −2 Original line number Diff line number Diff line Loading @@ -9,7 +9,25 @@ from .vehicle import PropellerParams, VehicleParams def moment_of_inertia_tensor_from_cooords( point_masses: Iterable[float], coords: Iterable[np.ndarray] ) -> np.matrix: ) -> np.ndarray: """ Calculate the inertial matrix given a distribution of point masses. Parameters ---------- point_masses : Iterable[float] A list of masses. coords : Iterable[np.ndarray] The corresponding coordinates of those masses about the center of rotation. Ideally, this would be the center of mass of the object. Returns ------- np.ndarray The 3x3 inertial matrix. """ # TODO: Conditionally calculate the center of mass and transform coordinates # about it if a boolean option is provided. coords = np.asarray(coords) masses = np.asarray(point_masses) x,y,z = coords[:,0], coords[:,1], coords[:,2] Loading @@ -30,7 +48,33 @@ def moment_of_inertia_tensor_from_cooords( def vehicle_params_factory( n: int, m_prop: float, d_prop: float, params: PropellerParams, m_body: float, body_shape: str='sphere_solid', body_size: float=0.1 ): ) -> VehicleParams: """ Create a simple multirotor vehicle parameters object. The multirotor has evenly spaced propellers and a simple core shape (shell, cube etc.) Parameters ---------- n : int The number of arms/propellers. m_prop : float The mass of each propeller. d_prop : float The distance of each propeller from the center of the multirotor. params : PropellerParams The parameters describing a propeller. m_body : float The mass of the central body. body_shape : str, optional The shape of the core, by default 'sphere_solid' body_size : float, optional The dimension of the core (m), by default 0.1 Returns ------- VehicleParams The parameters object. """ angle_spacing = 2 * np.pi / n angles = np.arange(angle_spacing / 2, 2 * np.pi, angle_spacing) masses = [m_prop] * n Loading Loading @@ -62,6 +106,21 @@ def vehicle_params_factory( def find_nominal_speed(thrust_fn: Callable[[float], float], weight: float) -> float: """ Calculate the speed a propeller must spin to balance the weight. Parameters ---------- thrust_fn : Callable[[float], float] A function taking the speed as input and outputting thrust (N). weight : float The weight to balance. Returns ------- float The speed to balance the weight. """ def balance(speed: float) -> float: thrust = thrust_fn(speed) residual = thrust - weight Loading @@ -73,6 +132,24 @@ def find_nominal_speed(thrust_fn: Callable[[float], float], weight: float) -> fl def learn_thrust_coefficient( thrust_fn: Callable[[float], float], domain: Tuple=(1, 10000) ) -> float: """ Assuming a quadratic relationship between thrust and propeller speed, estimate the coefficient of proportionality k_thrust, where thrust = k_thrust . speed^2 Parameters ---------- thrust_fn : Callable[[float], float] The function accepting speed and returning thrust. domain : Tuple, optional The range of speeds to try, by default (1, 10000) Returns ------- float The thrust coefficient. """ speeds = np.linspace(domain[0], domain[1], num=250) thrust = np.zeros_like(speeds) for i, speed in enumerate(speeds): Loading @@ -84,6 +161,24 @@ def learn_thrust_coefficient( def learn_speed_voltage_scaling( speed_fn: Callable[[float], float], domain: Tuple=(0,20) ) -> float: """ Assuming a linear relationship between voltage and motor speed, learn the scaling coefficient, k_scaling, where: Parameters ---------- speed_fn : Callable[[float], float] A function accepting voltage and returning speed. domain : Tuple, optional The range of voltages to try to learn the coefficient, by default (0,20) Returns ------- float The scaling coefficient. """ signals = np.linspace(domain[0], domain[1], num=10) speeds = np.zeros_like(signals) for i, signal in enumerate(signals): Loading
simulation.py +65 −9 Original line number Diff line number Diff line Loading @@ -149,13 +149,15 @@ class Motor: float The speed of the motor (rad /s) """ voltage, current, last_acc = self.voltage, self.current, self._last_angular_acc last_speed = self.speed # This method simply calls step() but restores the state of the object # afterwards, thus making it a "pure" function. voltage, current, last_acc, last_speed = \ self.voltage, self.current, self._last_angular_acc, self.speed speed = self.step(u) self.voltage = voltage self.current = current self._last_angular_acc = last_acc self.speed = last_speed self.voltage, self.current, self._last_angular_acc, self.speed = \ voltage, current, last_acc, last_speed return speed Loading Loading @@ -196,6 +198,7 @@ class Battery: """ Models the state of charge of the battery of the Multirotor. """ # TODO def __init__(self, params: BatteryParams, simulation: SimulationParams) -> None: self.params = deepcopy(params) Loading @@ -212,8 +215,19 @@ class Battery: class Multirotor: """ The multirotor class models dynamics and control allocation of a vehicle. """ def __init__(self, params: VehicleParams, simulation: SimulationParams) -> None: """ Parameters ---------- params : VehicleParams The vehicle parameters. These completely describe the vehicle's properties. simulation : SimulationParams The simulation parameters. """ self.params: VehicleParams = deepcopy(params) self.simulation: SimulationParams = simulation self.state: np.ndarray = None Loading @@ -226,7 +240,18 @@ class Multirotor: self.reset() def reset(self): def reset(self) -> np.ndarray: """ Reset the state of the vehicle. This includes resetting each propeller and re-calculating inertia and allocation matrices. Can simulate dynamics with propellers with/out motors. Returns ------- np.ndarray The state of the vehicle. """ self.t = 0. for p in self.propellers: p.reset() Loading Loading @@ -394,7 +419,22 @@ class Multirotor: return np.around(xdot, 3) def step_dynamics(self, u: np.ndarray): def step_dynamics(self, u: np.ndarray) -> np.ndarray: """ Given the 6-vector of x,y,z-forces and roll,pitch,yaw-torques, calculate the next state of the vehicle. Parameters ---------- u : np.ndarray The 6-vector, where the first 3 elements are forces (N) and the next 3 elements are the torques (Nm) Returns ------- np.ndarray The new state of the vehicle. """ self.t += self.simulation.dt self.state = odeint( self.dxdt_dynamics, self.state, (0, self.simulation.dt), args=(u,), Loading @@ -405,7 +445,23 @@ class Multirotor: return self.state def step_speeds(self, u: np.ndarray): def step_speeds(self, u: np.ndarray) -> np.ndarray: """ Given the n-vector of propeller speed signals, calculate the next state of the vehicle. Where n is number of propellers. Parameters ---------- u : np.ndarray The speed signals to be sent to each propeller's step() method. Can be the actual speed (rad/s) or the voltage signal (V) if a motor is used and MotorParams.speed_voltage_scaling constant is set. Returns ------- np.ndarray The new state of the vehicle. """ self.t += self.simulation.dt self.state = odeint( self.dxdt_speeds, self.state, (0, self.simulation.dt), args=(u,), Loading
trajectories.py +10 −0 Original line number Diff line number Diff line Loading @@ -32,6 +32,16 @@ class Trajectory: Iterate over waypoints for a multirotor. The trajectory class can segment a list of waypoints into smaller sections and feed them to the controller when the vehicle is within a radius of its current waypoint. For example: m = Multirotor(...) traj = Trajectory( points=[(0,0,0), (0,0,2), (10,0,2)], vehicle=m, proximity=0.1, resolution=0.5) for point in traj: Each point is spaced `resolution` units apart in euclidean distance. When m.position is within `proximity` of current point, the next point in the trajectory is yielded. """ Loading
