"""Angle operations."""
import math
import numpy as np
from ._constants import two_pi
[docs]
def norm_angle(a):
"""Normalize angle to (-pi, pi].
It is worth noting that using `numpy.ceil` to normalize angles will lose
more digits of precision as angles going larger but can keep more digits
of precision when angles are around zero. In common use cases, for example,
-10.0*pi to 10.0*pi, it performs well.
For more discussions on numerical precision:
https://github.com/dfki-ric/pytransform3d/pull/263
Parameters
----------
a : float or array-like, shape (n,)
Angle(s) in radians
Returns
-------
a_norm : float or array, shape (n,)
Normalized angle(s) in radians
"""
a = np.asarray(a, dtype=np.float64)
return a - (np.ceil((a + np.pi) / two_pi) - 1.0) * two_pi
[docs]
def passive_matrix_from_angle(basis, angle):
"""Compute passive rotation matrix from rotation about basis vector.
Parameters
----------
basis : int from [0, 1, 2]
The rotation axis (0: x, 1: y, 2: z)
angle : float
Rotation angle
Returns
-------
R : array-like, shape (3, 3)
Rotation matrix
Raises
------
ValueError
If basis is invalid
"""
c = np.cos(angle)
s = np.sin(angle)
if basis == 0:
R = np.array([[1.0, 0.0, 0.0], [0.0, c, s], [0.0, -s, c]])
elif basis == 1:
R = np.array([[c, 0.0, -s], [0.0, 1.0, 0.0], [s, 0.0, c]])
elif basis == 2:
R = np.array([[c, s, 0.0], [-s, c, 0.0], [0.0, 0.0, 1.0]])
else:
raise ValueError("Basis must be in [0, 1, 2]")
return R
[docs]
def active_matrix_from_angle(basis, angle):
r"""Compute active rotation matrix from rotation about basis vector.
With the angle :math:`\alpha` and :math:`s = \sin{\alpha}, c=\cos{\alpha}`,
we construct rotation matrices about the basis vectors as follows:
.. math::
\boldsymbol{R}_x(\alpha) =
\left(
\begin{array}{ccc}
1 & 0 & 0\\
0 & c & -s\\
0 & s & c
\end{array}
\right)
.. math::
\boldsymbol{R}_y(\alpha) =
\left(
\begin{array}{ccc}
c & 0 & s\\
0 & 1 & 0\\
-s & 0 & c
\end{array}
\right)
.. math::
\boldsymbol{R}_z(\alpha) =
\left(
\begin{array}{ccc}
c & -s & 0\\
s & c & 0\\
0 & 0 & 1
\end{array}
\right)
Parameters
----------
basis : int from [0, 1, 2]
The rotation axis (0: x, 1: y, 2: z)
angle : float
Rotation angle
Returns
-------
R : array, shape (3, 3)
Rotation matrix
Raises
------
ValueError
If basis is invalid
"""
c = np.cos(angle)
s = np.sin(angle)
if basis == 0:
R = np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])
elif basis == 1:
R = np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])
elif basis == 2:
R = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
else:
raise ValueError("Basis must be in [0, 1, 2]")
return R
[docs]
def quaternion_from_angle(basis, angle):
"""Compute quaternion from rotation about basis vector.
Parameters
----------
basis : int from [0, 1, 2]
The rotation axis (0: x, 1: y, 2: z)
angle : float
Rotation angle
Returns
-------
q : array, shape (4,)
Unit quaternion to represent rotation: (w, x, y, z)
Raises
------
ValueError
If basis is invalid
"""
half_angle = 0.5 * angle
c = math.cos(half_angle)
s = math.sin(half_angle)
if basis == 0:
q = np.array([c, s, 0.0, 0.0])
elif basis == 1:
q = np.array([c, 0.0, s, 0.0])
elif basis == 2:
q = np.array([c, 0.0, 0.0, s])
else:
raise ValueError("Basis must be in [0, 1, 2]")
return q
