setpoint_position plugin sets yaw incorrectly

[madsciencetist]

When I use the setpoint_position plugin to command a pose, the orientation does not behave as expected. To test, I am using the "2D Nav Goal" button in rviz with /move_base_simple/goal mapped to /mavros/setpoint_position/local. When I command a North heading, a quaternion of (0,0,0,1) is sent, which mavros converts to a yaw of zero, as expected. As I increase the commanded heading clockwise to South, the quaternion approaches (-1,0,0,0), which converts to a yaw of Pi, as expected. Any further though, and the angle goes to zero. Observe:

[/mavros]: q = (-0.999456, 0.000000, -0.000000, -0.032994) -> yaw = 3.075592
[/mavros]: q = (-0.999550, 0.000000, -0.000000, 0.029986) -> yaw = 0.059980

Thus, yaw setpoints can only be commanded in the [0,pi] range, as the other half of the circle will also map to [0,pi]. The problem must lie in UAS::quaternion_to_rpy() in uas_frame_conversions.cpp, but I'm not sure what the right answer is.

[vooon]

I wrote tests and may confirm that q->rpy results is strange.
@TSC21 may you check math please?

vovan@robert:~/ros/src/mavros/mavros$ catkin run_tests --this -j1
...
[==========] Running 10 tests from 1 test case.
[----------] Global test environment set-up.
[----------] 10 tests from UAS
[ RUN      ] UAS.transform_frame__vector3d_123
[       OK ] UAS.transform_frame__vector3d_123 (0 ms)
[ RUN      ] UAS.transform_frame__quaterniond_123
[       OK ] UAS.transform_frame__quaterniond_123 (0 ms)
[ RUN      ] UAS.transform_frame__covariance3x3
[       OK ] UAS.transform_frame__covariance3x3 (0 ms)
[ RUN      ] UAS.quaternion_from_rpy__check_compatibility
[       OK ] UAS.quaternion_from_rpy__check_compatibility (0 ms)
[ RUN      ] UAS.quaternion_from_rpy__paranoic_check
[       OK ] UAS.quaternion_from_rpy__paranoic_check (0 ms)
[ RUN      ] UAS.quaternion_to_rpy__123
[       OK ] UAS.quaternion_to_rpy__123 (0 ms)
[ RUN      ] UAS.quaternion_to_rpy__123_negative
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:151: Failure
The difference between -1.0 and rpy.x() is 3.1415926535897936, which exceeds epsilon, where
-1.0 evaluates to -1,
rpy.x() evaluates to 2.1415926535897936, and
epsilon evaluates to 1.0000000000000001e-09.
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:152: Failure
The difference between -2.0 and rpy.y() is 0.85840734641020666, which exceeds epsilon, where
-2.0 evaluates to -2,
rpy.y() evaluates to -1.1415926535897933, and
epsilon evaluates to 1.0000000000000001e-09.
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:153: Failure
The difference between -3.0 and rpy.z() is 3.1415926535897931, which exceeds epsilon, where
-3.0 evaluates to -3,
rpy.z() evaluates to 0.141592653589793, and
epsilon evaluates to 1.0000000000000001e-09.
[  FAILED  ] UAS.quaternion_to_rpy__123_negative (0 ms)
[ RUN      ] UAS.quaternion_to_rpy__check_compatibility
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:173: Failure
The difference between bt_roll and eigen_rpy.x() is 3.1415926535897931, which exceeds epsilon, where
bt_roll evaluates to -2.1415926535897931,
eigen_rpy.x() evaluates to 1, and
epsilon evaluates to 1.0000000000000001e-09.
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:174: Failure
The difference between bt_pitch and eigen_rpy.y() is 0.85840734641020644, which exceeds epsilon, where
bt_pitch evaluates to 1.1415926535897933,
eigen_rpy.y() evaluates to 1.9999999999999998, and
epsilon evaluates to 1.0000000000000001e-09.
/home/vovan/ros/src/mavros/mavros/test/test_frame_conv.cpp:175: Failure
The difference between bt_yaw and eigen_rpy.z() is 3.1415926535897931, which exceeds epsilon, where
bt_yaw evaluates to -0.14159265358979309,
eigen_rpy.z() evaluates to 3, and
epsilon evaluates to 1.0000000000000001e-09.
[  FAILED  ] UAS.quaternion_to_rpy__check_compatibility (0 ms)
[ RUN      ] UAS.getYaw__123
[       OK ] UAS.getYaw__123 (0 ms)
[ RUN      ] UAS.quaternion_to_mavlink__123
[       OK ] UAS.quaternion_to_mavlink__123 (0 ms)
[----------] 10 tests from UAS (1 ms total)

[----------] Global test environment tear-down
[==========] 10 tests from 1 test case ran. (1 ms total)
[  PASSED  ] 8 tests.
[  FAILED  ] 2 tests, listed below:
[  FAILED  ] UAS.quaternion_to_rpy__123_negative
[  FAILED  ] UAS.quaternion_to_rpy__check_compatibility

[TSC21]

Ok I'll check it.

[vooon]

@madsciencetist i hope that your data in wxyz format.
First q = yaw 356°, so it should be almost 2π. If i swap first and last elements roll 183°.
Second looks similar.

Also please tell it is before transform_frame_enu_ned?

[TSC21]

@vooon does this need .reverse()?

[vooon]

@TSC21 Without reverse getYaw test failed. But now i see that to_rpy gives incorrect result in other cases.
Interesting that (1 2 3)->tf2 q->(r p y) give different result from eigen, while (-1 -2 -3)->tf2 q->(r p y) equal.

But first i'll do #359.

[TSC21]

Well probably we are getting problems with singularities or something. I'm still trying to figure out what does this do?

[vooon]

Look stack overflow and eigen doc.

Because real order of RPY->Q is YPR i assumed that right is 2 1 0.

[madsciencetist]

@vooon My printout was (x,y,z,w) after transform_frame_enu_ned in send_position_target

[vooon]

@madsciencetist Could you please test master. I think that i fixed, unit-tests now are passed.

[LorenzMeier]

@vooon Have you still that Wiki algorithm in? I wouldn't be so sure if that doesn't have corner cases (numerically). The mavlink library contains a unit-tested conversion already, make sure to reuse it whenever possible:
https://github.com/mavlink/c_library/blob/master/mavlink_conversions.h#L146

[TSC21]

I'm kinda worried that the current test implemented is forcing the function to be "right" instead of getting the right euler angles. Corner cases were always a problem of euler angles and that's why probably Eigen developers changed the normalized range values that eulerAngles() method gives. My doubt is: should we normalize the values fed to the conversion method or normalize the output? Will we want to work in -PI..PI range or 0..2PI?

[vooon]

@LorenzMeier for now i used Eigen::Matrix3d::eulerAngles(2, 1, 0) for getting rpy vector and equation from wikipedia for UAS::quaternion_get_yaw().

Normalization in eulerAngles give us different angles, than passed to rpy->q.
And when i passed that values to rpy->q again i got complimentary quaternion.
So for check i do rotation of other q and then compare result. I can't be sure that it good from math's side of view, but >16k of iterations looks ok.

test_quaternion_utils.cpp L103

                auto tq1 = q1 * test_orientation * q1.inverse();
                auto tq2 = q2 * test_orientation * q2.inverse();

                EXPECT_QUATERNION(tq1, tq2, epsilon);

[vooon]

Using of mavlink's helpers little problematic because of mixing single and double precision.
But need to write a test...

[madsciencetist]

@vooon Tested master, confirmed that angles around the level circle are now mapped properly to [-pi,pi]. Thanks!

[vooon]

Thanks.