From e3a6179a7c4d3deff5543727c0908b89326c532a Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Thu, 14 Dec 2023 16:17:32 -0500 Subject: [PATCH] Added additional control abstractions to output. --- rotorpy/controllers/quadrotor_control.py | 36 ++++++++++++++++-------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/rotorpy/controllers/quadrotor_control.py b/rotorpy/controllers/quadrotor_control.py index ddcbf4f..32ff79d 100644 --- a/rotorpy/controllers/quadrotor_control.py +++ b/rotorpy/controllers/quadrotor_control.py @@ -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 \ No newline at end of file