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

Added additional control abstractions to output.

Browse Source
This commit is contained in:
spencerfolk
2023-12-14 16:17:32 -05:00
parent 9797c2bad7
commit e3a6179a7c
1 changed files with 25 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
rotorpy/controllers/quadrotor_control.py
View File

@@ -54,14 +54,17 @@ class SE3Control(object):
self.kd_pos = np.array([4.0, 4.0, 9])
self.kp_att = 544
self.kd_att = 46.64
self.kp_vel = 0.1*self.kp_pos # P gain for velocity controller (only used when the control abstraction is cmd_vel)
# Linear map from individual rotor forces to scalar thrust and vector
# moment applied to the vehicle.
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]), (k * self.rotor_dir).reshape(1,-1)))
# with the z axis and that the "sign" of each rotor yaw moment alternates starting with positive for r1. 'TM' = "thrust and moments"
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):
@@ -179,9 +182,12 @@ class SE3Control(object):
Outputs:
control_input, a dict describing the present computed control inputs with keys
cmd_motor_speeds, rad/s
cmd_motor_thrusts, N
cmd_thrust, N
cmd_moment, N*m
cmd_q, quaternion [i,j,k,w]
cmd_w, angular rates in the body frame, rad/s
cmd_v, velocity in the world frame, m/s
"""
cmd_motor_speeds = np.zeros((4,))
cmd_thrust = 0
@@ -196,7 +202,7 @@ class SE3Control(object):
"""Return vector corresponding to given skew symmetric matrix."""
return np.array([-S[1,2], S[0,2], -S[0,1]])
# Desired force vector.
# Get the 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
@@ -225,21 +231,29 @@ class SE3Control(object):
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)
# Desired torque, in units N-m.
u2 = self.inertia @ (-self.kp_att*att_err - self.kd_att*w_err) + np.cross(state['w'], self.inertia@state['w']) # Includes compensation for wxJw component
# Compute command body rates by doing PD on the attitude error.
cmd_w = -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_rotor_thrusts = self.TM_to_f @ TM
cmd_motor_speeds = cmd_rotor_thrusts / 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()
# Assign controller commands.
cmd_thrust = u1 # Commanded thrust, in units N.
cmd_moment = u2 # Commanded moment, in units N-m.
cmd_q = Rotation.from_matrix(R_des).as_quat() # Commanded attitude as a quaternion.
cmd_v = -self.kp_vel*pos_err + flat_output['x_dot'] # Commanded velocity in world frame (if using cmd_vel control abstraction), in units m/s
control_input = {'cmd_motor_speeds':cmd_motor_speeds,
'cmd_motor_thrusts':cmd_rotor_thrusts,
'cmd_thrust':cmd_thrust,
'cmd_moment':cmd_moment,
'cmd_q':cmd_q}
'cmd_q':cmd_q,
'cmd_w':cmd_w,
'cmd_v':cmd_v}
return control_input