| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
We are trying to get the heading of our turtlebot. We can get the Quaternion data about the robot's pose but want to convert to a simple euler angle. Is there C++ ROS code that is already available that supports this?
Yes, there is. Using the standard tf libraries:
tf:Quaternion q(x,y,z,w);
double yaw,pitch,roll;
btMatrix3x3(q).getEulerYPR(yaw,pitch,roll);
Yaw is what you want.
It's funny you should ask this question. I have been looking into something similar for a quadrotor. Specifically I want to extract the yaw rotation from a quaternion for some simple visual servoing. At this point I believe this can be done by the following procedure:
1) Set the x and y components of the quaternion to 0 2) Normalize the quaternion. In pseudo-code
mag = sqrt(q[0]^2 + q[4]^2)
q[0] /= mag
q[1] /= mag
3) Convert the quaternion to a rotation matrix. Again in pseudo-code (as per the Rotation matrices section of Conversion between quaternions and Euler angles -- Wikipedia
R[0][0] = q[0]^2 + q[1]^2 - q[2]^2 - q[3]^2
R[1][2] = 2*(q[1]*q[2] - q[0]*q[3])
R[1][3] = 2*(q[1]*q[3] + q[0]*q[2])
R[2][1] = 2*(q[1]*q[2] + q[0]*q[3])
R[2][2] = q[0]^2 - q[1]^2 + q[2]^2 - q[3]^2
R[2][3] = 2*(q[2]*q[3] - q[0]*q[1])
R[3][1] = 2*(q[1]*q[3] - q[0]*q[2])
R[3][2] = 2*(q[2]*q[3] + q[0]*q[1])
R[3][3] = q[0]^2 - q[1]^2 - q[2]^2 + q[3]^2
If you actually want all the Euler angles then the Quaternion section of the Rotation Matrix -- Wikipedia article may be helpful. However beware that the angles you get out may not be accurate due to the shortcomings of Euler angles. For example, if you create a rotation matrix R = RotZ(pi/2)*RotY(pi/2), convert it to a quaternion and convert it back you will get RotY(pi/2). I.e. the process doesn't return pi/2 for the rotation about z.
Asked: 2012-03-31 09:56:35 -0600
Seen: 3,466 times
Last updated: Apr 01 '12
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
