Ask Your Question
1

Get one(pitch) euler angle between two orientations (quaternions)

asked 2014-09-05 07:26:45 -0500

ToXeR gravatar image

Hi,

I want to get the angle between 2 quaternions (as euler angle), but the angle always needs to be from q1 to q2 (not the shortest angle). But since there are two ways to get an angle, I need to find the one I need. The following code works, but I'm looking for a nicer solution.

// calculate difference
ori_diff = q1.inverse() * q1;

//just use the pitch part
ori_diff.setValue(0., ori_diff.y(), 0., ori_diff.w());
ori_diff.normalize();

//get euler angle
tf::Matrix3x3 m;
m.setRotation(ori_diff);

tfScalar r1, r2, p1, p2, y1, y2;
m.getRPY(r1, p1, y1, 1);
m.getRPY(r2, p2, y2, 2);

if (        r1 == 0 && y1 == 0 ) {
  euler_angle = p1;
} else if ( r2 == 0 && y2 == 0) {
  euler_angle = p2;
} else {
  ROS_ERROR( ... );
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-11-07 03:51:27 -0500

ToXeR gravatar image

In the angles.h from the angles package one can found a lot of functions.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-09-05 07:26:45 -0500

Seen: 1,506 times

Last updated: Nov 07 '18