Gazebo: Q&A Forum - RSS feedhttp://answers.gazebosim.org/questions/Open source question and answer forum for GazeboenCopyright Askbot, 2010-2011.Sun, 21 Apr 2019 09:03:09 -0500Compute roll, pitch and yaw from 3D vectorhttp://answers.gazebosim.org/question/22154/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.
Sun, 24 Mar 2019 08:48:23 -0500http://answers.gazebosim.org/question/22154/compute-roll-pitch-and-yaw-from-3d-vector/Answer by josephcoombe for <p>Hi !</p>
<p>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.</p>
<p>Since the <code><pose></code> takes roll, pitch and yaw angles to build a cylinder, I have to compute them from the direction vector (vx, vy, vz).</p>
<p>Here's how I do it, using <code>numpy</code>:</p>
<p><code>roll = np.arctan2(vz, vy)</code></p>
<p><code>pitch = np.arctan2(vx, vz)</code></p>
<p><code>yaw = np.arctan2(vy, vx)</code></p>
<p>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 :</p>
<p><code>roll = np.arctan2(vz, vy) + 1.5708</code></p>
<p>without success.</p>
<p>If you have any advice on how to get these Euler angles right, feel free to share !</p>
<p>Have a nice day.</p>
http://answers.gazebosim.org/question/22154/compute-roll-pitch-and-yaw-from-3d-vector/?answer=22165#post-id-22165First 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](https://www.lfd.uci.edu/~gohlke/code/transformations.py.html), 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
Wed, 27 Mar 2019 10:48:12 -0500http://answers.gazebosim.org/question/22154/compute-roll-pitch-and-yaw-from-3d-vector/?answer=22165#post-id-22165Comment by altvali for <p>First of all, there exist multiple Euler Angle conventions.</p>
<p>URDF and SDF use the <strong>X-Y-Z (Tait–Bryan) Extrinsic</strong> Euler Angle convention - 3 consecutive rotations around the X, Y, and Z coordinate axes of the original (fixed) coordinate system. [1]</p>
<p>There are several gotcha edge cases when dealing with Euler Angles, so I recommend using a library like Christoph Gohlke's transformations.py (<a href="https://www.lfd.uci.edu/~gohlke/code/transformations.py.html">https://www.lfd.uci.edu/~gohlke/code/...</a>).</p>
<hr>
<p>Try something like this <em>(untested)</em>:</p>
<ul>
<li>Assume you have two unit vectors, vector <strong>a</strong> lies on axis of the previous cylinder, and vector <strong>b</strong> lies on the axis of the current cylinder.</li>
<li>Assume that length(<strong>a</strong>)==length(<strong>b</strong>) > 0.</li>
<li>Calculate the cross product of <strong>a</strong> and <strong>b</strong>: <strong>v</strong> = <strong>a</strong> x <strong>b</strong>. <strong>v</strong> is the <strong>axis</strong> of rotation between <strong>a</strong> and <strong>b</strong> [2]. If <strong>a</strong> and <strong>b</strong> 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').</li>
<li>Calculate the dot product of <strong>a</strong> and <strong>b</strong>: cos(angle)=dot(<strong>a</strong>, <strong>b</strong>)/(length(<strong>a</strong>)+length(<strong>b</strong>)). The result is the cosine of the <strong>angle of rotation</strong>. [2]</li>
<li>Calculate the acos of cos(angle) from previous step: angle=acos(cos(angle)). angle is the <strong>angle of rotation</strong>. [2]</li>
<li>Now, you have an <strong>axis angle representation</strong> of <strong>a rotation</strong> between <strong>a</strong> and <strong>b</strong>.</li>
<li>Using the <a href="https://www.lfd.uci.edu/~gohlke/code/transformations.py.html">transformations.py library</a>, you can calculate <strong>X-Y-Z extrinsic Euler Angles</strong> from the <strong>axis angle representation</strong>.
<ul>
<li>Given axis of rotation <strong>v</strong> and angle of rotation a.</li>
<li><strong>R</strong> = rotation_matrix(a, <strong>v</strong>):</li>
<li>x, y, z = euler_from_matrix(<strong>R</strong>, axes='sxyz'):</li>
</ul></li>
</ul>
<hr>
<p>References:</p>
<ul>
<li>[1] Euler Angles: <a href="https://en.wikipedia.org/wiki/Euler_angles">https://en.wikipedia.org/wiki/Euler_a...</a></li>
<li>[2] Euler angles between two 3d vectors: <a href="https://stackoverflow.com/a/15109028/8670609">https://stackoverflow.com/a/15109028/...</a></li>
</ul>
http://answers.gazebosim.org/question/22154/compute-roll-pitch-and-yaw-from-3d-vector/?comment=22338#post-id-22338Do 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.Sun, 21 Apr 2019 09:03:09 -0500http://answers.gazebosim.org/question/22154/compute-roll-pitch-and-yaw-from-3d-vector/?comment=22338#post-id-22338