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)
Exemple #2
0
 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]))
Exemple #3
0
    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]))
Exemple #5
0
 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)
Exemple #6
0
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
Exemple #7
0
    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
Exemple #8
0
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
Exemple #9
0
 def rotation(self, value: Rotation):
     if not isinstance(value, Rotation):
         raise ValueError
     self.matrix[:3, :3] = value.as_matrix()
Exemple #10
0
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
Exemple #12
0
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