Module Expr Str Import alias ImportFrom alias Import alias identifier Assign NameStore BinOpMult AttributeLoad Call AttributeLoad NameLoad attr NameLoad attr Num FunctionDef arguments args NameParam NameParam NameParam defaults Num Num Num body Expr Str Assign NameStore ListLoad If NameLoad body Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Expr Call AttributeLoad NameLoad attr Call AttributeLoad NameLoad attr ListLoad ListLoad NameLoad UnaryOpUSub NameLoad Num ListLoad NameLoad NameLoad Num ListLoad Num Num Num If NameLoad body Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Expr Call AttributeLoad NameLoad attr Call AttributeLoad NameLoad attr ListLoad ListLoad NameLoad Num NameLoad ListLoad Num Num Num ListLoad UnaryOpUSub NameLoad Num NameLoad If NameLoad body Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Expr Call AttributeLoad NameLoad attr Call AttributeLoad NameLoad attr ListLoad ListLoad Num Num Num ListLoad Num NameLoad UnaryOpUSub NameLoad ListLoad Num NameLoad NameLoad If NameLoad body Return Call NameLoad AttributeLoad NameLoad attr SubscriptLoad NameLoad Slice Num Return Call AttributeLoad NameLoad attr Num decorator_list FunctionDef arguments args NameParam NameParam defaults NameLoad body Expr Str Assign NameStore Call AttributeLoad NameLoad attr NameLoad If CompareIs NameLoad NameLoad body TryExcept body Assign NameStore BinOpMult AttributeLoad Call AttributeLoad NameLoad attr AttributeLoad NameLoad attr attr Num handlers ExceptHandler type NameLoad body Assign NameStore NameLoad Assign TupleStore NameStore NameStore NameStore NameStore NameStore NameStore NameStore NameStore NameStore AttributeLoad NameLoad attr Assign NameStore Call AttributeLoad NameLoad attr BinOpAdd BinOpMult NameLoad NameLoad BinOpMult NameLoad NameLoad If CompareGt NameLoad NameLoad body Assign NameStore Call AttributeLoad NameLoad attr UnaryOpUSub NameLoad NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad NameLoad Assign NameStore Call AttributeLoad NameLoad attr UnaryOpUSub NameLoad NameLoad orelse Assign NameStore Call AttributeLoad NameLoad attr NameLoad NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad NameLoad Assign NameStore Num Return TupleLoad NameLoad NameLoad NameLoad decorator_list FunctionDef arguments args NameParam NameParam NameParam defaults Num Num Num body Expr Str Assign NameStore BinOpDiv NameLoad Num Assign NameStore BinOpDiv NameLoad Num Assign NameStore BinOpDiv NameLoad Num Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Assign NameStore Call AttributeLoad NameLoad attr NameLoad Return Call AttributeLoad NameLoad attr ListLoad BinOpSub BinOpMult BinOpMult NameLoad NameLoad NameLoad BinOpMult BinOpMult NameLoad NameLoad NameLoad BinOpAdd BinOpMult BinOpMult NameLoad NameLoad NameLoad BinOpMult BinOpMult NameLoad NameLoad NameLoad BinOpSub BinOpMult BinOpMult NameLoad NameLoad NameLoad BinOpMult BinOpMult NameLoad NameLoad NameLoad BinOpAdd BinOpMult BinOpMult NameLoad NameLoad NameLoad BinOpMult BinOpMult NameLoad NameLoad NameLoad decorator_list FunctionDef arguments args NameParam defaults body Expr Str ImportFrom alias identifier Return Call NameLoad Call AttributeLoad NameLoad attr NameLoad decorator_list FunctionDef arguments args NameParam NameParam NameParam defaults Num Num Num body Expr Str ImportFrom alias identifier Return Call AttributeLoad NameLoad attr Call NameLoad NameLoad NameLoad NameLoad decorator_list FunctionDef arguments args NameParam NameParam NameParam defaults NameLoad body Expr Str ImportFrom alias identifier Assign NameStore Call AttributeLoad NameLoad attr NameLoad NameLoad NameLoad Return Call NameLoad NameLoad decorator_list
N/A N/A  Module implementing Euler angle rotations and their conversions

See:

* https://en.wikipedia.org/wiki/Rotation_matrix
* https://en.wikipedia.org/wiki/Euler_angles
* http://mathworld.wolfram.com/EulerAngles.html

See also: *Representing Attitude with Euler Angles and Quaternions: A
Reference* (2006) by James Diebel. A cached PDF link last found here:

http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134

Euler's rotation theorem tells us that any rotation in 3D can be
described by 3 angles.  Let's call the 3 angles the *Euler angle vector*
and call the angles in the vector :math:`alpha`, :math:`beta` and
:math:`gamma`.  The vector is [ :math:`alpha`,
:math:`beta`. :math:`gamma` ] and, in this description, the order of the
parameters specifies the order in which the rotations occur (so the
rotation corresponding to :math:`alpha` is applied first).

In order to specify the meaning of an *Euler angle vector* we need to
specify the axes around which each of the rotations corresponding to
:math:`alpha`, :math:`beta` and :math:`gamma` will occur.

There are therefore three axes for the rotations :math:`alpha`,
:math:`beta` and :math:`gamma`; let's call them :math:`i` :math:`j`,
:math:`k`.

Let us express the rotation :math:`alpha` around axis `i` as a 3 by 3
rotation matrix `A`.  Similarly :math:`beta` around `j` becomes 3 x 3
matrix `B` and :math:`gamma` around `k` becomes matrix `G`.  Then the
whole rotation expressed by the Euler angle vector [ :math:`alpha`,
:math:`beta`. :math:`gamma` ], `R` is given by::

   R = np.dot(G, np.dot(B, A))

See http://mathworld.wolfram.com/EulerAngles.html

The order :math:`G B A` expresses the fact that the rotations are
performed in the order of the vector (:math:`alpha` around axis `i` =
`A` first).

To convert a given Euler angle vector to a meaningful rotation, and a
rotation matrix, we need to define:

* the axes `i`, `j`, `k`
* whether a rotation matrix should be applied on the left of a vector to
  be transformed (vectors are column vectors) or on the right (vectors
  are row vectors).
* whether the rotations move the axes as they are applied (intrinsic
  rotations) - compared the situation where the axes stay fixed and the
  vectors move within the axis frame (extrinsic)
* the handedness of the coordinate system

See: https://en.wikipedia.org/wiki/Rotation_matrix#Ambiguities

We are using the following conventions:

* axes `i`, `j`, `k` are the `z`, `y`, and `x` axes respectively.  Thus
  an Euler angle vector [ :math:`alpha`, :math:`beta`. :math:`gamma` ]
  in our convention implies a :math:`alpha` radian rotation around the
  `z` axis, followed by a :math:`beta` rotation around the `y` axis,
  followed by a :math:`gamma` rotation around the `x` axis.
* the rotation matrix applies on the left, to column vectors on the
  right, so if `R` is the rotation matrix, and `v` is a 3 x N matrix
  with N column vectors, the transformed vector set `vdash` is given by
  ``vdash = np.dot(R, v)``.
* extrinsic rotations - the axes are fixed, and do not move with the
  rotations.
* a right-handed coordinate system

The convention of rotation around ``z``, followed by rotation around
``y``, followed by rotation around ``x``, is known (confusingly) as
"xyz", pitch-roll-yaw, Cardan angles, or Tait-Bryan angles.
 N/A math externals.six.moves reduce N/A numpy np N/A _FLOAT_EPS_4 N/A N/A N/A N/A np finfo float eps 4.0 euler2mat N/A N/A z y x N/A 0 0 0 N/A N/A  Return matrix for rotations around z, y and x axes

    Uses the z, then y, then x convention above

    Parameters
    ----------
    z : scalar
       Rotation angle in radians around z-axis (performed first)
    y : scalar
       Rotation angle in radians around y-axis
    x : scalar
       Rotation angle in radians around x-axis (performed last)

    Returns
    -------
    M : array shape (3,3)
       Rotation matrix giving same rotation as for given angles

    Examples
    --------
    >>> zrot = 1.3 # radians
    >>> yrot = -0.1
    >>> xrot = 0.2
    >>> M = euler2mat(zrot, yrot, xrot)
    >>> M.shape == (3, 3)
    True

    The output rotation matrix is equal to the composition of the
    individual rotations

    >>> M1 = euler2mat(zrot)
    >>> M2 = euler2mat(0, yrot)
    >>> M3 = euler2mat(0, 0, xrot)
    >>> composed_M = np.dot(M3, np.dot(M2, M1))
    >>> np.allclose(M, composed_M)
    True

    You can specify rotations by named arguments

    >>> np.all(M3 == euler2mat(x=xrot))
    True

    When applying M to a vector, the vector should column vector to the
    right of M.  If the right hand side is a 2D array rather than a
    vector, then each column of the 2D array represents a vector.

    >>> vec = np.array([1, 0, 0]).reshape((3,1))
    >>> v2 = np.dot(M, vec)
    >>> vecs = np.array([[1, 0, 0],[0, 1, 0]]).T # giving 3x2 array
    >>> vecs2 = np.dot(M, vecs)

    Rotations are counter-clockwise.

    >>> zred = np.dot(euler2mat(z=np.pi/2), np.eye(3))
    >>> np.allclose(zred, [[0, -1, 0],[1, 0, 0], [0, 0, 1]])
    True
    >>> yred = np.dot(euler2mat(y=np.pi/2), np.eye(3))
    >>> np.allclose(yred, [[0, 0, 1],[0, 1, 0], [-1, 0, 0]])
    True
    >>> xred = np.dot(euler2mat(x=np.pi/2), np.eye(3))
    >>> np.allclose(xred, [[1, 0, 0],[0, 0, -1], [0, 1, 0]])
    True

    Notes
    -----
    The direction of rotation is given by the right-hand rule (orient
    the thumb of the right hand along the axis around which the rotation
    occurs, with the end of the thumb at the positive end of the axis;
    curl your fingers; the direction your fingers curl is the direction
    of rotation).  Therefore, the rotations are counterclockwise if
    looking along the axis of rotation from positive to negative.
     N/A Ms N/A N/A z N/A N/A cosz N/A N/A math cos z N/A sinz N/A N/A math sin z N/A N/A N/A Ms append N/A N/A np array N/A N/A cosz N/A sinz 0 N/A sinz cosz 0 N/A 0 0 1 N/A y N/A N/A cosy N/A N/A math cos y N/A siny N/A N/A math sin y N/A N/A N/A Ms append N/A N/A np array N/A N/A cosy 0 siny N/A 0 1 0 N/A N/A siny 0 cosy N/A x N/A N/A cosx N/A N/A math cos x N/A sinx N/A N/A math sin x N/A N/A N/A Ms append N/A N/A np array N/A N/A 1 0 0 N/A 0 cosx N/A sinx N/A 0 sinx cosx N/A Ms N/A N/A N/A reduce N/A np dot N/A Ms N/A -1 N/A N/A N/A np eye 3 N/A mat2euler N/A N/A M cy_thresh N/A None N/A N/A  Discover Euler angle vector from 3x3 matrix

    Uses the conventions above.

    Parameters
    ----------
    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.

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

    Notes
    -----
    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:

    See: http://www.graphicsgems.org/

    The code appears to be licensed (from the website) as "can be used
    without restrictions".
     N/A M N/A N/A np asarray M N/A N/A cy_thresh None N/A N/A N/A N/A cy_thresh N/A N/A N/A N/A np finfo N/A M dtype eps 4 N/A N/A N/A ValueError N/A N/A cy_thresh _FLOAT_EPS_4 N/A N/A r11 r12 r13 r21 r22 r23 r31 r32 r33 N/A M flat N/A cy N/A N/A math sqrt N/A N/A r33 r33 N/A r23 r23 N/A N/A cy cy_thresh N/A N/A z N/A N/A math atan2 N/A r12 r11 N/A y N/A N/A math atan2 r13 cy N/A x N/A N/A math atan2 N/A r23 r33 N/A N/A z N/A N/A math atan2 r21 r22 N/A y N/A N/A math atan2 r13 cy N/A x 0.0 N/A N/A z y x N/A euler2quat N/A N/A z y x N/A 0 0 0 N/A N/A  Return quaternion corresponding to these Euler angles

    Uses the z, then y, then x convention above

    Parameters
    ----------
    z : scalar
       Rotation angle in radians around z-axis (performed first)
    y : scalar
       Rotation angle in radians around y-axis
    x : scalar
       Rotation angle in radians around x-axis (performed last)

    Returns
    -------
    quat : array shape (4,)
       Quaternion in w, x, y z (real, then vector) format

    Notes
    -----
    We can derive this formula in Sympy using:

    1. Formula giving quaternion corresponding to rotation of theta radians
       about arbitrary axis:
       http://mathworld.wolfram.com/EulerParameters.html
    2. Generated formulae from 1.) for quaternions corresponding to
       theta radians rotations about ``x, y, z`` axes
    3. Apply quaternion multiplication formula -
       https://en.wikipedia.org/wiki/Quaternions#Hamilton_product - to
       formulae from 2.) to give formula for combined rotations.
     N/A z N/A z 2.0 N/A y N/A y 2.0 N/A x N/A x 2.0 N/A cz N/A N/A math cos z N/A sz N/A N/A math sin z N/A cy N/A N/A math cos y N/A sy N/A N/A math sin y N/A cx N/A N/A math cos x N/A sx N/A N/A math sin x N/A N/A N/A np array N/A N/A N/A N/A cx cy cz N/A N/A sx sy sz N/A N/A N/A cx sy sz N/A N/A cy cz sx N/A N/A N/A cx cz sy N/A N/A sx cy sz N/A N/A N/A cx cy sz N/A N/A sx cz sy N/A quat2euler N/A N/A q N/A N/A N/A  Return Euler angles corresponding to quaternion `q`

    Parameters
    ----------
    q : 4 element sequence
       w, x, y, z of quaternion

    Returns
    -------
    z : scalar
       Rotation angle in radians around z-axis (performed first)
    y : scalar
       Rotation angle in radians around y-axis
    x : scalar
       Rotation angle in radians around x-axis (performed last)

    Notes
    -----
    It's possible to reduce the amount of calculation a little, by
    combining parts of the ``quat2mat`` and ``mat2euler`` functions, but
    the reduction in computation is small, and the code repetition is
    large.
     N/A quaternions nq N/A N/A mat2euler N/A N/A nq quat2mat q N/A euler2angle_axis N/A N/A z y x N/A 0 0 0 N/A N/A  Return angle, axis corresponding to these Euler angles

    Uses the z, then y, then x convention above

    Parameters
    ----------
    z : scalar
       Rotation angle in radians around z-axis (performed first)
    y : scalar
       Rotation angle in radians around y-axis
    x : scalar
       Rotation angle in radians around x-axis (performed last)

    Returns
    -------
    theta : scalar
       angle of rotation
    vector : array shape (3,)
       axis around which rotation occurs

    Examples
    --------
    >>> theta, vec = euler2angle_axis(0, 1.5, 0)
    >>> print(theta)
    1.5
    >>> np.allclose(vec, [0, 1, 0])
    True
     N/A quaternions nq N/A N/A N/A nq quat2angle_axis N/A euler2quat z y x N/A angle_axis2euler N/A N/A theta vector is_normalized N/A False N/A N/A  Convert angle, axis pair to Euler angles

    Parameters
    ----------
    theta : scalar
       angle of rotation
    vector : 3 element sequence
       vector specifying axis for rotation.
    is_normalized : bool, optional
       True if vector is already normalized (has norm of 1).  Default
       False

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

    Examples
    --------
    >>> z, y, x = angle_axis2euler(0, [1, 0, 0])
    >>> np.allclose((z, y, x), 0)
    True

    Notes
    -----
    It's possible to reduce the amount of calculation a little, by
    combining parts of the ``angle_axis2mat`` and ``mat2euler``
    functions, but the reduction in computation is small, and the code
    repetition is large.
     N/A quaternions nq N/A M N/A N/A nq angle_axis2mat theta vector is_normalized N/A N/A mat2euler M N/A