Skip to content

Commit

Permalink
Merge branch 'release/v1.5' into release-checklist
Browse files Browse the repository at this point in the history
  • Loading branch information
@aqitya
aqitya committed Jun 28, 2023
2 parents 981b410 + e57bd05 commit 2aac42d
Show file tree
Hide file tree
Showing 33 changed files with 243 additions and 231 deletions.
2 changes: 1 addition & 1 deletion examples/events.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ def event_state(self, state):

def threshold_met(self, x):
# Get threshold met from parent
t_met = super().threshold_met(x)
t_met = super().threshold_met(x)

# Add yell and red states from event_state
event_state = self.event_state(x)
Expand Down
2 changes: 1 addition & 1 deletion src/prog_models/data_models/dmd.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,7 +191,7 @@ def from_data(cls, times, inputs, outputs, states=None, event_states=None, **kwa
# Input validation
if not isinstance(config['trim_data_to'], Number) or config['trim_data_to'] > 1 or config['trim_data_to'] <= 0:
raise ValueError("Invalid 'trim_data_to' input value, must be between 0 and 1.")
if not isinstance(config['stability_tol'], Number) or config['stability_tol'] < 0:
if not isinstance(config['stability_tol'], Number) or config['stability_tol'] < 0:
raise ValueError(f"Invalid 'stability_tol' input value {config['stability_tol']}, must be a positive number.")
if not isinstance(config['training_noise'], Number) or config['training_noise'] < 0:
raise ValueError(f"Invalid 'training_noise' input value {config['training_noise']}, must be a positive number.")
Expand Down
9 changes: 4 additions & 5 deletions src/prog_models/loading/controllers.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,6 @@ def compute_input(self, gain, state_error):
return - np.dot(gain, state_error)

def build_scheduled_control(self, system_linear_model_fun, input_vector, state_vector_vals=None, index_scheduled_var=None):

if state_vector_vals is None:
# using psi (yaw angle) as scheduled variable as the LQR control cannot work with yaw=0 since it's in the inertial frame.
n_schedule_grid = 360*2 + 1
Expand All @@ -173,8 +172,8 @@ def build_scheduled_control(self, system_linear_model_fun, input_vector, state_v

for j in range(m):
phi, theta, psi = state_vector_vals[3:6, j]
p, q, r = state_vector_vals[-3:, j]
Aj, Bj = system_linear_model_fun(phi, theta, psi, p, q, r, input_vector[0])
p, q, r = state_vector_vals[-3:, j]
Aj, Bj = system_linear_model_fun(phi, theta, psi, p, q, r, input_vector[0])
self.control_gains[:, :, j], _ = self.compute_gain(Aj, Bj)

print('Control gain matrices complete.')
Expand Down Expand Up @@ -268,9 +267,9 @@ def __init__(self, x_ref, vehicle, **kwargs):
self.build_scheduled_control(vehicle.linear_model, input_vector=[self.ss_input])

def compute_gain(self, A, B):
""" Compute controller gain given state of the system described by linear model A, B"""
"""Compute controller gain given state of the system described by linear model A, B"""
self.Ai[:self.n_states, :self.n_states] = A
self.Bi[:self.n_states, :] = B
self.Bi[:self.n_states, :] = B
self.K, self.E = lqr_calc_k(self.Ai, self.Bi, self.parameters['Qi'], self.parameters['Ri'])
return self.K, self.E

Expand Down
114 changes: 56 additions & 58 deletions src/prog_models/models/aircraft_model/small_rotorcraft.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,7 @@ def dx(self, x, u):
# Extract useful values
m = self.mass['total'] # vehicle mass
Ixx, Iyy, Izz = self.mass['Ixx'], self.mass['Iyy'], self.mass['Izz'] # vehicle inertia

# Input vector
T = u['T'] # Thrust (along body z)
tp = u['mx'] # Moment along body x
Expand All @@ -196,7 +196,7 @@ def dx(self, x, u):
tan_theta = np.tan(theta)
sin_psi = np.sin(psi)
cos_psi = np.cos(psi)

# Compute drag forces
v_earth = np.array([vx_a, vy_a, vz_a]).reshape((-1,)) # velocity in Earth-fixed frame
v_body = np.dot(
Expand All @@ -223,27 +223,27 @@ def dx(self, x, u):
# Update state vector
# -------------------
dxdt = np.zeros((len(x),))
dxdt[0] = vx_a # x-position increment (airspeed along x-direction)
dxdt[1] = vy_a # y-position increment (airspeed along y-direction)
dxdt[2] = vz_a # z-position increment (airspeed along z-direction)
dxdt[3] = p + q * sin_phi * tan_theta + r * cos_phi * tan_theta # Euler's angle phi increment
dxdt[4] = q * cos_phi - r * sin_phi # Euler's angle theta increment
dxdt[5] = q * sin_phi / cos_theta + r * cos_phi / cos_theta # Euler's angle psi increment
dxdt[6] = ((sin_theta * cos_psi * cos_phi + sin_phi * sin_psi) * T - fe_drag[0]) / m # Acceleration along x-axis
dxdt[7] = ((sin_theta * sin_psi * cos_phi - sin_phi * cos_psi) * T - fe_drag[1]) / m # Acceleration along y-axis
dxdt[8] = - self.parameters['gravity'] + (cos_phi * cos_theta * T - fe_drag[2]) / m # Acceleration along z-axis

dxdt[9] = ((Iyy - Izz) * q * r + tp * self.geom['arm_length']) / Ixx # Angular acceleration along body x-axis: roll rate
dxdt[10] = ((Izz - Ixx) * p * r + tq * self.geom['arm_length']) / Iyy # Angular acceleration along body y-axis: pitch rate
dxdt[11] = ((Ixx - Iyy) * p * q + tr * 1 ) / Izz # Angular acceleration along body z-axis: yaw rate
dxdt[12] = 1 # Auxiliary time variable
dxdt[13] = (u['mission_complete'] - x['mission_complete'])/self.parameters['dt'] # Value to keep track of percentage of mission completed

dxdt[0] = vx_a # x-position increment (airspeed along x-direction)
dxdt[1] = vy_a # y-position increment (airspeed along y-direction)
dxdt[2] = vz_a # z-position increment (airspeed along z-direction)

dxdt[3] = p + q * sin_phi * tan_theta + r * cos_phi * tan_theta # Euler's angle phi increment
dxdt[4] = q * cos_phi - r * sin_phi # Euler's angle theta increment
dxdt[5] = q * sin_phi / cos_theta + r * cos_phi / cos_theta # Euler's angle psi increment

dxdt[6] = ((sin_theta * cos_psi * cos_phi + sin_phi * sin_psi) * T - fe_drag[0]) / m # Acceleration along x-axis
dxdt[7] = ((sin_theta * sin_psi * cos_phi - sin_phi * cos_psi) * T - fe_drag[1]) / m # Acceleration along y-axis
dxdt[8] = -self.parameters['gravity'] + (cos_phi * cos_theta * T - fe_drag[2]) / m # Acceleration along z-axis

dxdt[9] = ((Iyy - Izz) * q * r + tp * self.geom['arm_length']) / Ixx # Angular acceleration along body x-axis: roll rate
dxdt[10] = ((Izz - Ixx) * p * r + tq * self.geom['arm_length']) / Iyy # Angular acceleration along body y-axis: pitch rate
dxdt[11] = ((Ixx - Iyy) * p * q + tr * 1) / Izz # Angular acceleration along body z-axis: yaw rate
dxdt[12] = 1 # Auxiliary time variable
dxdt[13] = (u['mission_complete'] - x['mission_complete']) / self.parameters['dt'] # Value to keep track of percentage of mission completed

return self.StateContainer(np.array([np.atleast_1d(item) for item in dxdt]))

def event_state(self, x) -> dict:
# Based on percentage of reference trajectory completed
return {'TrajectoryComplete': x['mission_complete']}
Expand Down Expand Up @@ -273,14 +273,11 @@ def linear_model(self, phi, theta, psi, p, q, r, T):
"""
Linearized model of the small rotorcraft 6-dof.
The function returns the state-transition matrix A and the input matrix B that forms the linearized model as:
dx/dt = A x + B u
where x is the state vector in state-space form (namely x, \dot{x}), u is the input vector containing the thrust and three moments around the vehicle's main body axes.
To generate the linearized matrices, only the attitude angles, angular velocities, and thrust are necessary,
since the model model ignores gyroscopic effect and wind rate of change.
:param phi: rad, scalar, double, first Euler's angle
:param theta: rad, scalar, double, second Euler's angle
:param psi: rad, scalar, double, third Euler's angle
Expand All @@ -290,43 +287,44 @@ def linear_model(self, phi, theta, psi, p, q, r, T):
:param T: N, scalar, double, thrust
:return: Linearized state transition matrix A, n_states x n_states, and linearized input matrix B, n_states x n_inputs
"""
m = self.mass['total']
Ixx = self.mass['Ixx']
Iyy = self.mass['Iyy']
Izz = self.mass['Izz']
l = self.geom['arm_length']
sin_phi = np.sin(phi)
cos_phi = np.cos(phi)
m = self.mass['total']
Ixx = self.mass['Ixx']
Iyy = self.mass['Iyy']
Izz = self.mass['Izz']
length = self.geom['arm_length']
sin_phi = np.sin(phi)
cos_phi = np.cos(phi)
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
tan_theta = np.tan(theta)
sin_psi = np.sin(psi)
cos_psi = np.cos(psi)

A = np.array([[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, q*cos_phi*tan_theta - r*sin_phi*tan_theta, q*(tan_theta**2 + 1)*sin_phi + r*(tan_theta**2 + 1)*cos_phi, 0, 0, 0, 0, 1, sin_phi*tan_theta, cos_phi*tan_theta],
[0, 0, 0, -q*sin_phi - r*cos_phi, 0, 0, 0, 0, 0, 0, cos_phi, -sin_phi],
[0, 0, 0, q*cos_phi/cos_theta - r*sin_phi/cos_theta, q*sin_phi*sin_theta/cos_theta**2 + r*sin_theta*cos_phi/cos_theta**2, 0, 0, 0, 0, 0, sin_phi/cos_theta, cos_phi/cos_theta],
[0, 0, 0, T*(-sin_phi*sin_theta*cos_psi + sin_psi*cos_phi)/m, T*cos_phi*cos_psi*cos_theta/m, T*(sin_phi*cos_psi - sin_psi*sin_theta*cos_phi)/m, 0, 0, 0, 0, 0, 0],
[0, 0, 0, T*(-sin_phi*sin_psi*sin_theta - cos_phi*cos_psi)/m, T*sin_psi*cos_phi*cos_theta/m, T*(sin_phi*sin_psi + sin_theta*cos_phi*cos_psi)/m, 0, 0, 0, 0, 0, 0],
[0, 0, 0, -T*sin_phi*cos_theta/m, -T*sin_theta*cos_phi/m, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, r*(Iyy - Izz)/Ixx, q*(Iyy - Izz)/Ixx],
[0, 0, 0, 0, 0, 0, 0, 0, 0, r*(-Ixx + Izz)/Iyy, 0, p*(-Ixx + Izz)/Iyy],
sin_psi = np.sin(psi)
cos_psi = np.cos(psi)


A = np.array([[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, q*cos_phi*tan_theta - r*sin_phi*tan_theta, q*(tan_theta**2 + 1)*sin_phi + r*(tan_theta**2 + 1)*cos_phi, 0, 0, 0, 0, 1, sin_phi*tan_theta, cos_phi*tan_theta],
[0, 0, 0, -q*sin_phi - r*cos_phi, 0, 0, 0, 0, 0, 0, cos_phi, -sin_phi],
[0, 0, 0, q*cos_phi/cos_theta - r*sin_phi/cos_theta, q*sin_phi*sin_theta/cos_theta**2 + r*sin_theta*cos_phi/cos_theta**2, 0, 0, 0, 0, 0, sin_phi/cos_theta, cos_phi/cos_theta],
[0, 0, 0, T*(-sin_phi*sin_theta*cos_psi + sin_psi*cos_phi)/m, T*cos_phi*cos_psi*cos_theta/m, T*(sin_phi*cos_psi - sin_psi*sin_theta*cos_phi)/m, 0, 0, 0, 0, 0, 0],
[0, 0, 0, T*(-sin_phi*sin_psi*sin_theta - cos_phi*cos_psi)/m, T*sin_psi*cos_phi*cos_theta/m, T*(sin_phi*sin_psi + sin_theta*cos_phi*cos_psi)/m, 0, 0, 0, 0, 0, 0],
[0, 0, 0, -T*sin_phi*cos_theta/m, -T*sin_theta*cos_phi/m, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, r*(Iyy - Izz)/Ixx, q*(Iyy - Izz)/Ixx],
[0, 0, 0, 0, 0, 0, 0, 0, 0, r*(-Ixx + Izz)/Iyy, 0, p*(-Ixx + Izz)/Iyy],
[0, 0, 0, 0, 0, 0, 0, 0, 0, q*(Ixx - Iyy)/Izz, p*(Ixx - Iyy)/Izz, 0]])

B = np.array([ [0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[(sin_phi*sin_psi + sin_theta*cos_phi*cos_psi)/m, 0, 0, 0],
[(-sin_phi*cos_psi + sin_psi*sin_theta*cos_phi)/m, 0, 0, 0],
[cos_phi*cos_theta/m, 0, 0, 0],
[0, l/Ixx, 0, 0],
[0, 0, l/Iyy, 0],
B = np.array([ [0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[(sin_phi*sin_psi + sin_theta*cos_phi*cos_psi)/m, 0, 0, 0],
[(-sin_phi*cos_psi + sin_psi*sin_theta*cos_phi)/m, 0, 0, 0],
[cos_phi*cos_theta/m, 0, 0, 0],
[0, length/Ixx, 0, 0],
[0, 0, length/Iyy, 0],
[0, 0, 0, 1.0/Izz]])

return A, B
Expand Down
37 changes: 18 additions & 19 deletions src/prog_models/models/aircraft_model/vehicles/control/allocation_functions.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,52 +3,51 @@

import numpy as np

def rotorcraft_cam(n, l, b, d, constrained=False):
def rotorcraft_cam(n, length, b, d, constrained=False):
"""
Generate control allocation matrix (CAM) to transform rotor's angular velocity into thrust and torques around three main body axes.
[T, Mx, My, Mz]^{\top} = \Gamma [\Omega_1^2, \Omega_2^2, ..., \Omega_n^2]^{\top}
[T, Mx, My, Mz]^{\top} = \Gamma [\Omega_1^2, \Omega_2^2, ..., \Omega_n^2]^{\top}
Where:
[T, Mx, My, Mz]^{\top} is the (4 x 1) column vector containing thrust (T) and moments along main body axis (Mx, My, Mz)
\Gamma is the (4 x n) CAM
[\Omega_1^2, \Omega_2^2, ..., \Omega_n^2]^{\top} is the (n x 1) column vector of the rotor angular speed squared.
The control allocation matrix is built under the assumption of symmetric rotor configuration, for generality.
Special CAM should be built ad-hoc per UAV model.
:param n: number of rotors
:param l: rotor arm's length (from center of mass to rotor center)
:param length: rotor arm's length (from center of mass to rotor center)
:param b: thrust constant, function of rotor type
:param d: torque constant, function of rotor type
:return: control allocation matrix of dimensions (4, n) and its pseudo-inverse of dimensions (n, 4)
"""

Gamma = np.empty((4, n))
optional = {}
if n == 8 and not constrained:
l_b = l * b
l_b_sq2o2 = l_b * np.sqrt(2.0)/2.0
l_b = length * b
l_b_sq2o2 = l_b * np.sqrt(2.0) / 2.0
# This CAM is assuming there's no rotor pointing towards the drone forward direction (x-axis)
# See Dmitry Luchinsky's report for details (TO BE VERIFIED)
Gamma = np.array([[ b, b, b, b, b, b, b, b],
[ l_b, l_b_sq2o2, 0.0, -l_b_sq2o2, -l_b, -l_b_sq2o2, 0.0, l_b_sq2o2],
[ 0.0, -l_b_sq2o2, -l_b, -l_b_sq2o2, 0.0, l_b_sq2o2, l_b, l_b_sq2o2],
[ -d, d, -d, d, -d, d, -d, d]])
Gamma = np.array([[b, b, b, b, b, b, b, b],
[l_b, l_b_sq2o2, 0.0, -l_b_sq2o2, -l_b, -l_b_sq2o2, 0.0, l_b_sq2o2],
[0.0, -l_b_sq2o2, -l_b, -l_b_sq2o2, 0.0, l_b_sq2o2, l_b, l_b_sq2o2],
[-d, d, -d, d, -d, d, -d, d]])
if n == 8 and constrained:
bl = b * l
bl = b * length
b2 = 2.0 * b
d2 = 2.0 * d
blsqrt2 = np.sqrt(2.0) * bl

Gamma = np.array([[ b2, b2, b2, b2],
[ bl, 0.0, -bl, 0.0],
[-bl, -blsqrt2, bl, blsqrt2],
[-d2, d2, -d2, d2]])
Gamma = np.array([[b2, b2, b2, b2],
[bl, 0.0, -bl, 0.0],
[-bl, -blsqrt2, bl, blsqrt2],
[-d2, d2, -d2, d2]])
optional['selector'] = np.array([[1, 0, 1, 0, 0, 0, 0, 0],
[0, 1, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 1, 0],
[0, 0, 0, 0, 0, 1, 0, 1]]).T

return Gamma, np.linalg.pinv(Gamma), optional

Loading

0 comments on commit 2aac42d

Please sign in to comment.