ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

how to measure an angle?

asked 2012-01-17 10:48:39 -0500

maruchi gravatar image

updated 2012-01-23 12:52:38 -0500


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 ?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2012-01-17 12:21:44 -0500

rado0x54 gravatar image

updated 2012-01-17 21:20:01 -0500

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 very nice document. Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)

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);

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.

edit flag offensive delete link more


I edited the original question. Could you check it ?
maruchi gravatar image maruchi  ( 2012-01-23 12:53:16 -0500 )edit

answered 2012-01-17 13:18:35 -0500

ahendrix gravatar image

Take a look at the getYaw() function in tf:

In general, tf has a couple of handy functions for working with quaternions that represent angles in the plane.

edit flag offensive delete link more


I edited the original question. Could you check it?
maruchi gravatar image maruchi  ( 2012-01-23 12:53:44 -0500 )edit

Question Tools



Asked: 2012-01-17 10:48:39 -0500

Seen: 2,971 times

Last updated: Jan 23 '12