# Part of Spatial Math Toolbox for Python
# Copyright (c) 2000 Peter Corke
# MIT Licence, see details in top-level file: LICENCE
"""
Classes to abstract 3D pose and orientation using matrices in SE(3) and SO(3)
To use::
from spatialmath.pose3d import *
T = SE3.Rx(0.3)
import spatialmath as sm
T = sm.SE3.Rx(0.3)
.. inheritance-diagram:: spatialmath.pose3d
:top-classes: collections.UserList
:parts: 1
.. image:: ../figs/pose-values.png
"""
from __future__ import annotations
# pylint: disable=invalid-name
import numpy as np
import spatialmath.base as smb
from spatialmath.base.types import *
from spatialmath.base.vectors import orthogonalize
from spatialmath.baseposematrix import BasePoseMatrix
from spatialmath.pose2d import SE2
from spatialmath.twist import Twist3
from typing import TYPE_CHECKING
if TYPE_CHECKING:
from spatialmath.quaternion import UnitQuaternion
# ============================== SO3 =====================================#
[docs]class SO3(BasePoseMatrix):
"""
SO(3) matrix class
This subclass represents rotations in 3D space. Internally it is a 3x3
orthogonal matrix belonging to the group SO(3).
.. inheritance-diagram:: spatialmath.pose3d.SO3
:top-classes: collections.UserList
:parts: 1
"""
@overload
def __init__(self):
...
@overload
def __init__(self, arg: SO3, *, check=True):
...
@overload
def __init__(self, arg: SE3, *, check=True):
...
@overload
def __init__(self, arg: SO3Array, *, check=True):
...
@overload
def __init__(self, arg: List[SO3Array], *, check=True):
...
@overload
def __init__(self, arg: List[Union[SO3, SO3Array]], *, check=True):
...
[docs] def __init__(self, arg=None, *, check=True):
"""
Construct new SO(3) object
:rtype: SO3 instance
There are multiple call signatures:
- ``SO3()`` is an ``SO3`` instance with one value -- a 3x3 identity
matrix which corresponds to a null rotation
- ``SO3(R)`` is an ``SO3`` instance with with the value ``R`` which is a
3x3 numpy array representing an SO(3) rotation matrix. If ``check``
is ``True`` check the matrix belongs to SO(3).
- ``SO3([R1, R2, ... RN])`` is an ``SO3`` instance wwith ``N`` values
given by the elements ``Ri`` each of which is a 3x3 NumPy array
representing an SO(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SO(3).
- ``SO3([X1, X2, ... XN])`` is an ``SO3`` instance with ``N`` values
given by the elements ``Xi`` each of which is an SO3 or SE3 instance.
:SymPy: supported
"""
super().__init__()
if isinstance(arg, SE3):
self.data = [smb.t2r(x) for x in arg.data]
elif not super().arghandler(arg, check=check):
raise ValueError("bad argument to constructor")
@staticmethod
def _identity() -> R3x3:
return np.eye(3)
# ------------------------------------------------------------------------ #
@property
def shape(self) -> Tuple[int, int]:
"""
Shape of the object's interal matrix representation
:return: (3,3)
:rtype: tuple
Each value within the ``SO3`` instance is a NumPy array of this shape.
"""
return (3, 3)
@property
def R(self) -> SO3Array:
"""
SO(3) or SE(3) as rotation matrix
:return: rotational component
:rtype: ndarray(3,3)
``x.R`` is the rotation matrix component of ``x`` as an array with
shape (3,3). If ``len(x) > 1``, return an array with shape=(N,3,3).
.. warning:: The i'th rotation matrix is ``x[i,:,:]`` or simply
``x[i]``. This is different to the MATLAB version where the i'th
rotation matrix is ``x(:,:,i)``.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> x = SO3.Rx(0.3)
>>> x.R
:SymPy: supported
"""
if len(self) == 1:
return self.A[:3, :3] # type: ignore
else:
return np.array([x[:3, :3] for x in self.A]) # type: ignore
@property
def n(self) -> R3:
"""
Normal vector of SO(3) or SE(3)
:return: normal vector
:rtype: ndarray(3)
This is the first column of the rotation submatrix, sometimes called the
*normal vector*. It is parallel to the x-axis of the frame defined by
this pose.
"""
if len(self) != 1:
raise ValueError("can only determine n-vector for singleton pose")
return self.A[:3, 0] # type: ignore
@property
def o(self) -> R3:
"""
Orientation vector of SO(3) or SE(3)
:return: orientation vector
:rtype: ndarray(3)
This is the second column of the rotation submatrix, sometimes called
the *orientation vector*. It is parallel to the y-axis of the frame
defined by this pose.
"""
if len(self) != 1:
raise ValueError("can only determine o-vector for singleton pose")
return self.A[:3, 1] # type: ignore
@property
def a(self) -> R3:
"""
Approach vector of SO(3) or SE(3)
:return: approach vector
:rtype: ndarray(3)
This is the third column of the rotation submatrix, sometimes called the
*approach vector*. It is parallel to the z-axis of the frame defined by
this pose.
"""
if len(self) != 1:
raise ValueError("can only determine a-vector for singleton pose")
return self.A[:3, 2] # type: ignore
# ------------------------------------------------------------------------ #
[docs] def inv(self) -> Self:
"""
Inverse of SO(3)
:return: inverse
:rtype: SO2 instance
Efficiently compute the inverse of each of the SO(3) values taking into
account the matrix structure. For an SO(3) matrix the inverse is the
transpose.
"""
if len(self) == 1:
return SO3(self.A.T, check=False) # type: ignore
else:
return SO3([x.T for x in self.A], check=False)
[docs] def eul(self, unit: str = "rad", flip: bool = False) -> Union[R3, RNx3]:
r"""
SO(3) or SE(3) as Euler angles
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3-vector of Euler angles
:rtype: ndarray(3,), ndarray(n,3)
``x.eul`` is the Euler angle representation of the rotation. Euler angles are
a 3-vector :math:`(\phi, \theta, \psi)` which correspond to consecutive
rotations about the Z, Y, Z axes respectively.
If ``len(x)`` is:
- 1, return an ndarray with shape=(3,)
- N>1, return ndarray with shape=(3,N)
:seealso: :func:`~spatialmath.pose3d.SE3.Eul`, :func:`~spatialmath.base.transforms3d.tr2eul`
:SymPy: not supported
"""
if len(self) == 1:
return smb.tr2eul(self.A, unit=unit, flip=flip) # type: ignore
else:
return np.array([base.tr2eul(x, unit=unit, flip=flip) for x in self.A])
[docs] def rpy(self, unit: str = "rad", order: str = "zyx") -> Union[R3, RNx3]:
"""
SO(3) or SE(3) as roll-pitch-yaw angles
:param order: angle sequence order, default to 'zyx'
:type order: str
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3-vector of roll-pitch-yaw angles
:rtype: ndarray(3,), ndarray(n,3)
``x.rpy`` is the roll-pitch-yaw angle representation of the rotation. The angles are
a 3-vector :math:`(r, p, y)` which correspond to successive rotations about the axes
specified by ``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll 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.
If `len(x)` is:
- 1, return an ndarray with shape=(3,)
- N>1, return ndarray with shape=(3,N)
:seealso: :func:`~spatialmath.pose3d.SE3.RPY`, :func:`~spatialmath.base.transforms3d.tr2rpy`
:SymPy: not supported
"""
if len(self) == 1:
return smb.tr2rpy(self.A, unit=unit, order=order) # type: ignore
else:
return np.array([smb.tr2rpy(x, unit=unit, order=order) for x in self.A])
[docs] def angvec(self, unit: str = "rad") -> Tuple[float, R3]:
r"""
SO(3) or SE(3) as angle and rotation vector
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: :math:`(\theta, \hat{\bf v})`
:rtype: float or ndarray(3)
``x.angvec()`` is a tuple :math:`(\theta, v)` containing the rotation
angle and a rotation axis.
By default the angle is in radians but can be changed setting `unit='deg'`.
.. note::
- If the input is SE(3) the translation component is ignored.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> R = SO3.Rx(0.3)
>>> R.angvec()
:seealso: :meth:`eulervec` :meth:`AngVec` :meth:`~spatialmath.quaternion.UnitQuaternion.angvec` :meth:`~spatialmath.quaternion.AngVec`, :func:`~angvec2r`
"""
return smb.tr2angvec(self.R, unit=unit)
[docs] def eulervec(self) -> R3:
r"""
SO(3) or SE(3) as Euler vector (exponential coordinates)
:return: :math:`\theta \hat{\bf v}`
:rtype: ndarray(3)
``x.eulervec()`` is the Euler vector (or exponential coordinates) which
is related to angle-axis notation and is the product of the rotation
angle and the rotation axis.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> R = SO3.Rx(0.3)
>>> R.eulervec()
.. note::
- If the input is SE(3) the translation component is ignored.
:seealso: :meth:`angvec` :func:`~angvec2r`
"""
theta, v = smb.tr2angvec(self.R)
return theta * v
# ------------------------------------------------------------------------ #
[docs] @staticmethod
def isvalid(x: NDArray, check: bool = True) -> bool:
"""
Test if matrix is valid SO(3)
:param x: matrix to test
:type x: numpy.ndarray
:return: ``True`` if the matrix is a valid element of SO(3), ie. it is a 3x3
orthonormal matrix with determinant of +1.
:rtype: bool
:seealso: :func:`~spatialmath.base.transform3d.isrot`
"""
return smb.isrot(x, check=True)
# ---------------- variant constructors ---------------------------------- #
[docs] @classmethod
def Rx(cls, theta: float, unit: str = "rad") -> Self:
"""
Construct a new SO(3) from X-axis rotation
:param θ: rotation angle about the X-axis
:type θ: float or array_like
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SO(3) rotation
:rtype: SO3 instance
- ``SE3.Rx(θ)`` is an SO(3) rotation of ``θ`` radians about the x-axis
- ``SE3.Rx(θ, "deg")`` as above but ``θ`` is in degrees
If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
elements.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> import numpy as np
>>> x = SO3.Rx(np.linspace(0, math.pi, 20))
>>> len(x)
>>> x[7]
"""
return cls([smb.rotx(x, unit=unit) for x in smb.getvector(theta)], check=False)
[docs] @classmethod
def Ry(cls, theta, unit: str = "rad") -> Self:
"""
Construct a new SO(3) from Y-axis rotation
:param θ: rotation angle about Y-axis
:type θ: float or array_like
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.Ry(θ)`` is an SO(3) rotation of ``θ`` radians about the y-axis
- ``SO3.Ry(θ, "deg")`` as above but ``θ`` is in degrees
If ``θ`` is an array then the result is a sequence of rotations defined by consecutive
elements.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> import numpy as np
>>> x = SO3.Ry(np.linspace(0, math.pi, 20))
>>> len(x)
>>> x[7]
"""
return cls([smb.roty(x, unit=unit) for x in smb.getvector(theta)], check=False)
[docs] @classmethod
def Rz(cls, theta, unit: str = "rad") -> Self:
"""
Construct a new SO(3) from Z-axis rotation
:param θ: rotation angle about Z-axis
:type θ: float or array_like
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.Rz(θ)`` is an SO(3) rotation of ``θ`` radians about the z-axis
- ``SO3.Rz(θ, "deg")`` as above but ``θ`` is in degrees
If ``θ`` is an array then the result is a sequence of rotations defined by consecutive
elements.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> import numpy as np
>>> x = SO3.Rz(np.linspace(0, math.pi, 20))
>>> len(x)
>>> x[7]
"""
return cls([smb.rotz(x, unit=unit) for x in smb.getvector(theta)], check=False)
[docs] @classmethod
def Rand(cls, N: int = 1) -> Self:
"""
Construct a new SO(3) from random rotation
:param N: number of random rotations
:type N: int
:return: SO(3) rotation matrix
:rtype: SO3 instance
- ``SO3.Rand()`` is a random SO(3) rotation.
- ``SO3.Rand(N)`` is a sequence of N random rotations.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> x = SO3.Rand()
>>> x
:seealso: :func:`spatialmath.quaternion.UnitQuaternion.Rand`
"""
return cls([smb.q2r(smb.qrand()) for _ in range(0, N)], check=False)
@overload
@classmethod
def Eul(cls, *angles: float, unit: str = "rad") -> Self:
...
@overload
@classmethod
def Eul(cls, *angles: Union[ArrayLike3, RNx3], unit: str = "rad") -> Self:
...
[docs] @classmethod
def Eul(cls, *angles, unit: str = "rad") -> Self:
r"""
Construct a new SO(3) from Euler angles
:param 𝚪: Euler angles
:type 𝚪: 3 floats, array_like(3) or ndarray(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.Eul(𝚪)`` is an SO(3) rotation defined by a 3-vector of Euler
angles :math:`\Gamma = (\phi, \theta, \psi)` which correspond to
consecutive rotations about the Z, Y, Z axes respectively. If ``𝚪``
is an Nx3 matrix then the result is a sequence of rotations each
defined by Euler angles corresponding to the rows of ``angles``.
``SO3.Eul(φ, θ, ψ)`` as above but the angles are provided as three
scalars.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> SO3.Eul(0.1, 0.2, 0.3)
>>> SO3.Eul([0.1, 0.2, 0.3])
>>> SO3.Eul(10, 20, 30, unit="deg")
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`~spatialmath.base.transforms3d.eul2r`
"""
if len(angles) == 1:
angles = angles[0]
if smb.isvector(angles, 3):
return cls(smb.eul2r(angles, unit=unit), check=False)
else:
return cls([smb.eul2r(a, unit=unit) for a in angles], check=False)
@overload
@classmethod
def RPY(
cls,
*angles: float,
unit: str = "rad",
order="zyx",
) -> Self:
...
@overload
@classmethod
def RPY(
cls, *angles: Union[ArrayLike3, RNx3], unit: str = "rad", order="zyx"
) -> Self:
...
[docs] @classmethod
def RPY(cls, *angles, unit="rad", order="zyx"):
r"""
Construct a new SO(3) from roll-pitch-yaw angles
:param angles: roll-pitch-yaw angles
:type angles: array_like(3), array_like(n,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type order: str
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.RPY(angles)`` is an SO(3) rotation defined by a 3-vector of
roll, pitch, yaw angles :math:`(\alpha, \beta, \gamma)`. If ``angles``
is an Nx3 matrix then the result is a sequence of rotations each
defined by RPY angles corresponding to the rows of angles. The angles
correspond to successive rotations about the axes specified by
``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll 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.
- ``SO3.RPY(⍺, β, 𝛾)`` as above but the angles are provided as three
scalars.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> SO3.RPY(0.1, 0.2, 0.3)
>>> SO3.RPY([0.1, 0.2, 0.3])
>>> SO3.RPY(0.1, 0.2, 0.3, order='xyz')
>>> SO3.RPY(10, 20, 30, unit="deg")
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
"""
if len(angles) == 1:
angles = angles[0]
# angles = base.getmatrix(angles, (None, 3))
# return cls(base.rpy2r(angles, order=order, unit=unit), check=False)
if smb.isvector(angles, 3):
return cls(smb.rpy2r(angles, unit=unit, order=order), check=False)
else:
return cls(
[smb.rpy2r(a, unit=unit, order=order) for a in angles], check=False
)
[docs] @classmethod
def OA(cls, o: ArrayLike3, a: ArrayLike3) -> Self:
"""
Construct a new SO(3) from two vectors
:param o: 3-vector parallel to Y- axis
:type o: array_like
:param a: 3-vector parallel to the Z-axis
:type o: array_like
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.OA(O, A)`` is an SO(3) rotation defined in terms of
vectors parallel to the Y- and Z-axes of its 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.
.. note::
- Only the ``A`` vector is 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
:seealso: :func:`spatialmath.base.transforms3d.oa2r`
"""
return cls(smb.oa2r(o, a), check=False)
[docs] @classmethod
def TwoVectors(
cls,
x: Optional[Union[str, ArrayLike3]] = None,
y: Optional[Union[str, ArrayLike3]] = None,
z: Optional[Union[str, ArrayLike3]] = None,
) -> Self:
"""
Construct a new SO(3) from any two vectors
:param x: new x-axis, defaults to None
:type x: str, array_like(3), optional
:param y: new y-axis, defaults to None
:type y: str, array_like(3), optional
:param z: new z-axis, defaults to None
:type z: str, array_like(3), optional
Create a rotation by defining the direction of two of the new
axes in terms of the old axes. Axes are denoted by strings ``"x"``,
``"y"``, ``"z"``, ``"-x"``, ``"-y"``, ``"-z"``.
The directions can also be specified by 3-element vectors. If the vectors are not orthogonal,
they will orthogonalized w.r.t. the first available dimension. I.e. if x is available, it will be
normalized and the remaining vector will be orthogonalized w.r.t. x, else, y will be normalized
and z will be orthogonalized w.r.t. y.
To create a rotation where the new frame has its x-axis in -z-direction
of the previous frame, and its z-axis in the x-direction of the previous
frame is::
>>> SO3.TwoVectors(x='-z', z='x')
"""
def vval(v):
if isinstance(v, str):
sign = 1
if v[0] == "-":
sign = -1
v = v[1:] # skip sign char
elif v[0] == "+":
v = v[1:] # skip sign char
if v[0] == "x":
v = [sign, 0, 0]
elif v[0] == "y":
v = [0, sign, 0]
elif v[0] == "z":
v = [0, 0, sign]
return np.r_[v]
else:
return smb.unitvec(smb.getvector(v, 3))
if x is not None and y is not None and z is not None:
raise ValueError(
"Only two vectors should be provided. Please set one to None."
)
elif x is not None and y is not None and z is None:
# z = x x y
x = vval(x)
y = vval(y)
# Orthogonalizes y w.r.t. x
y = orthogonalize(y, x, normalize=True)
z = np.cross(x, y)
elif x is None and y is not None and z is not None:
# x = y x z
y = vval(y)
z = vval(z)
# Orthogonalizes z w.r.t. y
z = orthogonalize(z, y, normalize=True)
x = np.cross(y, z)
elif x is not None and y is None and z is not None:
# y = z x x
z = vval(z)
x = vval(x)
# Orthogonalizes z w.r.t. x
z = orthogonalize(z, x, normalize=True)
y = np.cross(z, x)
else:
raise ValueError(
"Insufficient number of vectors. Please provide exactly two vectors."
)
return cls(np.c_[x, y, z], check=True)
[docs] @classmethod
def AngleAxis(cls, theta: float, v: ArrayLike3, *, unit: str = "rad") -> Self:
r"""
Construct a new 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: rotation axis, 3-vector
:type v: array_like
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.AngleAxis(theta, V)`` is an SO(3) rotation defined by
a rotation of ``THETA`` about the vector ``V``.
.. note:: :math:`\theta \eq 0` the result in an identity matrix, otherwise
``V`` must have a finite length, ie. :math:`|V| > 0`.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`spatialmath.base.transforms3d.angvec2r`
"""
return cls(smb.angvec2r(theta, v, unit=unit), check=False)
[docs] @classmethod
def AngVec(cls, theta, v, *, unit="rad") -> Self:
r"""
Construct a new 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: rotation axis, 3-vector
:type v: array_like
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.AngVec(theta, V)`` is an SO(3) rotation defined by
a rotation of ``THETA`` about the vector ``V``.
.. deprecated:: 0.9.8
Use :meth:`AngleAxis` instead.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`spatialmath.base.transforms3d.angvec2r`
"""
return cls(smb.angvec2r(theta, v, unit=unit), check=False)
[docs] @classmethod
def EulerVec(cls, w) -> Self:
r"""
Construct a new SO(3) rotation matrix from an Euler rotation vector
:param ω: rotation axis
:type ω: 3-element array_like
:return: SO(3) rotation
:rtype: SO3 instance
``SO3.EulerVec(ω)`` is a unit quaternion that describes the 3D rotation
defined by a rotation of :math:`\theta = \lVert \omega \rVert` about the
unit 3-vector :math:`\omega / \lVert \omega \rVert`.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> SO3.EulerVec([0.5,0,0])
.. note:: :math:`\theta \eq 0` the result in an identity matrix, otherwise
``V`` must have a finite length, ie. :math:`|V| > 0`.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`~spatialmath.base.transforms3d.angvec2r`
"""
assert smb.isvector(w, 3), "w must be a 3-vector"
w = smb.getvector(w)
theta = smb.norm(w)
return cls(smb.angvec2r(theta, w), check=False)
[docs] @classmethod
def Exp(
cls,
S: Union[R3, RNx3],
check: bool = True,
so3: bool = True,
) -> Self:
r"""
Create an SO(3) rotation matrix from so(3)
:param S: Lie algebra so(3)
:type S: ndarray(3,3), ndarray(n,3)
:param check: check that passed matrix is valid so(3), default True
:bool check: bool, optional
:param so3: the input is interpretted as an so(3) matrix not a stack of three twists, default True
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra
which is a 3x3 so(3) matrix (skew symmetric)
- ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist
vector (the unique elements of the so(3) skew-symmetric matrix)
- ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix
of twist vectors, one per row.
.. note::
- if :math:`\theta \eq 0` the result in an identity matrix
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this
case the parameter `so3` is the decider.
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if smb.ismatrix(S, (-1, 3)) and not so3:
return cls([smb.trexp(s, check=check) for s in S], check=False)
else:
return cls(smb.trexp(cast(R3, S), check=check), check=False)
[docs] def UnitQuaternion(self) -> UnitQuaternion:
"""
SO3 as a unit quaternion instance
:return: a unit quaternion representation
:rtype: UnitQuaternion instance
``R.UnitQuaternion()`` is an ``UnitQuaternion`` instance representing the same rotation
as the SO3 rotation ``R``.
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> SO3.Rz(0.3).UnitQuaternion()
"""
# Function level import to avoid circular dependencies
from spatialmath import UnitQuaternion
return UnitQuaternion(smb.r2q(self.R), check=False)
[docs] def angdist(self, other: SO3, metric: int = 6) -> Union[float, ndarray]:
r"""
Angular distance metric between rotations
:param other: second rotation
:type other: SO3 instance
:param metric: metric, default is 6
:type metric: int
:raises TypeError: if other is not an SO3
:return: angle in radians
:rtype: float or ndarray
``R1.angdist(R2)`` is the geodesic norm, or geodesic distance between two
rotations.
Several metrics are supported, the first 5 are computed after conversion
to unit quaternions.
====== ===============================================================
Metric Details
====== ===============================================================
0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
5 :math:`\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]`
6 :math:`\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]`
====== ===============================================================
Example:
.. runblock:: pycon
>>> from spatialmath import SO3
>>> R1 = SO3.Rx(0.3)
>>> R2 = SO3.Ry(0.3)
>>> print(R1.angdist(R1))
>>> print(R1.angdist(R2))
.. note::
- metrics 1, 2, 4 can throw ValueError "math domain error" due to
numeric errors which push the argument of ``acos()`` marginally
outside its domain [0, 1].
- metrics 2 and 3 are equivalent, but 3 is more robust
:seealso: :func:`UnitQuaternion.angdist`
"""
if metric < 5:
from spatialmath.quaternion import UnitQuaternion
return UnitQuaternion(self).angdist(UnitQuaternion(other), metric=metric)
elif metric == 5:
op = lambda R1, R2: np.linalg.norm(np.eye(3) - R1 @ R2.T)
elif metric == 6:
op = lambda R1, R2: smb.norm(smb.trlog(R1 @ R2.T, twist=True))
else:
raise ValueError("unknown metric")
ad = self._op2(other, op)
if isinstance(ad, list):
return np.array(ad)
else:
return ad
# ============================== SE3 =====================================#
[docs]class SE3(SO3):
"""
SE(3) matrix class
This subclass represents rigid-body motion in 3D space. Internally it is a
4x4 homogeneous transformation matrix belonging to the group SE(3).
.. inheritance-diagram:: spatialmath.pose3d.SE3
:top-classes: collections.UserList
:parts: 1
"""
@overload
def __init__(self): # identity
...
@overload
def __init__(self, x: Union[SE3, SO3, SE2], *, check=True): # copy/promote
...
@overload
def __init__(self, x: List[SE3], *, check=True): # import list of SE3
...
@overload
def __init__(self, x: float, y: float, z: float, *, check=True): # pure translation
...
@overload
def __init__(self, x: ArrayLike3, *, check=True): # pure translation
...
@overload
def __init__(self, x: SE3Array, *, check=True): # import native array
...
@overload
def __init__(self, x: List[SE3Array], *, check=True): # import native arrays
...
[docs] def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:rtype: SE3 instance
There are multiple call signatures that return an ``SE3`` instance
with one or more values.
- ``SE3()`` null motion, value is the identity matrix.
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` where ``T`` is a 4x4 Numpy array representing an SE(3)
matrix. If ``check`` is ``True`` check the matrix belongs to SE(3).
- ``SE3([T1, T2, ... TN])`` has ``N`` values
given by the elements ``Ti`` each of which is a 4x4 NumPy array
representing an SE(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SE(3).
- ``SE3(X)`` where ``X`` is:
- ``SE3`` is a copy of ``X``
- ``SO3`` is the rotation of ``X`` with zero translation
- ``SE2`` is the z-axis rotation and x- and y-axis translation of
``X``
- ``SE3([X1, X2, ... XN])`` has ``N`` values
given by the elements ``Xi`` each of which is an SE3 instance.
:SymPy: supported
"""
if y is None and z is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif isinstance(x, SO3):
self.data = [smb.r2t(_x) for _x in x.data]
elif isinstance(x, SE2): # type(x).__name__ == "SE2":
self.data = x.SE3().data
elif smb.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [smb.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [smb.transl(T) for T in x]
else:
raise ValueError("bad argument to constructor")
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [smb.transl(x, y, z)]
else:
raise ValueError("Invalid arguments. See documentation for correct format.")
@staticmethod
def _identity() -> NDArray:
return np.eye(4)
# ------------------------------------------------------------------------ #
@property
def shape(self) -> Tuple[int, int]:
"""
Shape of the object's internal matrix representation
:return: (4,4)
:rtype: tuple
Each value within the ``SE3`` instance is a NumPy array of this shape.
"""
return (4, 4)
@property
def t(self) -> R3:
"""
Translational component of SE(3)
:return: translational component of SE(3)
:rtype: numpy.ndarray
``x.t`` is the translational component of ``x`` as an array with
shape (3,). If ``len(x) > 1``, return an array with shape=(N,3).
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> x = SE3(1,2,3)
>>> x.t
>>> x = SE3([ SE3(1,2,3), SE3(4,5,6)])
>>> x.t
:SymPy: supported
"""
if len(self) == 1:
return self.A[:3, 3]
else:
return np.array([x[:3, 3] for x in self.A])
@t.setter
def t(self, v: ArrayLike3):
if len(self) > 1:
raise ValueError("can only assign translation to length 1 object")
v = smb.getvector(v, 3)
self.A[:3, 3] = v
@property
def x(self) -> float:
"""
First element of translational component of SE(3)
:return: first element of translational component of SE(3)
:rtype: float
If ``len(v) > 1``, return an array with shape=(N,).
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> v = SE3(1,2,3)
>>> v.x
>>> v = SE3([ SE3(1,2,3), SE3(4,5,6)])
>>> v.x
:SymPy: supported
"""
if len(self) == 1:
return self.A[0, 3]
else:
return np.array([v[0, 3] for v in self.A])
@x.setter
def x(self, x: float):
if len(self) > 1:
raise ValueError("can only assign elements to length 1 object")
self.A[0, 3] = x
@property
def y(self) -> float:
"""
Second element of translational component of SE(3)
:return: second element of translational component of SE(3)
:rtype: float
If ``len(v) > 1``, return an array with shape=(N,).
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> v = SE3(1,2,3)
>>> v.y
>>> v = SE3([ SE3(1,2,3), SE3(4,5,6)])
>>> v.y
:SymPy: supported
"""
if len(self) == 1:
return self.A[1, 3]
else:
return np.array([v[1, 3] for v in self.A])
@y.setter
def y(self, y: float):
if len(self) > 1:
raise ValueError("can only assign elements to length 1 object")
self.A[1, 3] = y
@property
def z(self) -> float:
"""
Third element of translational component of SE(3)
:return: third element of translational component of SE(3)
:rtype: float
If ``len(v) > 1``, return an array with shape=(N,).
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> v = SE3(1,2,3)
>>> v.z
>>> v = SE3([ SE3(1,2,3), SE3(4,5,6)])
>>> v.z
:SymPy: supported
"""
if len(self) == 1:
return self.A[2, 3]
else:
return np.array([v[2, 3] for v in self.A])
@z.setter
def z(self, z: float):
if len(self) > 1:
raise ValueError("can only assign elements to length 1 object")
self.A[2, 3] = z
# ------------------------------------------------------------------------ #
[docs] def inv(self) -> SE3:
r"""
Inverse of SE(3)
:return: inverse
:rtype: SE3 instance
Efficiently compute the inverse of each of the SE(3) values taking into
account the matrix structure.
.. math::
T = \left[ \begin{array}{cc} \mat{R} & \vec{t} \\ 0 & 1 \end{array} \right],
\mat{T}^{-1} = \left[ \begin{array}{cc} \mat{R}^T & -\mat{R}^T \vec{t} \\ 0 & 1 \end{array} \right]`
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> x = SE3(1,2,3)
>>> x.inv()
:seealso: :func:`~spatialmath.base.transforms3d.trinv`
:SymPy: supported
"""
if len(self) == 1:
return SE3(smb.trinv(self.A), check=False)
else:
return SE3([smb.trinv(x) for x in self.A], check=False)
[docs] def yaw_SE2(self, order: str = "zyx") -> SE2:
"""
Create SE(2) from SE(3) yaw angle.
:param order: angle sequence order, default to 'zyx'
:type order: str
:return: SE(2) with same rotation as the yaw angle using the roll-pitch-yaw convention,
and translation along the roll-pitch axes.
:rtype: SE2 instance
Roll-pitch-yaw corresponds to successive rotations about the axes specified by ``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll 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.
"""
if len(self) == 1:
if order == "zyx":
return SE2(self.x, self.y, self.rpy(order = order)[2])
elif order == "xyz":
return SE2(self.z, self.y, self.rpy(order = order)[2])
elif order == "yxz":
return SE2(self.z, self.x, self.rpy(order = order)[2])
else:
return SE2([e.yaw_SE2() for e in self])
[docs] def delta(self, X2: Optional[SE3] = None) -> R6:
r"""
Infinitesimal difference of SE(3) values
:return: differential motion vector
:rtype: ndarray(6)
``X1.delta(X2)`` is the differential motion (6x1) corresponding to
infinitesimal motion (in the ``X1`` frame) from pose ``X1`` to ``X2``.
The vector :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]`
represents infinitesimal translation and rotation.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> x1 = SE3.Rx(0.3)
>>> x2 = SE3.Rx(0.3001)
>>> x1.delta(x2)
.. note::
- the displacement is only an approximation to the motion, and assumes
that ``X1`` ~ ``X2``.
- can be considered as an approximation to the effect of spatial velocity over a
a time interval, ie. the average spatial velocity multiplied by time.
:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
:seealso: :func:`~spatialmath.base.transforms3d.tr2delta`
"""
if X2 is None:
return smb.tr2delta(self.A)
else:
return smb.tr2delta(self.A, X2.A)
[docs] def Ad(self) -> R6x6:
r"""
Adjoint of SE(3)
:return: adjoint matrix
:rtype: ndarray(6,6)
``SE3.Ad`` is the 6x6 adjoint matrix
If spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T`
and the SE(3) represents the pose of {B} relative to {A},
ie. :math:`{}^A {\bf T}_B`, and the adjoint is :math:`\mathbf{A}` then
:math:`{}^{A}\!\nu = \mathbf{A} {}^{B}\!\nu`.
.. warning:: Do not use this method to map velocities
between robot base and end-effector frames - use ``jacob()``.
.. note:: Use this method to map velocities between two frames on
the same rigid-body.
:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
:seealso: SE3.jacob, Twist.ad, :func:`~spatialmath.base.tr2jac`
:SymPy: supported
"""
return smb.tr2adjoint(self.A)
[docs] def jacob(self) -> R6x6:
r"""
Velocity transform for SE(3)
:return: Jacobian matrix
:rtype: ndarray(6,6)
``SE3.jacob()`` is the 6x6 Jacobian that maps spatial velocity or
differential motion from frame {B} to frame {A} where the pose of {B}
relative to {A} is represented by the homogeneous transform T =
:math:`{}^A {\bf T}_B`.
.. note::
- To map from frame {A} to frame {B} use the transpose of this matrix.
- Use this method to map velocities between the robot end-effector frame
and the base frames.
.. warning:: Do not use this method to map velocities between two frames
on the same rigid-body.
:seealso: SE3.Ad, Twist.ad, :func:`~spatialmath.base.tr2jac`
:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
:SymPy: supported
"""
return smb.tr2jac(self.A)
[docs] def twist(self) -> Twist3:
"""
SE(3) as twist
:return: equivalent rigid-body motion as a twist vector
:rtype: Twist3 instance
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> x = SE3(1,2,3)
>>> x.twist()
:seealso: :func:`spatialmath.twist.Twist3`
"""
return Twist3(self.log(twist=True))
# ------------------------------------------------------------------------ #
[docs] @staticmethod
def isvalid(x: NDArray, check: bool = True) -> bool:
"""
Test if matrix is a valid SE(3)
:param x: matrix to test
:type x: numpy.ndarray
:return: ``True`` if the matrix is 4x4 and a valid element of SE(3), ie. it
is a valid homogeneous transformation matrix.
:rtype: bool
:seealso: :func:`~spatialmath.base.transforms3d.ishom`
"""
return smb.ishom(x, check=check)
# ---------------- variant constructors ---------------------------------- #
[docs] @classmethod
def Rx(
cls,
theta: ArrayLike,
unit: str = "rad",
t: Optional[ArrayLike3] = None,
) -> SE3:
"""
Create anSE(3) pure rotation about the X-axis
:param θ: rotation angle about X-axis
:type θ: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param t: translation, optional
:type t: 3-element array-like
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Rx(θ)`` is an SE(3) rotation of θ radians about the x-axis
- ``SE3.Rx(θ, "deg")`` as above but θ is in degrees
- ``SE3.Rx(θ, t=T)`` as above but also sets the translational component
If ``θ`` is an array then the result is a sequence of rotations defined
by consecutive elements.
.. note:: The translation option only works for the scalar θ case.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Rx(0.3)
>>> SE3.Rx([0.3, 0.4])
:seealso: :func:`~spatialmath.base.transforms3d.trotx`
:SymPy: supported
"""
return cls(
[smb.trotx(x, t=t, unit=unit) for x in smb.getvector(theta)],
check=False,
)
[docs] @classmethod
def Ry(
cls,
theta: ArrayLike,
unit: str = "rad",
t: Optional[ArrayLike3] = None,
) -> SE3:
"""
Create an SE(3) pure rotation about the Y-axis
:param θ: rotation angle about X-axis
:type θ: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param t: translation, optional
:type t: 3-element array-like
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Ry(θ)`` is an SO(3) rotation of θ radians about the y-axis
- ``SE3.Ry(θ, "deg")`` as above but θ is in degrees
- ``SE3.Ry(θ, t=T)`` as above but also sets the translational component
If ``θ`` is an array then the result is a sequence of rotations defined
by consecutive elements.
.. note:: The translation option only works for the scalar θ case.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Ry(0.3)
>>> SE3.Ry([0.3, 0.4])
:seealso: :func:`~spatialmath.base.transforms3d.troty`
:SymPy: supported
"""
return cls(
[smb.troty(x, t=t, unit=unit) for x in smb.getvector(theta)],
check=False,
)
[docs] @classmethod
def Rz(
cls,
theta: ArrayLike,
unit: str = "rad",
t: Optional[ArrayLike3] = None,
) -> SE3:
"""
Create an SE(3) pure rotation about the Z-axis
:param θ: rotation angle about Z-axis
:type θ: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param t: translation, optional
:type t: 3-element array-like
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis
- ``SE3.Rz(θ, "deg")`` as above but θ is in degrees
- ``SE3.Rz(θ, t=T)`` as above but also sets the translational component
If ``θ`` is an array then the result is a sequence of rotations defined
by consecutive elements.
.. note:: The translation option only works for the scalar θ case.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Rz(0.3)
>>> SE3.Rz([0.3, 0.4])
:seealso: :func:`~spatialmath.base.transforms3d.trotz`
:SymPy: supported
"""
return cls(
[smb.trotz(x, t=t, unit=unit) for x in smb.getvector(theta)],
check=False,
)
[docs] @classmethod
def Rand(
cls,
N: int = 1,
xrange: Optional[ArrayLike2] = (-1, 1),
yrange: Optional[ArrayLike2] = (-1, 1),
zrange: Optional[ArrayLike2] = (-1, 1),
) -> SE3: # pylint: disable=arguments-differ
"""
Create a random SE(3)
:param xrange: x-axis range [min,max], defaults to [-1, 1]
:type xrange: 2-element sequence, optional
:param yrange: y-axis range [min,max], defaults to [-1, 1]
:type yrange: 2-element sequence, optional
:param zrange: z-axis range [min,max], defaults to [-1, 1]
:type zrange: 2-element sequence, optional
:param N: number of random transforms
:type N: int
:return: SE(3) matrix
:rtype: SE3 instance
Return an SE3 instance with random rotation and translation.
- ``SE3.Rand()`` is a random SE(3) translation.
- ``SE3.Rand(N)`` is an SE3 object containing a sequence of N random
poses.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Rand(2)
:seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand`
"""
X = np.random.uniform(
low=xrange[0], high=xrange[1], size=N
) # random values in the range
Y = np.random.uniform(
low=yrange[0], high=yrange[1], size=N
) # random values in the range
Z = np.random.uniform(
low=zrange[0], high=zrange[1], size=N
) # random values in the range
R = SO3.Rand(N=N)
return cls(
[smb.transl(x, y, z) @ smb.r2t(r.A) for (x, y, z, r) in zip(X, Y, Z, R)],
check=False,
)
@overload
def Eul(cls, phi: float, theta: float, psi: float, unit: str = "rad") -> SE3:
...
@overload
def Eul(cls, angles: ArrayLike3, unit: str = "rad") -> SE3:
...
[docs] @classmethod
def Eul(cls, *angles, unit="rad") -> SE3:
r"""
Create an SE(3) pure rotation from Euler angles
:param 𝚪: Euler angles
:type 𝚪: 3 floats, array_like(3) or ndarray(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Eul(𝚪)`` is an SE(3) rotation defined by a 3-vector of Euler
angles :math:`\Gamma=(\phi, \theta, \psi)` which correspond to
consecutive rotations about the Z, Y, Z axes respectively.
If ``𝚪`` is an Nx3 matrix then the result is a sequence of
rotations each defined by Euler angles corresponding to the rows of
``𝚪``.
- ``SE3.Eul(φ, θ, ψ)`` as above but the angles are provided as three
scalars.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Eul(0.1, 0.2, 0.3)
>>> SE3.Eul([0.1, 0.2, 0.3])
>>> SE3.Eul(10, 20, 30, unit="deg")
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.base.transforms3d.eul2r`
:SymPy: supported
"""
if len(angles) == 1:
angles = angles[0]
if smb.isvector(angles, 3):
return cls(smb.eul2tr(angles, unit=unit), check=False)
else:
return cls([smb.eul2tr(a, unit=unit) for a in angles], check=False)
@overload
def RPY(cls, roll: float, pitch: float, yaw: float, unit: str = "rad") -> SE3:
...
@overload
def RPY(cls, angles: ArrayLike3, unit: str = "rad") -> SE3:
...
[docs] @classmethod
def RPY(cls, *angles, unit="rad", order="zyx") -> SE3:
r"""
Create an SE(3) pure rotation from roll-pitch-yaw angles
:param 𝚪: roll-pitch-yaw angles
:type 𝚪: 3 floats, array_like(3) or ndarray(N,3)
: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) matrix
:rtype: SE3 instance
- ``SE3.RPY(𝚪)`` is an SE(3) rotation defined by a 3-vector of roll,
pitch, yaw angles :math:`\Gamma=(r, p, y)` which correspond to
successive rotations about the axes specified by ``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. This is the **convention** for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. This is the **convention** for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. This is the **convention** for a camera with z-axis parallel
to the optical axis and x-axis parallel to the pixel rows.
If ``𝚪`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
corresponding to the rows of ``𝚪``.
- ``SE3.RPY(⍺, β, 𝛾)`` as above but the angles are provided as three
scalars.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.RPY(0.1, 0.2, 0.3)
>>> SE3.RPY([0.1, 0.2, 0.3])
>>> SE3.RPY(0.1, 0.2, 0.3, order='xyz')
>>> SE3.RPY(10, 20, 30, unit='deg')
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.base.transforms3d.rpy2r`
:SymPy: supported
"""
if len(angles) == 1:
angles = angles[0]
if smb.isvector(angles, 3):
return cls(smb.rpy2tr(angles, order=order, unit=unit), check=False)
else:
return cls(
[smb.rpy2tr(a, order=order, unit=unit) for a in angles], check=False
)
[docs] @classmethod
def OA(cls, o: ArrayLike3, a: ArrayLike3) -> SE3:
r"""
Create an SE(3) pure rotation from two vectors
:param o: 3-vector parallel to Y- axis
:type o: array_like(3)
:param a: 3-vector parallel to the Z-axis
:type a: array_like(3)
:return: SE(3) matrix
:rtype: SE3 instance
``SE3.OA(o, a)`` is an SE(3) rotation defined in terms of vectors ``o``
and ``a`` respectively parallel to the Y- and Z-axes of its reference
frame. In robotics these axes are respectively called the *orientation*
and *approach* vectors defined such that :math:`\mathbf{R} = [n, o, a]`
and :math:`n = o \times a`.
.. 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
``o`` is adjusted to be orthogonal to ``a``.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.OA([1, 0, 0], [0, 0, -1])
:seealso: :func:`~spatialmath.base.transforms3d.oa2r`
"""
return cls(smb.oa2tr(o, a), check=False)
[docs] @classmethod
def AngleAxis(
cls, theta: float, v: ArrayLike3, *, unit: Optional[unit] = "rad"
) -> SE3:
r"""
Create an SE(3) pure rotation matrix from rotation angle and axis
:param θ: rotation
:type θ: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param v: rotation axis
:type v: array_like(3)
:return: SE(3) matrix
:rtype: SE3 instance
``SE3.AngleAxis(θ, v)`` is an SE(3) rotation defined by
a rotation of ``θ`` about the vector ``v``.
.. math::
\mbox{if}\,\, \theta \left\{ \begin{array}{ll}
= 0 & \mbox{return identity matrix}\\
\ne 0 & \mbox{v must have a finite length}
\end{array}
\right.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`~spatialmath.pose3d.SE3.EulerVec`, :func:`~spatialmath.base.transforms3d.angvec2r`
"""
return cls(smb.angvec2tr(theta, v, unit=unit), check=False)
[docs] @classmethod
def AngVec(cls, theta: float, v: ArrayLike3, *, unit: str = "rad") -> SE3:
r"""
Create an SE(3) pure rotation matrix from rotation angle and axis
:param θ: rotation
:type θ: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param v: rotation axis
:type v: array_like(3)
:return: SE(3) matrix
:rtype: SE3 instance
``SE3.AngVec(θ, v)`` is an SE(3) rotation defined by
a rotation of ``θ`` about the vector ``v``.
.. deprecated:: 0.9.8
Use :meth:`AngleAxis` instead.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`~spatialmath.pose3d.SE3.EulerVec`, :func:`~spatialmath.base.transforms3d.angvec2r`
"""
return cls(smb.angvec2tr(theta, v, unit=unit), check=False)
[docs] @classmethod
def EulerVec(cls, w: ArrayLike3) -> SE3:
r"""
Construct a new SE(3) pure rotation matrix from an Euler rotation vector
:param ω: rotation axis
:type ω: array_like(3)
:return: SE(3) rotation
:rtype: SE3 instance
``SE3.EulerVec(ω)`` is a unit quaternion that describes the 3D rotation
defined by a rotation of :math:`\theta = \lVert \omega \rVert` about the
unit 3-vector :math:`\omega / \lVert \omega \rVert`.
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.EulerVec([0.5,0,0])
.. note:: :math:`\theta = 0` the result in an identity matrix, otherwise
``V`` must have a finite length, ie. :math:`|V| > 0`.
:seealso: :func:`~spatialmath.pose3d.SE3.AngVec`, :func:`~spatialmath.base.transforms3d.angvec2tr`
"""
assert smb.isvector(w, 3), "w must be a 3-vector"
w = smb.getvector(w)
theta = smb.norm(w)
return cls(smb.angvec2tr(theta, w), check=False)
[docs] @classmethod
def Exp(cls, S: Union[R6, R4x4], check: bool = True) -> SE3:
"""
Create an SE(3) matrix from se(3)
:param S: Lie algebra se(3) matrix
:type S: ndarray(6), ndarray(4,4)
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Exp(S)`` is an SE(3) rotation defined by its Lie algebra
which is a 4x4 se(3) matrix (skew symmetric)
- ``SE3.Exp(t)`` is an SE(3) rotation defined by a 6-element twist
vector (the unique elements of the se(3) skew-symmetric matrix)
:seealso: :func:`~spatialmath.base.transforms3d.trexp`, :func:`~spatialmath.base.transformsNd.skew`
"""
if smb.isvector(S, 6):
return cls(smb.trexp(smb.getvector(S)), check=False)
else:
return cls(smb.trexp(S), check=False)
[docs] @classmethod
def Delta(cls, d: ArrayLike6) -> SE3:
r"""
Create SE(3) from differential motion
:param d: differential motion
:type d: array_like(6)
:return: SE(3) matrix
:rtype: SE3 instance
``SE3.Delta2tr(d)`` is an SE(3) representing differential
motion :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]`.
:Reference: Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
:seealso: :meth:`~delta` :func:`~spatialmath.base.transform3d.delta2tr`
:SymPy: supported
"""
return cls(smb.trnorm(smb.delta2tr(d)))
@overload
def Trans(cls, x: float, y: float, z: float) -> SE3:
...
@overload
def Trans(cls, xyz: ArrayLike3) -> SE3:
...
[docs] @classmethod
def Trans(cls, x, y=None, z=None) -> SE3:
"""
Create SE(3) from translation vector
:param x: x-coordinate or translation vector
:type x: float or array_like(3)
:param y: y-coordinate, defaults to None
:type y: float, optional
:param z: z-coordinate, defaults to None
:type z: float, optional
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Trans(x, y, z)`` is an SE(3) representing pure translation.
- ``SE3.Trans([x, y, z])`` as above, but translation is given as an
array.
- ``SE3.Trans(t)`` where ``t`` is Nx3 then create an SE3 object with
N elements whose translation is defined by the rows of ``t``.
"""
if y is None and z is None:
# single passed value, assume is 3-vector or Nx3
t = smb.getmatrix(x, (None, 3))
return cls([smb.transl(_t) for _t in t], check=False)
else:
return cls(np.array([x, y, z]))
[docs] @classmethod
def Tx(cls, x: float) -> SE3:
"""
Create an SE(3) translation along the X-axis
:param x: translation distance along the X-axis
:type x: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Tx(x)` is an SE(3) translation of ``x`` along the x-axis
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Tx(2)
>>> SE3.Tx([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([smb.transl(_x, 0, 0) for _x in smb.getvector(x)], check=False)
[docs] @classmethod
def Ty(cls, y: float) -> SE3:
"""
Create an SE(3) translation along the Y-axis
:param y: translation distance along the Y-axis
:type y: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Ty(y) is an SE(3) translation of ``y`` along the y-axis
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Ty(2)
>>> SE3.Ty([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([smb.transl(0, _y, 0) for _y in smb.getvector(y)], check=False)
[docs] @classmethod
def Tz(cls, z: float) -> SE3:
"""
Create an SE(3) translation along the Z-axis
:param z: translation distance along the Z-axis
:type z: float
:return: SE(3) matrix
:rtype: SE3 instance
`SE3.Tz(z)` is an SE(3) translation of ``z`` along the z-axis
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> SE3.Tz(2)
>>> SE3.Tz([2,3])
:seealso: :func:`~spatialmath.base.transforms3d.transl`
:SymPy: supported
"""
return cls([smb.transl(0, 0, _z) for _z in smb.getvector(z)], check=False)
[docs] @classmethod
def Rt(
cls,
R: Union[SO3, SO3Array],
t: Optional[ArrayLike3] = None,
check: bool = True,
) -> SE3:
"""
Create an SE(3) from rotation and translation
:param R: rotation
:type R: SO3 or ndarray(3,3)
:param t: translation
:type t: array_like(3)
:param check: check rotation validity, defaults to True
:type check: bool, optional
:raises ValueError: bad rotation matrix
:return: SE(3) matrix
:rtype: SE3 instance
"""
if isinstance(R, SO3):
R = R.A
elif smb.isrot(R, check=check):
pass
else:
raise ValueError("expecting SO3 or rotation matrix")
if t is None:
t = np.zeros((3,))
return cls(smb.rt2tr(R, t, check=check), check=check)
[docs] @classmethod
def CopyFrom(
cls,
T: SE3Array,
check: bool = True
) -> SE3:
"""
Create an SE(3) from a 4x4 numpy array that is passed by value.
:param T: homogeneous transformation
:type T: ndarray(4, 4)
:param check: check rotation validity, defaults to True
:type check: bool, optional
:raises ValueError: bad rotation matrix, bad transformation matrix
:return: SE(3) matrix representing that transformation
:rtype: SE3 instance
"""
if T is None:
raise ValueError("Transformation matrix must not be None")
return cls(np.copy(T), check=check)
[docs] def angdist(self, other: SE3, metric: int = 6) -> float:
r"""
Angular distance metric between poses
:param other: second rotation
:type other: SE3 instance
:param metric: metric, default is 6
:type metric: int
:raises TypeError: if other is not an SE3
:return: angle in radians
:rtype: float or ndarray
``T1.angdist(T2)`` is the geodesic norm, or geodesic distance between the
rotational parts of the two poses.
Several metrics are supported, the first 5 are computed after conversion
to unit quaternions.
====== ===============================================================
Metric Details
====== ===============================================================
0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
5 :math:`\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]`
6 :math:`\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]`
====== ===============================================================
Example:
.. runblock:: pycon
>>> from spatialmath import SE3
>>> T1 = SE3.Rx(0.3)
>>> T2 = SE3.Ry(0.3)
>>> print(T1.angdist(T1))
>>> print(T1.angdist(T2))
.. note::
- metrics 1, 2, 4 can throw ValueError "math domain error" due to
numeric errors which push the argument of ``acos()`` marginally
outside its domain [0, 1].
- metrics 2 and 3 are equivalent, but 3 is more robust
:seealso: :func:`UnitQuaternion.angdist`
"""
if metric < 5:
from spatialmath.quaternion import UnitQuaternion
return UnitQuaternion(self).angdist(UnitQuaternion(other), metric=metric)
elif metric == 5:
op = lambda T1, T2: np.linalg.norm(np.eye(3) - T1[:3, :3] @ T2[:3, :3].T)
elif metric == 6:
op = lambda T1, T2: smb.norm(
smb.trlog(T1[:3, :3] @ T2[:3, :3].T, twist=True)
)
else:
raise ValueError("unknown metric")
ad = self._op2(other, op)
if isinstance(ad, list):
return np.array(ad)
else:
return ad
# @classmethod
# def SO3(cls, R, t=None, check=True):
# if isinstance(R, SO3):
# R = R.A
# elif base.isrot(R, check=check):
# pass
# else:
# raise ValueError('expecting SO3 or rotation matrix')
# if t is None:
# return cls(base.r2t(R))
# else:
# return cls(base.rt2tr(R, t))
if __name__ == "__main__": # pragma: no cover
import pathlib
exec(
open(
pathlib.Path(__file__).parent.parent.absolute() / "tests" / "test_pose3d.py"
).read()
) # pylint: disable=exec-used