I want to rotate a nut around its original Z axis. The nut is a free floating object and can have any orientation in space. mj_get_onebody() returns the current orientation as a quaternion. I need to extract from this quaternion: -- the rotation axis, i.e. what the original Z axis is now. Thats a vector with 3 cartesian coordinates. -- the rotation angle. That's a scalar between -PI and PI. Mathematics has always been difficult for me. As a first approach, I created an XML with some bodies rotated by different Euler angles. I run it in MuJoCo HAPTIX and saved the model as XML. The Euler angles are now converted to quaternions. Then I implemented the formulas from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles and compared them to the values from the saved XML: Code: #include <sstream> #include <iostream> #include <iomanip> #include <conio.h> using namespace std; #pragma warning(disable: 4244) // '=' : conversion from 'double' to 'float', possible loss of data #pragma warning(disable: 4305) // 'initializing' : truncation from 'double' to 'float' const double PI = 3.1415926535897932384626433832795; inline double deg(double rad) { return rad / PI * 180; } inline double rad(double deg) { return deg / 180 * PI; } void quat_to_euler(float q[4], float e[3]) { e[0] = deg(atan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]))); e[1] = deg(asin(2 * (q[0] * q[2] - q[3] * q[1]))); e[2] = deg(atan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]))); } void euler_to_quat(float e[3], float q[4]) { float a = 0; q[0] = cos(rad(a) / 2); q[1] = sin(rad(a) / 2) * cos(rad(e[0])); q[2] = sin(rad(a) / 2) * cos(rad(e[1])); q[3] = sin(rad(a) / 2) * cos(rad(e[2])); } struct SAMPLES_FROM_SAVED_XML { float e[3], q[4]; } samples[] = { 0, 0, 0, 1, 0, 0, 0, 90, 0, 0, 0.707107, 0.707107, 0, 0, 0, 90, 0, 0.707107, 0, 0.707107, 0, 90, 90, 0, 0.5, 0.5, 0.5, 0.5, 0, 0, 90, 0.707107, 0, 0, 0.707107, 90, 0, 90, 0.5, 0.5, -0.5, 0.5, 0, 90, 90, 0.5, 0.5, 0.5, 0.5, 90, 90, 90, 0, 0.707107, 0, 0.707107, -90, 0, 0, 0.707107, -0.707107, 0, 0, 0, -90, 0, 0.707107, 0, -0.707107, 0, -90, -90, 0, 0.5, -0.5, -0.5, 0.5, 0, 0, -90, 0.707107, 0, 0, -0.707107, -90, 0, -90, 0.5, -0.5, -0.5, -0.5, 0, -90, -90, 0.5, 0.5, -0.5, -0.5, -90, -90, -90, 0.707107, 0, -0.707107, 0, }; int _cdecl main(int argc, char* argv[]) { ostringstream ss; for (auto& x : samples) { ss.str(""), ss << "x.e={" << x.e[0] << "," << x.e[1] << "," << x.e[2] << "}"; cout << endl << left << setw(30) << ss.str() << "x.q={" << x.q[0] << "," << x.q[1] << "," << x.q[2] << "," << x.q[3] << "}"; float e[3], q[4]; quat_to_euler(x.q, e); euler_to_quat(x.e, q); ss.str(""), ss << " e={" << e[0] << "," << e[1] << "," << e[2] << "}"; cout << endl << left << setw(30) << ss.str() << " q={" << q[0] << "," << q[1] << "," << q[2] << "," << q[3] << "}"; } cout << endl << "READY." << endl; do _getch(); while (_kbhit()); return 0; } This gives the output: Code: x.e={0,0,0} x.q={1,0,0,0} e={0,0,0} q={1,0,0,0} x.e={90,0,0} x.q={0.707107,0.707107,0,0} e={90,0,0} q={1,0,0,0} x.e={0,90,0} x.q={0.707107,0,0.707107,0} e={180,-1.#IND,180} q={1,0,0,0} x.e={90,90,0} x.q={0.5,0.5,0.5,0.5} e={90,0,90} q={1,0,0,0} x.e={0,0,90} x.q={0.707107,0,0,0.707107} e={0,0,90} q={1,0,0,0} x.e={90,0,90} x.q={0.5,0.5,-0.5,0.5} e={0,-90,0} q={1,0,0,0} x.e={0,90,90} x.q={0.5,0.5,0.5,0.5} e={90,0,90} q={1,0,0,0} x.e={90,90,90} x.q={0,0.707107,0,0.707107} e={180,-1.#IND,180} q={1,0,0,0} x.e={-90,0,0} x.q={0.707107,-0.707107,0,0} e={-90,0,0} q={1,0,0,0} x.e={0,-90,0} x.q={0.707107,0,-0.707107,0} e={180,-1.#IND,180} q={1,0,0,0} x.e={-90,-90,0} x.q={0.5,-0.5,-0.5,0.5} e={-90,0,90} q={1,0,0,0} x.e={0,0,-90} x.q={0.707107,0,0,-0.707107} e={0,0,-90} q={1,0,0,0} x.e={-90,0,-90} x.q={0.5,-0.5,-0.5,-0.5} e={0,-90,0} q={1,0,0,0} x.e={0,-90,-90} x.q={0.5,0.5,-0.5,-0.5} e={90,0,-90} q={1,0,0,0} x.e={-90,-90,-90} x.q={0.707107,0,-0.707107,0} e={180,-1.#IND,180} q={1,0,0,0} quat_to_euler() is OK for some cases. For others, it swaps axes or returns numeric errors. euler_to_quat() always returns the null quaternion because the formula from Wikipedia requires some alpha angle which I don't know. I certainly need help with this kind of mathematics.
A unit quaternion encodes a 3D rotation around axis V by angle A as: quat = (cos(A/2), sin(A/2) Vx, sin(A/2) Vy, sin(A/2) Vz) where A is in radians and V is a unit vector. So you can convert a unit quaternion to an axis-angle representation of the 3D rotation as: double axis[3] = {quat[1], quat[2], quat[3]}; double sin_half_angle = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); double angle = 2 * atan2(sin_half_angle, quat[0]); This gives you a rotation angle (in radians) around a 3D rotation axis. Note that the axis vector is not normalized; you can normalize it if you want (its length is sin_half_angle computed above). The same 3D rotation can be encoded with two quaternions, having opposite axes and angles. Thus you may not recover the same angle-axis representation you started with, but it doesn't matter because the underlying 3D rotation will be the same. The above formula assumes that sin(A/2) is non-negative, which is legitimate because if A is between 0 and 2*pi, then A/2 is between 0 and pi.
An axis and a rotation angle seems to be what I need, so I don't have to care about Euler angles any more. Unfortunately, there is the little word "an". I don't need just some arbitrary axis and its accompanying rotation angle. I need the Z axis from the XML model file! For example: Code: object normalized heading mjOneBody.quat axis-angle should be Z+ 1 0 0 0 0 0 0 0 0 1 Z- 0 1 0 0 1 0 0 0 0 -1 Y+ .7 -.7 0 0 -1 0 0 0 1 0 Y- .7 .7 0 0 1 0 0 0 -1 0 X+ .7 0 .7 0 0 1 0 1 0 0 X- .7 0 -.7 0 0 -1 0 -1 0 0 The formula you gave me does not set axis[2] at all. It sets the same value for Z- and Y-. And it causes a numeric overflow at Z+ so that I had to check for (sin_half_angle != 0) before normalizing. Btw, yesterday evening I got my little euler_to_quat() function from the code above working: Code: void euler_to_quat(float e[3], float q[4]) { double heading = rad(e[0]); double attitude = rad(e[1]); double bank = rad(e[2]); // from http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm double c1 = cos(heading / 2); double s1 = sin(heading / 2); double c2 = cos(attitude / 2); double s2 = sin(attitude / 2); double c3 = cos(bank / 2); double s3 = sin(bank / 2); double c1c2 = c1*c2; double s1s2 = s1*s2; double w = c1c2*c3 - s1s2*s3; double x = c1c2*s3 + s1s2*c3; double y = s1*c2*c3 + c1*s2*s3; double z = c1*s2*c3 - s1*c2*s3; // confusing order between euclidianspace.com and MuJoCo q[0] = w; q[1] = y; q[2] = z; q[3] = x; for (int i = 0; i < 4; i++) if (q[i] > -1e-15 && q[i] < 1e-15) q[i] = 0; } Notice the strange order w-y-z-x in the conversion! He also mentions that his site may contain errors and should not be used in production.
We are talking about two different notions of "axis" here. The axis I was telling you about represents the 3D rotation needed to bring one rigid-body orientation in alignment with another. Each 3D rotation can be expressed as a single rotation around some axis by some angle, and these are unique modulo obvious degeneracies. For example, if you "rotate" by 0 deg around any axis, you remain in the same orientation. The quaternion corresponding to such null rotation is (1,0,0,0). In this case sin_half_anlge=0 and you cannot just normalize, as you noticed. The axis you are talking about is the axis of a coordinate frame attached to the body. In your table, +Z means nothing moved, so the quaterion is (1,0,0,0) and the rotation axis is impossible to recover (since there is no rotation). +Y means the Z axis rotated to the Y axis, meaning that the rotation was around the X axis -- which is what you got from the formula I gave you. Perhaps you want to transform a quaternion into a 3x3 orthonormal matrix (also known as a rotation matrix) whose columns are the body coordinate frame axes expressed in the world frame? If that is the case, you need a different formula, see here for example: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
Yes! That's it! The third column of the matrix is exactly the z_axis from the XML file that I was looking for. Thank you very much! The program for checking the conversion: Code: #include <iostream> #include <iomanip> #include <conio.h> using namespace std; void quatToMatrix(double q[4], double m[3][3]) { double w = q[0], x = q[1], y = q[2], z = q[3]; // from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ double sqw = w*w; double sqx = x*x; double sqy = y*y; double sqz = z*z; // invs (inverse square length) is only required if quaternion is not already normalised double invs = 1 / (sqx + sqy + sqz + sqw); m[0][0] = (sqx - sqy - sqz + sqw)*invs; // since sqw + sqx + sqy + sqz =1/invs*invs m[1][1] = (-sqx + sqy - sqz + sqw)*invs; m[2][2] = (-sqx - sqy + sqz + sqw)*invs; double tmp1 = x*y; double tmp2 = z*w; m[1][0] = 2.0 * (tmp1 + tmp2)*invs; m[0][1] = 2.0 * (tmp1 - tmp2)*invs; tmp1 = x*z; tmp2 = y*w; m[2][0] = 2.0 * (tmp1 - tmp2)*invs; m[0][2] = 2.0 * (tmp1 + tmp2)*invs; tmp1 = y*z; tmp2 = x*w; m[2][1] = 2.0 * (tmp1 + tmp2)*invs; m[1][2] = 2.0 * (tmp1 - tmp2)*invs; } struct { char* name; double q[4], needed_z_axis[3], needed_angle; } samples[] = { "Z+ 0", 1, 0, 0, 0, 0, 0, 1, 0, "Z+ 90", 0.707107, 0, 0, 0.707107, 0, 0, 1, 90, "Z+ 180", 0, 0, 0, 1, 0, 0, 1, 180, "Z+ 270", -0.707107, 0, 0, 0.707107, 0, 0, 1, 270, "Z- 0", 0, 1, 0, 0, 0, 0, -1, 0, "Z- 90", 0, 0.707107, -0.707107, 0, 0, 0, -1, 90, "Z- 180", 0, 0, -1, 0, 0, 0, -1, 180, "Z- 270", 0, -0.707107, -0.707107, 0, 0, 0, -1, 270, "Y+ 0", 0.707107, -0.707107, 0, 0, 0, 1, 0, 0, "Y+ 90", 0.5, -0.5, 0.5, 0.5, 0, 1, 0, 90, "Y+ 180", 0, 0, 0.707107, 0.707107, 0, 1, 0, 180, "Y+ 270", -0.5, 0.5, 0.5, 0.5, 0, 1, 0, 270, "Y- 0", 0.707107, 0.707107, 0, 0, 0, -1, 0, 0, "Y- 90", 0.5, 0.5, -0.5, 0.5, 0, -1, 0, 90, "Y- 180", 0, 0, -0.707107, 0.707107, 0, -1, 0, 180, "Y- 270", -0.5, -0.5, -0.5, 0.5, 0, -1, 0, 270, "X+ 0", 0.707107, 0, 0.707107, 0, 1, 0, 0, 0, "X+ 90", 0.5, 0.5, 0.5, 0.5, 1, 0, 0, 90, "X+ 180", 0, 0.707107, 0, 0.707107, 1, 0, 0, 180, "X+ 270", -0.5, 0.5, -0.5, 0.5, 1, 0, 0, 270, "X- 0", 0.707107, 0, -0.707107, 0, -1, 0, 0, 0, "X- 90", 0.5, -0.5, -0.5, 0.5, -1, 0, 0, 90, "X- 180", 0, -0.707107, 0, 0.707107, -1, 0, 0, 180, "X- 270", -0.5, -0.5, 0.5, 0.5, -1, 0, 0, 270, }; bool is_appr_eq(double a, double b) { return (a - b) > -1e-15 && (a - b) < 1e-15; } double deg(double x) { double d = x / 3.1415926535897932384626433832795 * 180; while (d < 0) d += 360; while (d >= 360) d -= 360; return d; } int _cdecl main(int argc, char* argv[]) { cout << setprecision(3); for (auto& x : samples) { double m[3][3]; quatToMatrix(x.q, m); double z_axis[3] = {m[0][2], m[1][2], m[2][2]}; double angle_from_quat = 0; // just another formula needed... cout << endl << x.name; if (is_appr_eq(z_axis[0], x.needed_z_axis[0]) && is_appr_eq(z_axis[1], x.needed_z_axis[1]) && is_appr_eq(z_axis[2], x.needed_z_axis[2])) { cout << " z_axis ok"; } else { cout << " ERROR z_axis=" << z_axis[0] << " " << z_axis[1] << " " << z_axis[2]; cout << " should be=" << x.needed_z_axis[0] << " " << x.needed_z_axis[1] << " " << x.needed_z_axis[2]; } if (is_appr_eq(deg(angle_from_quat), x.needed_angle)) { cout << " angle_from_quat ok"; } else { cout << " ERROR angle_from_quat=" << deg(angle_from_quat); cout << " should be=" << x.needed_angle; } } cout << endl << "READY." << endl; do _getch(); while (_kbhit()); return 0; } ... and its output: Code: Z+ 0 z_axis ok angle_from_quat ok Z+ 90 z_axis ok ERROR angle_from_quat=0 should be=90 Z+ 180 z_axis ok ERROR angle_from_quat=0 should be=180 Z+ 270 z_axis ok ERROR angle_from_quat=0 should be=270 Z- 0 z_axis ok angle_from_quat ok Z- 90 z_axis ok ERROR angle_from_quat=0 should be=90 Z- 180 z_axis ok ERROR angle_from_quat=0 should be=180 Z- 270 z_axis ok ERROR angle_from_quat=0 should be=270 Y+ 0 z_axis ok angle_from_quat ok Y+ 90 z_axis ok ERROR angle_from_quat=0 should be=90 Y+ 180 z_axis ok ERROR angle_from_quat=0 should be=180 Y+ 270 z_axis ok ERROR angle_from_quat=0 should be=270 Y- 0 z_axis ok angle_from_quat ok Y- 90 z_axis ok ERROR angle_from_quat=0 should be=90 Y- 180 z_axis ok ERROR angle_from_quat=0 should be=180 Y- 270 z_axis ok ERROR angle_from_quat=0 should be=270 X+ 0 z_axis ok angle_from_quat ok X+ 90 z_axis ok ERROR angle_from_quat=0 should be=90 X+ 180 z_axis ok ERROR angle_from_quat=0 should be=180 X+ 270 z_axis ok ERROR angle_from_quat=0 should be=270 X- 0 z_axis ok angle_from_quat ok X- 90 z_axis ok ERROR angle_from_quat=0 should be=90 X- 180 z_axis ok ERROR angle_from_quat=0 should be=180 X- 270 z_axis ok ERROR angle_from_quat=0 should be=270 The extracted z_axis from the quaternion is correct for every tested combination. Great! But I'm not finished yet. I still need to get the rotation angle from the quaternion. Any idea about that?
An idea: I could try http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ and the needed angle should be the third number. I'll try this tomorrow if noone comes up with something better.
Which rotation angle do you need? The first formula I gave you produces the rotation angle needed to align the two orientations. Or do you mean the angle between the original and rotated Z axis? If it is the latter, you can simply normalize the two vectors, and take their dot-product -- which equals the cosine of the angle between them. So you can use acos(dot(Z_original, Z_rotated)) to recover the angle. Note that this is not the same as the overall amount of rotation. To take an extreme case, suppose you rotated about the Z-axis. Now the angle between the original and rotated Z axes is 0, even though the body has been rotated.
The conversion from quaternion to Euler was a bad idea. If even the experts don't get it right, then how should a mathematical laymen? Code: quat_to_euler ERROR x.e = {0, 0, 6.26573201465964} e = {-0, 0, -0.0174533001800855} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 3.15904594610974} e = {9.196917933654e-011, 0.0174532834231338, -3.12413937016826} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 3.9095375244673} e = {1.45877732240117e-008, 0.017453283863287, -2.37364873093359} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 3.92699081698724} e = {-7.522409818963e-009, 0.0174533014245074, -2.35619396223312} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 3.94444410950718} e = {5.74883462170425e-009, 0.0174532949698867, -2.33874181278394} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 4.69493568786475} e = {-1.47336413553367e-008, 0.0174532981581024, -1.5882486064622} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 4.71238898038469} e = {0, 0.0174532821188298, -1.5707963267949} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 4.72984227290463} e = {1.47336413553367e-008, 0.0174532981581024, -1.5533440471276} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 5.48033385126219} e = {-5.74883462170425e-009, 0.0174532949698867, -0.802850840805857} quat_to_euler ERROR x.e = {0, 0.0174532925199433, 5.49778714378214} e = {7.522409818963e-009, 0.0174533014245074, -0.785398691356676} 13824 samples converted from euler_to_quat contain 0 errors 13824 samples converted from quat_to_euler contain 12995 errors At least the code for conversion from Euler to quaternion works. I have created 13k bodies with every combination of Euler angles ±1° around the critical angles every 45°, saved the XML from MuJoCo HAPTIX 1.31, and extracted the resulting quaternions. So this data should be correct because MuJoCo is the reference here and not the buggy Java code from the site of a wannabe internet teacher. The file is attached if someone wants to investigate this further. Which rotation angle do I need? The Z angle that is specified at <body euler="x y Z"> in the XML model file. I want to rotate a free floating nut. The nut is designed such that its rotation axis is the Z axis. All CAD programs seem to handle it that way. When the nut is attached to a thread somewhere in space, it can have any orientation. That depends on the thread. But I need to rotate the nut around its Z axis. So I need to know where the Z axis from the model file is with respect to that thread. get_one_body(nut_id) gives me only a quaternion for rotation. So I need to extract from this quaternion both the Z axis from the XML model file and a rotation angle around that axis. I have the Z axis, it is the 3rd column from the 3x3 orthogonal rotation matrix which you gave me. Now I need the rotation angle around that axis. Because I need to feel whether the model behaves physically correct. Later the experimenter has to tighten or loosen the nuts, so that the robot is not confronted with impossible torques. The robot will use just a virtual open-ended wrench, but the experimenter cannot get his hands into the simulation and feel the forces to decide if the experiment setup is correct. So he needs a torque wrench utility. This is what I'm trying to write. Maybe I don't need the absolute angle but only its derivative? This would solve the problem with the nasty singularities, as long as the update frequency is fast enough so that only small changes in angles are reported to get_one_body.
Another idea. If the orthogonal 3x3 matrix contains the Z axis from the model file as the 3rd column, then it probably contains the X and Y axes as the first and the second column. I save this matrix. At the next time step, I take acos(dot(col[0..2], saved_col[0..2])) and get 3 angles back. Then I do some safety calculation: max((abs(angle_x) + abs(angle_y)) / 2 - abs(angle_z), 0). This should give me the angle of rotation of the nut since the last timestep. I then divide by (time - saved_time) to get the absolute value of the rotation. The direction will have to be extracted from the appied force. I'll try this tomorrow. Too tired to program now.
I see what you are trying to do. The Euler angles from the XML are not easily recovered. The MuJoCo compiler applies a sequence of three rotations (specified by the Euler angles) to obtain the quaternion. These rotations do not commute, and in general there is no sensible mathematical operation you can perform with a vector of Euler angles. You can find formulas online showing how the 3x3 orientation matrix depends on the Euler angles, and find a way to recover them, but it is not a good idea. Instead it is better to think in terms of quaternions and/or 3x3 orientation matrices. In your case, just look at the X axis (or Y axis) and compute the angle between the old and new X axes. HOWEVER, both configurations should have the Z axis pointing in the same direction. Otherwise the rotation is around some oblique axis. So your latest idea is fine. Note also that mj_get_onebody gives you the angular velocity. This is a 3D vector whose direction is the axis of rotation (in world coordinates) and whose length is the speed of rotation in radians/second. If this vector is aligned with the Z axis of the body frame, the nut is spinning around its Z axis. If not, then you can find the rotation speed around the Z axis as dot(Z_axis, angvel). Again, this is in radians/second. The finite-difference approximation you are considering also works, but angvel is exact and does not involve any approximations.
Yes! The angvel trick did it! The orientation of the blue box with the thread can be changed with the two control sliders in the F3 dialog. Whatever the current orientation is, Cursor Right will always tighten the nut, and Cursor Left will loosen it. I didn't need the rotation angle at all, only the speed. Now I can fine tune the simulation so that it runs more stable, and use the rotation speed to control the torque such that the virtual wrench looks and feels realistic.