0
$\begingroup$

Rosanswers logo

There are two orientations given, as intrinsic yaw-roll-pitch (rzyx).

How to calculate the angle between the "Z" axes of them?

Converting the orientations to quaternions or to anything else is allowed.


Originally posted by okalachev on ROS Answers with karma: 11 on 2018-08-29

Post score: 0


Original comments

Comment by gvdhoorn on 2018-08-29:
Does #q10124 not answer this? See also tf2/Tutorials/Quaternions.

Comment by okalachev on 2018-08-29:
Looks like it's the finding of the full rotation between orientations. What I need, it the single angle between the Z axes of orientations.

Comment by stevemacenski on 2018-08-29:
I'd look into the axis angle representation. Alternatively, what you're describing could be interpreted as angular distance which you can formulate the same way you might an L2 distance.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I'd suggest converting both of the orientations into transformation objects, then create a point object 1m along the Z axis. Then use the two transformations to transform the point from the frame of one transform into the other. This will give you two unit vectors one if which is (0 0 1)^ the other which is something else, you can then use the dot product to find the angle between the two.

In a little bit more detail you'd create the two transform objects like this.

tf2::Quaternion q1;
q1.setRPY(roll, pitch, yaw); // node the order
tf2::Transform t1(q1);

Do the same with q2 and t2 for the other orientation then create the Z axis unit point.

tf2::Vector3 zPoint(0,0,1);

Transform it from one frame to the other.

tf2::Vector3 otherZPoint = t1.inverseTimes(t2) * zPoint;

Now you can calculate the angle between the two unit vectors zPoint and otherZPoint as so.

float angle = acos(zPoint.dot(otherZPoint) );

It's a bit long winded but I hope this solves the problem you're asking about!


Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-08-29

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by okalachev on 2018-08-30:
Yes, I think this is a working variant!

Isn't there an easier solution, without converting to transforms? I was given this formula: math.atan(math.hypot(math.tan(pitch), math.tan(roll))), it almost works, but doesn't give values more than 90 degrees (when Z axes are "upside down" to each other).

Comment by PeteBlackerThe3rd on 2018-08-30:
I'm sure it could be simplified, the question is do you need to? Is performance critical for this calculation? If not and you've got a working solution then I'd say the job's done.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.