Get one(pitch) euler angle between two orientations (quaternions)
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( ... );
}