ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Tue, 23 Dec 2014 12:24:52 -0600How to correct the "yaw" behavior of a quadrotor when crossing the angles' limits?!?!https://answers.ros.org/question/199989/how-to-correct-the-yaw-behavior-of-a-quadrotor-when-crossing-the-angles-limits/Hi guys,
My quadrotor has a strange behavior ([video](https://www.youtube.com/watch?v=Y6Trr7d4iro)) when it turns around 180° (the returned value of atan2()). After investigating for hours I 'm pretty sure I found what the problem.
Referring to this [tutorial](http://wiki.ros.org/urdf/Tutorials/Using%20urdf%20with%20robot_state_publisher) as a reference I realized that the angle around the vertical axis (which in my quadrotor is exactly the same structure, see my code [here](http://pastebin.com/dzRp42m3) from line 34) , in the tutorial expressed as *angle* :
tf::createQuaternionMsgFromYaw(angle+M_PI/2);
is simply added by a small quantity calculated at each pass and doesn't take care of the fact that for angles bigger than 180° (or smaller than -180°) the corresponding quaternion has the same value 8or the same orientation if you prefer). In other words, the method `tf::createQuaternionMsgFromYaw` is going to calculate the correct value even if the angles is crossing those *limits*.
The nagle in my application jumps about the Z-axis from the positive sector to the negative (and viceversa) because the returned value from atan2() is outputting *right* values but with the *wrong* interpretation. As you can see, in the following output (all values are in degree for simplify):
...
Yaw: -164.3665
Yaw: -174.0903
Goal reached
Moving to the 3 waypoint
Yaw: 172.8751
Yaw: 161.7627
Yaw: 153.9304
Yaw: 148.2633
Yaw: 144.0085
...
is clear that the quadrotor must move its nose from -174° to + 172°. But instead of to take the shortest way to to that, it turns counterclockwise along the longest path.
I think that this is the normal behavior of `tf::createQuaternionMsgFromYaw()` which doesn't consider the shortest way to reach the next angle, but it output the corresponding quaternion to that value 8could I have a confirmation about that?)
**Now:** one could say: "get rid of an absolut coordinate system for your quadrotor and calculate the angle about the Z-axis as a variable, which is going to be changed by small increments every time your code runs a loop...exact like in the tutorial you mentioned!!!"
That's what I thought at the beginning, but I have the problem that all the position and attitude controller for quadrotor are using a fixed coordinate system for their calculation:
![image description](/upfiles/1419358451960307.png)
Resume:
I want to correct the behavior of my quadrotor about the Z-axis.
To do that I could:
- Rewrite all my controllers for attitude and position to use a body coordinate system and treating the yaw a variable which increments by small changes in orientation without taking care about the limits I said above -> I ve really no idea how to rewrite my code to use such a *variable change* and I think it would lead to new problems;
- Use two "yaws": one absolut for the controllers and the other one "locally" which is going to be passed to the method `tf::createQuaternionMsgFromYaw()`. As above...I ve no idea how to implement it in my code;
What would you do guys? Could you me point to the right direction or give my the most used solution?
PS: Even mapping the output of atan2() to 0-360° doesn't change anything. It *moves* simply the problem between 0 and 360°
I hope my question is clear. If not tell me please and I'll update the question.
Merry Christmas !!
RegardsAndromedaTue, 23 Dec 2014 12:24:52 -0600https://answers.ros.org/question/199989/