def calc_att_cmd(self, thrust_vec, yaw_des):
thrust_mag = mt.norm2(thrust_vec)
s_yaw = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
R = np.empty([3,3])
R[:,2] = thrust_vec / thrust_mag
R[:,1] = mt.skew(R[:,2]) @ s_yaw
R[:,1] /= mt.norm2(R[:,1])
R[:,0] = mt.skew(R[:,1]) @ R[:,2]
return quat.from_R(R)
def Exp(vec3):
"""Map from R3 -> Lie Algebra -> Lie Group (quaternion)
Exp = exp(hat(vec3))
"""
assert vec3.size == 3
angle = mt.norm2(vec3)
if angle > 1e-6:
angle_half = angle / 2
axis = vec3 / angle
q_arr = np.array([np.cos(angle_half), *(np.sin(angle_half) * axis)])
else:
q_arr = np.array([1., *(vec3 / 2)])
q_arr /= mt.norm2(q_arr)
return Quaternion(q_arr)
def calc_att_cmd(self, Tc_vec, Tc, yaw_des):
s_yaw = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
R = np.empty([3, 3])
R[:, 2] = Tc_vec / Tc
R[:, 1] = mt.skew(R[:, 2]) @ s_yaw
R[:, 1] /= mt.norm2(R[:, 1])
R[:, 0] = mt.skew(R[:, 1]) @ R[:, 2]
return quat.from_R(R)
def Log(quat):
"""Map from Lie Group -> Lie Algebra -> R3
Log = vee(log(quat))
"""
assert isinstance(quat, Quaternion)
qbar_norm = mt.norm2(quat.qbar)
if qbar_norm < 1e-8:
vec3 = np.zeros(3)
else:
vec3 = 2 * np.arctan2(qbar_norm, quat.q0) * quat.qbar / qbar_norm
return vec3
def calc_command(self, vel, vel_des, R_bi, yaw, dt):
"""Calculate the throttle and attitude commands from desired
position and heading. Returns both scalar throttle command as
well as a unit quaternion attitude command.
Positions, velocities, and accel_des are 1D numpy arrays of
length 3. Headings and dt are scalars.
"""
vel_err = vel_des - vel
# yaw_des = -np.pi/2.0
yaw_des = 0.0
thrust_vec = self.g * self.e3 - self.Kd @ (vel_err)
thrust_cmd = mt.norm2(thrust_vec)
throttle_cmd = self.calc_throttle_cmd(thrust_cmd)
att_cmd = self.calc_att_cmd(thrust_vec, thrust_cmd, yaw_des)
ang_vel_des = self.calc_ang_vel_des(att_cmd, dt)
return throttle_cmd, att_cmd, ang_vel_des
def random():
"""Create a random quaternion"""
rand_arr = np.random.uniform(low=-1, high=1, size=4)
rand_arr /= mt.norm2(rand_arr)
return Quaternion(rand_arr)
def normalize(self):
"""Makes current quaternion unit length"""
self.arr /= mt.norm2(self.arr)