Compute roll, pitch and yaw from 3D vector

edit

First of all, there exist multiple Euler Angle conventions.

URDF and SDF use the X-Y-Z (Tait–Bryan) Extrinsic Euler Angle convention - 3 consecutive rotations around the X, Y, and Z coordinate axes of the original (fixed) coordinate system. [1]

There are several gotcha edge cases when dealing with Euler Angles, so I recommend using a library like Christoph Gohlke's transformations.py (https://www.lfd.uci.edu/~gohlke/code/...).

Try something like this (untested):

• Assume you have two unit vectors, vector a lies on axis of the previous cylinder, and vector b lies on the axis of the current cylinder.
• Assume that length(a)==length(b) > 0.
• Calculate the cross product of a and b: v = a x b. v is the axis of rotation between a and b [2]. If a and b are colinear, this will fail but that means they lie on the same axis and there's no rotation between them (technically, there could be a 180 degree rotation but let's assume that no branches go 'backwards').
• Calculate the dot product of a and b: cos(angle)=dot(a, b)/(length(a)+length(b)). The result is the cosine of the angle of rotation. [2]
• Calculate the acos of cos(angle) from previous step: angle=acos(cos(angle)). angle is the angle of rotation. [2]
• Now, you have an axis angle representation of a rotation between a and b.
• Using the transformations.py library, you can calculate X-Y-Z extrinsic Euler Angles from the axis angle representation.
  • Given axis of rotation v and angle of rotation a.
  • R = rotation_matrix(a, v):
  • x, y, z = euler_from_matrix(R, axes='sxyz'):

References:

• [1] Euler Angles: https://en.wikipedia.org/wiki/Euler_a...
• [2] Euler angles between two 3d vectors: https://stackoverflow.com/a/15109028/...