Ask Your Question
0

Odometry orientation in radians is giving me weird numbers

asked 2022-05-18 16:35:12 -0500

pitosalas gravatar image

updated 2022-05-19 08:02:23 -0500

gvdhoorn gravatar image

I am trying to better understand the information I am receiving from odometry.

oquat = msg.pose.pose.orientation
_, _, yaw = euler_from_quaternion([oquat.x, oquat.y, oquat.z, oquat.w])

I expect yaw to be a number between 0 and 2*pi or maybe between -pi and +pi. It doesn't seem to be doing that which means I am misunderstanding what yaw means in this context. Can you help me better understand? Here is the data for one full 360 degree rotation for a Turtlebot3 in Gazebo.

image description

edit retag flag offensive close merge delete

Comments

@pitosalas: please restore your original question.

The fact you made a mistake which resulted in what you reported is not sufficient reason to remove your question.

There's a good chance others could have made similar mistakes, and they would still be helped by seeing how you diagnosed and eventually found a solution.

I would recommend you revert the edit to your OP and post your solution as an answer. That will clearly show your question was answered.

gvdhoorn gravatar image gvdhoorn  ( 2022-05-19 08:01:46 -0500 )edit

2 Answers

Sort by » oldest newest most voted
1

answered 2022-05-19 13:56:31 -0500

pitosalas gravatar image

Hi @gvdhoorn! Ok it looks like you restored it for me. My bug was that in trying to understand better what was happening I had inserted a one line "normalization" function which had a bug in it. I removed the call to the normalization function call and odom rotation data, converted to yaw, made sense once again.

How did I solve this? By asking questions here, using the formula from wikipedia and noticing the same exact results as euler_from_quaternion. I also added print statements to generate the raw results in csv format so that i could easily load them into excel to see the graphs.

edit flag offensive delete link more
1

answered 2022-05-19 01:06:46 -0500

ljaniec gravatar image

I found this part on the Wikipedia (Quaternion to Euler angles conversion) which could explain it:

Note, however, that the arctan and arcsin functions implemented in computer languages only produce results between −π/2 and π/2, and for three rotations between −π/2 and π/2 one does not obtain all possible orientations. To generate all the orientations one needs to replace the arctan functions in computer code by atan2: (...)

But I think the problem in your case is different - this function takes [w, x, y, z] and you have [oquat.x, oquat.y, oquat.z, oquat.w]:

"""
Converts quaternion (w in last place) to euler roll, pitch, yaw
quaternion = [w, x, y, z]
"""
edit flag offensive delete link more

Comments

I saw that bit of documentation too. But I think the doc is wrong. Because when I change the order of params i as you suggest I get totally nonsense results. I wonder whether this is a tf vs tf2 issue?

pitosalas gravatar image pitosalas  ( 2022-05-19 05:29:34 -0500 )edit

I would implement the equation for the yaw angle from Wikipedia to be sure that this is not a problem arising from the tf / tf2. Checking the calculations with euler_from_quaternion and quaternion_from_euler with known RPY around Z axis should clear this up. Also, what exactly does "nonsense results" mean? I am using this snippet of code:

euler = euler_from_quaternion(
    [
        odometry_pose.orientation.w,
        odometry_pose.orientation.x,
        odometry_pose.orientation.y,
        odometry_pose.orientation.z,
    ]
)

without any problems...

ljaniec gravatar image ljaniec  ( 2022-05-19 06:42:52 -0500 )edit

I attempted to copy the code from Wikipedia and it matched what the Euler_from_quaternion said. I guess nonsense numbers depends on whether I have good sense. What I mean is that (lookk at the graph) it looks like if I rotate a robot 360 degrees around the z axis the quaternion reported from ODOM converts to a yaw angle that goes from -pi2 at "12 o'clock" to zero at "9 o'clock" to +pi/2 at 6, then jumps right back to -pi/2, followed by zero again at 3 to +pi/2 at 1 second after midnight and back to -pi/2. That's what I am seeing. I might have gotten something wrong, but I have double checked.

pitosalas gravatar image pitosalas  ( 2022-05-19 06:48:23 -0500 )edit

Hold on, I found a line in my code that is wrong and causing the results! Sorry, I will retract the question!

pitosalas gravatar image pitosalas  ( 2022-05-19 06:52:26 -0500 )edit

Usually, even when the solution is a fixed bug in the code, you want to leave the question and answers as they may be helpful to future readers. You should just add the information in an edit, upvote/accept the helpful comments or answers, and close the question rather than delete it.

ljaniec gravatar image ljaniec  ( 2022-05-19 07:44:52 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2022-05-18 16:35:12 -0500

Seen: 41 times

Last updated: May 19