swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init

Browse Source
This commit is contained in:
spencerfolk
2023-03-15 15:38:14 -04:00
commit 4d7fca10e4
62 changed files with 5891 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
rotorpy/controllers/README.md Normal file
View File

@@ -0,0 +1,5 @@
# Controller Module
The simulator is packaged with a geometric tracking controller for a quadrotor, found in `quadrotor_control.py`. Based on [this paper](https://mathweb.ucsd.edu/~mleok/pdf/LeLeMc2010_quadrotor.pdf) the controller takes flat outputs (position and yaw) and outputs a dictionary containing different control abstractions (angle, rate, motor speeds). The `Multirotor` vehicle will use the commanded motor speeds in the dynamics.
Other controllers can be developed but must complement the vehicle and the trajectory it is trying to stabilize to.

0
rotorpy/controllers/__init__.py Normal file
View File

61
rotorpy/controllers/controller_template.py Normal file
View File

@@ -0,0 +1,61 @@
"""
Imports
"""
import numpy as np
from scipy.spatial.transform import Rotation # This is a useful library for working with attitude.
class ControlTemplate(object):
"""
The controller is implemented as a class with two required methods: __init__() and update().
The __init__() is used to instantiate the controller, and this is where any model parameters or
controller gains should be set.
In update(), the current time, state, and desired flat outputs are passed into the controller at
each simulation step. The output of the controller should be the commanded motor speeds,
commanded thrust, commanded moment, and commanded attitude (in quaternion [x,y,z,w] format).
"""
def __init__(self, vehicle_params):
"""
Parameters:
vehicle_params, dict with keys specified in a python file under /rotorpy/vehicles/
"""
def update(self, t, state, flat_output):
"""
This function receives the current time, true state, and desired flat
outputs. It returns the command inputs.
Inputs:
t, present time in seconds
state, a dict describing the present state with keys
x, position, m
v, linear velocity, m/s
q, quaternion [i,j,k,w]
w, angular velocity, rad/s
flat_output, a dict describing the present desired flat outputs with keys
x, position, m
x_dot, velocity, m/s
x_ddot, acceleration, m/s**2
x_dddot, jerk, m/s**3
x_ddddot, snap, m/s**4
yaw, yaw angle, rad
yaw_dot, yaw rate, rad/s
Outputs:
control_input, a dict describing the present computed control inputs with keys
cmd_motor_speeds, rad/s
cmd_thrust, N (for debugging and laboratory; not used by simulator)
cmd_moment, N*m (for debugging; not used by simulator)
cmd_q, quaternion [i,j,k,w] (for laboratory; not used by simulator)
"""
cmd_motor_speeds = np.zeros((4,))
cmd_thrust = 0
cmd_moment = np.zeros((3,))
cmd_q = np.array([0,0,0,1])
control_input = {'cmd_motor_speeds':cmd_motor_speeds,
'cmd_thrust':cmd_thrust,
'cmd_moment':cmd_moment,
'cmd_q':cmd_q}
return control_input

153
rotorpy/controllers/quadrotor_control.py Normal file
View File

@@ -0,0 +1,153 @@
import numpy as np
from scipy.spatial.transform import Rotation
class SE3Control(object):
"""
"""
def __init__(self, quad_params):
"""
Parameters:
quad_params, dict with keys specified in rotorpy/vehicles
"""
# Quadrotor physical parameters.
# Inertial parameters
self.mass = quad_params['mass'] # kg
self.Ixx = quad_params['Ixx'] # kg*m^2
self.Iyy = quad_params['Iyy'] # kg*m^2
self.Izz = quad_params['Izz'] # kg*m^2
self.Ixy = quad_params['Ixy'] # kg*m^2
self.Ixz = quad_params['Ixz'] # kg*m^2
self.Iyz = quad_params['Iyz'] # kg*m^2
# Frame parameters
self.c_Dx = quad_params['c_Dx'] # drag coeff, N/(m/s)**2
self.c_Dy = quad_params['c_Dy'] # drag coeff, N/(m/s)**2
self.c_Dz = quad_params['c_Dz'] # drag coeff, N/(m/s)**2
self.num_rotors = quad_params['num_rotors']
self.rotor_pos = quad_params['rotor_pos']
# Rotor parameters
self.rotor_speed_min = quad_params['rotor_speed_min'] # rad/s
self.rotor_speed_max = quad_params['rotor_speed_max'] # rad/s
self.k_eta = quad_params['k_eta'] # thrust coeff, N/(rad/s)**2
self.k_m = quad_params['k_m'] # yaw moment coeff, Nm/(rad/s)**2
self.k_d = quad_params['k_d'] # rotor drag coeff, N/(m/s)
self.k_z = quad_params['k_z'] # induced inflow coeff N/(m/s)
self.k_flap = quad_params['k_flap'] # Flapping moment coefficient Nm/(m/s)
# Motor parameters
self.tau_m = quad_params['tau_m'] # motor reponse time, seconds
# You may define any additional constants you like including control gains.
self.inertia = np.array([[self.Ixx, self.Ixy, self.Ixz],
[self.Ixy, self.Iyy, self.Iyz],
[self.Ixz, self.Iyz, self.Izz]]) # kg*m^2
self.g = 9.81 # m/s^2
# Gains
self.kp_pos = np.array([6.5,6.5,15])
self.kd_pos = np.array([4.0, 4.0, 9])
self.kp_att = 544
self.kd_att = 46.64
# Linear map from individual rotor forces to scalar thrust and vector
# moment applied to the vehicle.
k = self.k_m/self.k_eta
# Below is an automated generation of the control allocator matrix. It assumes that all thrust vectors are aligned
# with the z axis and that the "sign" of each rotor yaw moment alternates starting with positive for r1.
self.f_to_TM = np.vstack((np.ones((1,self.num_rotors)),np.hstack([np.cross(self.rotor_pos[key],np.array([0,0,1])).reshape(-1,1)[0:2] for key in self.rotor_pos]), np.array([k*(-1)**i for i in range(self.num_rotors)]).reshape(1,-1)))
self.TM_to_f = np.linalg.inv(self.f_to_TM)
def update(self, t, state, flat_output):
"""
This function receives the current time, true state, and desired flat
outputs. It returns the command inputs.
Inputs:
t, present time in seconds
state, a dict describing the present state with keys
x, position, m
v, linear velocity, m/s
q, quaternion [i,j,k,w]
w, angular velocity, rad/s
flat_output, a dict describing the present desired flat outputs with keys
x, position, m
x_dot, velocity, m/s
x_ddot, acceleration, m/s**2
x_dddot, jerk, m/s**3
x_ddddot, snap, m/s**4
yaw, yaw angle, rad
yaw_dot, yaw rate, rad/s
Outputs:
control_input, a dict describing the present computed control inputs with keys
cmd_motor_speeds, rad/s
cmd_thrust, N
cmd_moment, N*m
cmd_q, quaternion [i,j,k,w]
"""
cmd_motor_speeds = np.zeros((4,))
cmd_thrust = 0
cmd_moment = np.zeros((3,))
cmd_q = np.zeros((4,))
def normalize(x):
"""Return normalized vector."""
return x / np.linalg.norm(x)
def vee_map(S):
"""Return vector corresponding to given skew symmetric matrix."""
return np.array([-S[1,2], S[0,2], -S[0,1]])
# Desired force vector.
pos_err = state['x'] - flat_output['x']
dpos_err = state['v'] - flat_output['x_dot']
F_des = self.mass * (- self.kp_pos*pos_err
- self.kd_pos*dpos_err
+ flat_output['x_ddot']
+ np.array([0, 0, self.g]))
# Desired thrust is force projects onto b3 axis.
R = Rotation.from_quat(state['q']).as_matrix()
b3 = R @ np.array([0, 0, 1])
u1 = np.dot(F_des, b3)
# Desired orientation to obtain force vector.
b3_des = normalize(F_des)
yaw_des = flat_output['yaw']
c1_des = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
b2_des = normalize(np.cross(b3_des, c1_des))
b1_des = np.cross(b2_des, b3_des)
R_des = np.stack([b1_des, b2_des, b3_des]).T
# Orientation error.
S_err = 0.5 * (R_des.T @ R - R.T @ R_des)
att_err = vee_map(S_err)
# Angular velocity error (this is oversimplified).
w_des = np.array([0, 0, flat_output['yaw_dot']])
w_err = state['w'] - w_des
# Angular control; vector units of N*m.
u2 = self.inertia @ (-self.kp_att*att_err - self.kd_att*w_err)
# Compute motor speeds. Avoid taking square root of negative numbers.
TM = np.array([u1, u2[0], u2[1], u2[2]])
cmd_motor_forces = self.TM_to_f @ TM
cmd_motor_speeds = cmd_motor_forces / self.k_eta
cmd_motor_speeds = np.sign(cmd_motor_speeds) * np.sqrt(np.abs(cmd_motor_speeds))
cmd_thrust = u1
cmd_moment = u2
cmd_q = Rotation.from_matrix(R_des).as_quat()
control_input = {'cmd_motor_speeds':cmd_motor_speeds,
'cmd_thrust':cmd_thrust,
'cmd_moment':cmd_moment,
'cmd_q':cmd_q}
return control_input