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.0Mon, 23 Jan 2012 12:53:44 -0600how to measure an angle?https://answers.ros.org/question/12668/how-to-measure-an-angle/Dear,
I want to measure yaw angle (radian). What command should I use ?
In my c++ file, I used like this;
double q0 = odom->pose.pose.orientation.w;
double q1 = odom->pose.pose.orientation.x;
double q2 = odom->pose.pose.orientation.y;
double q3 = odom->pose.pose.orientation.z;
double omega = odom->twist.twist.angular.z;
double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
This yaw angle produce only between -pi and pi.
But I want to measure directly the yaw angle in radian without the limitation of -pi and +pi, that is, measure the counter-clockwise one and half circling as +3 pi and the clockwise two circling as -4*pi.
One alternative can be coded this way:
double omega = odom->twist.twist.angular.z;
double delta_psi = omega*dt;
psi += delta_psi;
But the heading angle psi might accumulate some integration error. Do I have to implement more accurate integration method ?Tue, 17 Jan 2012 10:48:39 -0600https://answers.ros.org/question/12668/how-to-measure-an-angle/Answer by rado0x54 for <p>Dear,</p>
<p>I want to measure yaw angle (radian). What command should I use ?
In my c++ file, I used like this;</p>
<pre><code> double q0 = odom->pose.pose.orientation.w;
double q1 = odom->pose.pose.orientation.x;
double q2 = odom->pose.pose.orientation.y;
double q3 = odom->pose.pose.orientation.z;
double omega = odom->twist.twist.angular.z;
double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
</code></pre>
<p>This yaw angle produce only between -pi and pi.
But I want to measure directly the yaw angle in radian without the limitation of -pi and +pi, that is, measure the counter-clockwise one and half circling as +3 pi and the clockwise two circling as -4*pi. </p>
<p>One alternative can be coded this way:</p>
<pre><code> double omega = odom->twist.twist.angular.z;
double delta_psi = omega*dt;
psi += delta_psi;
</code></pre>
<p>But the heading angle psi might accumulate some integration error. Do I have to implement more accurate integration method ?</p>
https://answers.ros.org/question/12668/how-to-measure-an-angle/?answer=18694#post-id-18694To derive Euler Angles from Quaternions, you need to know (or define) the sequence of rotations around the coordinate axes. You can only get the Yaw in this context. Take a look at this **very** nice document. [Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)](http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134)
Otherwise I'd recommend to make use of a linear algebra library (e.g Eigen). Conversion can be done in one line:
Eigen::Quaterniond orientation(w,x,y,z);
orientation.toRotationMatrix().eulerAngles(0,1,2)
EDIT: I looked at the conversion formulas for all the angle sequences and could not find anything similar to your calculation. Should rather look similar to this (seq: 0,1,2):
atan2(2xy + 2wz, w*w + x*x - y*y - z*z)
compare to Formula (290) in linked document.
EDIT: Actually your formula is correct, because the norm of the quaternion is always 1:
w*w + x*x - y*y - z*z = 1-2*(y*y+z*z)
Output should be between -PI and PI.Tue, 17 Jan 2012 12:21:44 -0600https://answers.ros.org/question/12668/how-to-measure-an-angle/?answer=18694#post-id-18694Comment by maruchi for <p>To derive Euler Angles from Quaternions, you need to know (or define) the sequence of rotations around the coordinate axes. You can only get the Yaw in this context. Take a look at this <strong>very</strong> nice document. <a href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134">Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)</a></p>
<p>Otherwise I'd recommend to make use of a linear algebra library (e.g Eigen). Conversion can be done in one line:</p>
<pre><code>Eigen::Quaterniond orientation(w,x,y,z);
orientation.toRotationMatrix().eulerAngles(0,1,2)
</code></pre>
<p>EDIT: I looked at the conversion formulas for all the angle sequences and could not find anything similar to your calculation. Should rather look similar to this (seq: 0,1,2):</p>
<pre><code>atan2(2xy + 2wz, w*w + x*x - y*y - z*z)
</code></pre>
<p>compare to Formula (290) in linked document.</p>
<p>EDIT: Actually your formula is correct, because the norm of the quaternion is always 1:</p>
<pre><code>w*w + x*x - y*y - z*z = 1-2*(y*y+z*z)
</code></pre>
<p>Output should be between -PI and PI.</p>
https://answers.ros.org/question/12668/how-to-measure-an-angle/?comment=19838#post-id-19838I edited the original question. Could you check it ?Mon, 23 Jan 2012 12:53:16 -0600https://answers.ros.org/question/12668/how-to-measure-an-angle/?comment=19838#post-id-19838Answer by ahendrix for <p>Dear,</p>
<p>I want to measure yaw angle (radian). What command should I use ?
In my c++ file, I used like this;</p>
<pre><code> double q0 = odom->pose.pose.orientation.w;
double q1 = odom->pose.pose.orientation.x;
double q2 = odom->pose.pose.orientation.y;
double q3 = odom->pose.pose.orientation.z;
double omega = odom->twist.twist.angular.z;
double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
</code></pre>
<p>This yaw angle produce only between -pi and pi.
But I want to measure directly the yaw angle in radian without the limitation of -pi and +pi, that is, measure the counter-clockwise one and half circling as +3 pi and the clockwise two circling as -4*pi. </p>
<p>One alternative can be coded this way:</p>
<pre><code> double omega = odom->twist.twist.angular.z;
double delta_psi = omega*dt;
psi += delta_psi;
</code></pre>
<p>But the heading angle psi might accumulate some integration error. Do I have to implement more accurate integration method ?</p>
https://answers.ros.org/question/12668/how-to-measure-an-angle/?answer=18695#post-id-18695Take a look at the getYaw() function in tf: http://www.ros.org/doc/api/tf/html/c++/namespacetf.html#a33e93f413622e296d0a2a93b252bbb4b
In general, tf has a couple of handy functions for working with quaternions that represent angles in the plane.Tue, 17 Jan 2012 13:18:35 -0600https://answers.ros.org/question/12668/how-to-measure-an-angle/?answer=18695#post-id-18695Comment by maruchi for <p>Take a look at the getYaw() function in tf: <a href="http://www.ros.org/doc/api/tf/html/c++/namespacetf.html#a33e93f413622e296d0a2a93b252bbb4b">http://www.ros.org/doc/api/tf/html/c++/namespacetf.html#a33e93f413622e296d0a2a93b252bbb4b</a></p>
<p>In general, tf has a couple of handy functions for working with quaternions that represent angles in the plane.</p>
https://answers.ros.org/question/12668/how-to-measure-an-angle/?comment=19837#post-id-19837I edited the original question. Could you check it?Mon, 23 Jan 2012 12:53:44 -0600https://answers.ros.org/question/12668/how-to-measure-an-angle/?comment=19837#post-id-19837