# Compute roll, pitch and yaw from 3D vector

Hi !

I'm trying to build SDF tree models from a description file. The branches are modeled by cylinders. For each cylinder, I have : x, y, z : coords of the cylinder, vx, vy, vz : vector supporting the cylinder.

Since the <pose> takes roll, pitch and yaw angles to build a cylinder, I have to compute them from the direction vector (vx, vy, vz).

Here's how I do it, using numpy:

roll = np.arctan2(vz, vy)

pitch = np.arctan2(vx, vz)

yaw = np.arctan2(vy, vx)

I'm not getting the results I want, some of the branches are clearly misplaced. I noticed that Gazebo places a cylinder along the z axis if you provide yaw, pitch, roll = 0, so I tried to rotate by 90° on the x axis like so :

roll = np.arctan2(vz, vy) + 1.5708

without success.

If you have any advice on how to get these Euler angles right, feel free to share !

Have a nice day.

edit retag close merge delete

Sort by » oldest newest most voted

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:

more