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

Cleaning up documentation

Browse Source
This commit is contained in:
spencerfolk
2023-12-12 15:27:25 -05:00
parent f116d25965
commit 93113cd964
1 changed files with 6 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
rotorpy/controllers/quadrotor_control.py
View File

@@ -98,18 +98,12 @@ class SE3Control(object):
"""Return normalized vector.""" """Return normalized vector."""
return x / np.linalg.norm(x) 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. # Desired force vector.
t = flat_output['x_ddot']+ np.array([0, 0, self.g]) t = flat_output['x_ddot']+ np.array([0, 0, self.g])
b3 = normalize(t) b3 = normalize(t)
F_des = self.mass * (t)# this is vectorized F_des = self.mass * (t)# this is vectorized
# Desired thrust is force projects onto b3 axis. # Control input 1: collective thrust.
# 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])
u1 = np.dot(F_des, b3) u1 = np.dot(F_des, b3)
# Desired orientation to obtain force vector. # Desired orientation to obtain force vector.
@@ -121,9 +115,6 @@ class SE3Control(object):
R_des = np.stack([b1_des, b2_des, b3_des]).T R_des = np.stack([b1_des, b2_des, b3_des]).T
R = R_des # assume we have perfect tracking on rotation 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)
# Following section follows Mellinger paper to compute reference angular velocity # Following section follows Mellinger paper to compute reference angular velocity
dot_u1 = np.dot(b3,flat_output['x_dddot']) 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 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]) 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) 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]]) TM = np.array([u1, u2[0], u2[1], u2[2]])
cmd_motor_forces = self.TM_to_f @ TM cmd_motor_forces = self.TM_to_f @ TM
cmd_motor_speeds = cmd_motor_forces / self.k_eta cmd_motor_speeds = cmd_motor_forces / self.k_eta