Ask Your Question
0

how to measure an angle?

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

maruchi gravatar image

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

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 ?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

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

edit flag offensive delete link more

Comments

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

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

ahendrix gravatar image

Take 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.

edit flag offensive delete link more

Comments

I edited the original question. Could you check it?
maruchi gravatar imagemaruchi ( 2012-01-23 12:53:44 -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: 2012-01-17 10:48:39 -0500

Seen: 2,097 times

Last updated: Jan 23 '12