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

7
rotorpy/vehicles/README.md Normal file
View File

@@ -0,0 +1,7 @@
# Vehicle Module
Vehicle classes are where the continuous-time dynamics are stored. These dynamics are integrated using the `step` method. The IMU requires a `statedot` method which outputs the acceleration and body rate to be transformed by the sensor.
Besides that, integration must also be handled in `step`, which is done via `scipy.integrate.solve_ivp` [(reference)](https://docs.scipy.org/doc/scipy/reference/generated/scipy.integrate.solve_ivp.html#r179348322575-1). This is an RK45 method with adaptive step. It is recommended that other vehicles use this method for integration as well, since it preserves the integration accuracy for larger timesteps compared to forward/backward Euler.
Currently `Multirotor` is the only vehicle supported with automatic plotting and saving of the states and controller outputs.

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

65
rotorpy/vehicles/crazyflie_params.py Normal file
View File

@@ -0,0 +1,65 @@
"""
Physical parameters for a quadrotor. Values parameterize the inertia, motor dynamics,
rotor aerodynamics, parasitic drag, and rotor placement.
Additional sources:
https://bitcraze.io/2015/02/measuring-propeller-rpm-part-3
https://wiki.bitcraze.io/misc:investigations:thrust
https://commons.erau.edu/cgi/viewcontent.cgi?article=2057&context=publication
Notes:
k_thrust is inferred from 14.5 g thrust at 2500 rad/s
k_drag is mostly made up
"""
import numpy as np
d = 0.043
quad_params = {
# Inertial properties
'mass': 0.03, # kg
'Ixx': 1.43e-5, # kg*m^2
'Iyy': 1.43e-5, # kg*m^2
'Izz': 2.89e-5, # kg*m^2
'Ixy': 0.0, # kg*m^2
'Iyz': 0.0, # kg*m^2
'Ixz': 0.0, # kg*m^2
# Geometric properties, all vectors are relative to the center of mass.
'num_rotors': 4, # for looping over each actuator
# 'rotor_pos': {
# 'r1': np.array([0.046, 0, 0]), # Location of Rotor 1, meters
# 'r2': np.array([0, 0.046, 0]), # Location of Rotor 2, meters
# 'r3': np.array([-0.046, 0, 0]), # Location of Rotor 3, meters
# 'r4': np.array([0, -0.046, 0]), # Location of Rotor 4, meters
# },
'rotor_pos': {
'r1': d*np.array([ 0.70710678118, 0.70710678118, 0]), # Location of Rotor 1, meters
'r2': d*np.array([ 0.70710678118,-0.70710678118, 0]), # Location of Rotor 2, meters
'r3': d*np.array([-0.70710678118,-0.70710678118, 0]), # Location of Rotor 3, meters
'r4': d*np.array([-0.70710678118, 0.70710678118, 0]), # Location of Rotor 4, meters
},
'rotor_directions': np.array([1,-1,1,-1]), # This dictates the direction of the torque for each motor.
'rI': np.array([0,0,0]), # location of the IMU sensor, meters
# Frame aerodynamic properties
'c_Dx': 0.5e-2, # parasitic drag in body x axis, N/(m/s)**2
'c_Dy': 0.5e-2, # parasitic drag in body y axis, N/(m/s)**2
'c_Dz': 1e-2, # parasitic drag in body z axis, N/(m/s)**2
# Rotor properties
# See "System Identification of the Crazyflie 2.0 Nano Quadrocopter", Forster 2015.
'k_eta': 2.3e-08, # thrust coefficient N/(rad/s)**2
'k_m': 7.8e-10, # yaw moment coefficient Nm/(rad/s)**2
'k_d': 10.2506e-07, # rotor drag coefficient N/(rad*m/s**2) = kg/rad
'k_z': 7.553e-07, # induced inflow coefficient N/(rad*m/s**2) = kg/rad
'k_flap': 0.0, # Flapping moment coefficient Nm/(rad*m/s**2) = kg*m/rad
# Motor properties
'tau_m': 0.005, # motor response time, seconds
'rotor_speed_min': 0, # rad/s
'rotor_speed_max': 2500, # rad/s
'motor_noise_std': 100, # rad/s
}

58
rotorpy/vehicles/hummingbird_params.py Normal file
View File

@@ -0,0 +1,58 @@
"""
Physical parameters for the AscTec Hummingbird. Values parameterize the inertia, motor dynamics,
rotor aerodynamics, parasitic drag, and rotor placement.
Additional sources:
https://digitalrepository.unm.edu/cgi/viewcontent.cgi?article=1189&context=ece_etds
https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=7991501
Notes:
The k_d and k_z terms are an order of magnitude smaller because 10^-3 was too large.
"""
import numpy as np
d = 0.17
quad_params = {
# Inertial properties
'mass': 0.500, # kg
'Ixx': 3.65e-3, # kg*m^2
'Iyy': 3.68e-3, # kg*m^2
'Izz': 7.03e-3, # kg*m^2
'Ixy': 0.0, # kg*m^2
'Iyz': 0.0, # kg*m^2
'Ixz': 0.0, # kg*m^2
# Geometric properties, all vectors are relative to the center of mass.
'num_rotors': 4, # for looping over each actuator
'rotor_radius': 0.10, # rotor radius, in meters
'rotor_pos': {
'r1': d*np.array([ 0.70710678118, 0.70710678118, 0]), # Location of Rotor 1, meters
'r2': d*np.array([ 0.70710678118,-0.70710678118, 0]), # Location of Rotor 2, meters
'r3': d*np.array([-0.70710678118,-0.70710678118, 0]), # Location of Rotor 3, meters
'r4': d*np.array([-0.70710678118, 0.70710678118, 0]), # Location of Rotor 4, meters
},
'rotor_directions': np.array([1,-1,1,-1]), # This dictates the direction of the torque for each motor.
'rI': np.array([0,0,0]), # location of the IMU sensor, meters
# Frame aerodynamic properties
'c_Dx': 0.5e-2, # parasitic drag in body x axis, N/(m/s)**2
'c_Dy': 0.5e-2, # parasitic drag in body y axis, N/(m/s)**2
'c_Dz': 1e-2, # parasitic drag in body z axis, N/(m/s)**2
# Rotor properties
# See "System Identification of the Crazyflie 2.0 Nano Quadrocopter", Forster 2015.
'k_eta': 5.57e-06, # thrust coefficient N/(rad/s)**2
'k_m': 1.36e-07, # yaw moment coefficient Nm/(rad/s)**2
'k_d': 1.19e-04, # rotor drag coefficient N/(rad*m/s**2) = kg/rad
'k_z': 2.32e-04, # induced inflow coefficient N/(rad*m/s**2) = kg/rad
'k_flap': 0.0, # Flapping moment coefficient Nm/(rad*m/s**2) = kg*m/rad
# Motor properties
'tau_m': 0.005, # motor response time, seconds
'rotor_speed_min': 0, # rad/s
'rotor_speed_max': 1500, # rad/s
'motor_noise_std': 50, # rad/s
}

313
rotorpy/vehicles/multirotor.py Normal file
View File

@@ -0,0 +1,313 @@
import numpy as np
from numpy.linalg import inv, norm
import scipy.integrate
from scipy.spatial.transform import Rotation
from rotorpy.vehicles.hummingbird_params import quad_params
"""
Multirotor models
"""
def quat_dot(quat, omega):
"""
Parameters:
quat, [i,j,k,w]
omega, angular velocity of body in body axes
Returns
duat_dot, [i,j,k,w]
"""
# Adapted from "Quaternions And Dynamics" by Basile Graf.
(q0, q1, q2, q3) = (quat[0], quat[1], quat[2], quat[3])
G = np.array([[ q3, q2, -q1, -q0],
[-q2, q3, q0, -q1],
[ q1, -q0, q3, -q2]])
quat_dot = 0.5 * G.T @ omega
# Augment to maintain unit quaternion.
quat_err = np.sum(quat**2) - 1
quat_err_grad = 2 * quat
quat_dot = quat_dot - quat_err * quat_err_grad
return quat_dot
class Multirotor(object):
"""
Quadrotor forward dynamics model.
"""
def __init__(self, quad_params, initial_state = {'x': np.array([0,0,0]),
'v': np.zeros(3,),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.zeros(3,),
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])},
):
"""
Initialize 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']
self.rotor_dir = quad_params['rotor_directions']
self.extract_geometry()
# 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
self.motor_noise = quad_params['motor_noise_std'] # noise added to the actual motor speed, rad/s / sqrt(Hz)
# Additional constants.
self.inertia = np.array([[self.Ixx, self.Ixy, self.Ixz],
[self.Ixy, self.Iyy, self.Iyz],
[self.Ixz, self.Iyz, self.Izz]])
self.rotor_drag_matrix = np.array([[self.k_d, 0, 0],
[0, self.k_d, 0],
[0, 0, self.k_z]])
self.drag_matrix = np.array([[self.c_Dx, 0, 0],
[0, self.c_Dy, 0],
[0, 0, self.c_Dz]])
self.g = 9.81 # m/s^2
self.inv_inertia = inv(self.inertia)
self.weight = np.array([0, 0, -self.mass*self.g])
# Set the initial state
self.initial_state = initial_state
def extract_geometry(self):
"""
Extracts the geometry in self.rotors for efficient use later on in the computation of
wrenches acting on the rigid body.
The rotor_geometry is an array of length (n,3), where n is the number of rotors.
Each row corresponds to the position vector of the rotor relative to the CoM.
"""
self.rotor_geometry = np.array([]).reshape(0,3)
for rotor in self.rotor_pos:
r = self.rotor_pos[rotor]
self.rotor_geometry = np.vstack([self.rotor_geometry, r])
return
def statedot(self, state, cmd_rotor_speeds, t_step):
"""
Integrate dynamics forward from state given constant cmd_rotor_speeds for time t_step.
"""
# The true motor speeds can not fall below min and max speeds.
cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max)
# Form autonomous ODE for constant inputs and integrate one time step.
def s_dot_fn(t, s):
return self._s_dot_fn(t, s, cmd_rotor_speeds)
s = Multirotor._pack_state(state)
s_dot = s_dot_fn(0, s)
v_dot = s_dot[3:6]
w_dot = s_dot[10:13]
state_dot = {'vdot': v_dot,'wdot': w_dot}
return state_dot
def step(self, state, cmd_rotor_speeds, t_step):
"""
Integrate dynamics forward from state given constant cmd_rotor_speeds for time t_step.
"""
# The true motor speeds can not fall below min and max speeds.
cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max)
# Form autonomous ODE for constant inputs and integrate one time step.
def s_dot_fn(t, s):
return self._s_dot_fn(t, s, cmd_rotor_speeds)
s = Multirotor._pack_state(state)
# Option 1 - RK45 integration
sol = scipy.integrate.solve_ivp(s_dot_fn, (0, t_step), s, first_step=t_step)
s = sol['y'][:,-1]
# Option 2 - Euler integration
# s = s + s_dot_fn(0, s) * t_step # first argument doesn't matter. It's time invariant model
state = Multirotor._unpack_state(s)
# Re-normalize unit quaternion.
state['q'] = state['q'] / norm(state['q'])
# Add noise to the motor speed measurement
state['rotor_speeds'] += np.random.normal(scale=np.abs(self.motor_noise), size=(self.num_rotors,))
return state
def _s_dot_fn(self, t, s, cmd_rotor_speeds):
"""
Compute derivative of state for quadrotor given fixed control inputs as
an autonomous ODE.
"""
state = Multirotor._unpack_state(s)
rotor_speeds = state['rotor_speeds']
inertial_velocity = state['v']
wind_velocity = state['wind']
R = Rotation.from_quat(state['q']).as_matrix()
# Compute airspeed vector in the body frame
body_airspeed_vector = R.T@(inertial_velocity - wind_velocity)
# Rotor speed derivative
rotor_accel = (1/self.tau_m)*(cmd_rotor_speeds - rotor_speeds)
# Position derivative.
x_dot = state['v']
# Orientation derivative.
q_dot = quat_dot(state['q'], state['w'])
# Compute total wrench in the body frame based on the current rotor speeds and their location w.r.t. CoM
(FtotB, Mtot) = self.compute_body_wrench(state['w'], rotor_speeds, body_airspeed_vector)
# Rotate the force from the body frame to the inertial frame
Ftot = R@FtotB
# Velocity derivative.
v_dot = (self.weight + Ftot) / self.mass
# Angular velocity derivative.
w = state['w']
w_hat = Multirotor.hat_map(w)
w_dot = self.inv_inertia @ (Mtot - w_hat @ (self.inertia @ w))
# NOTE: the wind dynamics are currently handled in the wind_profile object.
# The line below doesn't do anything, as the wind state is assigned elsewhere.
wind_dot = np.zeros(3,)
# Pack into vector of derivatives.
s_dot = np.zeros((16+self.num_rotors,))
s_dot[0:3] = x_dot
s_dot[3:6] = v_dot
s_dot[6:10] = q_dot
s_dot[10:13] = w_dot
s_dot[13:16] = wind_dot
s_dot[16:] = rotor_accel
return s_dot
def compute_body_wrench(self, body_rates, rotor_speeds, body_airspeed_vector):
"""
Computes the wrench acting on the rigid body based on the rotor speeds for thrust and airspeed
for aerodynamic forces.
The airspeed is represented in the body frame.
The net force Ftot is represented in the body frame.
The net moment Mtot is represented in the body frame.
"""
FtotB = np.zeros((3,))
MtotB = np.zeros((3,))
for i in range(self.num_rotors):
# Loop through each rotor, compute the forces
r = self.rotor_geometry[i,:] # the position of rotor i relative to the CoM, in body coordinates
# Compute the local airspeed by adding on the rotational component to the airspeed.
local_airspeed_vector = body_airspeed_vector + Multirotor.hat_map(body_rates)@r
T = np.array([0, 0, self.k_eta*rotor_speeds[i]**2]) # thrust vector in body frame
H = -rotor_speeds[i]*self.rotor_drag_matrix@local_airspeed_vector # rotor drag force
# Compute the moments
M_force = Multirotor.hat_map(r)@(T+H)
M_yaw = self.rotor_dir[i]*np.array([0, 0, self.k_m*rotor_speeds[i]**2])
M_flap = -rotor_speeds[i]*self.k_flap*Multirotor.hat_map(local_airspeed_vector)@np.array([0,0,1])
FtotB += (T+H)
MtotB += (M_force + M_yaw + M_flap)
# Compute the drag force acting on the frame
D = -Multirotor._norm(body_airspeed_vector)*self.drag_matrix@body_airspeed_vector
FtotB += D
return (FtotB, MtotB)
@classmethod
def rotate_k(cls, q):
"""
Rotate the unit vector k by quaternion q. This is the third column of
the rotation matrix associated with a rotation by q.
"""
return np.array([ 2*(q[0]*q[2]+q[1]*q[3]),
2*(q[1]*q[2]-q[0]*q[3]),
1-2*(q[0]**2 +q[1]**2) ])
@classmethod
def hat_map(cls, s):
"""
Given vector s in R^3, return associate skew symmetric matrix S in R^3x3
"""
return np.array([[ 0, -s[2], s[1]],
[ s[2], 0, -s[0]],
[-s[1], s[0], 0]])
@classmethod
def _pack_state(cls, state):
"""
Convert a state dict to Quadrotor's private internal vector representation.
"""
s = np.zeros((20,)) # FIXME: this shouldn't be hardcoded. Should vary with the number of rotors.
s[0:3] = state['x'] # inertial position
s[3:6] = state['v'] # inertial velocity
s[6:10] = state['q'] # orientation
s[10:13] = state['w'] # body rates
s[13:16] = state['wind'] # wind vector
s[16:] = state['rotor_speeds'] # rotor speeds
return s
@classmethod
def _norm(cls, v):
"""
Given a vector v in R^3, return the 2 norm (length) of the vector
"""
norm = (v[0]**2 + v[1]**2 + v[2]**2)**0.5
return norm
@classmethod
def _unpack_state(cls, s):
"""
Convert Quadrotor's private internal vector representation to a state dict.
x = inertial position
v = inertial velocity
q = orientation
w = body rates
wind = wind vector
rotor_speeds = rotor speeds
"""
state = {'x':s[0:3], 'v':s[3:6], 'q':s[6:10], 'w':s[10:13], 'wind':s[13:16], 'rotor_speeds':s[16:]}
return state