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

Use rotor_directions from param file for control allocation

Browse Source
This commit is contained in:
spencerfolk
2023-12-12 14:04:42 -05:00
parent b215519cca
commit 29ff3b9ddf
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
rotorpy/controllers/quadrotor_control.py
View File

@@ -28,6 +28,7 @@ class SE3Control(object):
self.num_rotors = quad_params['num_rotors']
self.rotor_pos = quad_params['rotor_pos']
self.rotor_dir = quad_params['rotor_directions']
# Rotor parameters
self.rotor_speed_min = quad_params['rotor_speed_min'] # rad/s
@@ -56,11 +57,11 @@ class SE3Control(object):
# Linear map from individual rotor forces to scalar thrust and vector
# moment applied to the vehicle.
k = self.k_m/self.k_eta
k = self.k_m/self.k_eta # Ratio of torque to thrust coefficient.
# 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.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]), (k * self.rotor_dir).reshape(1,-1)))
self.TM_to_f = np.linalg.inv(self.f_to_TM)
def update_ref(self, t, flat_output):