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
......@@ -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])
......@@ -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
......@@ -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)
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[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[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 + \
# Position
xdot[0] = 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 + \
xdot[1] = 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
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
# 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
......@@ -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)
......
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