Source code for spatialmath.base.transforms3d

# 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")
[docs]def angvelxform(𝚪, inverse=False, full=True, representation="rpy/xyz"): """ DEPRECATED, use :func:`rotvelxform` instead """ raise DeprecationWarning("use rotvelxform instead")
[docs]def angvelxform_dot(𝚪, 𝚪d, full=True, representation="rpy/xyz"): """ DEPRECATED, use :func:`rotvelxform` instead """ raise DeprecationWarning("use rotvelxform_inv_dot 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: ...
[docs]def rotvelxform( 𝚪, inverse=False, full=False, representation="rpy/xyz", ): r""" Rotational velocity transformation :param 𝚪: angular representation or rotation matrix :type 𝚪: array_like(3) or ndarray(3,3) :param representation: defaults to 'rpy/xyz' :type representation: str, optional :param inverse: compute mapping from analytical rates to angular velocity :type inverse: bool :param full: return 6x6 transform for spatial velocity :type full: bool :return: rotation rate transformation matrix :rtype: ndarray(3,3) or ndarray(6,6) Computes the transformation from analytical rates :math:`\dvec{x}` where the rotational part is expressed as the rate of change in some angular representation to spatial velocity :math:`\omega`, where rotation rate is expressed as angular velocity. .. math:: \vec{\omega} = \mat{A}(\Gamma) \dvec{x} where :math:`\mat{A}` is a 3x3 matrix and :math:`\Gamma \in \mathbb{R}^3` is a minimal angular representation. :math:`\mat{A}(\Gamma)` is a function of the rotational representation which can be specified by the parameter ``𝚪`` as a 1D array, or by an SO(3) rotation matrix which will be converted to the ``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 ============================ ======================================== If ``inverse==True`` return :math:`\mat{A}^{-1}` computed using a closed-form solution rather than matrix inverse. If ``full=True`` a block diagonal 6x6 matrix is returned which transforms analytic velocity to spatial velocity. .. note:: Similar to :func:`eul2jac` :func:`rpy2jac` :func:`exp2jac` with ``full=False``. The analytical Jacobian is .. math:: \mat{J}_a(q) = \mat{A}^{-1}(\Gamma)\, \mat{J}(q) where :math:`\mat{A}` is computed with ``inverse==True`` and ``full=True``. Reference: - ``symbolic/angvelxform.ipynb`` in this Toolbox - 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_inv_dot` :func:`eul2jac` :func:`rpy2r` :func:`exp2jac` """ if isrot(𝚪): # passed a rotation matrix # convert to the representation 𝚪 = r2x(𝚪, representation=representation) if sym.issymbol(𝚪): C = sym.cos S = sym.sin T = sym.tan else: C = math.cos S = math.sin T = math.tan if representation in ("rpy/xyz", "arm"): alpha, beta, gamma = 𝚪 # autogenerated by symbolic/angvelxform.ipynb if not inverse: # analytical rates -> angular velocity # fmt: off A = np.array([ [ S(beta), 0, 1], [-S(gamma)*C(beta), C(gamma), 0], # type: ignore [ C(beta)*C(gamma), S(gamma), 0] # type: ignore ]) # fmt: on else: # angular velocity -> analytical rates # fmt: off A = np.array([ [0, -S(gamma)/C(beta), C(gamma)/C(beta)], # type: ignore [0, C(gamma), S(gamma)], [1, S(gamma)*T(beta), -C(gamma)*T(beta)] # type: ignore ]) # fmt: on elif representation in ("rpy/zyx", "vehicle"): alpha, beta, gamma = 𝚪 # autogenerated by symbolic/angvelxform.ipynb if not inverse: # analytical rates -> angular velocity # fmt: off A = np.array([ [C(beta)*C(gamma), -S(gamma), 0], # type: ignore [S(gamma)*C(beta), C(gamma), 0], # type: ignore [-S(beta), 0, 1] # type: ignore ]) # type: ignore # fmt: on else: # angular velocity -> analytical rates # fmt: off A = np.array([ [C(gamma)/C(beta), S(gamma)/C(beta), 0], # type: ignore [-S(gamma), C(gamma), 0], # type: ignore [C(gamma)*T(beta), S(gamma)*T(beta), 1] # type: ignore ]) # fmt: on elif representation in ("rpy/yxz", "camera"): alpha, beta, gamma = 𝚪 # autogenerated by symbolic/angvelxform.ipynb if not inverse: # analytical rates -> angular velocity # fmt: off A = np.array([ [ S(gamma)*C(beta), C(gamma), 0], # type: ignore [-S(beta), 0, 1], # type: ignore [ C(beta)*C(gamma), -S(gamma), 0] # type: ignore ]) # fmt: on else: # angular velocity -> analytical rates # fmt: off A = np.array([ [S(gamma)/C(beta), 0, C(gamma)/C(beta)], # type: ignore [C(gamma), 0, -S(gamma)], # type: ignore [S(gamma)*T(beta), 1, C(gamma)*T(beta)] # type: ignore ]) # type: ignore # fmt: on elif representation == "eul": phi, theta, psi = 𝚪 # autogenerated by symbolic/angvelxform.ipynb if not inverse: # analytical rates -> angular velocity # fmt: off A = np.array([ [0, -S(phi), S(theta)*C(phi)], # type: ignore [0, C(phi), S(phi)*S(theta)], # type: ignore [1, 0, C(theta)] ]) # fmt: on else: # angular velocity -> analytical rates # fmt: off A = np.array([ [-C(phi)/T(theta), -S(phi)/T(theta), 1], # type: ignore [-S(phi), C(phi), 0], # type: ignore [ C(phi)/S(theta), S(phi)/S(theta), 0] # type: ignore ]) # fmt: on elif representation == "exp": # from ETHZ class notes sk = skew(𝚪) theta = norm(𝚪) if not inverse: # analytical rates -> angular velocity # (2.106) A = ( np.eye(3) + sk * (1 - C(theta)) / theta**2 + sk @ sk * (theta - S(theta)) / theta**3 ) else: # angular velocity -> analytical rates # (2.107) A = ( np.eye(3) - sk / 2 + sk @ sk / theta**2 * (1 - (theta / 2) * (S(theta) / (1 - C(theta)))) ) else: raise ValueError("unknown representation") if full: AA = np.eye(6) AA[3:, 3:] = A return AA else: return A
@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: ...
[docs]def rotvelxform_inv_dot( 𝚪: ArrayLike3, 𝚪d: ArrayLike3, full: bool = False, representation: str = "rpy/xyz" ) -> Union[R3x3, R6x6]: r""" Derivative of angular velocity transformation :param 𝚪: angular representation :type 𝚪: array_like(3) :param 𝚪d: angular representation rate :math:`\dvec{\Gamma}` :type 𝚪d: array_like(3) :param representation: defaults to 'rpy/xyz' :param full: return 6x6 transform for spatial velocity :return: derivative of inverse angular velocity transformation matrix :rtype: ndarray(6,6) or ndarray(3,3) The angular rate transformation matrix :math:`\mat{A} \in \mathbb{R}^{6 \times 6}` is such that .. math:: \dvec{x} = \mat{A}^{-1}(\Gamma) \vec{\nu} where :math:`\dvec{x} \in \mathbb{R}^6` is analytic velocity :math:`(\vec{v}, \dvec{\Gamma})`, :math:`\vec{\nu} \in \mathbb{R}^6` is spatial velocity :math:`(\vec{v}, \vec{\omega})`, and :math:`\vec{\Gamma} \in \mathbb{R}^3` is a minimal rotational representation. The relationship between spatial and analytic acceleration is .. math:: \ddvec{x} = \dmat{A}^{-1}(\Gamma, \dot{\Gamma}) \vec{\nu} + \mat{A}^{-1}(\Gamma) \dvec{\nu} and :math:`\dmat{A}^{-1}(\Gamma, \dot{\Gamma})` is computed by this function. ============================ ======================================== ``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 ============================ ======================================== If ``full=True`` a block diagonal 6x6 matrix is returned which transforms analytic analytic rotational acceleration to angular acceleration. Reference: - ``symbolic/angvelxform.ipynb`` in this Toolbox - ``symbolic/angvelxform_dot.ipynb`` in this Toolbox :seealso: :func:`rotvelxform` :func:`eul2jac` :func:`rpy2r` :func:`exp2jac` """ if sym.issymbol(𝚪): C = sym.cos S = sym.sin T = sym.tan else: C = math.cos S = math.sin T = math.tan if representation in ("rpy/xyz", "arm"): # autogenerated by symbolic/angvelxform.ipynb alpha, beta, gamma = 𝚪 alpha_dot, beta_dot, gamma_dot = 𝚪d Ainv_dot = np.array( [ [ 0, -( beta_dot * math.sin(beta) * S(gamma) / C(beta) + gamma_dot * C(gamma) ) / C(beta), (beta_dot * S(beta) * C(gamma) / C(beta) - gamma_dot * S(gamma)) / C(beta), ], [0, -gamma_dot * S(gamma), gamma_dot * C(gamma)], [ 0, beta_dot * S(gamma) / C(beta) ** 2 + gamma_dot * C(gamma) * math.tan(beta), -beta_dot * C(gamma) / C(beta) ** 2 + gamma_dot * S(gamma) * math.tan(beta), ], ] # type: ignore ) elif representation in ("rpy/zyx", "vehicle"): # autogenerated by symbolic/angvelxform.ipynb alpha, beta, gamma = 𝚪 alpha_dot, beta_dot, gamma_dot = 𝚪d Ainv_dot = np.array( [ [ (beta_dot * S(beta) * C(gamma) / C(beta) - gamma_dot * S(gamma)) / C(beta), (beta_dot * S(beta) * S(gamma) / C(beta) + gamma_dot * C(gamma)) / C(beta), 0, ], [-gamma_dot * C(gamma), -gamma_dot * S(gamma), 0], [ beta_dot * C(gamma) / C(beta) ** 2 - gamma_dot * S(gamma) * math.tan(beta), beta_dot * S(gamma) / C(beta) ** 2 + gamma_dot * C(gamma) * math.tan(beta), 0, ], ] # type: ignore ) elif representation in ("rpy/yxz", "camera"): # autogenerated by symbolic/angvelxform.ipynb alpha, beta, gamma = 𝚪 alpha_dot, beta_dot, gamma_dot = 𝚪d Ainv_dot = np.array( [ [ (beta_dot * S(beta) * S(gamma) / C(beta) + gamma_dot * C(gamma)) / C(beta), 0, (beta_dot * S(beta) * C(gamma) / C(beta) - gamma_dot * S(gamma)) / C(beta), ], [-gamma_dot * S(gamma), 0, -gamma_dot * C(gamma)], [ beta_dot * S(gamma) / C(beta) ** 2 + gamma_dot * C(gamma) * T(beta), 0, beta_dot * C(gamma) / C(beta) ** 2 - gamma_dot * S(gamma) * T(beta), ], ] # type: ignore ) elif representation == "eul": # autogenerated by symbolic/angvelxform.ipynb phi, theta, psi = 𝚪 phi_dot, theta_dot, psi_dot = 𝚪d Ainv_dot = np.array( [ [ phi_dot * S(phi) / math.tan(theta) + theta_dot * C(phi) / S(theta) ** 2, -phi_dot * C(phi) / math.tan(theta) + theta_dot * S(phi) / S(theta) ** 2, 0, ], [-phi_dot * C(phi), -phi_dot * S(phi), 0], [ -(phi_dot * S(phi) + theta_dot * C(phi) * C(theta) / S(theta)) / S(theta), (phi_dot * C(phi) - theta_dot * S(phi) * C(theta) / S(theta)) / S(theta), 0, ], ] # type: ignore ) elif representation == "exp": sk = skew(𝚪) theta = norm(𝚪) skd = skew(𝚪d) theta_dot = np.inner(𝚪, 𝚪d) / norm(𝚪) Theta = (1.0 - theta / 2.0 * np.sin(theta) / (1.0 - np.cos(theta))) / theta**2 # hand optimized version of code from notebook symbolic/angvelxform_dot.ipynb Theta_dot = ( -theta * C(theta) - S(theta) + theta * S(theta) ** 2 / (1 - C(theta)) ) * theta_dot / 2 / (1 - C(theta)) / theta**2 - ( 2 - theta * S(theta) / (1 - C(theta)) ) * theta_dot / theta**3 Ainv_dot = -0.5 * skd + (sk @ skd + skd @ sk) * Theta + sk @ sk * Theta_dot else: raise ValueError("bad representation specified") if full: Afull = np.zeros((6, 6)) Afull[3:, 3:] = Ainv_dot return Afull else: return Ainv_dot
@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)