Home | Tutorials | Wiki | Issues
Ask Your Question

Revision history [back]

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/transformations.py.html).


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_angles
  • [2] Euler angles between two 3d vectors: https://stackoverflow.com/a/15109028/8670609