"""
This modules contains functions to create and transform 3D rotation matrices
and homogeneous tranformation matrices.
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.
TODO:
- trinterp
- trjac, trjac2
- tranimate, tranimate2
"""
# This file is part of the SpatialMath toolbox for Python
# https://github.com/petercorke/spatialmath-python
#
# MIT License
#
# Copyright (c) 1993-2020 Peter Corke
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
# Contributors:
#
# 1. Luis Fernando Lara Tobar and Peter Corke, 2008
# 2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)
# 3. Peter Corke, 2020
import sys
import math
import numpy as np
from spatialmath.base import argcheck
from spatialmath.base import vectors as vec
from spatialmath.base import transformsNd as trn
from spatialmath.base import quaternions as quat
try: # pragma: no cover
# print('Using SymPy')
import sympy as sym
def issymbol(x):
return isinstance(x, sym.Symbol)
except BaseException:
[docs] def issymbol(x):
return False
_eps = np.finfo(np.float64).eps
[docs]def colvec(v):
return np.array(v).reshape((len(v), 1))
# ---------------------------------------------------------------------------------------#
def _cos(theta):
if issymbol(theta):
return sym.cos(theta)
else:
return math.cos(theta)
def _sin(theta):
if issymbol(theta):
return sym.sin(theta)
else:
return math.sin(theta)
[docs]def rotx(theta, unit="rad"):
"""
Create SO(3) rotation about X-axis
:param theta: rotation angle about X-axis
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpy.ndarray, shape=(3,3)
- ``rotx(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation
of THETA radians about the x-axis
- ``rotx(THETA, "deg")`` as above but THETA is in degrees
:seealso: :func:`~trotx`
"""
theta = argcheck.getunit(theta, unit)
ct = _cos(theta)
st = _sin(theta)
R = np.array([
[1, 0, 0],
[0, ct, -st],
[0, st, ct]])
return R
# ---------------------------------------------------------------------------------------#
[docs]def roty(theta, unit="rad"):
"""
Create SO(3) rotation about Y-axis
:param theta: rotation angle about Y-axis
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpy.ndarray, shape=(3,3)
- ``roty(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation
of THETA radians about the y-axis
- ``roty(THETA, "deg")`` as above but THETA is in degrees
:seealso: :func:`~troty`
"""
theta = argcheck.getunit(theta, unit)
ct = _cos(theta)
st = _sin(theta)
R = np.array([
[ct, 0, st],
[0, 1, 0],
[-st, 0, ct]])
return R
# ---------------------------------------------------------------------------------------#
[docs]def rotz(theta, unit="rad"):
"""
Create SO(3) rotation about Z-axis
:param theta: rotation angle about Z-axis
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpy.ndarray, shape=(3,3)
- ``rotz(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation
of THETA radians about the z-axis
- ``rotz(THETA, "deg")`` as above but THETA is in degrees
:seealso: :func:`~yrotz`
"""
theta = argcheck.getunit(theta, unit)
ct = _cos(theta)
st = _sin(theta)
R = np.array([
[ct, -st, 0],
[st, ct, 0],
[0, 0, 1]])
return R
# ---------------------------------------------------------------------------------------#
[docs]def trotx(theta, unit="rad", t=None):
"""
Create SE(3) pure rotation about X-axis
:param theta: rotation angle about X-axis
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param t: translation 3-vector, defaults to [0,0,0]
:type t: array_like :return: 4x4 homogeneous transformation matrix
:rtype: numpy.ndarray, shape=(4,4)
- ``trotx(THETA)`` is a homogeneous transformation (4x4) representing a rotation
of THETA radians about the x-axis.
- ``trotx(THETA, 'deg')`` as above but THETA is in degrees
- ``trotx(THETA, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]
:seealso: :func:`~rotx`
"""
T = np.pad(rotx(theta, unit), (0, 1), mode='constant')
if t is not None:
T[:3, 3] = argcheck.getvector(t, 3, 'array')
T[3, 3] = 1.0
return T
# ---------------------------------------------------------------------------------------#
[docs]def troty(theta, unit="rad", t=None):
"""
Create SE(3) pure rotation about Y-axis
:param theta: rotation angle about Y-axis
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param t: translation 3-vector, defaults to [0,0,0]
:type t: array_like
:return: 4x4 homogeneous transformation matrix as a numpy array
:rtype: numpy.ndarray, shape=(4,4)
- ``troty(THETA)`` is a homogeneous transformation (4x4) representing a rotation
of THETA radians about the y-axis.
- ``troty(THETA, 'deg')`` as above but THETA is in degrees
- ``troty(THETA, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]
:seealso: :func:`~roty`
"""
T = np.pad(roty(theta, unit), (0, 1), mode='constant')
if t is not None:
T[:3, 3] = argcheck.getvector(t, 3, 'array')
T[3, 3] = 1.0
return T
# ---------------------------------------------------------------------------------------#
[docs]def trotz(theta, unit="rad", t=None):
"""
Create SE(3) pure rotation about Z-axis
:param theta: rotation angle about Z-axis
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param t: translation 3-vector, defaults to [0,0,0]
:type t: array_like
:return: 4x4 homogeneous transformation matrix
:rtype: numpy.ndarray, shape=(4,4)
- ``trotz(THETA)`` is a homogeneous transformation (4x4) representing a rotation
of THETA radians about the z-axis.
- ``trotz(THETA, 'deg')`` as above but THETA is in degrees
- ``trotz(THETA, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]
:seealso: :func:`~rotz`
"""
T = np.pad(rotz(theta, unit), (0, 1), mode='constant')
if t is not None:
T[:3, 3] = argcheck.getvector(t, 3, 'array')
T[3, 3] = 1.0
return T
# ---------------------------------------------------------------------------------------#
[docs]def transl(x, y=None, z=None):
"""
Create SE(3) pure translation, or extract translation from 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: 4x4 homogeneous transformation matrix
:rtype: numpy.ndarray, shape=(4,4)
Create a translational SE(3) matrix:
- ``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.
Extract the translational part of an SE(3) matrix:
- ``P = TRANSL(T)`` is the translational part of a homogeneous transform T as a
3-element numpy array.
:seealso: :func:`~spatialmath.base.transforms2d.transl2`
"""
if np.isscalar(x):
T = np.identity(4)
T[:3, 3] = [x, y, z]
return T
elif argcheck.isvector(x, 3):
T = np.identity(4)
T[:3, 3] = argcheck.getvector(x, 3, out='array')
return T
elif argcheck.ismatrix(x, (4, 4)):
return x[:3, 3]
else:
ValueError('bad argument')
[docs]def ishom(T, check=False, tol=10):
"""
Test if matrix belongs to SE(3)
:param T: matrix to test
:type T: numpy.ndarray
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SE(3) homogeneous transformation matrix
:rtype: bool
- ``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.
: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 (trn.isR(T[:3, :3], tol=tol) and np.all(T[3, :] == np.array([0, 0, 0, 1]))))
[docs]def isrot(R, check=False, tol=10):
"""
Test if matrix belongs to SO(3)
:param R: matrix to test
:type R: numpy.ndarray
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SO(3) rotation matrix
:rtype: bool
- ``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.
: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 trn.isR(R, tol=tol))
# ---------------------------------------------------------------------------------------#
[docs]def rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx'):
"""
Create an SO(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 unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
- ``rpy2r(ROLL, PITCH, YAW)`` 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 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. Covention 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.
- ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken
from ``RPY`` which is a 3-vector (array_like) with values
(ROLL, PITCH, YAW).
:seealso: :func:`~eul2r`, :func:`~rpy2tr`, :func:`~tr2rpy`
"""
if np.isscalar(roll):
angles = [roll, pitch, yaw]
else:
angles = argcheck.getvector(roll, 3)
angles = argcheck.getunit(angles, unit)
if order == 'xyz' or order == 'arm':
R = rotx(angles[2]) @ roty(angles[1]) @ rotz(angles[0])
elif order == 'zyx' or order == 'vehicle':
R = rotz(angles[2]) @ roty(angles[1]) @ rotx(angles[0])
elif order == 'yxz' or order == 'camera':
R = roty(angles[2]) @ rotx(angles[1]) @ rotz(angles[0])
else:
raise ValueError('Invalid angle order')
return R
# ---------------------------------------------------------------------------------------#
[docs]def rpy2tr(roll, pitch=None, yaw=None, unit='rad', order='zyx'):
"""
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 unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
- ``rpy2tr(ROLL, PITCH, YAW)`` 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 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.
- ``rpy2tr(RPY)`` as above but the roll, pitch, yaw angles are taken
from ``RPY`` which is a 3-vector (array_like) with values
(ROLL, PITCH, YAW).
Notes:
- The translational part is zero.
:seealso: :func:`~eul2tr`, :func:`~rpy2r`, :func:`~tr2rpy`
"""
R = rpy2r(roll, pitch, yaw, order=order, unit=unit)
return trn.r2t(R)
# ---------------------------------------------------------------------------------------#
[docs]def eul2r(phi, theta=None, psi=None, unit='rad'):
"""
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: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
- ``R = eul2r(PHI, THETA, PSI)`` 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 (array_like) with values
(PHI THETA PSI).
:seealso: :func:`~rpy2r`, :func:`~eul2tr`, :func:`~tr2eul`
"""
if np.isscalar(phi):
angles = [phi, theta, psi]
else:
angles = argcheck.getvector(phi, 3)
angles = argcheck.getunit(angles, unit)
return rotz(angles[0]) @ roty(angles[1]) @ rotz(angles[2])
# ---------------------------------------------------------------------------------------#
[docs]def eul2tr(phi, theta=None, psi=None, unit='rad'):
"""
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: 4x4 homogeneous transformation matrix
:rtype: numpdy.ndarray, shape=(4,4)
- ``R = eul2tr(PHI, THETA, 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 (array_like) with values
(PHI THETA PSI).
Notes:
- The translational part is zero.
:seealso: :func:`~rpy2tr`, :func:`~eul2r`, :func:`~tr2eul`
"""
R = eul2r(phi, theta, psi, unit=unit)
return trn.r2t(R)
# ---------------------------------------------------------------------------------------#
[docs]def angvec2r(theta, v, unit='rad'):
"""
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: rotation axis, 3-vector
:type v: array_like
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
``angvec2r(THETA, V)`` is an SO(3) orthonormal rotation matrix
equivalent to a rotation of ``THETA`` about the vector ``V``.
Notes:
- If ``THETA == 0`` then return identity matrix.
- If ``THETA ~= 0`` then ``V`` must have a finite length.
:seealso: :func:`~angvec2tr`, :func:`~tr2angvec`
"""
assert np.isscalar(theta) and argcheck.isvector(v, 3), "Arguments must be theta and vector"
if np.linalg.norm(v) < 10 * _eps:
return np.eye(3)
theta = argcheck.getunit(theta, unit)
# Rodrigue's equation
sk = trn.skew(vec.unitvec(v))
R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
return R
# ---------------------------------------------------------------------------------------#
[docs]def angvec2tr(theta, v, unit='rad'):
"""
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: rotation axis, 3-vector
:type v: : array_like
:return: 4x4 homogeneous transformation matrix
:rtype: numpdy.ndarray, shape=(4,4)
``angvec2tr(THETA, V)`` is an SE(3) homogeneous transformation matrix
equivalent to a rotation of ``THETA`` about the vector ``V``.
Notes:
- If ``THETA == 0`` then return identity matrix.
- If ``THETA ~= 0`` then ``V`` must have a finite length.
- The translational part is zero.
:seealso: :func:`~angvec2r`, :func:`~tr2angvec`
"""
return trn.r2t(angvec2r(theta, v, unit=unit))
# ---------------------------------------------------------------------------------------#
[docs]def oa2r(o, a=None):
"""
Create SO(3) rotation matrix 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: 3x3 rotation matrix
:rtype: numpy.ndarray, shape=(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
Notes:
- 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`
"""
o = argcheck.getvector(o, 3, out='array')
a = argcheck.getvector(a, 3, out='array')
n = np.cross(o, a)
o = np.cross(a, n)
R = np.stack((vec.unitvec(n), vec.unitvec(o), vec.unitvec(a)), axis=1)
return R
# ---------------------------------------------------------------------------------------#
[docs]def oa2tr(o, a=None):
"""
Create SE(3) pure rotation 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: 4x4 homogeneous transformation matrix
:rtype: numpy.ndarray, shape=(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
Notes:
- 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`
"""
return trn.r2t(oa2r(o, a))
# ------------------------------------------------------------------------------------------------------------------- #
[docs]def tr2angvec(T, unit='rad', check=False):
r"""
Convert SO(3) or SE(3) to angle and rotation vector
:param R: SO(3) or SE(3) matrix
:type R: numpy.ndarray, shape=(3,3) or (4,4)
: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, numpy.ndarray, shape=(3,)
``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'`.
Notes:
- If the input is SE(3) the translation component is ignored.
:seealso: :func:`~angvec2r`, :func:`~angvec2tr`, :func:`~tr2rpy`, :func:`~tr2eul`
"""
if argcheck.ismatrix(T, (4, 4)):
R = trn.t2r(T)
else:
R = T
assert isrot(R, check=check)
v = trn.vex(trlog(R))
if vec.iszerovec(v):
theta = 0
v = np.r_[0, 0, 0]
else:
theta = vec.norm(v)
v = vec.unitvec(v)
if unit == 'deg':
theta *= 180 / math.pi
return (theta, v)
# ------------------------------------------------------------------------------------------------------------------- #
[docs]def tr2eul(T, unit='rad', flip=False, check=False):
r"""
Convert SO(3) or SE(3) to ZYX Euler angles
:param R: SO(3) or SE(3) matrix
:type R: numpy.ndarray, shape=(3,3) or (4,4)
: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
:return: ZYZ Euler angles
:rtype: numpy.ndarray, shape=(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'`.
Notes:
- There is a singularity for the case where :math:`\theta=0` in which case :math:`\phi` is arbitrarily set to zero 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`
"""
if argcheck.ismatrix(T, (4, 4)):
R = trn.t2r(T)
else:
R = T
assert isrot(R, check=check)
eul = np.zeros((3,))
if abs(R[0, 2]) < 10 * _eps and abs(R[1, 2]) < 10 * _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
# ------------------------------------------------------------------------------------------------------------------- #
[docs]def tr2rpy(T, unit='rad', order='zyx', check=False):
"""
Convert SO(3) or SE(3) to roll-pitch-yaw angles
:param R: SO(3) or SE(3) matrix
:type R: numpy.ndarray, shape=(3,3) or (4,4)
:param unit: 'rad' or 'deg'
:type unit: str
:param order: 'xyz', 'zyx' or 'yxz' [default 'zyx']
:type unit: str
:param check: check that rotation matrix is valid
:type check: bool
:return: Roll-pitch-yaw angles
:rtype: numpy.ndarray, shape=(3,)
``tr2rpy(R)`` are the roll-pitch-yaw angles corresponding to
the rotation part of ``R``.
The 3 angles RPY=[R,P,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'`.
Notes:
- There is a singularity for the case where P=:math:`\pi/2` in which case R is arbitrarily set to zero and Y is the sum (R+Y).
- If the input is SE(3) the translation component is ignored.
:seealso: :func:`~rpy2r`, :func:`~rpy2tr`, :func:`~tr2eul`, :func:`~tr2angvec`
"""
if argcheck.ismatrix(T, (4, 4)):
R = trn.t2r(T)
else:
R = T
assert isrot(R, check=check)
rpy = np.zeros((3,))
if order == 'xyz' or order == 'arm':
# XYZ order
if abs(abs(R[0, 2]) - 1) < 10 * _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(R[0, 2])
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 == 'zyx' or order == 'vehicle':
# old ZYX order (as per Paul book)
if abs(abs(R[2, 0]) - 1) < 10 * _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(R[2, 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 == 'yxz' or order == 'camera':
if abs(abs(R[1, 2]) - 1) < 10 * _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(R[1, 2]) # 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
# ---------------------------------------------------------------------------------------#
[docs]def trlog(T, check=True):
"""
Logarithm of SO(3) or SE(3) matrix
:param T: SO(3) or SE(3) matrix
:type T: numpy.ndarray, shape=(3,3) or (4,4)
:return: logarithm
:rtype: numpy.ndarray, shape=(3,3) or (4,4)
:raises: ValueError
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].
:seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`, :func:`~spatialmath.base.transformsNd.vexa`
"""
if ishom(T, check=check):
# SE(3) matrix
if trn.iseye(T):
# is identity matrix
return np.zeros((4, 4))
else:
[R, t] = trn.tr2rt(T)
if trn.iseye(R):
# rotation matrix is identity
skw = np.zeros((3, 3))
v = t
theta = 1
else:
S = trlog(R, check=False) # recurse
w = trn.vex(S)
theta = vec.norm(w)
skw = trn.skew(w / theta)
Ginv = np.eye(3) / theta - skw / 2 + (1 / theta - 1 / np.tan(theta / 2) / 2) * skw @ skw
v = Ginv @ t
return trn.rt2m(skw, v) * theta
elif isrot(T, check=check):
# deal with rotation matrix
R = T
if trn.iseye(R):
# matrix is identity
return np.zeros((3, 3))
elif abs(np.trace(R) + 1) < 100 * _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
return trn.skew(w * theta)
else:
# general case
theta = np.arccos((np.trace(R) - 1) / 2)
skw = (R - R.T) / 2 / np.sin(theta)
return skw * theta
else:
raise ValueError("Expect SO(3) or SE(3) matrix")
# ---------------------------------------------------------------------------------------#
[docs]def trexp(S, theta=None):
"""
Exponential of so(3) or se(3) matrix
:param S: so(3), se(3) matrix or equivalent velctor
:type T: numpy.ndarray, shape=(3,3), (3,), (4,4), or (6,)
:param theta: motion
:type theta: float
:return: 3x3 or 4x4 matrix exponential in SO(3) or SE(3)
:rtype: numpy.ndarray, shape=(3,3) or (4,4)
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(S)`` is the matrix exponential of the so(3) element ``S`` which is a 3x3
skew-symmetric matrix.
- ``trexp(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is
unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
given by ``THETA``.
- ``trexp(W)`` is the matrix exponential of the so(3) element ``W`` expressed as
a 3-vector (array_like).
- ``trexp(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
unit-norm vector representing a rotation axis and a rotation magnitude
given by ``THETA``. ``W`` is expressed as a 3-vector (array_like).
For se(3) the results is an SE(3) homogeneous transformation matrix:
- ``trexp(SIGMA)`` is the matrix exponential of the se(3) element ``SIGMA`` which is
a 4x4 augmented skew-symmetric matrix.
- ``trexp(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
- ``trexp(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as
a 6-vector which can be considered a screw motion.
- ``trexp(TW, THETA)`` as above but for an se(3) motion of TW*THETA, where ``TW``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
:seealso: :func:`~trlog, :func:`~spatialmath.base.transforms2d.trexp2`
"""
if argcheck.ismatrix(S, (4, 4)) or argcheck.isvector(S, 6):
# se(3) case
if argcheck.ismatrix(S, (4, 4)):
# augmentented skew matrix
tw = trn.vexa(S)
else:
# 6 vector
tw = argcheck.getvector(S)
if vec.iszerovec(tw):
return np.eye(4)
if theta is None:
(tw, theta) = vec.unittwist_norm(tw)
else:
if theta == 0:
return np.eye(4)
else:
assert vec.isunittwist(tw), 'If theta is specified S must be a unit twist'
t = tw[0:3]
w = tw[3:6]
R = trn._rodrigues(w, theta)
skw = trn.skew(w)
V = np.eye(3) * theta + (1.0 - math.cos(theta)) * skw + (theta - math.sin(theta)) * skw @ skw
return trn.rt2tr(R, V@t)
elif argcheck.ismatrix(S, (3, 3)) or argcheck.isvector(S, 3):
# so(3) case
if argcheck.ismatrix(S, (3, 3)):
# skew symmetric matrix
w = trn.vex(S)
else:
# 3 vector
w = argcheck.getvector(S)
if theta is not None:
assert vec.isunitvec(w), 'If theta is specified S must be a unit twist'
# do Rodrigues' formula for rotation
return trn._rodrigues(w, theta)
else:
raise ValueError(" First argument must be SO(3), 3-vector, SE(3) or 6-vector")
[docs]def trnorm(T):
"""
Normalize an SO(3) or SE(3) matrix
:param T: SO(3) or SE(3) matrix
:type T1: np.ndarray, shape=(3,3) or (4,4)
:param T1: second SE(3) matrix
:return: SO(3) or SE(3) matrix
:rtype: np.ndarray, shape=(3,3) or (4,4)
- ``trnorm(R)`` is guaranteed to be a proper orthogonal matrix rotation
matrix (3x3) which is "close" to the input matrix R (3x3). If R
= [N,O,A] the O and A vectors are made unit length and the normal vector
is formed from N = O x A, and then we ensure that O and A are orthogonal
by O = A x N.
- ``trnorm(T)`` as above but the rotational submatrix of the homogeneous
transformation T (4x4) is normalised while the translational part is
unchanged.
Notes:
- Only the direction of A (the z-axis) is unchanged.
- Used to prevent finite word length arithmetic causing transforms to
become 'unnormalized'.
"""
assert ishom(T) or isrot(T), 'expecting 3x3 or 4x4 hom xform'
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((vec.unitvec(n), vec.unitvec(o), vec.unitvec(a)), axis=1)
if ishom(T):
return trn.rt2tr( R, T[:3,3] )
else:
return R
[docs]def trinterp(T0, T1=None, s=None):
"""
Interpolate SE(3) matrices
:param T0: first SE(3) matrix
:type T0: np.ndarray, shape=(4,4)
:param T1: second SE(3) matrix
:type T1: np.ndarray, shape=(4,4)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: SE(3) matrix
:rtype: np.ndarray, shape=(4,4)
- ``trinterp(T0, T1, S)`` is a homogeneous transform (4x4) interpolated
between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous
transforms (4x4).
- ``trinterp(T1, S)`` as above but interpolated between the identity matrix
when S=0 to T1 when S=1.
Notes:
- Rotation is interpolated using quaternion spherical linear interpolation (slerp).
:seealso: :func:`spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms3d.trinterp2`
"""
assert 0 <= s <= 1, 's outside interval [0,1]'
if T1 is None:
# TRINTERP(T, s)
q0 = quat.r2q(trn.t2r(T0))
p0 = transl(T0)
qr = quat.slerp(quat.eye(), q0, s)
pr = s * p0
else:
# TRINTERP(T0, T1, s)
q0 = quat.r2q(trn.t2r(T0))
q1 = quat.r2q(trn.t2r(T1))
p0 = transl(T0)
p1 = transl(T1)
qr = quat.slerp(q0, q1, s)
pr = p0 * (1 - s) + s * p1;
return trn.rt2tr(quat.q2r(qr), pr)
[docs]def delta2tr(d):
r"""
Convert differential motion to SE(3)
:param d: differential motion as a 6-vector
:type d: array_like
:return: SE(3) matrix
:rtype: np.ndarray, shape=(4,4)
``T = delta2tr(d)`` is an SE(3) matrix representing differential
motion :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`.
Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.
:seealso: :func:`~tr2delta`
"""
return np.eye(4,4) + trn.skewa(d)
[docs]def trinv(T):
r"""
Invert an SE(3) matrix
:param T: an SE(3) matrix
:type T: np.ndarray, shape=(4,4)
:return: SE(3) matrix
:rtype: np.ndarray, shape=(4,4)
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}`
"""
assert ishom(T), 'expecting SE(3) matrix'
(R, t) = trn.tr2rt(T)
return trn.rt2tr(R.T, -R.T@t)
[docs]def tr2delta(T0, T1=None):
"""
Difference of SE(3) matrices as differential motion
:param T0: first SE(3) matrix
:type T0: np.ndarray, shape=(4,4)
:param T1: second SE(3) matrix
:type T1: np.ndarray, shape=(4,4)
:return: Sdifferential motion as a 6-vector
:rtype: np.ndarray, shape=(6,)
- ``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:`d = [\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.
Notes:
- D 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: Second Edition, P. Corke, Springer 2016; p67.
:seealso: :func:`~delta2tr`
"""
if T1 is None:
# tr2delta(T)
assert ishom(T0), '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), trn.vex(trn.t2r(Td) - np.eye(3))]
[docs]def tr2jac(T, samebody=False):
"""
SE(3) adjoint
:param T: an SE(3) matrix
:type T: np.ndarray, shape=(4,4)
:return: adjoint matrix
:rtype: np.ndarray, shape=(6,6)
Computes an adjoint matrix that maps spatial velocity between two frames defined by
an SE(3) matrix. It acts like a Jacobian matrix.
- ``tr2jac(T)`` is a Jacobian matrix (6x6) that maps spatial velocity or
differential motion from frame {A} to frame {B} where the pose of {B}
relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.
- ``tr2jac(T, True)`` as above but for the case when frame {A} to frame {B} are both
attached to the same moving body.
"""
assert ishom(T), 'expecting an SE(3) matrix'
Z = np.zeros((3,3))
if samebody:
(R,t) = trn.tr2rt(T)
return np.block([[R.T, (trn.skew(t)@R).T], [Z, R.T]])
else:
R = trn.t2r(T);
return np.block([[R.T, Z], [Z, R.T]])
[docs]def trprint(T, orient='rpy/zyx', label=None, file=sys.stdout, fmt='{:8.2g}', unit='deg'):
"""
Compact display of SO(3) or SE(3) matrices
:param T: matrix to format
:type T: numpy.ndarray, shape=(3,3) or (4,4)
: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: str
:param fmt: conversion format for each number
:type fmt: str
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: optional formatted string
:rtype: str
The matrix is formatted and written to ``file`` or if ``file=None`` then the
string is returned.
- ``trprint(R)`` displays the SO(3) rotation matrix in a compact
single-line format:
[LABEL:] ORIENTATION UNIT
- ``trprint(T)`` displays the SE(3) homogoneous transform in a compact
single-line format:
[LABEL:] [t=X, Y, Z;] ORIENTATION UNIT
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
Example:
>>> T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg')
>>> trprint(T, file=None, label='T')
'T: t = 1, 2, 3; rpy/zyx = 10, 20, 30 deg'
>>> trprint(T, file=None, label='T', orient='angvec')
'T: t = 1, 2, 3; angvec = ( 56 deg | 0.12, 0.62, 0.78)'
>>> trprint(T, file=None, label='T', orient='angvec', fmt='{:8.4g}')
'T: t = 1, 2, 3; angvec = ( 56.04 deg | 0.124, 0.6156, 0.7782)'
Notes:
- 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 readable columns of data
:seealso: :func:`~spatialmath.base.transforms2d.trprint2`, :func:`~tr2eul`, :func:`~tr2rpy`, :func:`~tr2angvec`
"""
s = ''
if label is not None:
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
a = orient.split('/')
if a[0] == 'rpy':
if len(a) == 2:
seq = a[1]
else:
seq = None
angles = tr2rpy(T, order=seq, unit=unit)
s += ' {} = {} {}'.format(orient, _vec2s(fmt, angles), unit)
elif a[0].startswith('eul'):
angles = tr2eul(T, unit)
s += ' eul = {} {}'.format(_vec2s(fmt, angles), unit)
elif a[0] == 'angvec':
pass
# as a vector and angle
(theta, v) = tr2angvec(T, unit)
if theta == 0:
s += ' R = nil'
else:
s += ' angvec = ({} {} | {})'.format(fmt.format(theta), unit, _vec2s(fmt, v))
else:
raise ValueError('bad orientation format')
if file:
print(s, file=file)
else:
return s
def _vec2s(fmt, v):
v = [x if np.abs(x) > 100 * _eps 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 BaseException: # pragma: no cover
def trplot(*args, **kwargs):
print('** trplot: no plot produced -- matplotlib not installed')
_matplotlib_exists = False
if _matplotlib_exists:
[docs] def trplot(T, axes=None, dims=None, color='blue', frame=None, textcolor=None, labels=['X', 'Y', 'Z'], length=1, arrow=True, projection='ortho', rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs):
"""
Plot a 3D coordinate frame
:param T: an SO(3) or SE(3) pose to be displayed as coordinate frame
:type: numpy.ndarray, shape=(3,3) or (4,4)
:param axes: the axes to plot into, defaults to current axes
:type axes: Axes3D reference
: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
:param color: color of the lines defining the frame
:type color: str
:param textcolor: color of text labels for the frame, default color of lines above
: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 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
:param arrow: show arrow heads, default True
:type arrow: bool
:param wtl: width-to-length ratio for arrows, default 0.2
:type wtl: float
:param rviz: show Rviz style arrows, default False
:type rviz: bool
:param projection: 3D projection: ortho [default] or persp
:type projection: str
:param width: width of lines, default 1
:type width: float
:param d1: distance of frame axis label text from origin, default 1.15
:type d2: distance of frame label text from origin, default 0.05
Adds a 3D coordinate frame represented by the SO(3) or SE(3) matrix to the current axes.
- If no current figure, one is created
- If current figure, but no axes, a 3d Axes is created
Examples:
trplot(T, frame='A')
trplot(T, frame='A', color='green')
trplot(T1, 'labels', 'NOA');
"""
# TODO
# animation
# anaglyph
# check input types
if isrot(T, check=True):
T = trn.r2t(T)
else:
assert ishom(T, check=True)
if axes is None:
# create an axes
fig = plt.gcf()
if fig.axes == []:
# no axes in the figure, create a 3D axes
ax = fig.add_subplot(111, projection='3d', proj_type=projection)
ax.autoscale(enable=True, axis='both')
# ax.set_aspect('equal')
ax.set_xlabel(labels[0])
ax.set_ylabel(labels[1])
ax.set_zlabel(labels[2])
else:
# reuse an existing axis
ax = plt.gca()
else:
ax = axes
if dims is not None:
if len(dims) == 2:
dims = dims * 3
ax.set_xlim(dims[0:2])
ax.set_ylim(dims[2:4])
ax.set_zlim(dims[4:6])
# create unit vectors in homogeneous form
o = T @ np.array([0, 0, 0, 1])
x = T @ np.array([1, 0, 0, 1]) * length
y = T @ np.array([0, 1, 0, 1]) * length
z = T @ np.array([0, 0, 1, 1]) * length
# draw the axes
if rviz:
ax.plot([o[0], x[0]], [o[1], x[1]], [o[2], x[2]], color='red', linewidth=5 * width)
ax.plot([o[0], y[0]], [o[1], y[1]], [o[2], y[2]], color='lime', linewidth=5 * width)
ax.plot([o[0], z[0]], [o[1], z[1]], [o[2], z[2]], color='blue', linewidth=5 * width)
elif 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, edgecolor=color)
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, edgecolor=color)
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, edgecolor=color)
# plot an 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=[20, 0, 0, 0])
else:
ax.plot([o[0], x[0]], [o[1], x[1]], [o[2], x[2]], color=color, linewidth=width)
ax.plot([o[0], y[0]], [o[1], y[1]], [o[2], y[2]], color=color, linewidth=width)
ax.plot([o[0], z[0]], [o[1], z[1]], [o[2], z[2]], color=color, linewidth=width)
# label the frame
if frame:
if textcolor is not None:
color = textcolor
o1 = T @ np.array([-d1, -d1, -d1, 1])
ax.text(o1[0], o1[1], o1[2], r'$\{' + frame + r'\}$', color=color, verticalalignment='top', horizontalalignment='center')
# add the labels to each axis
x = (x - o) * d2 + o
y = (y - o) * d2 + o
z = (z - o) * d2 + o
ax.text(x[0], x[1], x[2], "$%c_{%s}$" % (labels[0], frame), color=color, horizontalalignment='center', verticalalignment='center')
ax.text(y[0], y[1], y[2], "$%c_{%s}$" % (labels[1], frame), color=color, horizontalalignment='center', verticalalignment='center')
ax.text(z[0], z[1], z[2], "$%c_{%s}$" % (labels[2], frame), color=color, horizontalalignment='center', verticalalignment='center')
from spatialmath.base import animate as animate
[docs] def tranimate(T, **kwargs):
"""
Animate a 3D coordinate frame
:param T: an SO(3) or SE(3) pose to be displayed as coordinate frame
:type: numpy.ndarray, shape=(3,3) or (4,4)
:param nframes: number of steps in the animation [defaault 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 movie: name of file to write MP4 movie into
:type movie: str
Animates a 3D coordinate frame moving from the world frame to a frame represented by the SO(3) or SE(3) matrix to the current axes.
- If no current figure, one is created
- If current figure, but no axes, a 3d Axes is created
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')
"""
anim = animate.Animate(**kwargs)
anim.trplot(T, **kwargs)
anim.run(**kwargs)
if __name__ == '__main__': # pragma: no cover
import pathlib
import os.path
exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_transforms.py")).read())