From 93113cd964d1f792922c52672c02746ff0864f1e Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Tue, 12 Dec 2023 15:27:25 -0500 Subject: [PATCH] Cleaning up documentation --- rotorpy/controllers/quadrotor_control.py | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/rotorpy/controllers/quadrotor_control.py b/rotorpy/controllers/quadrotor_control.py index bc68875..ddcbf4f 100644 --- a/rotorpy/controllers/quadrotor_control.py +++ b/rotorpy/controllers/quadrotor_control.py @@ -98,18 +98,12 @@ class SE3Control(object): """Return normalized vector.""" return x / np.linalg.norm(x) - # def vee_map(S): - # """Return vector corresponding to given skew symmetric matrix.""" - # return np.array([-S[1,2], S[0,2], -S[0,1]]) - # Desired force vector. t = flat_output['x_ddot']+ np.array([0, 0, self.g]) b3 = normalize(t) F_des = self.mass * (t)# this is vectorized - # Desired thrust is force projects onto b3 axis. - # R = Rotation.from_quat(state['q']).as_matrix() #this is where most of the problem is, there is no error in rotation! - # b3 = R @ np.array([0, 0, 1]) + # Control input 1: collective thrust. u1 = np.dot(F_des, b3) # Desired orientation to obtain force vector. @@ -120,10 +114,7 @@ class SE3Control(object): b1_des = np.cross(b2_des, b3_des) R_des = np.stack([b1_des, b2_des, b3_des]).T - R = R_des# assume we have perfect tracking on rotation - # Orientation error. - # S_err = 0.5 * (R_des.T @ R - R.T @ R_des) - # att_err = vee_map(S_err) + R = R_des # assume we have perfect tracking on rotation # Following section follows Mellinger paper to compute reference angular velocity dot_u1 = np.dot(b3,flat_output['x_dddot']) @@ -143,10 +134,11 @@ class SE3Control(object): r_dot = flat_output['yaw_ddot'] *np.dot(np.array([0,0,1.0]), b3_des) #uniquely need yaw_ddot Alpha = np.array([p_dot, q_dot, r_dot]) - - + # Control input 2: moment on each body axis + u2 = self.inertia @ Alpha + np.cross(Omega, self.inertia @ Omega) - # print(u1,u2) + + # Convert to cmd motor speeds. 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