mhkabir
commented
9 years ago @TSC21 Can you please have a look and verify the quaternion order for tranforms?
Closed vooon closed 9 years ago
mhkabir
commented
9 years ago @TSC21 Can you please have a look and verify the quaternion order for tranforms?
vooon
commented
9 years ago Test from https://github.com/mavlink/mavros/commit/c91e39590dfcf056590a1c0a20b1129889643fc8 dont work! @TSC21 what wrong?
-- run_tests.py: execute commands
/home/vovan/ros/devel/lib/mavros/libmavros-frame-test --gtest_output=xml:/home/vovan/ros/build/mavros/test_results/mavros/gtest-libmavros-frame-test.xml
[==========] Running 12 tests from 2 test cases.
[----------] Global test environment set-up.
[----------] 8 tests from VECTOR
[ RUN ] VECTOR.transform_frame_xyz_111
[ INFO] [1435595758.726338027]: In (x y z): 1.000000 1.000000 1.000000
[ INFO] [1435595758.726378237]: Expected: 1.000000 -1.000000 -1.000000
[ INFO] [1435595758.726400802]: Out: 1.000000 -1.000000 -1.000000
[ OK ] VECTOR.transform_frame_xyz_111 (0 ms)
[ RUN ] VECTOR.transform_frame_xyz_pi00
[ INFO] [1435595758.726458002]: In (x y z): 3.141593 0.000000 0.000000
[ INFO] [1435595758.726477923]: Expected: 3.141593 0.000000 0.000000
[ INFO] [1435595758.726496865]: Out: 3.141593 -0.000000 -0.000000
[ OK ] VECTOR.transform_frame_xyz_pi00 (0 ms)
[ RUN ] VECTOR.transform_frame_xyz_0pi0
[ INFO] [1435595758.726540654]: In (x y z): 0.000000 3.141593 0.000000
[ INFO] [1435595758.726560370]: Expected: 0.000000 -3.141593 0.000000
[ INFO] [1435595758.726579140]: Out: 0.000000 -3.141593 -0.000000
[ OK ] VECTOR.transform_frame_xyz_0pi0 (0 ms)
[ RUN ] VECTOR.transform_frame_xyz_00pi
[ INFO] [1435595758.726621050]: In (x y z): 0.000000 0.000000 3.141593
[ INFO] [1435595758.726640458]: Expected: 0.000000 0.000000 -3.141593
[ INFO] [1435595758.726658737]: Out: 0.000000 -0.000000 -3.141593
[ OK ] VECTOR.transform_frame_xyz_00pi (0 ms)
[ RUN ] VECTOR.transform_frame_attitude_rpy_111
[ INFO] [1435595758.726701352]: In (x y z): 1.000000 1.000000 1.000000
[ INFO] [1435595758.726720770]: Expected: 0.000000 -1.000000 -1.000000
[ INFO] [1435595758.726739243]: Out: 4.141593 1.000000 1.000000
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:85: Failure
Value of: out
Actual: 32-byte object <8C-16 22-AA FD-90 10-40 00-00 00-00 00-00 F0-3F 00-00 00-00 00-00 F0-3F 00-00 00-00 00-00 00-00>
Expected: expected_out
Which is: 32-byte object <00-00 00-00 00-00 00-00 00-00 00-00 00-00 F0-BF 00-00 00-00 00-00 F0-BF 00-00 00-00 00-00 00-00>
[ FAILED ] VECTOR.transform_frame_attitude_rpy_111 (0 ms)
[ RUN ] VECTOR.transform_frame_attitude_rpy_pi00
[ INFO] [1435595758.726987526]: In (x y z): 3.141593 0.000000 0.000000
[ INFO] [1435595758.727021779]: Expected: 3.141593 0.000000 0.000000
[ INFO] [1435595758.727040340]: Out: 6.283185 0.000000 0.000000
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:96: Failure
Value of: out
Actual: 32-byte object <18-2D 44-54 FB-21 19-40 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00>
Expected: expected_out
Which is: 32-byte object <18-2D 44-54 FB-21 09-40 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00>
[ FAILED ] VECTOR.transform_frame_attitude_rpy_pi00 (1 ms)
[ RUN ] VECTOR.transform_frame_attitude_rpy_0pi0
[ INFO] [1435595758.727162224]: In (x y z): 0.000000 3.141593 0.000000
[ INFO] [1435595758.727182810]: Expected: 0.000000 -3.141593 0.000000
[ INFO] [1435595758.727201497]: Out: 3.141593 3.141593 0.000000
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:107: Failure
Value of: out
Actual: 32-byte object <18-2D 44-54 FB-21 09-40 18-2D 44-54 FB-21 09-40 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00>
Expected: expected_out
Which is: 32-byte object <00-00 00-00 00-00 00-00 18-2D 44-54 FB-21 09-C0 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00>
[ FAILED ] VECTOR.transform_frame_attitude_rpy_0pi0 (0 ms)
[ RUN ] VECTOR.transform_frame_attitude_rpy_00pi
[ INFO] [1435595758.727323345]: In (x y z): 0.000000 0.000000 3.141593
[ INFO] [1435595758.727343568]: Expected: 0.000000 0.000000 -3.141593
[ INFO] [1435595758.727361578]: Out: 3.141593 0.000000 3.141593
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:118: Failure
Value of: out
Actual: 32-byte object <18-2D 44-54 FB-21 09-40 00-00 00-00 00-00 00-00 18-2D 44-54 FB-21 09-40 00-00 00-00 00-00 00-00>
Expected: expected_out
Which is: 32-byte object <00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 18-2D 44-54 FB-21 09-C0 00-00 00-00 00-00 00-00>
[ FAILED ] VECTOR.transform_frame_attitude_rpy_00pi (0 ms)
[----------] 8 tests from VECTOR (1 ms total)
[----------] 4 tests from QUATERNION
[ RUN ] QUATERNION.transform_frame_attitude_q_111
[ INFO] [1435595758.727580559]: In (x y z w): 0.167519 0.570941 0.167519 0.786067
[ INFO] [1435595758.727607931]: Expected: 0.167519 -0.570941 -0.167519 0.786067
[ INFO] [1435595758.727629748]: Out: 0.786067 0.167519 -0.570941 -0.167519
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:132: Failure
Value of: out
Actual: 32-byte object <A0-D4 13-34 75-27 E9-3F 6A-32 EF-78 41-71 C5-3F A8-2B 6D-0C 27-45 E2-BF 67-32 EF-78 41-71 C5-BF>
Expected: expected_out
Which is: 32-byte object <69-32 EF-78 41-71 C5-3F A8-2B 6D-0C 27-45 E2-BF 69-32 EF-78 41-71 C5-BF A0-D4 13-34 75-27 E9-3F>
[ FAILED ] QUATERNION.transform_frame_attitude_q_111 (0 ms)
[ RUN ] QUATERNION.transform_frame_attitude_q_pi00
[ INFO] [1435595758.727805481]: In (x y z w): 1.000000 0.000000 0.000000 0.000000
[ INFO] [1435595758.727838588]: Expected: 1.000000 0.000000 0.000000 0.000000
[ INFO] [1435595758.727873648]: Out: 0.000000 0.000000 0.000000 -1.000000
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:143: Failure
Value of: out
Actual: 32-byte object <07-5C 14-33 26-A6 A1-3C 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 F0-BF>
Expected: expected_out
Which is: 32-byte object <00-00 00-00 00-00 F0-3F 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 07-5C 14-33 26-A6 91-3C>
[ FAILED ] QUATERNION.transform_frame_attitude_q_pi00 (0 ms)
[ RUN ] QUATERNION.transform_frame_attitude_q_0pi0
[ INFO] [1435595758.727994172]: In (x y z w): 0.000000 1.000000 0.000000 0.000000
[ INFO] [1435595758.728014659]: Expected: 0.000000 -1.000000 0.000000 0.000000
[ INFO] [1435595758.728033967]: Out: 0.000000 0.000000 -1.000000 0.000000
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:154: Failure
Value of: out
Actual: 32-byte object <07-5C 14-33 26-A6 91-3C 07-5C 14-33 26-A6 91-3C 00-00 00-00 00-00 F0-BF 48-5D 8A-85 CE-77 33-39>
Expected: expected_out
Which is: 32-byte object <00-00 00-00 00-00 00-00 00-00 00-00 00-00 F0-BF 00-00 00-00 00-00 00-00 07-5C 14-33 26-A6 91-3C>
[ FAILED ] QUATERNION.transform_frame_attitude_q_0pi0 (1 ms)
[ RUN ] QUATERNION.transform_frame_attitude_q_00pi
[ INFO] [1435595758.728160120]: In (x y z w): 0.000000 0.000000 1.000000 0.000000
[ INFO] [1435595758.728180721]: Expected: 0.000000 0.000000 -1.000000 0.000000
[ INFO] [1435595758.728199710]: Out: 0.000000 1.000000 0.000000 0.000000
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:165: Failure
Value of: out
Actual: 32-byte object <07-5C 14-33 26-A6 91-3C 00-00 00-00 00-00 F0-3F 07-5C 14-33 26-A6 91-3C 48-5D 8A-85 CE-77 33-39>
Expected: expected_out
Which is: 32-byte object <00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 00-00 F0-BF 07-5C 14-33 26-A6 91-3C>
[ FAILED ] QUATERNION.transform_frame_attitude_q_00pi (0 ms)
[----------] 4 tests from QUATERNION (1 ms total)
[----------] Global test environment tear-down
[==========] 12 tests from 2 test cases ran. (2 ms total)
[ PASSED ] 4 tests.
[ FAILED ] 8 tests, listed below:
[ FAILED ] VECTOR.transform_frame_attitude_rpy_111
[ FAILED ] VECTOR.transform_frame_attitude_rpy_pi00
[ FAILED ] VECTOR.transform_frame_attitude_rpy_0pi0
[ FAILED ] VECTOR.transform_frame_attitude_rpy_00pi
[ FAILED ] QUATERNION.transform_frame_attitude_q_111
[ FAILED ] QUATERNION.transform_frame_attitude_q_pi00
[ FAILED ] QUATERNION.transform_frame_attitude_q_0pi0
[ FAILED ] QUATERNION.transform_frame_attitude_q_00pi
8 FAILED TESTS
TSC21
commented
9 years ago GTests are failing cause the expected outputs are wrong. For example in https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L160,
the result should be tf::createQuaternionFromRPY(M_PI, 0, M_PI);
and not tf::createQuaternionFromRPY(0, 0, -M_PI);. Don't mess rotation matrices with resulting RPY values.
TSC21
commented
9 years ago Also you are feeding 1 rad values here: https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L127. Not a problem but values like these should be applied to positions, and not angles.
TSC21
commented
9 years ago Please change outputs in https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L91, https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L102, https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L113, https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L127, https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L138, https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L149
and https://github.com/mavlink/mavros/blob/master/mavros/test/test_frame_conv.cpp#L160. You have to sum M_PI on Roll angle.
TSC21
commented
9 years ago Relatively to M_PI vs boost::math::constants::pi<double>(): https://meetingcpp.com/index.php/br/items/cpp-and-pi.html:
Thats why M_PI or boost can offer an alternative, especially if you don't know pi from your head, so no need to look it up. Boost has another advantage: It works with types of multiple precision, as stated on reddit, and as you can see in the example of boost.multiprecision.
So I think that boost wins in this case, since we already use boost lib in our code. But again, if "like" or "not like" is a valid metric, seems I don't have much of an opinion here.
TSC21
commented
9 years ago Solved with https://github.com/mavlink/mavros/pull/322
vooon
commented
9 years ago Attitude transform are wrong: https://youtu.be/Q1_wX4SEb1A
TSC21
commented
9 years ago why you have the axis upside-down? Are you mistaking ENU and NED representations?
TSC21
commented
9 years ago The same rotation is both applied on rpy and quaternion transformation, and both return exactly the same result. So transform is right, and I have confirmed it with math. Also @blackcoder was able to flight test this transformation (even with POSCTL), so I don't see where this is wrong.
The previous conversion you had was roll, -pitch, -yaw and adding PI to Roll results exactly in the same rotation 'cause (+PI,0,0) results in a rotation matrix with the following form {{1,0,0},{0,-1,0},{0,0,-1}}. If you apply this rotation to {roll, pitch, yaw}, you get {roll, -pitch, -yaw}.
vooon
commented
9 years ago why you have the axis upside-down?
Good question to You! I replaced attitude math with xyz and got normal position of axes. With your math pitch and yaw has wrong sign, you may notice that in GCS nose upper horizon, while in rviz down.
vooon
commented
9 years ago So simple add 180° to roll DO NOT WORK. If you want do rotation, do rotation matrix and apply it.
vooon
commented
9 years ago And i'm going to release in current state, without that math. While transforms on "local" (previous y x -z) changed to common x -y -z, i think that it is better, than completely broken system.
TSC21
commented
9 years ago Well that's unexpected... I wonder how @blackcoder got it to work then
TSC21
commented
9 years ago So the current transforms are then x,-y,-z,roll,-pitch,-yaw?
TSC21
commented
9 years ago Ok now i know the reason why it worked with Tarek: mocap plugin uses quaternions and the rotation on roll is transformed into a quaternion which acts as a rotation to the other one. Multiplying both results on the correct transformation. So my mistake on this, as I thougth summing the rotation was similar to applying the rotation matrix which is obviously wrong. Thanks for finding that out @vooon.
TSC21
commented
9 years ago Why you changed quaternions transform too @vooon? Those are right cause quaternions act as a form of rotation... So th PI on Roll is converted to quaternion, and that math is right!
vooon
commented
9 years ago Here covariance transform with eigen: https://github.com/ros-perception/imu_pipeline/blob/indigo-devel/imu_transformer/include/imu_transformer/tf2_sensor_msgs.h#L69-L75
@TSC21 math looks exact what you statement :)
TSC21
commented
9 years ago Here covariance transform with eigen: https://github.com/ros-perception/imu_pipeline/blob/indigo-devel/imu_transformer/include/imu_transformer/tf2_sensor_msgs.h#L69-L75
That's exactly what I need :D thanks for pointing that @vooon. Now we need Eigen on business...
vooon
commented
9 years ago Can you please run imu test that i commited in test_package branch?
TSC21
commented
9 years ago Can you please run imu test that i commited in test_package branch?
Hi will, but I'm out of bandwidth now :s
Note: What imu test are you referring too? Can't see any new commit on test_package...
vooon
commented
9 years ago APM SITL + mavros: https://youtu.be/mUIptiNbmS4
Here tested base plugins.
vooon
commented
9 years ago @TSC21 please check. I wrote simple eigen-based transforms. Vector's one seems ok, while quaternion require check.
There is no function similar to tf::quaternionFromRPY(), and use Eigen::AxisAngle() in different order gives different result.
We should check that order.
vooon
commented
9 years ago Added a test, and seems that right order is same as in MIRA quaternionFromYawPitchRoll()
vooon
commented
9 years ago Almost all test implemented, except UAS::transform_frame(Covariance6d) (not implemented).
vooon
commented
9 years ago All used transforms have tests. Closing.
Wrote part, got error: