The setup is a measuring device giving me Quaternions and Euler Angles. The measuring device is mounted on a disk, which itself is mounted on an arm. The arm can rotate up and down while being fixed on one end, about 240 degrees, since it's sitting on a podest. The disc can rotate clockwise and counterclockwise, unlimited rotation in both directions.
Left is the side view, right is the top view. The possible rotations i want to measure are drawn as arrows.
The goal is displaying two values, one for the arm angle and one for the disc rotation angle, both between -180 and 180 degrees.
The rotation sequence of the measuring device's data is ZYX (roll, pitch, yaw), the Quaternion is WXYZ. X is positive to the East, Y to the North, Z to the top. The order of rotating the measuring device itself is not fixed (arm and disk movements can be mixed).
I understood that the Euler Angles are relative to the object, and thus subject to change when the object is rotated. Furthermore I understood that the rotation order is important. I read that reversing the rotation order (to XYZ) would give me the extrinsic rotation (= the rotations used for obtaining the same end orientation of the object based on the world's non-changing axes instead of the object's intrinsic axes), but I have no idea if/ how this could be used for solving my problem.
I did not find any python functions for achiving my goal and do not understand the math required for going that route. Am I overlooking a simple way of solving that problem? If not, how would you approach it?
EDIT1: Found a solution for calculating the arm angle:
- Define base vector (0,0,1), which is the z axis in the neutral position (meaning the arm is parallel to the earth)
- Get current z axis by applying the rotation defined by the device's quaternion to the base vector
- Use arccos(numpy.clip(dot(rotatedVectorZ, baseVectorZ), -1.0, 1.0)) to calculate the angle between the original z axis and the rotated one, which equals the arm angle.
The calculated angles are never negative, so there are two possible positions for each value, but that is sufficient for my case.
For calculating the rotation angle, my idea was projecting the x vector into the x-y-plane by setting z to 0 and then normalizing the vector, and then calculating the angle between the (1,0,0) axis and the projected vector, but this does not work, since there is some shift in the arm angles nearing 90 degrees.
Now I am thinking about rotating the device's x,y,z coordinates (which I get by applying the rotation defined by the quaternion to each of the vectors (1,0,0), (0,1,0) and (0,0,1)) in such a way in the sphere that the z axes align, which would enable me to calculate the difference between the original x axis (1,0,0) and the rotated one, but I am not sure if that works as expected.
Edit 2: Found a working approach for determining the angles of disk rotation between 0 and 180 degrees:
- Define base vector (1,0,0), which is the x axis in the neutral position (meaning the disk is not rotated)
- Get current x axis by applying the rotation defined by the device's quaternion to the base vector
- Calculate the axis, around which the arm rotates, by calculating the vector perpendicular to the original z axis (which is 0,0,1) and the current z axis (which equals 0,0,1 and the application of the device's current quaternion value)
- Normalize that axis vector and calculate the quaternion that rotates the current z axis to the original z axis (0,0,1)
- Use that quaternion for rotating the current x axis (see step 2). This simulates an arm movement into the position parallel to the earth.
- Calculate the angle between that new x axis and the base x axis (1,0,0)
Now we have the rotation angle of the disk in the 180 degrees space.
Edit 3: Found the final solution for getting the angles for the rotation between 0 and 360 degrees. Instead of using the arccos of the dot product we can use arctan2(determinant, dot(current_x-vector, base_x-axis)), since both vectors are in the same plane now (with z beeing 0).
[0.7934709, 0.006922651, 0.016842736, 0.60833573] [-0.24577184, 0.015660709, -0.0057019624, 0.96918446] [-0.23589617, -0.24023984, 0.22534007, 0.9142537] [-0.92398167, 0.22448266, 0.24135745, -0.19393821] [-0.71125966, 0.45272928, 0.5107461, -0.16817938] [-0.31564975, 0.68132704, -0.023805361, -0.659994]
– Gustave