tf::Quaternion getAngle() eqivalent for rospy [closed]

edit

I don't know if there's a built in function to do this. But the relationship between the w element of a quaternion and the angle it's rotating by is fairly simple.

w = cos(angle / 2)

therefore:

angle = 2 x arccos(w)

So you can calculate this yourself using this equation from the w element.

Update:

This is mathematically equivalent to the more complex form shown on the Wikipedia page here. See their curves here:

angle = 2 x arccos(w))

angle = 2*atan2(sqrt(1-w^2), w),+w))

Note: I've replaced sqrt(qi^2 + qj^2 + qk^2) with sqrt(1-qr^2) since this expression can more easily be calculated using just the w value.