79 lines
3.6 KiB
Python
79 lines
3.6 KiB
Python
"""
|
|
Imports
|
|
"""
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation # This is a useful library for working with attitude.
|
|
|
|
class MultirotorControlTemplate(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 depends on the control abstraction for Multirotor...
|
|
if cmd_motor_speeds: the output dict should contain the key 'cmd_motor_speeds'
|
|
if cmd_motor_thrusts: the output dict should contain the key 'cmd_rotor_thrusts'
|
|
if cmd_vel: the output dict should contain the key 'cmd_v'
|
|
if cmd_ctatt: the output dict should contain the keys 'cmd_thrust' and 'cmd_q'
|
|
if cmd_ctbr: the output dict should contain the keys 'cmd_thrust' and 'cmd_w'
|
|
if cmd_ctbm: the output dict should contain the keys 'cmd_thrust' and 'cmd_moment'
|
|
"""
|
|
def __init__(self, vehicle_params):
|
|
"""
|
|
Use this constructor to save vehicle parameters, set controller gains, etc.
|
|
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
|
|
|
|
key, description, unit, (applicable control abstraction)
|
|
cmd_motor_speeds, the commanded speed for each motor, rad/s, (cmd_motor_speeds)
|
|
cmd_thrust, the collective thrust of all rotors, N, (cmd_ctatt, cmd_ctbr, cmd_ctbm)
|
|
cmd_moment, the control moments on each boxy axis, N*m, (cmd_ctbm)
|
|
cmd_q, desired attitude as a quaternion [i,j,k,w], , (cmd_ctatt)
|
|
cmd_w, desired angular rates in body frame, rad/s, (cmd_ctbr)
|
|
cmd_v, desired velocity vector in world frame, m/s (cmd_vel)
|
|
"""
|
|
|
|
# Only some of these are necessary depending on your desired control abstraction.
|
|
cmd_motor_speeds = np.zeros((4,))
|
|
cmd_motor_thrusts = np.zeros((4,))
|
|
cmd_thrust = 0
|
|
cmd_moment = np.zeros((3,))
|
|
cmd_q = np.array([0,0,0,1])
|
|
cmd_w = np.zeros((3,))
|
|
cmd_v = np.zeros((3,))
|
|
|
|
control_input = {'cmd_motor_speeds':cmd_motor_speeds,
|
|
'cmd_motor_thrusts':cmd_motor_thrusts,
|
|
'cmd_thrust':cmd_thrust,
|
|
'cmd_moment':cmd_moment,
|
|
'cmd_q':cmd_q,
|
|
'cmd_w':cmd_w,
|
|
'cmd_v':cmd_v}
|
|
return control_input
|