Gazebo | Ignition | Community
Ask Your Question

How to apply an extrinsic rotation to a link, programmatically?

asked 2019-04-21 14:24:07 -0500

altvali gravatar image

I have models written in SDF that I want to rotate at specific extrinsic angles before starting a Gazebo simulation with them. I have the roll, pitch and yaw of the pose of the link, I have the extrinsic roll, pitch and yaw of the desired rotation, and I want to find out the formula for obtaining the roll, pitch and yaw of the link, after the rotation, so that I can update the pose values in the SDF file.

I couldn't find a source for the Euler angle convention that Gazebo uses, but I can tell that it is using intrinsic angles because when I rotate a link on the X axis by changing its roll pose number, it rotates around the extrinsic X axis, but if I then change the pitch pose number, it doesn't rotate around the extrinsic Y axis but its own, intrinsic, Y axis.

I'll greatly appreciate any help, thank you!

Edit: I've managed to find out about a widely used transformations library and I've managed to write the following code:

Re = euler_matrix(arr[3]-np.pi, arr[4], arr[5], 'rxyz')
Rx = rotation_matrix(r_arr[0], [1, 0, 0])
Ry = rotation_matrix(r_arr[1], [0, 1, 0])
Rz = rotation_matrix(r_arr[2], [0, 0, 1])
M  = concatenate_matrices(Re, Rx, Ry, Rz)
euler = euler_from_matrix(M, 'rxyz')
arr[3] = euler[0]
arr[4] = euler[1]
arr[5] = euler[2]

where r_arr contains the desired extrinsic rotations and arr[3:6] contains the current intrinsic rotations.

Here is the result: hand rotated 90 degrees on the Y axis

The hand has been rotated 90 degrees on the Y axis. The little finger didn't come out right, I think this is a numerical inaccuracy, the links of the little finger being the only ones with negative pitch from all the other fingers. If there is a better way, I'm open to suggestions.

I'm also attaching the hand SDF file.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2019-04-22 11:37:43 -0500

updated 2019-04-22 11:40:54 -0500

Based on my own experience and this Gazebo Answer, I believe that Gazebo poses do in fact represent extrinsic (fixed axis) xyz rotations.

I have used Christoph Gohlke's transformation library and also encountered some numerical inaccuracy issue.

Numerical Inaccuracy Workaround: In, I changed the value of _EPS from numpy.finfo(float).eps * 4.0 to 1e-10

Explanation: As best as I can tell, most of the recommended algorithms/libraries for converting rotation matrices to euler angles (including use derivatives of an algorithm by Ken Shoemake found in Graphics Gems IV (1994).


def mat2euler(M, cy_thresh=None):
    ''' Discover Euler angle vector from 3x3 matrix

    Uses the conventions above.

    M : array-like, shape (3,3)
    cy_thresh : None or scalar, optional
       threshold below which to give up on straightforward arctan for
       estimating x rotation.  If None (default), estimate from
       precision of input.

    z : scalar
    y : scalar
    x : scalar
       Rotations in radians around z, y, x axes, respectively

    If there was no numerical error, the routine could be derived using
    Sympy expression for z then y then x rotation matrix, which is::

      [                       cos(y)*cos(z),                       -cos(y)*sin(z),         sin(y)],
      [cos(x)*sin(z) + cos(z)*sin(x)*sin(y), cos(x)*cos(z) - sin(x)*sin(y)*sin(z), -cos(y)*sin(x)],
      [sin(x)*sin(z) - cos(x)*cos(z)*sin(y), cos(z)*sin(x) + cos(x)*sin(y)*sin(z),  cos(x)*cos(y)]

    with the obvious derivations for z, y, and x

       z = atan2(-r12, r11)
       y = asin(r13)
       x = atan2(-r23, r33)

    Problems arise when cos(y) is close to zero, because both of::

       z = atan2(cos(y)*sin(z), cos(y)*cos(z))
       x = atan2(cos(y)*sin(x), cos(x)*cos(y))

    will be close to atan2(0, 0), and highly unstable.

    The ``cy`` fix for numerical instability below is from: *Graphics
    Gems IV*, Paul Heckbert (editor), Academic Press, 1994, ISBN:
    0123361559.  Specifically it comes from EulerAngles.c by Ken
    Shoemake, and deals with the case where cos(y) is close to zero:


    The code appears to be licensed (from the website) as "can be used
    without restrictions".
    M = np.asarray(M)
    if cy_thresh is None:
            cy_thresh = np.finfo(M.dtype).eps * 4
        except ValueError:
            cy_thresh = _FLOAT_EPS_4
    r11, r12, r13, r21, r22, r23, r31, r32, r33 = M.flat
    # cy: sqrt((cos(y)*cos(z))**2 + (cos(x)*cos(y))**2)
    cy = math.sqrt(r33*r33 + r23*r23)
    if cy > cy_thresh: # cos(y) not close to zero, standard form
        z = math.atan2(-r12,  r11) # atan2(cos(y)*sin(z), cos(y)*cos(z))
        y = math.atan2(r13,  cy) # atan2(sin(y), cy)
        x = math.atan2(-r23, r33) # atan2(cos(y)*sin(x), cos(x)*cos(y))
    else: # cos(y) (close to) zero, so x -> 0.0 (see above)
        # so r21 -> sin(z), r22 -> cos(z) and
        z = math.atan2(r21,  r22)
        y = math.atan2(r13,  cy) # atan2(sin(y), cy)
        x ...
edit flag offensive delete link more

Question Tools

1 follower


Asked: 2019-04-21 09:23:17 -0500

Seen: 551 times

Last updated: Apr 22 '19