def trans_and_rot_to_tmat(trans, rot: Rotation):
rot_mat = rot.as_matrix()
tmat = [[rot_mat[0][0], rot_mat[0][1], rot_mat[0][2], trans[0]],
[rot_mat[1][0], rot_mat[1][1], rot_mat[1][2], trans[1]],
[rot_mat[2][0], rot_mat[2][1], rot_mat[2][2], trans[2]],
[0, 0, 0, 1]]
return np.array(tmat)
def _compute_normalizing_rotation(center: np.ndarray,
head_rot: Rotation) -> Rotation:
z_axis = _normalize_vector(center.ravel())
head_rot = head_rot.as_matrix()
head_x_axis = head_rot[:, 0]
y_axis = _normalize_vector(np.cross(z_axis, head_x_axis))
x_axis = _normalize_vector(np.cross(y_axis, z_axis))
return Rotation.from_matrix(np.vstack([x_axis, y_axis, z_axis]))
def to_tree(cls, node: Rotation, ctx):
"""
Convert an instance of the 'Dimension' type into YAML representations.
Parameters
----------
node :
Instance of the 'Dimension' type to be serialized.
ctx :
An instance of the 'AsdfFile' object that is being written out.
Returns
-------
A basic YAML type ('dict', 'list', 'str', 'int', 'float', or
'complex') representing the properties of the 'Dimension' type to be
serialized.
"""
tree = {}
if not hasattr(node,
"wx_meta"): # default to quaternion representation
tree["quaternions"] = node.as_quat()
elif node.wx_meta["constructor"] == "from_quat":
tree["quaternions"] = node.as_quat()
elif node.wx_meta["constructor"] == "from_matrix":
tree["matrix"] = node.as_matrix()
elif node.wx_meta["constructor"] == "from_rotvec":
tree["rotvec"] = node.as_rotvec()
elif node.wx_meta["constructor"] == "from_euler":
seq_str = node.wx_meta["seq"]
if not len(seq_str) == 3:
if all([c in "xyz" for c in seq_str]):
seq_str = seq_str + "".join(
[c for c in "xyz" if c not in seq_str])
elif all([c in "XYZ" for c in seq_str]):
seq_str = seq_str + "".join(
[c for c in "XYZ" if c not in seq_str])
else: # pragma: no cover
raise ValueError(
"Mix of intrinsic and extrinsic euler angles.")
angles = node.as_euler(seq_str, degrees=node.wx_meta["degrees"])
angles = np.squeeze(angles[..., :len(node.wx_meta["seq"])])
if node.wx_meta["degrees"]:
angles = Q_(angles, "degree")
else:
angles = Q_(angles, "rad")
tree["sequence"] = node.wx_meta["seq"]
tree["angles"] = angles
else: # pragma: no cover
raise NotImplementedError("unknown or missing constructor")
return tree
def _compute_normalizing_rotation(center: np.ndarray,
head_rot: Rotation) -> Rotation:
# See section 4.2 and Figure 9 of https://arxiv.org/abs/1711.09017
z_axis = _normalize_vector(center.ravel())
head_rot = head_rot.as_matrix()
head_x_axis = head_rot[:, 0]
y_axis = _normalize_vector(np.cross(z_axis, head_x_axis))
x_axis = _normalize_vector(np.cross(y_axis, z_axis))
return Rotation.from_matrix(np.vstack([x_axis, y_axis, z_axis]))
def from_rotation_translation(cls, rotation: Rotation,
translation: ndarray):
if not isinstance(rotation, Rotation):
raise ValueError
if not isinstance(translation, ndarray):
raise ValueError
if translation.shape != (3, ):
raise ValueError
matrix = np.identity(4)
matrix[:3, :3] = rotation.as_matrix()
matrix[:3, 3] = translation
return cls(matrix)
def solve_w_t(uvd1, uvd2, R0):
"""
solve_w_t core routine used to compute best fit w and t given a set of stereo correspondences
:param uvd1: 3xn ndarray : normailzed stereo results from frame 1
:param uvd2: 3xn ndarray : normailzed stereo results from frame 1
:param R0: Rotation type - base rotation estimate Rotation object
:return: w, t : 3x1 ndarray estimate for rotation vector, 3x1 ndarray estimate for translation
"""
# TODO Your code here replace the dummy return value with a value you compute
n = uvd1.shape[1] # number of eqns always 3
print('n = ', str(n))
# print('current n number:', str(n))
for i in range(n):
# Rot0 = R0.as_matrix()
Rot0 = Rotation.as_matrix(R0)
temp_uvd1 = uvd1[:, i]
temp_uvd2 = uvd2[:, i]
temp_y = np.dot(Rot0, [[temp_uvd2[0]], [temp_uvd2[1]], [1]])
[y1, y2, y3] = temp_y
temp_b = -np.dot([[1, 0, -temp_uvd1[0]], [0, 1, -temp_uvd1[1]]],
temp_y) #.astype(float)
temp_mat1 = [[1, 0, -temp_uvd1[0]], [0, 1, -temp_uvd1[1]]]
temp_mat2 = [[0, y3, -y2, temp_uvd2[2], 0, 0],
[-y3, 0, y1, 0, temp_uvd2[2], 0],
[y2, -y1, 0, 0, 0, temp_uvd2[2]]]
temp_A = np.dot(temp_mat1, temp_mat2)
if i == 0:
A = temp_A
b = temp_b
else:
A = np.vstack((A, temp_A))
b = np.vstack((b, temp_b))
# print(A)
x = np.linalg.lstsq(A, b)[0] # only get the solution 6*1
# x = np.dot(np.linalg.inv(A), b)
w = np.asarray(x[0:3]).reshape((3, 1))
t = np.asarray(x[3:6]).reshape((3, 1))
print('THIS IS THE LSQ ESTIMATE', str(w))
return w, t
def to_yaml_tree(self, obj: Rotation, tag: str, ctx) -> dict:
"""Convert to python dict."""
tree = {}
if not hasattr(obj, ROT_META): # default to quaternion representation
tree["quaternions"] = obj.as_quat()
elif getattr(obj, ROT_META)["constructor"] == "from_quat":
tree["quaternions"] = obj.as_quat()
elif getattr(obj, ROT_META)["constructor"] == "from_matrix":
tree["matrix"] = obj.as_matrix()
elif getattr(obj, ROT_META)["constructor"] == "from_rotvec":
tree["rotvec"] = obj.as_rotvec()
elif getattr(obj, ROT_META)["constructor"] == "from_euler":
seq_str = getattr(obj, ROT_META)["seq"]
if not len(seq_str) == 3:
if all(c in "xyz" for c in seq_str):
seq_str = seq_str + "".join(
[c for c in "xyz" if c not in seq_str])
elif all(c in "XYZ" for c in seq_str):
seq_str = seq_str + "".join(
[c for c in "XYZ" if c not in seq_str])
else: # pragma: no cover
raise ValueError(
"Mix of intrinsic and extrinsic euler angles.")
angles = obj.as_euler(seq_str,
degrees=getattr(obj, ROT_META)["degrees"])
angles = np.squeeze(
angles[..., :len(getattr(obj, ROT_META)["seq"])])
if getattr(obj, ROT_META)["degrees"]:
angles = Q_(angles, "degree")
else:
angles = Q_(angles, "rad")
tree["sequence"] = getattr(obj, ROT_META)["seq"]
tree["angles"] = angles
else: # pragma: no cover
raise NotImplementedError("unknown or missing constructor")
return tree
def find_inliers(w, t, uvd1, uvd2, R0, threshold):
"""
find_inliers core routine used to detect which correspondences are inliers
:param w: ndarray with 3 entries angular velocity vector in radians/sec
:param t: ndarray with 3 entries, translation vector
:param uvd1: 3xn ndarray : normailzed stereo results from frame 1
:param uvd2: 3xn ndarray : normailzed stereo results from frame 2
:param R0: Rotation type - base rotation estimate
:param threshold: Threshold to use
:return: ndarray with n boolean entries : Only True for correspondences that pass the test
"""
n = uvd1.shape[1]
inliers = np.zeros(n, dtype='bool')
# TODO Your code here replace the dummy return value with a value you compute
Rot0 = Rotation.as_matrix(R0)
for i in range(n):
temp_uvd1 = uvd1[:, i]
temp_uvd2 = uvd2[:, i]
mat1 = [[1, 0, -temp_uvd1[0]], [0, 1, -temp_uvd1[1]]]
[w1, w2, w3] = w
# w_skew = [[0, -w3, w2], [w3, 0, -w1], [-w2, w1, 0]]
# inter_mat = np.dot(np.eye(3)+w_skew, Rot0)
w_skew_I = [[1, -w3, w2], [w3, 1, -w1], [-w2, w1, 1]]
inter_mat = np.dot(w_skew_I, Rot0)
mat2 = np.dot(inter_mat, [temp_uvd2[0], temp_uvd2[1], 1])
mat3 = np.dot(temp_uvd2[2], t)
sigma = np.dot(mat1, (mat2 + mat3))
print(sigma)
sigma_norm = np.linalg.norm(sigma)
if sigma_norm < threshold:
inliers[i] = 1
else:
inliers[i] = 0
return inliers
def rotation(self, value: Rotation):
if not isinstance(value, Rotation):
raise ValueError
self.matrix[:3, :3] = value.as_matrix()
def euler2RotMat(z, y, x):
# takes as input 3 rotation angles in degrees
# in the order z rot, y rot and x rot
# return a 3x3 rotation matrix
r = Rotation.from_euler('zyx', [z, y, x], degrees=True)
return Rotation.as_matrix(r)
def csv_to_txt(sensor, trial_dir_path, csvfilename, device, t_calib, freq,
delay, t_range, t0):
#------------------------------------------------------------------------------
#---import csv data------------------------------------------------------------
with open(trial_dir_path + csvfilename) as csvfile:
data = csv.reader(csvfile)
#-------sensor Xsens-----------------------------------------------------------
if sensor == 'Xsens':
PacketCounter, SampleTimeFine, OriInc_w, OriInc_x, OriInc_y, OriInc_z, VelInc_x, VelInc_y, VelInc_z, Mag_X, Mag_Y, Mag_Z, Quat_w, Quat_x, Quat_y, Quat_z, FreeAcc_X, FreeAcc_Y, FreeAcc_Z, Statusword = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
#-----------Android Device-----------------------------------------------------
if device == 'android':
print('not validated yet')
count = 0
for row in data:
if count > 6:
PacketCounter.append(row[0])
SampleTimeFine.append(row[1])
OriInc_w.append(row[2])
OriInc_x.append(row[3])
OriInc_y.append(row[4])
OriInc_z.append(row[5])
VelInc_x.append(row[6])
VelInc_y.append(row[7])
VelInc_z.append(row[8])
Mag_X.append(row[9])
Mag_Y.append(row[10])
Mag_Z.append(row[11])
Quat_w.append(row[12])
Quat_x.append(row[13])
Quat_y.append(row[14])
Quat_z.append(row[15])
FreeAcc_X.append(row[16])
FreeAcc_Y.append(row[17])
FreeAcc_Z.append(row[18])
Statusword.append(row[19])
count += 1
#-----------Apple Device-------------------------------------------------------
if device == 'apple':
count = 0
for row in data:
if count > 8:
PacketCounter.append(row[0])
SampleTimeFine.append(row[1])
Quat_w.append(row[2])
Quat_x.append(row[3])
Quat_y.append(row[4])
Quat_z.append(row[5])
count += 1
#-------sensor Vicon-----------------------------------------------------------
if sensor == 'Vicon':
time_s, type_, Acc_x, Acc_y, Acc_z, Angvel_x, Angvel_y, Angvel_z, Mag_x, Mag_y, Mag_z, Quat_x, Quat_y, Quat_z, Quat_w = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
#-----------Android Device-----------------------------------------------------
if device == 'android':
print('the vicon trident is not compatible with android')
#-----------Apple Device-------------------------------------------------------
if device == 'apple':
count = 0
for row in data:
if count > 0:
time_s.append(row[0])
type_.append(row[1])
Acc_x.append(row[2])
Acc_y.append(row[3])
Acc_z.append(row[4])
Angvel_x.append(row[5])
Angvel_y.append(row[6])
Angvel_z.append(row[7])
Mag_x.append(row[8])
Mag_y.append(row[9])
Mag_z.append(row[10])
Quat_x.append(row[11])
Quat_y.append(row[12])
Quat_z.append(row[13])
Quat_w.append(row[14])
count += 1
#------------------------------------------------------------------------------
#---interpolation--------------------------------------------------------------
if sensor == 'Xsens':
PacketCounter, SampleTimeFine, Quat_w, Quat_x, Quat_y, Quat_z = Xsens_interpolation_slerp(
t_range, t_calib, freq, PacketCounter, SampleTimeFine, Quat_w,
Quat_x, Quat_y, Quat_z)
length = len(PacketCounter)
if sensor == 'Vicon':
time_s, Quat_w, Quat_x, Quat_y, Quat_z = Vicon_interpolation_slerp(
freq, time_s, delay, t_calib, Quat_w, Quat_x, Quat_y, Quat_z)
PacketCounter = list(range(1, len(time_s) + 1))
length = len(time_s)
#---creating rotation matrices-------------------------------------------------
Mat11raw, Mat21raw, Mat31raw, Mat12raw, Mat22raw, Mat32raw, Mat13raw, Mat23raw, Mat33raw = [
0
] * length, [0] * length, [0] * length, [0] * length, [0] * length, [
0
] * length, [0] * length, [0] * length, [0] * length
count = 0
for x in range(length):
Matraw = R.as_matrix(
R.from_quat([
Quat_x[count], Quat_y[count], Quat_z[count], Quat_w[count]
]))
Mat11raw[count] = Matraw[0][0]
Mat12raw[count] = Matraw[0][1]
Mat13raw[count] = Matraw[0][2]
Mat21raw[count] = Matraw[1][0]
Mat22raw[count] = Matraw[1][1]
Mat23raw[count] = Matraw[1][2]
Mat31raw[count] = Matraw[2][0]
Mat32raw[count] = Matraw[2][1]
Mat33raw[count] = Matraw[2][2]
count += 1
#---sync Tridents--------------------------------------------------------------
if sensor == 'Vicon':
time_s, PacketCounter, Mat11raw, Mat21raw, Mat31raw, Mat12raw, Mat22raw, Mat32raw, Mat13raw, Mat23raw, Mat33raw = sync_tridents(
time_s, t0, PacketCounter, Mat11raw, Mat21raw, Mat31raw,
Mat12raw, Mat22raw, Mat32raw, Mat13raw, Mat23raw, Mat33raw)
length = len(time_s)
#---reset heading--------------------------------------------------------------
Mat11, Mat21, Mat31, Mat12, Mat22, Mat32, Mat13, Mat23, Mat33 = reset_heading(
sensor, freq, delay, Mat11raw, Mat21raw, Mat31raw, Mat12raw,
Mat22raw, Mat32raw, Mat13raw, Mat23raw, Mat33raw)
#---create .txt files----------------------------------------------------------
if sensor == 'Xsens':
txtfilename = '{}_{}.txt'.format(csvfilename[13:28],
csvfilename[0:12])
if sensor == 'Vicon':
TSindex = csvfilename.index('TS')
csvfilename_short = csvfilename.replace('-', '')
txtfilename = '{}_{}_Vicon_TS_{}.txt'.format(
csvfilename_short[TSindex + 8:TSindex + 16],
csvfilename_short[TSindex + 16:TSindex + 22],
csvfilename_short[TSindex + 2:TSindex + 7])
txtfilepath = trial_dir_path
completename = os.path.join(txtfilepath, txtfilename)
with open(completename, 'w') as fh:
fh.write('// Start Time: Unknown'
"\n"
'// Update Rate: {}.0Hz'
"\n"
'// Filter Profile: human (46.1)'
"\n"
'// Option Flags: AHS Disabled ICC Disabled '
"\n"
'// Firmware Version: 4.0.2'
"\n".format(freq))
count = t_calib * freq
if sensor == 'Xsens':
fh.write(
'PacketCounter\tSampleTimeFine\tMat[1][1]\tMat[2][1]\tMat[3][1]\tMat[1][2]\tMat[2][2]\tMat[3][2]\tMat[1][3]\tMat[2][3]\tMat[3][3]'
"\n")
for x in range(length):
fh.write("%s" % '{}'.format(PacketCounter[count]) +
'\t{}'.format(SampleTimeFine[count]) +
'\t{}'.format(Mat11[count]) +
'\t{}'.format(Mat21[count]) +
'\t{}'.format(Mat31[count]) +
'\t{}'.format(Mat12[count]) +
'\t{}'.format(Mat22[count]) +
'\t{}'.format(Mat32[count]) +
'\t{}'.format(Mat13[count]) +
'\t{}'.format(Mat23[count]) +
'\t{}\n'.format(Mat33[count]))
count += 1
if count == length:
break
if sensor == 'Vicon':
fh.write(
'PacketCounter\tSampleTimeFine\tMat[1][1]\tMat[2][1]\tMat[3][1]\tMat[1][2]\tMat[2][2]\tMat[3][2]\tMat[1][3]\tMat[2][3]\tMat[3][3]'
"\n")
for x in range(length):
fh.write("%s" % '{}'.format(PacketCounter[count]) +
'\t{}'.format(time_s[count]) +
'\t{}'.format(Mat11[count]) +
'\t{}'.format(Mat21[count]) +
'\t{}'.format(Mat31[count]) +
'\t{}'.format(Mat12[count]) +
'\t{}'.format(Mat22[count]) +
'\t{}'.format(Mat32[count]) +
'\t{}'.format(Mat13[count]) +
'\t{}'.format(Mat23[count]) +
'\t{}\n'.format(Mat33[count]))
count += 1
if count == length:
break
def rotvec2mat(x):
return R.as_matrix(R.from_rotvec(x))
def update(self, t, state, flat_output):
"""
This function receives the current time, true state, and desired flat
outputs. It returns the command inputs.
Inputs:
t, present time in seconds
state, a dict describing the present state with keys # State
x, position, m
v, linear velocity, m/s
q, quaternion [i,j,k,w]
w, angular velocity, rad/s
flat_output, a dict describing the present desired flat outputs with keys # Desired
x, position, m
x_dot, velocity, m/s
x_ddot, acceleration, m/s**2
x_dddot, jerk, m/s**3
x_ddddot, snap, m/s**4
yaw, yaw angle, rad
yaw_dot, yaw rate, rad/s
Outputs:
control_input, a dict describing the present computed control inputs with keys # Output
cmd_motor_speeds, rad/s
cmd_thrust, N (for debugging and laboratory; not used by simulator)
cmd_moment, N*m (for debugging; not used by simulator)
cmd_q, quaternion [i,j,k,w] (for laboratory; not used by simulator)
"""
cmd_motor_speeds = np.zeros((4,))
cmd_thrust = 0
cmd_moment = np.zeros((3,))
cmd_q = np.zeros((4,))
# STUDENT CODE HERE --------------------------------------------------------------------------------------------
# define error
error_pos = state.get('x') - flat_output.get('x')
error_vel = state.get('v') - flat_output.get('x_dot')
error_vel = np.array(error_vel).reshape(3, 1)
error_pos = np.array(error_pos).reshape(3, 1)
# Equation 26
rdd_des = np.array(flat_output.get('x_ddot')).reshape(3, 1) - np.matmul(self.Kd, error_vel) - np.matmul(self.Kp,
error_pos)
# Equation 28
F_des = (self.mass * rdd_des) + np.array([0, 0, self.mass * self.g]).reshape(3, 1) # (3 * 1)
# Find Rotation matrix
R = Rotation.as_matrix(Rotation.from_quat(state.get('q'))) # Quaternions to Rotation Matrix
# print(R.shape)
# Equation 29, Find u1
b3 = R[0:3, 2:3]
# print(b3)
u1 = np.matmul(b3.T, F_des) # u1[0,0] to access value
# print(np.transpose(b3))
# ----------------------- the following is to find u2 ---------------------------------------------------------
# Equation 30
b3_des = F_des / np.linalg.norm(F_des) # 3 * 1
a_Psi = np.array([np.cos(flat_output.get('yaw')), np.sin(flat_output.get('yaw')), 0]).reshape(3, 1) # 3 * 1
b2_des = np.cross(b3_des, a_Psi, axis=0) / np.linalg.norm(np.cross(b3_des, a_Psi, axis=0))
b1_des = np.cross(b2_des, b3_des, axis=0)
# Equation 33
R_des = np.hstack((b1_des, b2_des, b3_des))
# print(R_des)
# Equation 34
# R_temp = 0.5 * (np.matmul(np.transpose(R_des), R) - np.matmul(np.transpose(R), R_des))
temp = R_des.T @ R - R.T @ R_des
R_temp = 0.5 * temp
# orientation error vector
e_R = 0.5 * np.array([-R_temp[1, 2], R_temp[0, 2], -R_temp[0, 1]]).reshape(3, 1)
# Equation 35
u2 = self.inertia @ (-self.K_R @ e_R - self.K_w @ (np.array(state.get('w')).reshape(3, 1)))
gama = self.k_drag / self.k_thrust
Len = self.arm_length
cof_temp = np.array(
[1, 1, 1, 1, 0, Len, 0, -Len, -Len, 0, Len, 0, gama, -gama, gama, -gama]).reshape(4, 4)
u = np.vstack((u1, u2))
F_i = np.matmul(np.linalg.inv(cof_temp), u)
for i in range(4):
if F_i[i, 0] < 0:
F_i[i, 0] = 0
cmd_motor_speeds[i] = self.rotor_speed_max
cmd_motor_speeds[i] = np.sqrt(F_i[i, 0] / self.k_thrust)
if cmd_motor_speeds[i] > self.rotor_speed_max:
cmd_motor_speeds[i] = self.rotor_speed_max
cmd_thrust = u1[0, 0]
cmd_moment[0] = u2[0, 0]
cmd_moment[1] = u2[1, 0]
cmd_moment[2] = u2[2, 0]
cmd_q = Rotation.as_quat(Rotation.from_matrix(R_des))
control_input = {'cmd_motor_speeds': cmd_motor_speeds,
'cmd_thrust': cmd_thrust,
'cmd_moment': cmd_moment,
'cmd_q': cmd_q}
return control_input
def control_attitude(self, rot_sp_full: Rotation,
rot_cur_attitude: Rotation):
# An implementation of the quaternion-based attitude control described in this paper:
# https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
# PX4 implementation can be found in update() here:
# https://github.com/PX4/PX4-Autopilot/blob/master/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
# Notes on terminology:
# For target orientation quaternion:
# Paper "q_cmd"
# Px4 code "q_d"
# Here "q_sp" or "rot_sp"
# For frame-of-reference basis vectors:
# Paper "e^B"
# Px4 code "e"
# Here "body"
# For distinguishing between Reduced and Full:
# Paper "red"
# Px4 code "red"
# Here "reduced"
##############################
# Generate quaternion setpoint
##############################
## Generate quaternion setpoint via Reduced (yaw-uncontrolled) Attitude Control:
# See section 3.2.1 in the paper
# Get current bodyZ:
# The third column of rotation matrices encode the corresponding desired position of the Z basis vector
body_z_cur = rot_cur_attitude.as_matrix()[:, 2]
body_z_sp = rot_sp_full.as_matrix()[:, 2]
# Calculate reduced rotation setpoint
q_sp_reduced = self.quat_between_vectors(body_z_cur, body_z_sp)
rot_sp_reduced = Rotation.from_quat(q_sp_reduced)
if np.abs(q_sp_reduced)[0] == 1 or np.abs(q_sp_reduced)[1] == 1:
# Corner case: vehicle and thrust have opposite directions
# Here we just ignore the reduced control
rot_sp_reduced = rot_sp_full
else:
# Transform rot_sp_reduced from body to world frame
rot_sp_reduced = rot_sp_reduced * rot_cur_attitude
# Finished calculating reduced rotation setpoint
## Mixing reduced and full attitude control to get final quaternion setpoint
# See section 3.2.3 and eq(53) in the paper
rot_reduced_to_full = rot_sp_reduced.inv() * rot_sp_full
rot_reduced_to_full = self.canonicalize_rotation(rot_reduced_to_full)
q_reduced_to_full = rot_reduced_to_full.as_quat()
# Constrain domains for arccos and arcsin
q_reduced_to_full[2] = np.clip(q_reduced_to_full[2], -1, 1)
q_reduced_to_full[3] = np.clip(q_reduced_to_full[3], -1, 1)
yaw_weight = self.attitude_params.yaw_weight
# Mixing quaternion. See eq(53)
q_mixer = np.array([
0, 0,
np.sin(yaw_weight * np.arcsin(q_reduced_to_full[2])),
np.cos(yaw_weight * np.arccos(q_reduced_to_full[3]))
])
# Apply reduced-to-full rotation to the mixing orientation
rot_mixer = Rotation.from_quat(q_mixer)
rot_sp_mixed = rot_sp_reduced * rot_mixer
# Finished mixing reduced and full rotation setpoints
##########################################
# Apply quaternion setpoint to Control Law
##########################################
## Convert quaternion setpoint to angular rates via the control law described by eq(23) in the paper
# Get attitude error (as rotation) between current and mixed setpoint
rot_error = rot_cur_attitude.inv() * rot_sp_mixed
# Canonicalize and get error quaternion
rot_error = self.canonicalize_rotation(rot_error)
q_error = rot_error.as_quat()
# Apply control law
bodyrates_sp = 2 * self.attitude_params.kP * q_error[0:3]
bodyrates_sp = np.reshape(bodyrates_sp, (3, 1))
return bodyrates_sp
def set_rotation_part(H, r:Rotation):
R = r.as_matrix()
H[0:3,0:3] = R
return H