Home | Tutorials | Wiki | Issues
Ask Your Question
0

Compute roll, pitch and yaw from 3D vector

asked 2019-03-24 08:48:23 -0500

Loic gravatar image

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 flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2019-03-27 10:48:12 -0500

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:

edit flag offensive delete link more

Comments

Do you have a source for the claim that Gazebo uses X-Y-Z Extrinsic Euler Angle convention? I'm using Gazebo 7 and it looks intrinsic to me: if I add a cube as link to a model, and put 0,785398 (45 degrees) in the pose -> roll field, it rotates around the global (and local) X axis; if I then add 0,785398 to the pose -> pitch axes, it rotates the cube around it's new Y axis, not around the global Y axis.

altvali gravatar imagealtvali ( 2019-04-21 09:03:09 -0500 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2019-03-24 08:48:23 -0500

Seen: 403 times

Last updated: Mar 27