search

jsk-ros-pkg / jsk_aerial_robot

The platfrom for aerial robot (e.g. general multirotor, hydrus, di, dragon, etc)
http://www.jsk.t.u-tokyo.ac.jp/index-j.html
34 stars 34 forks source link

[Navigation][Trajectory] solve the discontinued problem of yaw rotation #634

Open Liyunong20000 opened 1 week ago

Liyunong20000 commented 1 week ago

What is this

The yaw angle has the discontinued problem around PI. This PR solve the wrong rotation path in trajectory generation, that is, the trajectory must choose the shortest path.

Details

This is achieved by checking diff angle with previous yaw angle, and add an offset to remove a big gap 

if (diff > M_PI) yaw_angle -= 2 * M_PI;
if (diff < -M_PI) yaw_angle += 2 * M_PI;
tongtybj commented 6 days ago

@Liyunong20000

Have you confirmed that this patch solve your problem with the real robot?

tongtybj commented 2 days ago

@Liyunong20000

Have you reproduced the corner case?

sugikazu75 commented 2 days ago

@Liyunong20000 FYI: You can use angles::normalize_angle https://docs.ros.org/en/jade/api/angles/html/namespaceangles.html#a90ae8baf394c3ce775a3a14e48c7e7be

tongtybj commented 14 hours ago

@sugikazu75

Thx for your suggenstion! That's the case for https://github.com/jsk-ros-pkg/jsk_aerial_robot/pull/634/files#diff-9c1ce395efa7ef3a077e6e1471de383649423c7453dd7c227c41adf5fd490eb8R1006-R1007.

But for https://github.com/jsk-ros-pkg/jsk_aerial_robot/pull/634/files#diff-c6a4f3e29ec6e7e00ea548e81f405d43d9fe45cd6776cf42c285133d81122b06R124-R125, the purpose is now to normalize yaw_angle, but to find the closest value from the previous yaw angle. So the modified yaw_angle might be out of the range of (-pi, pi).