def calc_command(self, att_des: quat.Quaternion, ang_vel_des,
att: quat.Quaternion):
"""Compute the angular velocity command from current attitude
and desired attitude. Returns 1D numpy array of length 3.
"""
q_des2body = att_des @ att
feedforward = q_des2body.rotp(ang_vel_des)
error = att_des.inverse().box_minus(att)
feedback = self.kp * error
ang_vel_cmd = self.saturate(feedforward + feedback)
return ang_vel_cmd
def calc_command(self, att_des: quat.Quaternion, ang_vel_des,
att: quat.Quaternion):
"""Compute the angular velocity command from current attitude
and desired attitude. Returns 1D numpy array of length 3. att_des and
att are quaternions equivalent to rotations from inertial to body
"""
q_des2body = att_des.inverse(
) @ att # return quaternion rotation from desired to body
feedforward = q_des2body.rotp(
ang_vel_des) #rotate ang_vel_des from des frame to body frame
error = att_des.box_minus(
att
) #returns Axis-Angle representation of how much you should rotate about each of the body frame axes
feedback = self.kp * error
ang_vel_cmd = self.saturate(feedforward + feedback)
return ang_vel_cmd