Commit fd140001 authored by Ibrahim's avatar Ibrahim
Browse files

added PID control

parent fa3d2d30
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
...@@ -82,4 +82,18 @@ def learn_thrust_coefficient( ...@@ -82,4 +82,18 @@ def learn_thrust_coefficient(
def moment_of_inertia_disk(m: float, r: float) -> float: def moment_of_inertia_disk(m: float, r: float) -> float:
return 0.5 * m * r**2 return 0.5 * m * r**2
\ No newline at end of file
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
...@@ -51,12 +51,19 @@ def thrust( ...@@ -51,12 +51,19 @@ def thrust(
@njit @njit
def torque( def torque(
position_vector: np.ndarray, force: np.ndarray, 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: ) -> np.ndarray:
# TODO: See here
# https://andrew.gibiansky.com/downloads/pdf/Quadcopter%20Dynamics,%20Simulation,%20and%20Control.pdf
# Total moments in the body frame # Total moments in the body frame
# yaw moments # yaw moments
# tau = I . d omega/dt # 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 = r x F
tau = np.cross(position_vector, force) tau = np.cross(position_vector, force)
# print(moment_of_inertia, prop_angular_acceleration) # print(moment_of_inertia, prop_angular_acceleration)
...@@ -102,12 +109,11 @@ def apply_forces_torques( ...@@ -102,12 +109,11 @@ def apply_forces_torques(
xdot[2] = 1/mass * (fz) + g * cphi * cthe + q * ub - p * vb # = wdot xdot[2] = 1/mass * (fz) + g * cphi * cthe + q * ub - p * vb # = wdot
# xdot[3] = 1/Ixx * (tx + (Iyy - Izz) * q * r) # = pdot # xdot[3] = 1/I[0,0] * (tx + (I[1,1] - I[2,2]) * q * r) # = pdot
# xdot[4] = 1/Iyy * (ty + (Izz - Ixx) * p * r) # = qdot # xdot[4] = 1/I[1,1] * (ty + (I[2,2] - I[0,0]) * p * r) # = qdot
# xdot[5] = 1/Izz * (tz + (Ixx - Iyy) * p * q) # = rdot # 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) # 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])
# 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[3:6] = I_inv @ (torques - gyro) # TODO: verify whether it is + or - gyro
xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot xdot[6] = p + (q*sphi + r*cphi) * sthe / cthe # = phidot
...@@ -123,10 +129,3 @@ def apply_forces_torques( ...@@ -123,10 +129,3 @@ def apply_forces_torques(
xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot xdot[11] = (-sthe * ub + sphi*cthe * vb + cphi*cthe * wb) # = zIdot
return xdot 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
...@@ -7,6 +7,7 @@ from scipy.integrate import odeint ...@@ -7,6 +7,7 @@ from scipy.integrate import odeint
from vehicle import MotorParams, PropellerParams, SimulationParams, VehicleParams from vehicle import MotorParams, PropellerParams, SimulationParams, VehicleParams
from coords import body_to_inertial, direction_cosine_matrix, rotating_frame_derivative, angular_to_euler_rate 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 physics import thrust, torque, apply_forces_torques
from helpers import control_allocation_matrix
...@@ -32,10 +33,38 @@ class Propeller: ...@@ -32,10 +33,38 @@ class Propeller:
def apply_speed(self, speed: float) -> float: 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 return speed
def step(self, speed: float) -> float: 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 self.speed = speed
...@@ -87,7 +116,7 @@ class Multirotor: ...@@ -87,7 +116,7 @@ class Multirotor:
self.simulation: SimulationParams = simulation self.simulation: SimulationParams = simulation
self.state: np.ndarray = None self.state: np.ndarray = None
self.propellers: List[Propeller] = None self.propellers: List[Propeller] = None
self.propeller_vectors: np.matrix = None self.propeller_vectors: np.ndarray = None
self.t: float = 0. self.t: float = 0.
self.reset() self.reset()
...@@ -104,6 +133,7 @@ class Multirotor: ...@@ -104,6 +133,7 @@ class Multirotor:
self.propeller_vectors = np.vstack((x, y, z)) self.propeller_vectors = np.vstack((x, y, z))
self.inertial_matrix_inverse = np.asmatrix(np.linalg.inv(self.params.inertia_matrix)) 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) self.state = np.zeros(12)
...@@ -178,9 +208,10 @@ class Multirotor: ...@@ -178,9 +208,10 @@ class Multirotor:
) )
torque_vec[:, i] = torque( torque_vec[:, i] = torque(
self.propeller_vectors[:,i], thrust_vec[:,i], 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) forces = thrust_vec.sum(axis=1)
torques = torque_vec.sum(axis=1) torques = torque_vec.sum(axis=1)
return forces, torques return forces, torques
...@@ -199,7 +230,11 @@ class Multirotor: ...@@ -199,7 +230,11 @@ class Multirotor:
def step(self, u: np.ndarray): def step(self, u: np.ndarray):
self.t += self.simulation.dt 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): for u_, prop in zip(u, self.propellers):
prop.step(u_) prop.step(u_)
return self.state return self.state
...@@ -208,8 +243,8 @@ class Multirotor: ...@@ -208,8 +243,8 @@ class Multirotor:
def allocate_control(self, thrust: float, torques: np.ndarray) -> np.ndarray: def allocate_control(self, thrust: float, torques: np.ndarray) -> np.ndarray:
# TODO: njit it? np.linalg.lstsq can be compiled # 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.params.alloc, vec, rcond=None)[0]) # return np.sqrt(np.linalg.lstsq(self.alloc, vec, rcond=None)[0])
return np.sqrt( 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)
) )
...@@ -85,26 +85,6 @@ class VehicleParams: ...@@ -85,26 +85,6 @@ class VehicleParams:
self.clockwise = np.ones(len(self.propellers), dtype=int) self.clockwise = np.ones(len(self.propellers), dtype=int)
self.clockwise[::2] = 1 self.clockwise[::2] = 1
self.clockwise[1::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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment