+# Part of Spatial Math Toolbox for Python
+# Copyright (c) 2000 Peter Corke
+# MIT Licence, see details in top-level file: LICENCE
+
+"""
+These functions create and manipulate 3D rotation matrices and rigid-body
+transformations as 3x3 SO(3) matrices and 4x4 SE(3) matrices respectively.
+These matrices are represented as 2D NumPy arrays.
+
+Vector arguments are what numpy refers to as ``array_like`` and can be a list,
+tuple, numpy array, numpy row vector or numpy column vector.
+
+"""
+
+# pylint: disable=invalid-name
+
+import sys
+from collections.abc import Iterable
+import math
+import numpy as np
+
+from spatialmath.base.argcheck import getunit, getvector, isvector, isscalar, ismatrix
+from spatialmath.base.vectors import (
+ unitvec,
+ unitvec_norm,
+ norm,
+ isunitvec,
+ iszerovec,
+ unittwist_norm,
+ isunittwist,
+)
+from spatialmath.base.transformsNd import (
+ r2t,
+ t2r,
+ rt2tr,
+ skew,
+ skewa,
+ vex,
+ vexa,
+ isskew,
+ isskewa,
+ isR,
+ tr2rt,
+ Ab2M,
+)
+from spatialmath.base.quaternions import r2q, q2r, qeye, qslerp
+from spatialmath.base.graphics import plotvol3, axes_logic
+from spatialmath.base.animate import Animate
+import spatialmath.base.symbolic as sym
+
+from spatialmath.base.types import *
+
+_eps = np.finfo(np.float64).eps
+
+# ---------------------------------------------------------------------------------------#
+
+
+[docs]def rotx(theta: float, unit: str = "rad") -> SO3Array:
+
"""
+
Create SO(3) rotation about X-axis
+
+
:param theta: rotation angle about X-axis
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
+
- ``rotx(θ)`` is an SO(3) rotation matrix (3x3) representing a rotation
+
of θ radians about the x-axis
+
- ``rotx(θ, "deg")`` as above but θ is in degrees
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import rotx
+
>>> rotx(0.3)
+
>>> rotx(45, 'deg')
+
+
:seealso: :func:`~trotx`
+
:SymPy: supported
+
"""
+
+
theta = getunit(theta, unit, vector=False)
+
ct = sym.cos(theta)
+
st = sym.sin(theta)
+
# fmt: off
+
R = np.array([
+
[1, 0, 0],
+
[0, ct, -st],
+
[0, st, ct]]) # type: ignore
+
# fmt: on
+
return R
+
+
+a = rotx(1) @ rotx(2)
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def roty(theta: float, unit: str = "rad") -> SO3Array:
+
"""
+
Create SO(3) rotation about Y-axis
+
+
:param theta: rotation angle about Y-axis
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
+
- ``roty(θ)`` is an SO(3) rotation matrix (3x3) representing a rotation
+
of θ radians about the y-axis
+
- ``roty(θ, "deg")`` as above but θ is in degrees
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import roty
+
>>> roty(0.3)
+
>>> roty(45, 'deg')
+
+
:seealso: :func:`~troty`
+
:SymPy: supported
+
"""
+
+
theta = getunit(theta, unit, vector=False)
+
ct = sym.cos(theta)
+
st = sym.sin(theta)
+
# fmt: off
+
return np.array([
+
[ ct, 0, st],
+
[ 0, 1, 0],
+
[-st, 0, ct]]) # type: ignore
+ # fmt: on
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def rotz(theta: float, unit: str = "rad") -> SO3Array:
+
"""
+
Create SO(3) rotation about Z-axis
+
+
:param theta: rotation angle about Z-axis
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
+
- ``rotz(θ)`` is an SO(3) rotation matrix (3x3) representing a rotation
+
of θ radians about the z-axis
+
- ``rotz(θ, "deg")`` as above but θ is in degrees
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import rotz
+
>>> rotz(0.3)
+
>>> rotz(45, 'deg')
+
+
:seealso: :func:`~trotz`
+
:SymPy: supported
+
"""
+
theta = getunit(theta, unit, vector=False)
+
ct = sym.cos(theta)
+
st = sym.sin(theta)
+
# fmt: off
+
return np.array([
+
[ct, -st, 0],
+
[st, ct, 0],
+
[0, 0, 1]]) # type: ignore
+ # fmt: on
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def trotx(theta: float, unit: str = "rad", t: Optional[ArrayLike3] = None) -> SE3Array:
+
"""
+
Create SE(3) pure rotation about X-axis
+
+
:param theta: rotation angle about X-axis
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:param t: 3D translation vector, defaults to [0,0,0]
+
:type t: array_like(3)
+
:return: SE(3) transformation matrix
+
:rtype: ndarray(4,4)
+
+
- ``trotx(θ)`` is a homogeneous transformation (4x4) representing a rotation
+
of θ radians about the x-axis.
+
- ``trotx(θ, 'deg')`` as above but θ is in degrees
+
- ``trotx(θ, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import trotx
+
>>> trotx(0.3)
+
>>> trotx(45, 'deg', t=[1,2,3])
+
+
:seealso: :func:`~rotx`
+
:SymPy: supported
+
"""
+
T = r2t(rotx(theta, unit))
+
if t is not None:
+
T[:3, 3] = getvector(t, 3, "array")
+
return T
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def troty(theta: float, unit: str = "rad", t: Optional[ArrayLike3] = None) -> SE3Array:
+
"""
+
Create SE(3) pure rotation about Y-axis
+
+
:param theta: rotation angle about Y-axis
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:param t: 3D translation vector, defaults to [0,0,0]
+
:type t: array_like(3)
+
:return: SE(3) transformation matrix
+
:rtype: ndarray(4,4)
+
+
- ``troty(θ)`` is a homogeneous transformation (4x4) representing a rotation
+
of θ radians about the y-axis.
+
- ``troty(θ, 'deg')`` as above but θ is in degrees
+
- ``troty(θ, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import troty
+
>>> troty(0.3)
+
>>> troty(45, 'deg', t=[1,2,3])
+
+
:seealso: :func:`~roty`
+
:SymPy: supported
+
"""
+
T = r2t(roty(theta, unit))
+
if t is not None:
+
T[:3, 3] = getvector(t, 3, "array")
+
return T
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def trotz(theta: float, unit: str = "rad", t: Optional[ArrayLike3] = None) -> SE3Array:
+
"""
+
Create SE(3) pure rotation about Z-axis
+
+
:param theta: rotation angle about Z-axis
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:param t: 3D translation vector, defaults to [0,0,0]
+
:type t: array_like(3)
+
:return: SE(3) transformation matrix
+
:rtype: ndarray(4,4)
+
+
- ``trotz(θ)`` is a homogeneous transformation (4x4) representing a rotation
+
of θ radians about the z-axis.
+
- ``trotz(θ, 'deg')`` as above but θ is in degrees
+
- ``trotz(θ, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import trotz
+
>>> trotz(0.3)
+
>>> trotz(45, 'deg', t=[1,2,3])
+
+
:seealso: :func:`~rotz`
+
:SymPy: supported
+
"""
+
T = r2t(rotz(theta, unit))
+
if t is not None:
+
T[:3, 3] = getvector(t, 3, "array")
+
return T
+
+
+# ---------------------------------------------------------------------------------------#
+
+
+@overload # pragma: no cover
+def transl(x: float, y: float, z: float) -> SE3Array:
+ ...
+
+
+@overload # pragma: no cover
+def transl(x: ArrayLike3) -> SE3Array:
+ ...
+
+
+@overload # pragma: no cover
+def transl(x: SE3Array) -> R3:
+ ...
+
+
+[docs]def transl(x, y=None, z=None):
+
"""
+
Create SE(3) pure translation, or extract translation from SE(3) matrix
+
+
**Create a translational SE(3) matrix**
+
+
:param x: translation along X-axis
+
:type x: float
+
:param y: translation along Y-axis
+
:type y: float
+
:param z: translation along Z-axis
+
:type z: float
+
:return: SE(3) transformation matrix
+
:rtype: numpy(4,4)
+
:raises ValueError: bad argument
+
+
- ``T = transl( X, Y, Z )`` is an SE(3) homogeneous transform (4x4)
+
representing a pure translation of X, Y and Z.
+
- ``T = transl( V )`` as above but the translation is given by a 3-element
+
list, dict, or a numpy array, row or column vector.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import transl
+
>>> import numpy as np
+
>>> transl(3, 4, 5)
+
>>> transl([3, 4, 5])
+
>>> transl(np.array([3, 4, 5]))
+
+
**Extract the translational part of an SE(3) matrix**
+
+
:param x: SE(3) transformation matrix
+
:type x: numpy(4,4)
+
:return: translation elements of SE(2) matrix
+
:rtype: ndarray(3)
+
:raises ValueError: bad argument
+
+
- ``t = transl(T)`` is the translational part of a homogeneous transform T as a
+
3-element numpy array.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import transl
+
>>> import numpy as np
+
>>> T = np.array([[1, 0, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]])
+
>>> transl(T)
+
+
.. note:: This function is compatible with the MATLAB version of the
+
Toolbox. It is unusual/weird in doing two completely different things
+
inside the one function.
+
:seealso: :func:`~spatialmath.base.transforms2d.transl2`
+
:SymPy: supported
+
"""
+
+
if isscalar(x) and y is not None and z is not None:
+
t = np.r_[x, y, z]
+
elif isvector(x, 3):
+
t = getvector(x, 3, out="array")
+
elif ismatrix(x, (4, 4)):
+
# SE(3) -> R3
+
return x[:3, 3]
+
else:
+
raise ValueError("bad argument")
+
+
if t.dtype != "O":
+
t = t.astype("float64")
+
+
T = np.identity(4, dtype=t.dtype)
+
T[:3, 3] = t
+
return T
+
+
+[docs]def ishom(T: Any, check: bool = False, tol: float = 20) -> bool:
+
"""
+
Test if matrix belongs to SE(3)
+
+
:param T: SE(3) matrix to test
+
:type T: numpy(4,4)
+
:param check: check validity of rotation submatrix
+
:param tol: Tolerance in units of eps for rotation submatrix check, defaults to 20
+
:return: whether matrix is an SE(3) homogeneous transformation matrix
+
+
- ``ishom(T)`` is True if the argument ``T`` is of dimension 4x4
+
- ``ishom(T, check=True)`` as above, but also checks orthogonality of the
+
rotation sub-matrix and validitity of the bottom row.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import ishom
+
>>> import numpy as np
+
>>> T = np.array([[1, 0, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]])
+
>>> ishom(T)
+
>>> T = np.array([[1, 1, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]]) # invalid SE(3)
+
>>> ishom(T) # a quick check says it is an SE(3)
+
>>> ishom(T, check=True) # but if we check more carefully...
+
>>> R = np.array([[1, 1, 0], [0, 1, 0], [0, 0, 1]])
+
>>> ishom(R)
+
+
:seealso: :func:`~spatialmath.base.transformsNd.isR` :func:`~isrot` :func:`~spatialmath.base.transforms2d.ishom2`
+
"""
+
return (
+
isinstance(T, np.ndarray)
+
and T.shape == (4, 4)
+
and (
+
not check
+
or (isR(T[:3, :3], tol=tol) and all(T[3, :] == np.array([0, 0, 0, 1])))
+
)
+
)
+
+
+[docs]def isrot(R: Any, check: bool = False, tol: float = 20) -> bool:
+
"""
+
Test if matrix belongs to SO(3)
+
+
:param R: SO(3) matrix to test
+
:type R: numpy(3,3)
+
:param check: check validity of rotation submatrix
+
:param tol: Tolerance in units of eps for rotation matrix test, defaults to 20
+
:return: whether matrix is an SO(3) rotation matrix
+
+
- ``isrot(R)`` is True if the argument ``R`` is of dimension 3x3
+
- ``isrot(R, check=True)`` as above, but also checks orthogonality of the
+
rotation matrix.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import isrot
+
>>> import numpy as np
+
>>> T = np.array([[1, 0, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]])
+
>>> isrot(T)
+
>>> R = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
+
>>> isrot(R)
+
>>> R = R = np.array([[1, 1, 0], [0, 1, 0], [0, 0, 1]]) # invalid SO(3)
+
>>> isrot(R) # a quick check says it is an SO(3)
+
>>> isrot(R, check=True) # but if we check more carefully...
+
+
:seealso: :func:`~spatialmath.base.transformsNd.isR` :func:`~spatialmath.base.transforms2d.isrot2`, :func:`~ishom`
+
"""
+
return (
+
isinstance(R, np.ndarray)
+
and R.shape == (3, 3)
+
and (not check or isR(R, tol=tol))
+
)
+
+
+# ---------------------------------------------------------------------------------------#
+@overload # pragma: no cover
+def rpy2r(
+ roll: float, pitch: float, yaw: float, *, unit: str = "rad", order: str = "zyx"
+) -> SO3Array:
+ ...
+
+
+@overload # pragma: no cover
+def rpy2r(
+ roll: ArrayLike3,
+ pitch: None = None,
+ yaw: None = None,
+ *,
+ unit: str = "rad",
+ order: str = "zyx",
+) -> SO3Array:
+ ...
+
+
+[docs]def rpy2r(
+
roll: Union[ArrayLike3, float],
+
pitch: Optional[float] = None,
+
yaw: Optional[float] = None,
+
*,
+
unit: str = "rad",
+
order: str = "zyx",
+
) -> SO3Array:
+
"""
+
Create an SO(3) rotation matrix from roll-pitch-yaw angles
+
+
:param roll: roll angle
+
:type roll: float or array_like(3)
+
:param pitch: pitch angle
+
:type pitch: float
+
:param yaw: yaw angle
+
:type yaw: float
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
:raises ValueError: bad argument
+
+
- ``rpy2r(⍺, β, γ)`` is an SO(3) orthonormal rotation matrix (3x3)
+
equivalent to the specified roll (⍺), pitch (β), yaw (γ) angles angles.
+
These correspond to successive rotations about the axes specified by
+
``order``:
+
+
- 'zyx' [default], rotate by γ about the z-axis, then by β about the new
+
y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot
+
with x-axis forward and y-axis sideways.
+
- 'xyz', rotate by γ about the x-axis, then by β about the new y-axis,
+
then by ⍺ about the new z-axis. Convention for a robot gripper with
+
z-axis forward and y-axis between the gripper fingers.
+
- 'yxz', rotate by γ about the y-axis, then by β about the new x-axis,
+
then by ⍺ about the new z-axis. Convention for a camera with z-axis
+
parallel to the optic axis and x-axis parallel to the pixel rows.
+
+
- ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken
+
from ``RPY`` which is a 3-vector with values (⍺, β, γ).
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import rpy2r
+
>>> rpy2r(0.1, 0.2, 0.3)
+
>>> rpy2r([0.1, 0.2, 0.3])
+
>>> rpy2r([10, 20, 30], unit='deg')
+
+
:seealso: :func:`~eul2r` :func:`~rpy2tr` :func:`~tr2rpy`
+
"""
+
+
if isscalar(roll):
+
angles = [roll, pitch, yaw]
+
else:
+
angles = getvector(roll, 3)
+
+
angles = getunit(angles, unit)
+
+
a = rotx(0)
+
if order in ("xyz", "arm"):
+
R = rotx(angles[2]) @ roty(angles[1]) @ rotz(angles[0])
+
elif order in ("zyx", "vehicle"):
+
R = rotz(angles[2]) @ roty(angles[1]) @ rotx(angles[0])
+
elif order in ("yxz", "camera"):
+
R = roty(angles[2]) @ rotx(angles[1]) @ rotz(angles[0])
+
else:
+
raise ValueError("Invalid angle order")
+
+
return R
+
+
+# ---------------------------------------------------------------------------------------#
+@overload # pragma: no cover
+def rpy2tr(
+ roll: float, pitch: float, yaw: float, unit: str = "rad", order: str = "zyx"
+) -> SE3Array:
+ ...
+
+
+@overload # pragma: no cover
+def rpy2tr(
+ roll: ArrayLike3,
+ pitch: None = None,
+ yaw: None = None,
+ unit: str = "rad",
+ order: str = "zyx",
+) -> SE3Array:
+ ...
+
+
+[docs]def rpy2tr(
+
roll,
+
pitch=None,
+
yaw=None,
+
unit: str = "rad",
+
order: str = "zyx",
+
) -> SE3Array:
+
"""
+
Create an SE(3) rotation matrix from roll-pitch-yaw angles
+
+
:param roll: roll angle
+
:type roll: float
+
:param pitch: pitch angle
+
:type pitch: float
+
:param yaw: yaw angle
+
:type yaw: float
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:type unit: str
+
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
+
:type order: str
+
:return: SE(3) transformation matrix
+
:rtype: ndarray(4,4)
+
+
- ``rpy2tr(⍺, β, γ)`` is an SE(3) matrix (4x4) equivalent to the specified
+
roll (⍺), pitch (β), yaw (γ) angles angles. These correspond to successive
+
rotations about the axes specified by ``order``:
+
+
- 'zyx' [default], rotate by γ about the z-axis, then by β about the new
+
y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot
+
with x-axis forward and y-axis sideways.
+
- 'xyz', rotate by γ about the x-axis, then by β about the new y-axis,
+
then by ⍺ about the new z-axis. Convention for a robot gripper with
+
z-axis forward and y-axis between the gripper fingers.
+
- 'yxz', rotate by γ about the y-axis, then by β about the new x-axis,
+
then by ⍺ about the new z-axis. Convention for a camera with z-axis
+
parallel to the optic axis and x-axis parallel to the pixel rows.
+
+
- ``rpy2tr(RPY)`` as above but the roll, pitch, yaw angles are taken
+
from ``RPY`` which is a 3-vector with values (⍺, β, γ).
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import rpy2tr
+
>>> rpy2tr(0.1, 0.2, 0.3)
+
>>> rpy2tr([0.1, 0.2, 0.3])
+
>>> rpy2tr([10, 20, 30], unit='deg')
+
+
.. note:: By default, the translational component is zero but it can be
+
set to a non-zero value.
+
+
:seealso: :func:`~eul2tr` :func:`~rpy2r` :func:`~tr2rpy`
+
"""
+
+
R = rpy2r(roll, pitch, yaw, order=order, unit=unit)
+
return r2t(R)
+
+
+# ---------------------------------------------------------------------------------------#
+
+
+@overload # pragma: no cover
+def eul2r(phi: float, theta: float, psi: float, unit: str = "rad") -> SO3Array:
+ ...
+
+
+@overload # pragma: no cover
+def eul2r(
+ phi: ArrayLike3, theta: None = None, psi: None = None, unit: str = "rad"
+) -> SO3Array:
+ ...
+
+
+[docs]def eul2r(
+
phi: Union[ArrayLike3, float],
+
theta: Optional[float] = None,
+
psi: Optional[float] = None,
+
unit: str = "rad",
+
) -> SO3Array:
+
"""
+
Create an SO(3) rotation matrix from Euler angles
+
+
:param phi: Z-axis rotation
+
:type phi: float
+
:param theta: Y-axis rotation
+
:type theta: float
+
:param psi: Z-axis rotation
+
:type psi: float
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:type unit: str
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
+
- ``R = eul2r(φ, θ, ψ)`` is an SO(3) orthonornal rotation
+
matrix equivalent to the specified Euler angles. These correspond
+
to rotations about the Z, Y, Z axes respectively.
+
- ``R = eul2r(EUL)`` as above but the Euler angles are taken from
+
``EUL`` which is a 3-vector with values (φ θ ψ).
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import eul2r
+
>>> eul2r(0.1, 0.2, 0.3)
+
>>> eul2r([0.1, 0.2, 0.3])
+
>>> eul2r([10, 20, 30], unit='deg')
+
+
:seealso: :func:`~rpy2r` :func:`~eul2tr` :func:`~tr2eul`
+
+
:SymPy: supported
+
"""
+
+
if np.isscalar(phi):
+
angles = [phi, theta, psi]
+
else:
+
angles = getvector(phi, 3)
+
+
angles = getunit(angles, unit)
+
+
return rotz(angles[0]) @ roty(angles[1]) @ rotz(angles[2])
+
+
+# ---------------------------------------------------------------------------------------#
+@overload # pragma: no cover
+def eul2tr(phi: float, theta: float, psi: float, unit: str = "rad") -> SE3Array:
+ ...
+
+
+@overload # pragma: no cover
+def eul2tr(phi: ArrayLike3, theta=None, psi=None, unit: str = "rad") -> SE3Array:
+ ...
+
+
+[docs]def eul2tr(
+
phi,
+
theta=None,
+
psi=None,
+
unit="rad",
+
) -> SE3Array:
+
"""
+
Create an SE(3) pure rotation matrix from Euler angles
+
+
:param phi: Z-axis rotation
+
:type phi: float
+
:param theta: Y-axis rotation
+
:type theta: float
+
:param psi: Z-axis rotation
+
:type psi: float
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:type unit: str
+
:return: SE(3) transformation matrix
+
:rtype: ndarray(4,4)
+
+
- ``R = eul2tr(PHI, θ, PSI)`` is an SE(3) homogeneous transformation
+
matrix equivalent to the specified Euler angles. These correspond
+
to rotations about the Z, Y, Z axes respectively.
+
- ``R = eul2tr(EUL)`` as above but the Euler angles are taken from
+
``EUL`` which is a 3-vector with values
+
(PHI θ PSI).
+
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import eul2tr
+
>>> eul2tr(0.1, 0.2, 0.3)
+
>>> eul2tr([0.1, 0.2, 0.3])
+
>>> eul2tr([10, 20, 30], unit='deg')
+
+
.. note:: By default, the translational component is zero but it can be
+
set to a non-zero value.
+
+
:seealso: :func:`~rpy2tr` :func:`~eul2r` :func:`~tr2eul`
+
+
:SymPy: supported
+
"""
+
+
R = eul2r(phi, theta, psi, unit=unit)
+
return r2t(R)
+
+
+# ---------------------------------------------------------------------------------------#
+
+
+[docs]def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Array:
+
"""
+
Create an SO(3) rotation matrix from rotation angle and axis
+
+
:param theta: rotation
+
:type theta: float
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:type unit: str
+
:param v: 3D rotation axis
+
:type v: array_like(3)
+
:param tol: Tolerance in units of eps for zero-rotation case, defaults to 20
+
:type: float
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
:raises ValueError: bad arguments
+
+
``angvec2r(θ, V)`` is an SO(3) orthonormal rotation matrix
+
equivalent to a rotation of ``θ`` about the vector ``V``.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import angvec2r
+
>>> angvec2r(0.3, [1, 0, 0]) # rotx(0.3)
+
>>> angvec2r(0, [1, 0, 0]) # rotx(0)
+
+
.. note::
+
+
- If ``θ == 0`` then return identity matrix.
+
- If ``θ ~= 0`` then ``V`` must have a finite length.
+
+
:seealso: :func:`~angvec2tr` :func:`~tr2angvec`
+
+
:SymPy: not supported
+
"""
+
if not isscalar(theta) or not isvector(v, 3):
+
raise ValueError("Arguments must be angle and vector")
+
+
if np.linalg.norm(v) < tol * _eps:
+
return np.eye(3)
+
+
θ = getunit(theta, unit)
+
+
# Rodrigue's equation
+
+
sk = skew(cast(ArrayLike3, unitvec(v)))
+
R = np.eye(3) + math.sin(θ) * sk + (1.0 - math.cos(θ)) * sk @ sk
+
return R
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def angvec2tr(theta: float, v: ArrayLike3, unit="rad") -> SE3Array:
+
"""
+
Create an SE(3) pure rotation from rotation angle and axis
+
+
:param theta: rotation
+
:type theta: float
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:type unit: str
+
:param v: 3D rotation axis
+
:type v: : array_like(3)
+
:return: SE(3) transformation matrix
+
:rtype: ndarray(4,4)
+
+
``angvec2tr(θ, V)`` is an SE(3) homogeneous transformation matrix
+
equivalent to a rotation of ``θ`` about the vector ``V``.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import angvec2tr
+
>>> angvec2tr(0.3, [1, 0, 0]) # rtotx(0.3)
+
+
.. note::
+
+
- If ``θ == 0`` then return identity matrix.
+
- If ``θ ~= 0`` then ``V`` must have a finite length.
+
- The translational part is zero.
+
+
:seealso: :func:`~angvec2r` :func:`~tr2angvec`
+
+
:SymPy: not supported
+
"""
+
return r2t(angvec2r(theta, v, unit=unit))
+
+
+# ---------------------------------------------------------------------------------------#
+
+
+[docs]def exp2r(w: ArrayLike3) -> SE3Array:
+
r"""
+
Create an SO(3) rotation matrix from exponential coordinates
+
+
:param w: exponential coordinate vector
+
:type w: array_like(3)
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
:raises ValueError: bad arguments
+
+
``exp2r(w)`` is an SO(3) orthonormal rotation matrix
+
equivalent to a rotation of :math:`\| w \|` about the vector :math:`\hat{w}`.
+
+
If ``w`` is zero then result is the identity matrix.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import exp2r, rotx
+
>>> exp2r([0.3, 0, 0])
+
>>> rotx(0.3)
+
+
.. note:: Exponential coordinates are also known as an Euler vector
+
+
:seealso: :func:`~angvec2r` :func:`~tr2angvec`
+
+
:SymPy: not supported
+
"""
+
if not isvector(w, 3):
+
raise ValueError("Arguments must be a 3-vector")
+
+
try:
+
v, theta = unitvec_norm(w)
+
except ValueError:
+
return np.eye(3)
+
+
# Rodrigue's equation
+
+
sk = skew(cast(ArrayLike3, v))
+
R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
+
return R
+
+
+[docs]def exp2tr(w: ArrayLike3) -> SE3Array:
+
r"""
+
Create an SE(3) pure rotation matrix from exponential coordinates
+
+
:param w: exponential coordinate vector
+
:type w: array_like(3)
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
:raises ValueError: bad arguments
+
+
``exp2r(w)`` is an SO(3) orthonormal rotation matrix
+
equivalent to a rotation of :math:`\| w \|` about the vector :math:`\hat{w}`.
+
+
If ``w`` is zero then result is the identity matrix.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import exp2tr, trotx
+
>>> exp2tr([0.3, 0, 0])
+
>>> trotx(0.3)
+
+
.. note:: Exponential coordinates are also known as an Euler vector
+
+
:seealso: :func:`~angvec2r` :func:`~tr2angvec`
+
+
:SymPy: not supported
+
"""
+
if not isvector(w, 3):
+
raise ValueError("Arguments must be a 3-vector")
+
+
try:
+
v, theta = unitvec_norm(w)
+
except ValueError:
+
return np.eye(4)
+
+
# Rodrigue's equation
+
+
sk = skew(cast(ArrayLike3, v))
+
R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
+
return r2t(cast(SO3Array, R))
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def oa2r(o: ArrayLike3, a: ArrayLike3) -> SO3Array:
+
"""
+
Create SO(3) rotation matrix from two vectors
+
+
:param o: 3D vector parallel to Y- axis
+
:type o: array_like(3)
+
:param a: 3D vector parallel to the Z-axis
+
:type o: array_like(3)
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
+
``T = oa2tr(O, A)`` is an SO(3) orthonormal rotation matrix for a frame
+
defined in terms of vectors parallel to its Y- and Z-axes with respect to a
+
reference frame. In robotics these axes are respectively called the
+
orientation and approach vectors defined such that R = [N O A] and N = O x
+
A.
+
+
Steps:
+
+
1. N' = O x A
+
2. O' = A x N
+
3. normalize N', O', A
+
4. stack horizontally into rotation matrix
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import oa2r
+
>>> oa2r([0, 1, 0], [0, 0, -1]) # Y := Y, Z := -Z
+
+
.. note::
+
+
- The A vector is the only guaranteed to have the same direction in the
+
resulting rotation matrix
+
- O and A do not have to be unit-length, they are normalized
+
- O and A do not have to be orthogonal, so long as they are not parallel
+
- The vectors O and A are parallel to the Y- and Z-axes of the
+
equivalent coordinate frame.
+
+
:seealso: :func:`~oa2tr`
+
+
:SymPy: not supported
+
"""
+
o = getvector(o, 3, out="array")
+
a = getvector(a, 3, out="array")
+
n = np.cross(o, a)
+
o = np.cross(a, n)
+
R = np.stack((unitvec(n), unitvec(o), unitvec(a)), axis=1)
+
return R
+
+
+# ---------------------------------------------------------------------------------------#
+[docs]def oa2tr(o: ArrayLike3, a: ArrayLike3) -> SE3Array:
+
"""
+
Create SE(3) pure rotation from two vectors
+
+
:param o: 3D vector parallel to Y- axis
+
:type o: array_like(3)
+
:param a: 3D vector parallel to the Z-axis
+
:type o: array_like(3)
+
:return: SE(3) transformation matrix
+
:rtype: ndarray(4,4)
+
+
``T = oa2tr(O, A)`` is an SE(3) homogeneous transformation matrix for a
+
frame defined in terms of vectors parallel to its Y- and Z-axes with respect
+
to a reference frame. In robotics these axes are respectively called the
+
orientation and approach vectors defined such that R = [N O A] and N = O x
+
A.
+
+
Steps:
+
+
1. N' = O x A
+
2. O' = A x N
+
3. normalize N', O', A
+
4. stack horizontally into rotation matrix
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import oa2tr
+
>>> oa2tr([0, 1, 0], [0, 0, -1]) # Y := Y, Z := -Z
+
+
.. note:
+
+
- The A vector is the only guaranteed to have the same direction in the
+
resulting rotation matrix
+
- O and A do not have to be unit-length, they are normalized
+
- O and A do not have to be orthogonal, so long as they are not parallel
+
- The translational part is zero.
+
- The vectors O and A are parallel to the Y- and Z-axes of the
+
equivalent coordinate frame.
+
+
:seealso: :func:`~oa2r`
+
+
:SymPy: not supported
+
"""
+
return r2t(oa2r(o, a))
+
+
+# ------------------------------------------------------------------------------------------------------------------- #
+[docs]def tr2angvec(
+
T: Union[SO3Array, SE3Array], unit: str = "rad", check: bool = False
+
) -> Tuple[float, R3]:
+
r"""
+
Convert SO(3) or SE(3) to angle and rotation vector
+
+
:param R: SE(3) or SO(3) matrix
+
:type R: ndarray(4,4) or ndarray(3,3)
+
:param unit: 'rad' or 'deg'
+
:type unit: str
+
:param check: check that rotation matrix is valid
+
:type check: bool
+
:return: :math:`(\theta, {\bf v})`
+
:rtype: float, ndarray(3)
+
:raises ValueError: bad arguments
+
+
``(v, θ) = tr2angvec(R)`` is a rotation angle and a vector about which the
+
rotation acts that corresponds to the rotation part of ``R``.
+
+
By default the angle is in radians but can be changed setting `unit='deg'`.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import troty, tr2angvec
+
>>> T = troty(45, 'deg')
+
>>> v, theta = tr2angvec(T)
+
>>> print(v, theta)
+
+
.. note::
+
+
- If the input is SE(3) the translation component is ignored.
+
+
:seealso: :func:`~angvec2r` :func:`~angvec2tr` :func:`~tr2rpy` :func:`~tr2eul`
+
"""
+
+
if ismatrix(T, (4, 4)):
+
R = t2r(T)
+
else:
+
R = T
+
if not isrot(R, check=check):
+
raise ValueError("argument is not SO(3)")
+
+
v = vex(trlog(cast(SO3Array, R), check=check))
+
+
try:
+
theta = norm(v)
+
v = unitvec(v)
+
except ValueError:
+
theta = 0
+
v = np.r_[0, 0, 0]
+
+
if unit == "deg":
+
theta *= 180 / math.pi
+
+
return (theta, v)
+
+
+# ------------------------------------------------------------------------------------------------------------------- #
+[docs]def tr2eul(
+
T: Union[SO3Array, SE3Array],
+
unit: str = "rad",
+
flip: bool = False,
+
check: bool = False,
+
tol: float = 20,
+
) -> R3:
+
r"""
+
Convert SO(3) or SE(3) to ZYX Euler angles
+
+
:param R: SE(3) or SO(3) matrix
+
:type R: ndarray(4,4) or ndarray(3,3)
+
:param unit: 'rad' or 'deg'
+
:type unit: str
+
:param flip: choose first Euler angle to be in quadrant 2 or 3
+
:type flip: bool
+
:param check: check that rotation matrix is valid
+
:type check: bool
+
:param tol: Tolerance in units of eps for near-zero checks, defaults to 20
+
:type: float
+
:return: ZYZ Euler angles
+
:rtype: ndarray(3)
+
+
``tr2eul(R)`` are the Euler angles corresponding to
+
the rotation part of ``R``.
+
+
The 3 angles :math:`[\phi, \theta, \psi]` correspond to sequential rotations
+
about the Z, Y and Z axes respectively.
+
+
By default the angles are in radians but can be changed setting `unit='deg'`.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import tr2eul, eul2tr
+
>>> T = eul2tr(0.2, 0.3, 0.5)
+
>>> print(T)
+
>>> tr2eul(T)
+
+
.. note::
+
+
- There is a singularity for the case where :math:`\theta=0` in which
+
case we arbitrarily set :math:`\phi = 0` and :math:`\phi` is set to
+
:math:`\phi+\psi`.
+
- If the input is SE(3) the translation component is ignored.
+
+
:seealso: :func:`~eul2r` :func:`~eul2tr` :func:`~tr2rpy` :func:`~tr2angvec`
+
:SymPy: not supported
+
+
"""
+
+
if ismatrix(T, (4, 4)):
+
R = t2r(T)
+
else:
+
R = T
+
if not isrot(R, check=check, tol=tol):
+
raise ValueError("argument is not SO(3)")
+
+
eul = np.zeros((3,))
+
if abs(R[0, 2]) < tol * _eps and abs(R[1, 2]) < tol * _eps:
+
eul[0] = 0
+
sp = 0
+
cp = 1
+
eul[1] = math.atan2(cp * R[0, 2] + sp * R[1, 2], R[2, 2])
+
eul[2] = math.atan2(-sp * R[0, 0] + cp * R[1, 0], -sp * R[0, 1] + cp * R[1, 1])
+
else:
+
if flip:
+
eul[0] = math.atan2(-R[1, 2], -R[0, 2])
+
else:
+
eul[0] = math.atan2(R[1, 2], R[0, 2])
+
sp = math.sin(eul[0])
+
cp = math.cos(eul[0])
+
eul[1] = math.atan2(cp * R[0, 2] + sp * R[1, 2], R[2, 2])
+
eul[2] = math.atan2(-sp * R[0, 0] + cp * R[1, 0], -sp * R[0, 1] + cp * R[1, 1])
+
+
if unit == "deg":
+
eul *= 180 / math.pi
+
+
return eul # type: ignore
+
+
+# ------------------------------------------------------------------------------------------------------------------- #
+
+
+[docs]def tr2rpy(
+
T: Union[SO3Array, SE3Array],
+
unit: str = "rad",
+
order: str = "zyx",
+
check: bool = False,
+
tol: float = 20,
+
) -> R3:
+
r"""
+
Convert SO(3) or SE(3) to roll-pitch-yaw angles
+
+
:param R: SE(3) or SO(3) matrix
+
:type R: ndarray(4,4) or ndarray(3,3)
+
:param unit: 'rad' or 'deg'
+
:type unit: str
+
:param order: 'xyz', 'zyx' or 'yxz' [default 'zyx']
+
:type order: str
+
:param check: check that rotation matrix is valid
+
:type check: bool
+
:param tol: Tolerance in units of eps, defaults to 20
+
:type: float
+
:return: Roll-pitch-yaw angles
+
:rtype: ndarray(3)
+
:raises ValueError: bad arguments
+
+
``tr2rpy(R)`` are the roll-pitch-yaw angles corresponding to
+
the rotation part of ``R``.
+
+
The 3 angles RPY = :math:`[\theta_R, \theta_P, \theta_Y]` correspond to
+
sequential rotations about the Z, Y and X axes respectively. The axis order
+
sequence can be changed by setting:
+
+
- ``order='xyz'`` for sequential rotations about X, Y, Z axes
+
- ``order='yxz'`` for sequential rotations about Y, X, Z axes
+
+
By default the angles are in radians but can be changed setting
+
``unit='deg'``.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import tr2rpy, rpy2tr
+
>>> T = rpy2tr(0.2, 0.3, 0.5)
+
>>> print(T)
+
>>> tr2rpy(T)
+
+
.. note::
+
+
- There is a singularity for the case where :math:`\theta_P = \pi/2` in
+
which case we arbitrarily set :math:`\theta_R=0` and
+
:math:`\theta_Y = \theta_R + \theta_Y`.
+
- If the input is SE(3) the translation component is ignored.
+
+
:seealso: :func:`~rpy2r` :func:`~rpy2tr` :func:`~tr2eul`,
+
:func:`~tr2angvec`
+
:SymPy: not supported
+
"""
+
+
if ismatrix(T, (4, 4)):
+
R = t2r(T)
+
else:
+
R = T
+
if not isrot(R, check=check, tol=tol):
+
raise ValueError("not a valid SO(3) matrix")
+
+
rpy = np.zeros((3,))
+
if order in ("xyz", "arm"):
+
# XYZ order
+
if abs(abs(R[0, 2]) - 1) < tol * _eps: # when |R13| == 1
+
# singularity
+
rpy[0] = 0 # roll is zero
+
if R[0, 2] > 0:
+
rpy[2] = math.atan2(R[2, 1], R[1, 1]) # R+Y
+
else:
+
rpy[2] = -math.atan2(R[1, 0], R[2, 0]) # R-Y
+
rpy[1] = math.asin(np.clip(R[0, 2], -1.0, 1.0))
+
else:
+
rpy[0] = -math.atan2(R[0, 1], R[0, 0])
+
rpy[2] = -math.atan2(R[1, 2], R[2, 2])
+
+
k = np.argmax(np.abs([R[0, 0], R[0, 1], R[1, 2], R[2, 2]]))
+
if k == 0:
+
rpy[1] = math.atan(R[0, 2] * math.cos(rpy[0]) / R[0, 0])
+
elif k == 1:
+
rpy[1] = -math.atan(R[0, 2] * math.sin(rpy[0]) / R[0, 1])
+
elif k == 2:
+
rpy[1] = -math.atan(R[0, 2] * math.sin(rpy[2]) / R[1, 2])
+
elif k == 3:
+
rpy[1] = math.atan(R[0, 2] * math.cos(rpy[2]) / R[2, 2])
+
+
elif order in ("zyx", "vehicle"):
+
# old ZYX order (as per Paul book)
+
if abs(abs(R[2, 0]) - 1) < tol * _eps: # when |R31| == 1
+
# singularity
+
rpy[0] = 0 # roll is zero
+
if R[2, 0] < 0:
+
rpy[2] = -math.atan2(R[0, 1], R[0, 2]) # R-Y
+
else:
+
rpy[2] = math.atan2(-R[0, 1], -R[0, 2]) # R+Y
+
rpy[1] = -math.asin(np.clip(R[2, 0], -1.0, 1.0))
+
else:
+
rpy[0] = math.atan2(R[2, 1], R[2, 2]) # R
+
rpy[2] = math.atan2(R[1, 0], R[0, 0]) # Y
+
+
k = np.argmax(np.abs([R[0, 0], R[1, 0], R[2, 1], R[2, 2]]))
+
if k == 0:
+
rpy[1] = -math.atan(R[2, 0] * math.cos(rpy[2]) / R[0, 0])
+
elif k == 1:
+
rpy[1] = -math.atan(R[2, 0] * math.sin(rpy[2]) / R[1, 0])
+
elif k == 2:
+
rpy[1] = -math.atan(R[2, 0] * math.sin(rpy[0]) / R[2, 1])
+
elif k == 3:
+
rpy[1] = -math.atan(R[2, 0] * math.cos(rpy[0]) / R[2, 2])
+
+
elif order in ("yxz", "camera"):
+
if abs(abs(R[1, 2]) - 1) < tol * _eps: # when |R23| == 1
+
# singularity
+
rpy[0] = 0
+
if R[1, 2] < 0:
+
rpy[2] = -math.atan2(R[2, 0], R[0, 0]) # R-Y
+
else:
+
rpy[2] = math.atan2(-R[2, 0], -R[2, 1]) # R+Y
+
rpy[1] = -math.asin(np.clip(R[1, 2], -1.0, 1.0)) # P
+
else:
+
rpy[0] = math.atan2(R[1, 0], R[1, 1])
+
rpy[2] = math.atan2(R[0, 2], R[2, 2])
+
+
k = np.argmax(np.abs([R[1, 0], R[1, 1], R[0, 2], R[2, 2]]))
+
if k == 0:
+
rpy[1] = -math.atan(R[1, 2] * math.sin(rpy[0]) / R[1, 0])
+
elif k == 1:
+
rpy[1] = -math.atan(R[1, 2] * math.cos(rpy[0]) / R[1, 1])
+
elif k == 2:
+
rpy[1] = -math.atan(R[1, 2] * math.sin(rpy[2]) / R[0, 2])
+
elif k == 3:
+
rpy[1] = -math.atan(R[1, 2] * math.cos(rpy[2]) / R[2, 2])
+
+
else:
+
raise ValueError("Invalid order")
+
+
if unit == "deg":
+
rpy *= 180 / math.pi
+
+
return rpy # type: ignore
+
+
+# ---------------------------------------------------------------------------------------#
+@overload # pragma: no cover
+def trlog(
+ T: SO3Array, check: bool = True, twist: bool = False, tol: float = 20
+) -> so3Array:
+ ...
+
+
+@overload # pragma: no cover
+def trlog(
+ T: SE3Array, check: bool = True, twist: bool = False, tol: float = 20
+) -> se3Array:
+ ...
+
+
+@overload # pragma: no cover
+def trlog(T: SO3Array, check: bool = True, twist: bool = True, tol: float = 20) -> R3:
+ ...
+
+
+@overload # pragma: no cover
+def trlog(T: SE3Array, check: bool = True, twist: bool = True, tol: float = 20) -> R6:
+ ...
+
+
+[docs]def trlog(
+
T: Union[SO3Array, SE3Array],
+
check: bool = True,
+
twist: bool = False,
+
tol: float = 20,
+
) -> Union[R3, R6, so3Array, se3Array]:
+
"""
+
Logarithm of SO(3) or SE(3) matrix
+
+
:param R: SE(3) or SO(3) matrix
+
:type R: ndarray(4,4) or ndarray(3,3)
+
:param check: check that matrix is valid
+
:type check: bool
+
:param twist: return a twist vector instead of matrix [default]
+
:type twist: bool
+
:param tol: Tolerance in units of eps for zero-rotation case, defaults to 20
+
:type: float
+
:return: logarithm
+
:rtype: ndarray(4,4) or ndarray(3,3)
+
:raises ValueError: bad argument
+
+
An efficient closed-form solution of the matrix logarithm for arguments that
+
are SO(3) or SE(3).
+
+
- ``trlog(R)`` is the logarithm of the passed rotation matrix ``R`` which
+
will be 3x3 skew-symmetric matrix. The equivalent vector from ``vex()``
+
is parallel to rotation axis and its norm is the amount of rotation about
+
that axis.
+
- ``trlog(T)`` is the logarithm of the passed homogeneous transformation
+
matrix ``T`` which will be 4x4 augumented skew-symmetric matrix. The
+
equivalent vector from ``vexa()`` is the twist vector (6x1) comprising [v
+
w].
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import trlog, rotx, trotx
+
>>> trlog(trotx(0.3))
+
>>> trlog(trotx(0.3), twist=True)
+
>>> trlog(rotx(0.3))
+
>>> trlog(rotx(0.3), twist=True)
+
+
:seealso: :func:`~trexp` :func:`~spatialmath.base.transformsNd.vex` :func:`~spatialmath.base.transformsNd.vexa`
+
"""
+
+
if ishom(T, check=check, tol=tol):
+
# SE(3) matrix
+
+
[R, t] = tr2rt(T)
+
+
# S = trlog(R, check=False) # recurse
+
S = trlog(cast(SO3Array, R), check=False, tol=tol) # recurse
+
w = vex(S)
+
theta = norm(w)
+
if theta == 0:
+
# rotation matrix is identity
+
if twist:
+
return np.r_[t, 0, 0, 0]
+
else:
+
return Ab2M(np.zeros((3, 3)), t)
+
else:
+
# general case
+
Ginv = (
+
np.eye(3)
+
- S / 2
+
+ (1 / theta - 1 / math.tan(theta / 2) / 2) / theta * S @ S
+
)
+
v = Ginv @ t
+
if twist:
+
return np.r_[v, w]
+
else:
+
return Ab2M(S, v)
+
+
elif isrot(T, check=check, tol=tol):
+
# deal with rotation matrix
+
R = T
+
if abs(np.trace(R) + 1) < tol * _eps:
+
# check for trace = -1
+
# rotation by +/- pi, +/- 3pi etc.
+
diagonal = R.diagonal()
+
k = diagonal.argmax()
+
mx = diagonal[k]
+
I = np.eye(3)
+
col = R[:, k] + I[:, k]
+
w = col / np.sqrt(2 * (1 + mx))
+
theta = math.pi
+
if twist:
+
return w * theta
+
else:
+
return skew(w * theta)
+
else:
+
# general case
+
tr = (np.trace(R) - 1) / 2
+
# min for inaccuracies near identity yielding trace > 3
+
theta = math.acos(min(tr, 1.0))
+
st = math.sin(theta)
+
if st == 0:
+
if twist:
+
return np.zeros((3,))
+
else:
+
return np.zeros((3, 3))
+
else:
+
skw = (R - R.T) / 2 / st
+
if twist:
+
return vex(skw * theta)
+
else:
+
return skw * theta
+
else:
+
raise ValueError("Expect SO(3) or SE(3) matrix")
+
+
+# ---------------------------------------------------------------------------------------#
+@overload # pragma: no cover
+def trexp(S: so3Array, theta: Optional[float] = None, check: bool = True) -> SO3Array:
+ ...
+
+
+@overload # pragma: no cover
+def trexp(S: se3Array, theta: Optional[float] = None, check: bool = True) -> SE3Array:
+ ...
+
+
+@overload # pragma: no cover
+def trexp(S: ArrayLike3, theta: Optional[float] = None, check=True) -> SO3Array:
+ ...
+
+
+@overload # pragma: no cover
+def trexp(S: ArrayLike6, theta: Optional[float] = None, check=True) -> SE3Array:
+ ...
+
+
+[docs]def trexp(S, theta=None, check=True):
+
"""
+
Exponential of se(3) or so(3) matrix
+
+
:param S: se(3), so(3) matrix or equivalent twist vector
+
:type T: ndarray(4,4) or ndarray(6); or ndarray(3,3) or ndarray(3)
+
:param θ: motion
+
:type θ: float
+
:return: matrix exponential in SE(3) or SO(3)
+
:rtype: ndarray(4,4) or ndarray(3,3)
+
:raises ValueError: bad arguments
+
+
An efficient closed-form solution of the matrix exponential for arguments
+
that are so(3) or se(3).
+
+
For so(3) the results is an SO(3) rotation matrix:
+
+
- ``trexp(Ω)`` is the matrix exponential of the so(3) element ``Ω`` which is
+
a 3x3 skew-symmetric matrix.
+
- ``trexp(Ω, θ)`` as above but for an so(3) motion of Ωθ, where ``Ω`` is
+
unit-norm skew-symmetric matrix representing a rotation axis and a
+
rotation magnitude given by ``θ``.
+
- ``trexp(ω)`` is the matrix exponential of the so(3) element ``ω``
+
expressed as a 3-vector.
+
- ``trexp(ω, θ)`` as above but for an so(3) motion of ωθ where ``ω`` is a
+
unit-norm vector representing a rotation axis and a rotation magnitude
+
given by ``θ``. ``ω`` is expressed as a 3-vector.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import trexp, skew
+
>>> trexp(skew([1, 2, 3]))
+
>>> trexp(skew([1, 0, 0]), 2) # revolute unit twist
+
>>> trexp([1, 2, 3])
+
>>> trexp([1, 0, 0], 2) # revolute unit twist
+
+
For se(3) the results is an SE(3) homogeneous transformation matrix:
+
+
- ``trexp(Σ)`` is the matrix exponential of the se(3) element ``Σ`` which is
+
a 4x4 augmented skew-symmetric matrix.
+
- ``trexp(Σ, θ)`` as above but for an se(3) motion of Σθ, where ``Σ`` must
+
represent a unit-twist, ie. the rotational component is a unit-norm
+
skew-symmetric matrix.
+
- ``trexp(S)`` is the matrix exponential of the se(3) element ``S``
+
represented as a 6-vector which can be considered a screw motion.
+
- ``trexp(S, θ)`` as above but for an se(3) motion of Sθ, where ``S`` must
+
represent a unit-twist, ie. the rotational component is a unit-norm
+
skew-symmetric matrix.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import trexp, skewa
+
>>> trexp(skewa([1, 2, 3, 4, 5, 6]))
+
>>> trexp(skewa([1, 0, 0, 0, 0, 0]), 2) # prismatic unit twist
+
>>> trexp([1, 2, 3, 4, 5, 6])
+
>>> trexp([1, 0, 0, 0, 0, 0], 2)
+
+
:seealso: :func:`~trlog :func:`~spatialmath.base.transforms2d.trexp2`
+
"""
+
+
if ismatrix(S, (4, 4)) or isvector(S, 6):
+
# se(3) case
+
if ismatrix(S, (4, 4)):
+
# augmentented skew matrix
+
if check and not isskewa(S):
+
raise ValueError("argument must be a valid se(3) element")
+
tw = vexa(cast(se3Array, S))
+
else:
+
# 6 vector
+
tw = getvector(S)
+
+
if iszerovec(tw):
+
return np.eye(4)
+
+
if theta is None:
+
(tw, theta) = unittwist_norm(tw)
+
else:
+
if theta == 0:
+
return np.eye(4)
+
elif not isunittwist(tw):
+
raise ValueError("If theta is specified S must be a unit twist")
+
+
# tw is a unit twist, th is its magnitude
+
t = tw[0:3]
+
w = tw[3:6]
+
+
R = rodrigues(w, theta)
+
+
skw = skew(w)
+
V = (
+
np.eye(3) * theta
+
+ (1.0 - math.cos(theta)) * skw
+
+ (theta - math.sin(theta)) * skw @ skw
+
)
+
+
return rt2tr(R, V @ t)
+
+
elif ismatrix(S, (3, 3)) or isvector(S, 3):
+
# so(3) case
+
if ismatrix(S, (3, 3)):
+
# skew symmetric matrix
+
if check and not isskew(S):
+
raise ValueError("argument must be a valid so(3) element")
+
w = vex(S)
+
else:
+
# 3 vector
+
w = getvector(S)
+
+
if theta is not None and not isunitvec(w):
+
raise ValueError("If theta is specified S must be a unit twist")
+
+
# do Rodrigues' formula for rotation
+
return rodrigues(w, theta)
+
else:
+
raise ValueError(" First argument must be SO(3), 3-vector, SE(3) or 6-vector")
+
+
+@overload # pragma: no cover
+def trnorm(R: SO3Array) -> SO3Array:
+ ...
+
+
+[docs]def trnorm(T: SE3Array) -> SE3Array:
+
r"""
+
Normalize an SO(3) or SE(3) matrix
+
+
:param T: SE(3) or SO(3) matrix
+
:type T: ndarray(4,4) or ndarray(3,3)
+
:return: normalized SE(3) or SO(3) matrix
+
:rtype: ndarray(4,4) or ndarray(3,3)
+
:raises ValueError: bad arguments
+
+
- ``trnorm(R)`` is guaranteed to be a proper orthogonal matrix rotation
+
matrix (3x3) which is *close* to the input matrix R (3x3).
+
- ``trnorm(T)`` as above but the rotational submatrix of the homogeneous
+
transformation T (4x4) is normalised while the translational part is
+
unchanged.
+
+
The steps in normalization are:
+
+
#. If :math:`\mathbf{R} = [n, o, a]`
+
#. Form unit vectors :math:`\hat{o}, \hat{a}` from :math:`o, a` respectively
+
#. Form the normal vector :math:`\hat{n} = \hat{o} \times \hat{a}`
+
#. Recompute :math:`\hat{o} = \hat{a} \times \hat{n}` to ensure that :math:`\hat{o}, \hat{a}` are orthogonal
+
#. Form the normalized SO(3) matrix :math:`\mathbf{R} = [\hat{n}, \hat{o}, \hat{a}]`
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import trnorm, troty
+
>>> from numpy import linalg
+
>>> T = troty(45, 'deg', t=[3, 4, 5])
+
>>> linalg.det(T[:3,:3]) - 1 # is a valid SO(3)
+
>>> T = T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T
+
>>> linalg.det(T[:3,:3]) - 1 # not quite a valid SE(3) anymore
+
>>> T = trnorm(T)
+
>>> linalg.det(T[:3,:3]) - 1 # once more a valid SE(3)
+
+
.. note::
+
+
- Only the direction of a-vector (the z-axis) is unchanged.
+
- Used to prevent finite word length arithmetic causing transforms to
+
become 'unnormalized', ie. determinant :math:`\ne 1`.
+
"""
+
+
if not ishom(T) and not isrot(T):
+
raise ValueError("expecting SO(3) or SE(3)")
+
+
o = T[:3, 1]
+
a = T[:3, 2]
+
+
n = np.cross(o, a) # N = O x A
+
o = np.cross(a, n) # (a)];
+
R = np.stack((unitvec(n), unitvec(o), unitvec(a)), axis=1)
+
+
if ishom(T):
+
return rt2tr(cast(SO3Array, R), T[:3, 3])
+
else:
+
return R
+
+
+@overload
+def trinterp(
+ start: Optional[SO3Array], end: SO3Array, s: float, shortest: bool = True
+) -> SO3Array:
+ ...
+
+
+@overload
+def trinterp(
+ start: Optional[SE3Array], end: SE3Array, s: float, shortest: bool = True
+) -> SE3Array:
+ ...
+
+
+[docs]def trinterp(start, end, s, shortest=True):
+
"""
+
Interpolate SE(3) matrices
+
+
:param start: initial SE(3) or SO(3) matrix value when s=0, if None then identity is used
+
:type start: ndarray(4,4) or ndarray(3,3)
+
:param end: final SE(3) or SO(3) matrix, value when s=1
+
:type end: ndarray(4,4) or ndarray(3,3)
+
:param s: interpolation coefficient, range 0 to 1
+
:type s: float
+
:param shortest: take the shortest path along the great circle for the rotation
+
:type shortest: bool, default to True
+
:return: interpolated SE(3) or SO(3) matrix value
+
:rtype: ndarray(4,4) or ndarray(3,3)
+
:raises ValueError: bad arguments
+
+
- ``trinterp(None, T, S)`` is a homogeneous transform (4x4) interpolated
+
between identity when S=0 and T (4x4) when S=1.
+
- ``trinterp(T0, T1, S)`` as above but interpolated
+
between T0 (4x4) when S=0 and T1 (4x4) when S=1.
+
- ``trinterp(None, R, S)`` is a rotation matrix (3x3) interpolated
+
between identity when S=0 and R (3x3) when S=1.
+
- ``trinterp(R0, R1, S)`` as above but interpolated
+
between R0 (3x3) when S=0 and R1 (3x3) when S=1.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import transl, trinterp
+
>>> T1 = transl(1, 2, 3)
+
>>> T2 = transl(4, 5, 6)
+
>>> trinterp(T1, T2, 0)
+
>>> trinterp(T1, T2, 1)
+
>>> trinterp(T1, T2, 0.5)
+
>>> trinterp(None, T2, 0)
+
>>> trinterp(None, T2, 1)
+
>>> trinterp(None, T2, 0.5)
+
+
.. note:: Rotation is interpolated using quaternion spherical linear interpolation (slerp).
+
+
:seealso: :func:`spatialmath.base.quaternions.qlerp` :func:`~spatialmath.base.transforms3d.trinterp2`
+
"""
+
+
if not 0 <= s <= 1:
+
raise ValueError("s outside interval [0,1]")
+
+
if ismatrix(end, (3, 3)):
+
# SO(3) case
+
+
if start is None:
+
# TRINTERP(T, s)
+
q0 = r2q(end)
+
qr = qslerp(qeye(), q0, s, shortest=shortest)
+
else:
+
# TRINTERP(T0, T1, s)
+
q0 = r2q(start)
+
q1 = r2q(end)
+
qr = qslerp(q0, q1, s, shortest=shortest)
+
+
return q2r(qr)
+
+
elif ismatrix(end, (4, 4)):
+
# SE(3) case
+
if start is None:
+
# TRINTERP(T, s)
+
q0 = r2q(t2r(end))
+
p0 = transl(end)
+
+
qr = qslerp(qeye(), q0, s, shortest=shortest)
+
pr = s * p0
+
else:
+
# TRINTERP(T0, T1, s)
+
q0 = r2q(t2r(start))
+
q1 = r2q(t2r(end))
+
+
p0 = transl(start)
+
p1 = transl(end)
+
+
qr = qslerp(q0, q1, s, shortest=shortest)
+
pr = p0 * (1 - s) + s * p1
+
+
return rt2tr(q2r(qr), pr)
+
else:
+
return ValueError("Argument must be SO(3) or SE(3)")
+
+
+[docs]def delta2tr(d: R6) -> SE3Array:
+
r"""
+
Convert differential motion to SE(3)
+
+
:param Δ: differential motion as a 6-vector
+
:type Δ: array_like(6)
+
:return: SE(3) matrix
+
:rtype: ndarray(4,4)
+
+
``delta2tr(Δ)`` is an SE(3) matrix representing differential
+
motion :math:`\Delta = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]`.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import delta2tr
+
>>> delta2tr([0.001, 0, 0, 0, 0.002, 0])
+
+
:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
+
+
:seealso: :func:`~tr2delta`
+
:SymPy: supported
+
"""
+
+
return np.eye(4, 4) + skewa(d)
+
+
+[docs]def trinv(T: SE3Array) -> SE3Array:
+
r"""
+
Invert an SE(3) matrix
+
+
:param T: SE(3) matrix
+
:type T: ndarray(4,4)
+
:return: inverse of SE(3) matrix
+
:rtype: ndarray(4,4)
+
:raises ValueError: bad arguments
+
+
Computes an efficient inverse of an SE(3) matrix:
+
+
:math:`\begin{pmatrix} {\bf R} & t \\ 0\,0\,0 & 1 \end{pmatrix}^{-1} = \begin{pmatrix} {\bf R}^T & -{\bf R}^T t \\ 0\,0\, 0 & 1 \end{pmatrix}`
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import trinv, trotx
+
>>> T = trotx(0.3, t=[4,5,6])
+
>>> trinv(T)
+
>>> T @ trinv(T)
+
+
:SymPy: supported
+
"""
+
if not ishom(T):
+
raise ValueError("expecting SE(3) matrix")
+
# inline this code for speed, don't use tr2rt and rt2tr
+
R = T[:3, :3]
+
t = T[:3, 3]
+
Ti = np.zeros((4, 4), dtype=T.dtype)
+
Ti[:3, :3] = R.T
+
Ti[:3, 3] = -R.T @ t
+
Ti[3, 3] = 1
+
return Ti
+
+
+[docs]def tr2delta(T0: SE3Array, T1: Optional[SE3Array] = None) -> R6:
+
r"""
+
Difference of SE(3) matrices as differential motion
+
+
:param T0: first SE(3) matrix
+
:type T0: ndarray(4,4)
+
:param T1: second SE(3) matrix
+
:type T1: ndarray(4,4)
+
:return: Differential motion as a 6-vector
+
:rtype: ndarray(6)
+
:raises ValueError: bad arguments
+
+
- ``tr2delta(T0, T1)`` is the differential motion Δ (6x1) corresponding to
+
infinitessimal motion (in the T0 frame) from pose T0 to T1 which are SE(3)
+
matrices.
+
+
- ``tr2delta(T)`` as above but the motion is from the world frame to the
+
pose represented by T.
+
+
The vector :math:`\Delta = [\delta_x, \delta_y, \delta_z, \theta_x,
+
\theta_y, \theta_z]` represents infinitessimal translation and rotation, and
+
is an approximation to the instantaneous spatial velocity multiplied by time
+
step.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import tr2delta, trotx
+
>>> T1 = trotx(0.3, t=[4,5,6])
+
>>> T2 = trotx(0.31, t=[4,5.02,6])
+
>>> tr2delta(T1, T2)
+
+
.. note::
+
+
- Δ is only an approximation to the motion T, and assumes
+
that T0 ~ T1 or T ~ eye(4,4).
+
- Can be considered as an approximation to the effect of spatial velocity over a
+
a time interval, average spatial velocity multiplied by time.
+
+
:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
+
+
:seealso: :func:`~delta2tr`
+
:SymPy: supported
+
"""
+
+
if T1 is None:
+
# tr2delta(T)
+
+
if not ishom(T0):
+
raise ValueError("expecting SE(3) matrix")
+
Td = T0
+
+
else:
+
# incremental transformation from T0 to T1 in the T0 frame
+
Td = trinv(T0) @ T1
+
+
return np.r_[transl(Td), vex(t2r(Td) - np.eye(3))]
+
+
+[docs]def tr2jac(T: SE3Array) -> R6x6:
+
r"""
+
SE(3) Jacobian matrix
+
+
:param T: SE(3) matrix
+
:type T: ndarray(4,4)
+
:return: Jacobian matrix
+
:rtype: ndarray(6,6)
+
+
Computes an Jacobian matrix that maps spatial velocity between two frames
+
defined by an SE(3) matrix.
+
+
``tr2jac(T)`` is a Jacobian matrix (6x6) that maps spatial velocity or
+
differential motion from frame {B} to frame {A} where the pose of {B}
+
elative to {A} is represented by the homogeneous transform T = :math:`{}^A
+
{\bf T}_B`.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import tr2jac, trotx
+
>>> T = trotx(0.3, t=[4,5,6])
+
>>> tr2jac(T)
+
+
:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
+
:SymPy: supported
+
"""
+
+
if not ishom(T):
+
raise ValueError("expecting an SE(3) matrix")
+
+
Z = np.zeros((3, 3), dtype=T.dtype)
+
R = t2r(T)
+
return np.block([[R, Z], [Z, R]])
+
+
+[docs]def eul2jac(angles: ArrayLike3) -> R3x3:
+
"""
+
Euler angle rate Jacobian
+
+
:param angles: Euler angles (φ, θ, ψ)
+
:type angles: array_like(3)
+
:return: Jacobian matrix
+
:rtype: ndarray(3,3)
+
+
- ``eul2jac(φ, θ, ψ)`` is a Jacobian matrix (3x3) that maps ZYZ Euler angle
+
rates to angular velocity at the operating point specified by the Euler
+
angles φ, ϴ, ψ.
+
- ``eul2jac(𝚪)`` as above but the Euler angles are taken from ``𝚪`` which
+
is a 3-vector with values (φ θ ψ).
+
+
Example:
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import eul2jac
+
>>> eul2jac([0.1, 0.2, 0.3])
+
+
.. note::
+
- Used in the creation of an analytical Jacobian.
+
- Angles in radians, rates in radians/sec.
+
+
:Reference: Robotics, Vision & Control for Python, Section 8.1.3, P. Corke, Springer 2023.
+
+
:SymPy: supported
+
+
:seealso: :func:`angvelxform` :func:`rpy2jac` :func:`exp2jac`
+
"""
+
phi = angles[0]
+
theta = angles[1]
+
+
ctheta = sym.cos(theta)
+
stheta = sym.sin(theta)
+
cphi = sym.cos(phi)
+
sphi = sym.sin(phi)
+
+
# fmt: off
+
return np.array([
+
[ 0.0, -sphi, cphi * stheta],
+
[ 0.0, cphi, sphi * stheta],
+
[ 1.0, 0.0, ctheta ]
+
] # type: ignore
+
)
+ # fmt: on
+
+
+[docs]def rpy2jac(angles: ArrayLike3, order: str = "zyx") -> R3x3:
+
"""
+
Jacobian from RPY angle rates to angular velocity
+
+
:param angles: roll-pitch-yaw angles (⍺, β, γ)
+
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
+
:return: Jacobian matrix
+
+
- ``rpy2jac(⍺, β, γ)`` is a Jacobian matrix (3x3) that maps roll-pitch-yaw
+
angle rates to angular velocity at the operating point (⍺, β, γ). These
+
correspond to successive rotations about the axes specified by ``order``:
+
+
- 'zyx' [default], rotate by γ about the z-axis, then by β about the new
+
y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot
+
with x-axis forward and y-axis sideways.
+
- 'xyz', rotate by γ about the x-axis, then by β about the new y-axis,
+
then by ⍺ about the new z-axis. Convention for a robot gripper with
+
z-axis forward and y-axis between the gripper fingers.
+
- 'yxz', rotate by γ about the y-axis, then by β about the new x-axis,
+
then by ⍺ about the new z-axis. Convention for a camera with z-axis
+
parallel to the optic axis and x-axis parallel to the pixel rows.
+
+
- ``rpy2jac(𝚪)`` as above but the roll, pitch, yaw angles are taken
+
from ``𝚪`` which is a 3-vector with values (⍺, β, γ).
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import rpy2jac
+
>>> rpy2jac([0.1, 0.2, 0.3])
+
+
.. note::
+
- Used in the creation of an analytical Jacobian.
+
- Angles in radians, rates in radians/sec.
+
+
:Reference: Robotics, Vision & Control for Python, Section 8.1.3, P. Corke, Springer 2023.
+
+
:SymPy: supported
+
+
:seealso: :func:`rotvelxform` :func:`eul2jac` :func:`exp2jac`
+
"""
+
+
pitch = angles[1]
+
yaw = angles[2]
+
+
cp = sym.cos(pitch)
+
sp = sym.sin(pitch)
+
cy = sym.cos(yaw)
+
sy = sym.sin(yaw)
+
+
if order == "xyz":
+
# fmt: off
+
J = np.array([
+
[ sp, 0, 1],
+
[-cp * sy, cy, 0],
+
[ cp * cy, sy, 0]
+
]) # type: ignore
+
# fmt: on
+
elif order == "zyx":
+
# fmt: off
+
J = np.array([
+
[ cp * cy, -sy, 0],
+
[ cp * sy, cy, 0],
+
[-sp, 0, 1],
+
]) # type: ignore
+
# fmt: on
+
elif order == "yxz":
+
# fmt: off
+
J = np.array([
+
[ cp * sy, cy, 0],
+
[-sp, 0, 1],
+
[ cp * cy, -sy, 0]
+
]) # type: ignore
+
# fmt: on
+
else:
+
raise ValueError("unknown order")
+
return J
+
+
+[docs]def exp2jac(v: R3) -> R3x3:
+
"""
+
Jacobian from exponential coordinate rates to angular velocity
+
+
:param v: Exponential coordinates
+
:type v: array_like(3)
+
:return: Jacobian matrix
+
:rtype: ndarray(3,3)
+
+
- ``exp2jac(v)`` is a Jacobian matrix (3x3) that maps exponential coordinate
+
rates to angular velocity at the operating point ``v``.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import exp2jac
+
>>> exp2jac([0.3, 0, 0])
+
+
.. note::
+
- Used in the creation of an analytical Jacobian.
+
+
Reference::
+
+
- A compact formula for the derivative of a 3-D rotation in
+
exponential coordinate
+
Guillermo Gallego, Anthony Yezzi
+
https://arxiv.org/pdf/1312.0788v1.pdf
+
- Robot Dynamics Lecture Notes
+
Robotic Systems Lab, ETH Zurich, 2018
+
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf
+
+
:SymPy: supported
+
+
:seealso: :func:`rotvelxform` :func:`eul2jac` :func:`rpy2jac`
+
"""
+
+
try:
+
vn, theta = unitvec_norm(v)
+
except ValueError:
+
return np.eye(3)
+
+
# R = trexp(v)
+
# z = np.eye(3,3) - R
+
# # build the derivative columnwise
+
# A = []
+
# for i in range(3):
+
# # (III.7)
+
# dRdvi = vn[i] * skew(vn) + skew(np.cross(vn, z[:,i])) / theta
+
# x = vex(dRdvi)
+
# A.append(x)
+
# return np.c_[A].T
+
+
# from ETH paper
+
theta = norm(v)
+
sk = skew(v)
+
+
# (2.106)
+
E = (
+
np.eye(3)
+
+ sk * (1 - np.cos(theta)) / theta**2
+
+ sk @ sk * (theta - np.sin(theta)) / theta**3
+
)
+
return E
+
+
+[docs]def r2x(R: SO3Array, representation: str = "rpy/xyz") -> R3:
+
r"""
+
Convert SO(3) matrix to angular representation
+
+
:param R: SO(3) rotation matrix
+
:type R: ndarray(3,3)
+
:param representation: rotational representation, defaults to "rpy/xyz"
+
:type representation: str, optional
+
:return: angular representation
+
:rtype: ndarray(3)
+
+
Convert an SO(3) rotation matrix to a minimal rotational representation
+
:math:`\vec{\Gamma} \in \mathbb{R}^3`.
+
+
============================ ========================================
+
``representation`` Rotational representation
+
============================ ========================================
+
``"rpy/xyz"`` ``"arm"`` RPY angular rates in XYZ order (default)
+
``"rpy/zyx"`` ``"vehicle"`` RPY angular rates in XYZ order
+
``"rpy/yxz"`` ``"camera"`` RPY angular rates in YXZ order
+
``"eul"`` Euler angular rates in ZYZ order
+
``"exp"`` exponential coordinate rates
+
============================ ========================================
+
+
:SymPy: supported
+
+
:seealso: :func:`x2r` :func:`tr2rpy` :func:`tr2eul` :func:`trlog`
+
"""
+
if representation == "eul":
+
r = tr2eul(R)
+
elif representation.startswith("rpy/"):
+
r = tr2rpy(R, order=representation[4:])
+
elif representation in ("arm", "vehicle", "camera"):
+
r = tr2rpy(R, order=representation)
+
elif representation == "exp":
+
r = trlog(R, twist=True)
+
else:
+
raise ValueError(f"unknown representation: {representation}")
+
return r
+
+
+[docs]def x2r(r: ArrayLike3, representation: str = "rpy/xyz") -> SO3Array:
+
r"""
+
Convert angular representation to SO(3) matrix
+
+
:param r: angular representation
+
:type r: array_like(3)
+
:param representation: rotational representation, defaults to "rpy/xyz"
+
:type representation: str, optional
+
:return: SO(3) rotation matrix
+
:rtype: ndarray(3,3)
+
+
Convert a minimal rotational representation :math:`\vec{\Gamma} \in
+
\mathbb{R}^3` to an SO(3) rotation matrix.
+
+
============================ ========================================
+
``representation`` Rotational representation
+
============================ ========================================
+
``"rpy/xyz"`` ``"arm"`` RPY angular rates in XYZ order (default)
+
``"rpy/zyx"`` ``"vehicle"`` RPY angular rates in XYZ order
+
``"rpy/yxz"`` ``"camera"`` RPY angular rates in YXZ order
+
``"eul"`` Euler angular rates in ZYZ order
+
``"exp"`` exponential coordinate rates
+
============================ ========================================
+
+
:SymPy: supported
+
+
:seealso: :func:`r2x` :func:`rpy2r` :func:`eul2r` :func:`trexp`
+
"""
+
if representation == "eul":
+
R = eul2r(r)
+
elif representation.startswith("rpy/"):
+
R = rpy2r(r, order=representation[4:])
+
elif representation in ("arm", "vehicle", "camera"):
+
R = rpy2r(r, order=representation)
+
elif representation == "exp":
+
R = trexp(r)
+
else:
+
raise ValueError(f"unknown representation: {representation}")
+
return R
+
+
+[docs]def tr2x(T: SE3Array, representation: str = "rpy/xyz") -> R6:
+
r"""
+
Convert SE(3) to an analytic representation
+
+
:param T: pose as an SE(3) matrix
+
:type T: ndarray(4,4)
+
:param representation: angular representation to use, defaults to "rpy/xyz"
+
:type representation: str, optional
+
:return: analytic vector representation
+
:rtype: ndarray(6)
+
+
Convert an SE(3) matrix into an equivalent vector representation
+
:math:`\vec{x} = (\vec{t},\vec{r}) \in \mathbb{R}^6` where rotation
+
:math:`\vec{r} \in \mathbb{R}^3` is encoded in a minimal representation.
+
+
============================ ========================================
+
``representation`` Rotational representation
+
============================ ========================================
+
``"rpy/xyz"`` ``"arm"`` RPY angular rates in XYZ order (default)
+
``"rpy/zyx"`` ``"vehicle"`` RPY angular rates in XYZ order
+
``"rpy/yxz"`` ``"camera"`` RPY angular rates in YXZ order
+
``"eul"`` Euler angular rates in ZYZ order
+
``"exp"`` exponential coordinate rates
+
============================ ========================================
+
+
:SymPy: supported
+
+
:seealso: :func:`r2x`
+
"""
+
t = transl(T)
+
R = t2r(T)
+
r = r2x(R, representation=representation)
+
return np.r_[t, r]
+
+
+[docs]def x2tr(x: R6, representation="rpy/xyz") -> SE3Array:
+
r"""
+
Convert analytic representation to SE(3)
+
+
:param x: analytic vector representation
+
:type x: array_like(6)
+
:param representation: angular representation to use, defaults to "rpy/xyz"
+
:type representation: str, optional
+
:return: pose as an SE(3) matrix
+
:rtype: ndarray(4,4)
+
+
Convert a vector representation of pose :math:`\vec{x} = (\vec{t},\vec{r})
+
\in \mathbb{R}^6` to SE(3), where rotation :math:`\vec{r} \in \mathbb{R}^3` is encoded
+
in a minimal representation to an equivalent SE(3) matrix.
+
+
============================ ========================================
+
``representation`` Rotational representation
+
============================ ========================================
+
``"rpy/xyz"`` ``"arm"`` RPY angular rates in XYZ order (default)
+
``"rpy/zyx"`` ``"vehicle"`` RPY angular rates in XYZ order
+
``"rpy/yxz"`` ``"camera"`` RPY angular rates in YXZ order
+
``"eul"`` Euler angular rates in ZYZ order
+
``"exp"`` exponential coordinate rates
+
============================ ========================================
+
+
:SymPy: supported
+
+
:seealso: :func:`r2x`
+
"""
+
t = x[:3]
+
R = x2r(x[3:], representation=representation)
+
+
return rt2tr(R, t)
+
+
+[docs]def rot2jac(R, representation="rpy/xyz"):
+
"""
+
DEPRECATED, use :func:`rotvelxform` instead
+
"""
+
raise DeprecationWarning("use rotvelxform instead")
+
+
+
+
+
+
+
+
+@overload # pragma: no cover
+def rotvelxform(
+ 𝚪: ArrayLike3,
+ inverse: bool = False,
+ full: bool = False,
+ representation="rpy/xyz",
+) -> R3x3:
+ ...
+
+
+@overload # pragma: no cover
+def rotvelxform(
+ 𝚪: SO3Array,
+ inverse: bool = False,
+ full: bool = False,
+) -> R3x3:
+ ...
+
+
+@overload # pragma: no cover
+def rotvelxform(
+ 𝚪: ArrayLike3,
+ inverse: bool = False,
+ full: bool = True,
+ representation="rpy/xyz",
+) -> R6x6:
+ ...
+
+
+@overload # pragma: no cover
+def rotvelxform(
+ 𝚪: SO3Array,
+ inverse: bool = False,
+ full: bool = True,
+) -> R6x6:
+ ...
+
+
+
+
+
+@overload # pragma: no cover
+def rotvelxform_inv_dot(
+ 𝚪: ArrayLike3, 𝚪d: ArrayLike3, full: bool = False, representation: str = "rpy/xyz"
+) -> R3x3:
+ ...
+
+
+@overload # pragma: no cover
+def rotvelxform_inv_dot(
+ 𝚪: ArrayLike3, 𝚪d: ArrayLike3, full: bool = True, representation: str = "rpy/xyz"
+) -> R6x6:
+ ...
+
+
+
+
+
+@overload # pragma: no cover
+def tr2adjoint(T: SO3Array) -> R3x3:
+ ...
+
+
+@overload # pragma: no cover
+def tr2adjoint(T: SE3Array) -> R6x6:
+ ...
+
+
+[docs]def tr2adjoint(T):
+
r"""
+
Adjoint matrix
+
+
:param T: SE(3) or SO(3) matrix
+
:type T: ndarray(4,4) or ndarray(3,3)
+
:return: adjoint matrix
+
:rtype: ndarray(6,6) or ndarray(3,3)
+
+
Computes an adjoint matrix that maps the Lie algebra between frames.
+
+
.. math:
+
+
Ad(\mat{T}) \vec{X} X = \vee \left( \mat{T} \skew{\vec{X} \mat{T}^{-1} \right)
+
+
where :math:`\mat{T} \in \SE3`.
+
+
``tr2jac(T)`` is an adjoint matrix (6x6) that maps spatial velocity or
+
differential motion between frame {B} to frame {A} which are attached to the
+
same moving body. The pose of {B} relative to {A} is represented by the
+
homogeneous transform T = :math:`{}^A {\bf T}_B`.
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import tr2adjoint, trotx
+
>>> T = trotx(0.3, t=[4,5,6])
+
>>> tr2adjoint(T)
+
+
:Reference:
+
- Robotics, Vision & Control for Python, Section 3, P. Corke, Springer 2023.
+
- `Lie groups for 2D and 3D Transformations <http://ethaneade.com/lie.pdf>`_
+
+
:SymPy: supported
+
"""
+
+
Z = np.zeros((3, 3), dtype=T.dtype)
+
if T.shape == (3, 3):
+
# SO(3) adjoint
+
R = T
+
return R
+
elif T.shape == (4, 4):
+
# SE(3) adjoint
+
(R, t) = tr2rt(T)
+
# fmt: off
+
return np.block([
+
[R, skew(t) @ R],
+
[Z, R]
+
])
+
# fmt: on
+
else:
+
raise ValueError("bad argument")
+
+
+[docs]def rodrigues(w: ArrayLike3, theta: Optional[float] = None) -> SO3Array:
+
r"""
+
Rodrigues' formula for 3D rotation
+
+
:param w: rotation vector
+
:type w: array_like(3)
+
:param theta: rotation angle
+
:type theta: float or None
+
:return: SO(3) matrix
+
:rtype: ndarray(3,3)
+
+
Compute Rodrigues' formula for a rotation matrix given a rotation axis
+
and angle.
+
+
.. math::
+
+
\mat{R} = \mat{I}_{3 \times 3} + \sin \theta \skx{\hat{\vec{v}}} + (1 - \cos \theta) \skx{\hat{\vec{v}}}^2
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import *
+
>>> rodrigues([1, 0, 0], 0.3)
+
>>> rodrigues([0.3, 0, 0])
+
+
"""
+
w = getvector(w, 3)
+
if iszerovec(w):
+
# for a zero so(3) return unit matrix, theta not relevant
+
return np.eye(3)
+
+
if theta is None:
+
try:
+
w, theta = unitvec_norm(w)
+
except ValueError:
+
return np.eye(3)
+
+
skw = skew(cast(ArrayLike3, w))
+
return (
+
np.eye(skw.shape[0])
+
+ math.sin(theta) * skw
+
+ (1.0 - math.cos(theta)) * skw @ skw
+
)
+
+
+[docs]def trprint(
+
T: Union[SO3Array, SE3Array],
+
orient: str = "rpy/zyx",
+
label: str = "",
+
file: TextIO = sys.stdout,
+
fmt: str = "{:.3g}",
+
degsym: bool = True,
+
unit: str = "deg",
+
) -> str:
+
"""
+
Compact display of SO(3) or SE(3) matrices
+
+
:param T: SE(3) or SO(3) matrix
+
:type T: ndarray(4,4) or ndarray(3,3)
+
:param label: text label to put at start of line
+
:type label: str
+
:param orient: 3-angle convention to use
+
:type orient: str
+
:param file: file to write formatted string to. [default, stdout]
+
:type file: file object
+
:param fmt: conversion format for each number in the format used with ``format``
+
:type fmt: str
+
:param unit: angular units: 'rad' [default], or 'deg'
+
:type unit: str
+
:return: formatted string
+
:rtype: str
+
:raises ValueError: bad argument
+
+
The matrix is formatted and written to ``file`` and the
+
string is returned. To suppress writing to a file, set ``file=None``.
+
+
- ``trprint(R)`` prints the SO(3) rotation matrix to stdout in a compact
+
single-line format:
+
+
[LABEL:] ORIENTATION UNIT
+
+
- ``trprint(T)`` prints the SE(3) homogoneous transform to stdout in a
+
compact single-line format:
+
+
[LABEL:] [t=X, Y, Z;] ORIENTATION UNIT
+
+
- ``trprint(X, file=None)`` as above but returns the string rather than
+
printing to a file
+
+
Orientation is expressed in one of several formats:
+
+
- 'rpy/zyx' roll-pitch-yaw angles in ZYX axis order [default]
+
- 'rpy/yxz' roll-pitch-yaw angles in YXZ axis order
+
- 'rpy/zyx' roll-pitch-yaw angles in ZYX axis order
+
- 'eul' Euler angles in ZYZ axis order
+
- 'angvec' angle and axis
+
+
+
.. runblock:: pycon
+
+
>>> from spatialmath.base import transl, rpy2tr, trprint
+
>>> T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg')
+
>>> trprint(T, file=None)
+
>>> trprint(T, file=None, label='T', orient='angvec')
+
>>> trprint(T, file=None, label='T', orient='angvec', fmt='{:8.4g}')
+
+
.. note::
+
+
- If the 'rpy' option is selected, then the particular angle sequence can be
+
specified with the options 'xyz' or 'yxz' which are passed through to ``tr2rpy``.
+
'zyx' is the default.
+
- Default formatting is for compact display of data
+
- For tabular data set ``fmt`` to a fixed width format such as
+
``fmt='{:.3g}'``
+
+
:seealso: :func:`~spatialmath.base.transforms2d.trprint2` :func:`~tr2eul` :func:`~tr2rpy` :func:`~tr2angvec`
+
:SymPy: not supported
+
"""
+
+
s = ""
+
+
if label != "":
+
s += "{:s}: ".format(label)
+
+
# print the translational part if it exists
+
if ishom(T):
+
s += "t = {};".format(_vec2s(fmt, transl(T)))
+
+
# print the angular part in various representations
+
+
# define some aliases for rpy conventions for arms, vehicles and cameras
+
aliases = {"arm": "rpy/xyz", "vehicle": "rpy/zyx", "camera": "rpy/yxz"}
+
if orient in aliases:
+
orient = aliases[orient]
+
+
a = orient.split("/")
+
if a[0] == "rpy":
+
if len(a) == 2:
+
seq = a[1]
+
else:
+
seq = "zyx"
+
angles = tr2rpy(T, order=seq, unit=unit)
+
if degsym and unit == "deg":
+
fmt += "\u00b0"
+
s += " {} = {}".format(orient, _vec2s(fmt, angles))
+
+
elif a[0].startswith("eul"):
+
angles = tr2eul(T, unit)
+
if degsym and unit == "deg":
+
fmt += "\u00b0"
+
s += " eul = {}".format(_vec2s(fmt, angles))
+
+
elif a[0] == "angvec":
+
# as a vector and angle
+
(theta, v) = tr2angvec(T, unit)
+
if theta == 0:
+
s += " R = nil"
+
else:
+
theta = fmt.format(theta)
+
if degsym and unit == "deg":
+
theta += "\u00b0"
+
s += " angvec = ({} | {})".format(theta, _vec2s(fmt, v))
+
else:
+
raise ValueError("bad orientation format")
+
+
if file:
+
print(s, file=file)
+
+
return s
+
+
+def _vec2s(fmt, v):
+ v = [x if np.abs(x) > 1e-6 else 0.0 for x in v]
+ return ", ".join([fmt.format(x) for x in v])
+
+
+try:
+ import matplotlib.pyplot as plt
+ from mpl_toolkits.mplot3d import Axes3D
+
+ _matplotlib_exists = True
+except ImportError:
+ _matplotlib_exists = False
+
+if _matplotlib_exists:
+
+[docs] def trplot(
+
T: Union[SO3Array, SE3Array],
+
style: str = "arrow",
+
color: Union[str, Tuple[str, str, str], List[str]] = "blue",
+
frame: str = "",
+
axislabel: bool = True,
+
axissubscript: bool = True,
+
textcolor: str = "",
+
labels: Tuple[str, str, str] = ("X", "Y", "Z"),
+
length: float = 1,
+
originsize: float = 20,
+
origincolor: str = "",
+
projection: str = "ortho",
+
block: Optional[bool] = None,
+
anaglyph: Optional[Union[bool, str, Tuple[str, float]]] = None,
+
wtl: float = 0.2,
+
width: Optional[float] = None,
+
ax: Optional[Axes3D] = None,
+
dims: Optional[ArrayLikePure] = None,
+
d2: float = 1.15,
+
flo: Tuple[float, float, float] = (-0.05, -0.05, -0.05),
+
**kwargs,
+
):
+
"""
+
Plot a 3D coordinate frame
+
+
:param T: SE(3) or SO(3) matrix
+
:type T: ndarray(4,4) or ndarray(3,3) or an iterable returning same
+
:param style: axis style: 'arrow' [default], 'line', 'rgb', 'rviz' (Rviz style)
+
:type style: str
+
:param color: color of the lines defining the frame
+
:type color: str or list(3) or tuple(3) of str
+
:param textcolor: color of text labels for the frame, default ``color``
+
:type textcolor: str
+
:param frame: label the frame, name is shown below the frame and as subscripts on the frame axis labels
+
:type frame: str
+
:param axislabel: display labels on axes, default True
+
:type axislabel: bool
+
:param axissubscript: display subscripts on axis labels, default True
+
:type axissubscript: bool
+
:param labels: labels for the axes, defaults to X, Y and Z
+
:type labels: 3-tuple of strings
+
:param length: length of coordinate frame axes, default 1
+
:type length: float or array_like(3)
+
:param originsize: size of dot to draw at the origin, 0 for no dot (default 20)
+
:type originsize: int
+
:param origincolor: color of dot to draw at the origin, default is ``color``
+
:type origincolor: str
+
:param ax: the axes to plot into, defaults to current axes
+
:type ax: Axes3D reference
+
:param block: run the GUI main loop until all windows are closed, default True
+
:type block: bool
+
:param dims: dimension of plot volume as [xmin, xmax, ymin, ymax,zmin, zmax].
+
If dims is [min, max] those limits are applied to the x-, y- and z-axes.
+
:type dims: array_like(6) or array_like(2)
+
:param anaglyph: 3D anaglyph display, if True use use red-cyan glasses. To
+
set the color pass a string like ``'gb'`` for green-blue glasses. To set the
+
disparity (default 0.1) provide second argument in a tuple, eg. ``('rc', 0.2)``.
+
Bigger disparity exagerates the 3D "pop out" effect.
+
:type anaglyph: bool, str or (str, float)
+
:param wtl: width-to-length ratio for arrows, default 0.2
+
:type wtl: float
+
:param projection: 3D projection: ortho [default] or persp
+
:type projection: str
+
:param width: width of lines, default 1
+
:type width: float
+
:param flo: frame label offset, a vector for frame label text string relative
+
to frame origin, default (-0.05, -0.05, -0.05)
+
:type flo: array_like(3)
+
:param d2: distance of frame axis label text from origin, default 1.15
+
:type d2: float
+
:return: axes containing the frame
+
:rtype: Axes3DSubplot
+
:raises ValueError: bad arguments
+
+
Adds a 3D coordinate frame represented by the SO(3) or SE(3) matrix to the
+
current axes. If ``T`` is iterable then multiple frames will be drawn.
+
+
The appearance of the coordinate frame depends on many parameters:
+
+
- coordinate axes depend on:
+
- ``color`` of axes
+
- ``width`` of line
+
- ``length`` of line
+
- ``style`` which is one of:
+
+
- ``'arrow'`` [default], draw line with arrow head in ``color``
+
- ``'line'``, draw line with no arrow head in ``color``
+
- ``'rgb'``, frame axes are lines with no arrow head and red for X, green
+
for Y, blue for Z; no origin dot
+
- ``'rviz'``, frame axes are thick lines with no arrow head and red for X,
+
green for Y, blue for Z; no origin dot
+
+
- coordinate axis labels depend on:
+
+
- ``axislabel`` if True [default] label the axis, default labels are X, Y, Z
+
- ``labels`` 3-list of alternative axis labels
+
- ``textcolor`` which defaults to ``color``
+
- ``axissubscript`` if True [default] add the frame label ``frame`` as a subscript
+
for each axis label
+
+
- coordinate frame label depends on:
+
+
- `frame` the label placed inside {} near the origin of the frame
+
+
- a dot at the origin
+
+
- ``originsize`` size of the dot, if zero no dot
+
- ``origincolor`` color of the dot, defaults to ``color``
+
+
Examples::
+
+
trplot(T, frame='A')
+
trplot(T, frame='A', color='green')
+
trplot(T1, 'labels', 'UVW');
+
+
.. plot::
+
+
import matplotlib.pyplot as plt
+
from spatialmath.base import trplot, transl, rpy2tr
+
fig = plt.figure(figsize=(10,10))
+
text_opts = dict(bbox=dict(boxstyle="round",
+
fc="w",
+
alpha=0.9),
+
zorder=20,
+
family='monospace',
+
fontsize=8,
+
verticalalignment='top')
+
T = transl(2, 1, 1)@ rpy2tr(0, 0, 0)
+
+
ax = fig.add_subplot(331, projection='3d')
+
trplot(T, ax=ax, dims=[0,4])
+
ax.text(0.5, 0.5, 4.5, "trplot(T)", **text_opts)
+
ax = fig.add_subplot(332, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], originsize=0)
+
ax.text(0.5, 0.5, 4.5, "trplot(T, originsize=0)", **text_opts)
+
ax = fig.add_subplot(333, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], style='line')
+
ax.text(0.5, 0.5, 4.5, "trplot(T, style='line')", **text_opts)
+
ax = fig.add_subplot(334, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], axislabel=False)
+
ax.text(0.5, 0.5, 4.5, "trplot(T, axislabel=False)", **text_opts)
+
ax = fig.add_subplot(335, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], width=3)
+
ax.text(0.5, 0.5, 4.5, "trplot(T, width=3)", **text_opts)
+
ax = fig.add_subplot(336, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], frame='B')
+
ax.text(0.5, 0.5, 4.5, "trplot(T, frame='B')", **text_opts)
+
ax = fig.add_subplot(337, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], color='r', textcolor='k')
+
ax.text(0.5, 0.5, 4.5, "trplot(T, color='r', textcolor='k')", **text_opts)
+
ax = fig.add_subplot(338, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], labels=("u", "v", "w"))
+
ax.text(0.5, 0.5, 4.5, "trplot(T, labels=('u', 'v', 'w'))", **text_opts)
+
ax = fig.add_subplot(339, projection='3d')
+
trplot(T, ax=ax, dims=[0,4], style='rviz')
+
ax.text(0.5, 0.5, 4.5, "trplot(T, style='rviz')", **text_opts)
+
+
+
.. note:: If ``axes`` is specified the plot is drawn there, otherwise:
+
- it will draw in the current figure (as given by ``gca()``)
+
- if no axes in the current figure, it will create a 3D axes
+
- if no current figure, it will create one, and a 3D axes
+
+
.. note:: ``width`` can be set in the ``rgb`` or ``rviz`` styles to override the
+
defaults which are 1 and 8 respectively.
+
+
.. note:: The ``anaglyph`` effect is induced by drawing two versions of the
+
frame in different colors: one that corresponds to lens over the left
+
eye and one to the lens over the right eye. The view for the right eye
+
is from a view point shifted in the positive x-direction.
+
+
.. note:: The origin is normally indicated with a marker of the same color
+
as the frame. The default size is 20. This can be disabled by setting
+
its size to zero by ``originsize=0``. For ``'rgb'`` style the default is 0
+
but it can be set explicitly, and the color is as per the ``color``
+
option.
+
+
:SymPy: not supported
+
+
:seealso: :func:`tranimate` :func:`plotvol3` :func:`axes_logic`
+
"""
+
+
# TODO
+
# animation
+
# anaglyph
+
+
if dims is None:
+
ax = axes_logic(ax, 3, projection)
+
else:
+
ax = plotvol3(dims, ax=ax)
+
+
try:
+
if not ax.get_xlabel():
+
ax.set_xlabel(labels[0])
+
if not ax.get_ylabel():
+
ax.set_ylabel(labels[1])
+
if not ax.get_zlabel():
+
ax.set_zlabel(labels[2])
+
except AttributeError:
+
pass # if axes are an Animate object
+
+
if anaglyph is not None:
+
# enforce perspective projection
+
ax.set_proj_type("persp")
+
+
# collect all the arguments to use for left and right views
+
args = {
+
"ax": ax,
+
"frame": frame,
+
"length": length,
+
"style": style,
+
"wtl": wtl,
+
"flo": flo,
+
"d2": d2,
+
}
+
args = {**args, **kwargs}
+
+
# unpack the anaglyph parameters
+
shift = 0.1
+
if anaglyph is True:
+
colors = "rc"
+
elif isinstance(anaglyph, str):
+
colors = anaglyph
+
elif isinstance(anaglyph, tuple):
+
colors = anaglyph[0]
+
shift = anaglyph[1]
+
else:
+
raise ValueError("bad anaglyph value")
+
+
# the left eye sees the normal trplot
+
trplot(T, color=colors[0], **args)
+
+
# the right eye sees a from a viewpoint in shifted in the X direction
+
if isrot(T):
+
T = r2t(cast(SO3Array, T))
+
trplot(transl(shift, 0, 0) @ T, color=colors[1], **args)
+
+
return
+
+
if style == "rviz":
+
if originsize is None:
+
originsize = 0
+
color = "rgb"
+
if width is None:
+
width = 8
+
style = "line"
+
axislabel = False
+
elif style == "rgb":
+
if originsize is None:
+
originsize = 0
+
color = "rgb"
+
if width is None:
+
width = 1
+
style = "arrow"
+
+
if isinstance(color, str):
+
if color == "rgb":
+
color = ("red", "green", "blue")
+
else:
+
color = (color,) * 3
+
+
# check input types
+
if isrot(T, check=True):
+
T = r2t(cast(SO3Array, T))
+
elif ishom(T, check=True):
+
pass
+
else:
+
# assume it is an iterable
+
for Tk in T:
+
trplot(
+
Tk,
+
ax=ax,
+
block=block,
+
dims=dims,
+
color=color,
+
frame=frame,
+
textcolor=textcolor,
+
labels=labels,
+
length=length,
+
style=style,
+
projection=projection,
+
originsize=originsize,
+
origincolor=origincolor,
+
wtl=wtl,
+
width=width,
+
d2=d2,
+
flo=flo,
+
anaglyph=anaglyph,
+
axislabel=axislabel,
+
**kwargs,
+
)
+
return
+
+
if dims is not None:
+
dims = tuple(dims)
+
if len(dims) == 2:
+
dims = dims * 3
+
ax.set_xlim(left=dims[0], right=dims[1])
+
ax.set_ylim(bottom=dims[2], top=dims[3])
+
ax.set_zlim(bottom=dims[4], top=dims[5])
+
+
# create unit vectors in homogeneous form
+
if isinstance(length, Iterable):
+
axlength = getvector(length, 3)
+
else:
+
axlength = (length,) * 3
+
+
o = T @ np.array([0, 0, 0, 1])
+
x = T @ np.array([axlength[0], 0, 0, 1])
+
y = T @ np.array([0, axlength[1], 0, 1])
+
z = T @ np.array([0, 0, axlength[2], 1])
+
+
# draw the axes
+
+
if style == "arrow":
+
ax.quiver(
+
o[0],
+
o[1],
+
o[2],
+
x[0] - o[0],
+
x[1] - o[1],
+
x[2] - o[2],
+
arrow_length_ratio=wtl,
+
linewidth=width,
+
facecolor=color[0],
+
edgecolor=color[0],
+
)
+
ax.quiver(
+
o[0],
+
o[1],
+
o[2],
+
y[0] - o[0],
+
y[1] - o[1],
+
y[2] - o[2],
+
arrow_length_ratio=wtl,
+
linewidth=width,
+
facecolor=color[1],
+
edgecolor=color[1],
+
)
+
ax.quiver(
+
o[0],
+
o[1],
+
o[2],
+
z[0] - o[0],
+
z[1] - o[1],
+
z[2] - o[2],
+
arrow_length_ratio=wtl,
+
linewidth=width,
+
facecolor=color[2],
+
edgecolor=color[2],
+
)
+
+
# plot some points
+
# invisible point at the end of each arrow to allow auto-scaling to work
+
ax.scatter(
+
xs=[o[0], x[0], y[0], z[0]],
+
ys=[o[1], x[1], y[1], z[1]],
+
zs=[o[2], x[2], y[2], z[2]],
+
s=[0, 0, 0, 0],
+
)
+
elif style == "line":
+
ax.plot(
+
[o[0], x[0]],
+
[o[1], x[1]],
+
[o[2], x[2]],
+
color=color[0],
+
linewidth=width,
+
)
+
ax.plot(
+
[o[0], y[0]],
+
[o[1], y[1]],
+
[o[2], y[2]],
+
color=color[1],
+
linewidth=width,
+
)
+
ax.plot(
+
[o[0], z[0]],
+
[o[1], z[1]],
+
[o[2], z[2]],
+
color=color[2],
+
linewidth=width,
+
)
+
+
if textcolor == "":
+
textcolor = color[0]
+
+
if origincolor == "":
+
origincolor = color[0]
+
+
# label the frame
+
if frame != "":
+
o1 = T @ np.array(np.r_[flo, 1])
+
ax.text(
+
o1[0],
+
o1[1],
+
o1[2],
+
r"$\{" + frame + r"\}$",
+
color=textcolor,
+
verticalalignment="top",
+
horizontalalignment="center",
+
)
+
+
if axislabel:
+
# add the labels to each axis
+
+
x = (x - o) * d2 + o
+
y = (y - o) * d2 + o
+
z = (z - o) * d2 + o
+
+
if frame is None or not axissubscript:
+
format = "${:s}$"
+
else:
+
format = "${:s}_{{{:s}}}$"
+
+
ax.text(
+
x[0],
+
x[1],
+
x[2],
+
format.format(labels[0], frame),
+
color=textcolor,
+
horizontalalignment="center",
+
verticalalignment="center",
+
)
+
ax.text(
+
y[0],
+
y[1],
+
y[2],
+
format.format(labels[1], frame),
+
color=textcolor,
+
horizontalalignment="center",
+
verticalalignment="center",
+
)
+
ax.text(
+
z[0],
+
z[1],
+
z[2],
+
format.format(labels[2], frame),
+
color=textcolor,
+
horizontalalignment="center",
+
verticalalignment="center",
+
)
+
+
if originsize > 0:
+
ax.scatter(xs=[o[0]], ys=[o[1]], zs=[o[2]], color=origincolor, s=originsize)
+
+
if block is not None:
+
# calling this at all, causes FuncAnimation to fail so when invoked from tranimate skip this bit
+
import matplotlib.pyplot as plt
+
+
# TODO move blocking into graphics
+
plt.show(block=block)
+
return ax
+
+[docs] def tranimate(T: Union[SO3Array, SE3Array], **kwargs) -> str:
+
"""
+
Animate a 3D coordinate frame
+
+
:param T: SE(3) or SO(3) matrix
+
:type T: ndarray(4,4) or ndarray(3,3) or an iterable returning same
+
:param nframes: number of steps in the animation [default 100]
+
:type nframes: int
+
:param repeat: animate in endless loop [default False]
+
:type repeat: bool
+
:param interval: number of milliseconds between frames [default 50]
+
:type interval: int
+
:param wait: wait until animation is complete, default False
+
:type wait: bool
+
:param movie: name of file to write MP4 movie into
+
:type movie: str
+
:param **kwargs: arguments passed to ``trplot``
+
+
- ``tranimate(T)`` where ``T`` is an SO(3) or SE(3) matrix, animates a 3D
+
coordinate frame moving from the world frame to the frame ``T`` in
+
``nsteps``.
+
+
- ``tranimate(I)`` where ``I`` is an iterable or generator, animates a 3D
+
coordinate frame representing the pose of each element in the sequence of
+
SO(3) or SE(3) matrices.
+
+
Examples:
+
+
>>> tranimate(transl(1,2,3)@trotx(1), frame='A', arrow=False, dims=[0, 5])
+
>>> tranimate(transl(1,2,3)@trotx(1), frame='A', arrow=False, dims=[0, 5], movie='spin.mp4')
+
+
.. note:: For Jupyter this works with the ``notebook`` and ``TkAgg``
+
backends.
+
+
.. note:: The animation occurs in the background after ``tranimate`` has
+
returned. If ``block=True`` this blocks after the animation has completed.
+
+
.. note:: When saving animation to a file the animation does not appear
+
on screen. A ``StopIteration`` exception may occur, this seems to
+
be a matplotlib bug #19599
+
+
:SymPy: not supported
+
+
:seealso: `trplot`, `plotvol3`
+
"""
+
dim = kwargs.pop("dims", None)
+
ax = kwargs.pop("ax", None)
+
anim = Animate(dim=dim, ax=ax, **kwargs)
+
anim.trplot(T, **kwargs)
+
return anim.run(**kwargs)
+
+
+if __name__ == "__main__": # pragma: no cover
+ # import sympy
+ # from spatialmath.base.symbolic import *
+
+ # p, q, r = symbol('phi theta psi')
+ # print(p)
+
+ # print(angvelxform([p, q, r], representation='eul'))
+
+ # exec(
+ # open(
+ # pathlib.Path(__file__).parent.parent.parent.absolute()
+ # / "tests"
+ # / "base"
+ # / "test_transforms3d.py"
+ # ).read()
+ # ) # pylint: disable=exec-used
+
+ # exec(
+ # open(
+ # pathlib.Path(__file__).parent.parent.parent.absolute()
+ # / "tests"
+ # / "base"
+ # / "test_transforms3d_plot.py"
+ # # ).read()
+ # ) # pylint: disable=exec-used
+ import numpy as np
+
+ T = np.array(
+ [
+ [1, 3.881e-14, 0, -1.985e-13],
+ [-3.881e-14, 1, 1.438e-11, 1.192e-13],
+ [0, -1.438e-11, 1, 0],
+ [0, 0, 0, 1],
+ ]
+ )
+ # theta, vec = tr2angvec(T)
+ # print(theta, vec)
+ # print(trlog(T, twist=True))
+ R = rotx(np.pi / 2)
+ s = tranimate(R, movie=True)
+ with open("z.html", "w") as f:
+ print(f"<html>{s}</html", file=f)
+
+
+
+
+
+
+
+
+
