# 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,
iseye,
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, dim=0)
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, dim=0)
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, dim=0)
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'))
import pathlib
# 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)