Quaternion-derived Rotation Matrix problem

user_15265540 0
Viewed 85

my problem is pretty simple: i have a device from which i can read linear accelerations and a quaternion representing his orientation. I need to add the gravity component to these linear accelerations. I found the solution of this problem by taking the quaternion, converting it to a rotation matrix and then doing a matrix product between the rotation matrix and the gravity vector that i wish to add: [0, 0, 10]. From the resulting vector i added each component to the linear acceleration vector. To create the rotation matrix i followed this link. The device has a RH Coordinate system:

enter image description here

The python code is shown below:

        q_init = Quaternion(data.pose.orientation.w, data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z)

        q = q_init.elements

        q0 = q[0]
        q1 = q[1]
        q2 = q[2]
        q3 = q[3]
        #i create the quaternion-derived rotation matrix

        r00 = 1 - 2*(q2*q2 + q3*q3)
        r01 = 2*(q1*q2 - q3*q0) 
        r02 = 2*(q1*q3 + q2*q0) 

        r10 = 2*(q1*q2 + q3*q0) 
        r11 = 1 - 2*(q1*q1 + q3*q3)
        r12 = 2*(q2*q3 - q1*q0)  

        r20 = 2*(q1*q3 - q2*q0)  
        r21 = 2*(q2*q3 + q1*q0) 
        r22 = 1 - 2*(q1*q1 + q2*q2)

        rot_matrix = np.array([[r00, r01, r02], [r10, r11, r12], [r20, r21, r22]])
        
        g_vect = np.array([[0], [0], [10]])

        g_rot = np.dot(rot_matrix, g_vect)

        if len(self.AccX) != 0:
            
            self.Output[3] = (self.AccX[-1] + g_rot[0, 0])
            self.Output[4] = (self.AccY[-1] + g_rot[1, 0])
            self.Output[5] = (self.AccZ[-1] + g_rot[2, 0])

With this code if i stay still i get as Acceleration_with_Gravity (AccG) vector [0,0,10] which is fine and its the same as another device. If i pitch by 90 deg i get [10,0,0] which is fine. If i roll by 90 deg. -> [0,10,0], fine.

The problem shows when i first pitch by 90 deg. and then roll by 90 deg:

If i rotate clockwise around Y Axe by 90 deg and the rotate clockwise my device around the X-axe it should just give me [10,0,0] for every angle i rotate but instead, as soon as i start rotating the device, the axe seems like to change on the Y one (negative, clearly) and it gives me [0,-10,0] instead of a constant [10,0,0]!

Explaining pictures/video:

Movement: link

Video of the rotation: Link

I don't know where is the problem, i tried to use other python libraries, to use only quaternions but the problem seems to persist, and the rotation matrix is taken from wikipedia, also i tried to change it with other alternatives of the same Rotation matrix but the problem it's still here.

Thank you very much for the help.

ros quaternions rotation matrix python
1 Answers

I just had the same issue, i think.

You cannot directly feed the axis and angle to the quaternions. They have to be modified a little beforehand.

// written in typescript

/**
 * @param axis  
 *  The axis along wich the rotation takes place
 * @param theta  
 *  The rotation (should be radians)
 */
function for_rotation(axis: Vector3I, theta: number): Quaternion {
        const cos_theta = Math.cos(theta / 2);
        const sin_theta = Math.sin(theta / 2);
        return new Quaternion(cos_theta, sin_theta * axis.x, sin_theta * axis.y, sin_theta * axis.z);
}

You can also see that part in your wiki source in the first paragraph 'Using quaternions as rotations' the second equation.

I don't know if it helps you 8 Months later, but better late than never :D

user_84926650
Related