Transforms in 3D
These functions create and manipulate 3D rotation matrices and rigid-body transformations as 3x3 SO(3) matrices and 4x4 SE(3) matrices respectively. These matrices are represented as 2D NumPy arrays.
Vector arguments are what numpy refers to as array_like
and can be a list,
tuple, numpy array, numpy row vector or numpy column vector.
- angvec2r(theta, v, unit='rad', tol=20)[source]
Create an SO(3) rotation matrix from rotation angle and axis
- Parameters:
theta (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (array_like(3)) – 3D rotation axis
tol (
float
) – Tolerance in units of eps for zero-rotation case, defaults to 20
- Type:
float
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
- Raises:
ValueError – bad arguments
angvec2r(θ, V)
is an SO(3) orthonormal rotation matrix equivalent to a rotation ofθ
about the vectorV
.>>> from spatialmath.base import angvec2r >>> angvec2r(0.3, [1, 0, 0]) # rotx(0.3) array([[ 1. , 0. , 0. ], [ 0. , 0.9553, -0.2955], [ 0. , 0.2955, 0.9553]]) >>> angvec2r(0, [1, 0, 0]) # rotx(0) array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
Note
If
θ == 0
then return identity matrix.If
θ ~= 0
thenV
must have a finite length.
- Seealso:
- SymPy:
not supported
- angvec2tr(theta, v, unit='rad')[source]
Create an SE(3) pure rotation from rotation angle and axis
- Parameters:
theta (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (: array_like(3)) – 3D rotation axis
- Returns:
SE(3) transformation matrix
- Return type:
ndarray(4,4)
angvec2tr(θ, V)
is an SE(3) homogeneous transformation matrix equivalent to a rotation ofθ
about the vectorV
.>>> from spatialmath.base import angvec2tr >>> angvec2tr(0.3, [1, 0, 0]) # rtotx(0.3) array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. ], [ 0. , 0.2955, 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]])
Note
If
θ == 0
then return identity matrix.If
θ ~= 0
thenV
must have a finite length.The translational part is zero.
- Seealso:
- SymPy:
not supported
- angvelxform(Γ, inverse=False, full=True, representation='rpy/xyz')[source]
DEPRECATED, use
rotvelxform()
instead
- angvelxform_dot(Γ, Γd, full=True, representation='rpy/xyz')[source]
DEPRECATED, use
rotvelxform()
instead
- delta2tr(d)[source]
Convert differential motion to SE(3)
- Parameters:
Δ (array_like(6)) – differential motion as a 6-vector
- Returns:
SE(3) matrix
- Return type:
ndarray(4,4)
delta2tr(Δ)
is an SE(3) matrix representing differential motion \(\Delta = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]\).>>> from spatialmath.base import delta2tr >>> delta2tr([0.001, 0, 0, 0, 0.002, 0]) array([[ 1. , 0. , 0.002, 0.001], [ 0. , 1. , 0. , 0. ], [-0.002, 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]])
- Reference:
Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
- Seealso:
- SymPy:
supported
- eul2jac(angles)[source]
Euler angle rate Jacobian
- Parameters:
angles (array_like(3)) – Euler angles (φ, θ, ψ)
- Returns:
Jacobian matrix
- Return type:
ndarray(3,3)
eul2jac(φ, θ, ψ)
is a Jacobian matrix (3x3) that maps ZYZ Euler angle rates to angular velocity at the operating point specified by the Euler angles φ, ϴ, ψ.eul2jac(𝚪)
as above but the Euler angles are taken from𝚪
which is a 3-vector with values (φ θ ψ).
Example:
>>> from spatialmath.base import eul2jac >>> eul2jac([0.1, 0.2, 0.3]) array([[ 0. , -0.0998, 0.1977], [ 0. , 0.995 , 0.0198], [ 1. , 0. , 0.9801]])
Note
Used in the creation of an analytical Jacobian.
Angles in radians, rates in radians/sec.
- Reference:
Robotics, Vision & Control for Python, Section 8.1.3, P. Corke, Springer 2023.
- SymPy:
supported
- Seealso:
- eul2r(phi, theta=None, psi=None, unit='rad')[source]
Create an SO(3) rotation matrix from Euler angles
- Parameters:
phi (float) – Z-axis rotation
theta (float) – Y-axis rotation
psi (float) – Z-axis rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
R = eul2r(φ, θ, ψ)
is an SO(3) orthonornal rotation matrix equivalent to the specified Euler angles. These correspond to rotations about the Z, Y, Z axes respectively.R = eul2r(EUL)
as above but the Euler angles are taken fromEUL
which is a 3-vector with values (φ θ ψ).
>>> from spatialmath.base import eul2r >>> eul2r(0.1, 0.2, 0.3) array([[ 0.9021, -0.3836, 0.1977], [ 0.3875, 0.9216, 0.0198], [-0.1898, 0.0587, 0.9801]]) >>> eul2r([0.1, 0.2, 0.3]) array([[ 0.9021, -0.3836, 0.1977], [ 0.3875, 0.9216, 0.0198], [-0.1898, 0.0587, 0.9801]]) >>> eul2r([10, 20, 30], unit='deg') array([[ 0.7146, -0.6131, 0.3368], [ 0.6337, 0.7713, 0.0594], [-0.2962, 0.171 , 0.9397]])
- eul2tr(phi, theta=None, psi=None, unit='rad')[source]
Create an SE(3) pure rotation matrix from Euler angles
- Parameters:
phi (float) – Z-axis rotation
theta (float) – Y-axis rotation
psi (float) – Z-axis rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
SE(3) transformation matrix
- Return type:
ndarray(4,4)
R = eul2tr(PHI, θ, PSI)
is an SE(3) homogeneous transformation matrix equivalent to the specified Euler angles. These correspond to rotations about the Z, Y, Z axes respectively.R = eul2tr(EUL)
as above but the Euler angles are taken fromEUL
which is a 3-vector with values (PHI θ PSI).
>>> from spatialmath.base import eul2tr >>> eul2tr(0.1, 0.2, 0.3) array([[ 0.9021, -0.3836, 0.1977, 0. ], [ 0.3875, 0.9216, 0.0198, 0. ], [-0.1898, 0.0587, 0.9801, 0. ], [ 0. , 0. , 0. , 1. ]]) >>> eul2tr([0.1, 0.2, 0.3]) array([[ 0.9021, -0.3836, 0.1977, 0. ], [ 0.3875, 0.9216, 0.0198, 0. ], [-0.1898, 0.0587, 0.9801, 0. ], [ 0. , 0. , 0. , 1. ]]) >>> eul2tr([10, 20, 30], unit='deg') array([[ 0.7146, -0.6131, 0.3368, 0. ], [ 0.6337, 0.7713, 0.0594, 0. ], [-0.2962, 0.171 , 0.9397, 0. ], [ 0. , 0. , 0. , 1. ]])
Note
By default, the translational component is zero but it can be set to a non-zero value.
- exp2jac(v)[source]
Jacobian from exponential coordinate rates to angular velocity
- Parameters:
v (array_like(3)) – Exponential coordinates
- Returns:
Jacobian matrix
- Return type:
ndarray(3,3)
exp2jac(v)
is a Jacobian matrix (3x3) that maps exponential coordinate rates to angular velocity at the operating pointv
.
>>> from spatialmath.base import exp2jac >>> exp2jac([0.3, 0, 0]) array([[ 1. , 0. , 0. ], [ 0. , 0.9851, -0.1489], [ 0. , 0.1489, 0.9851]])
Note
Used in the creation of an analytical Jacobian.
Reference:
- A compact formula for the derivative of a 3-D rotation in exponential coordinate Guillermo Gallego, Anthony Yezzi https://arxiv.org/pdf/1312.0788v1.pdf - Robot Dynamics Lecture Notes Robotic Systems Lab, ETH Zurich, 2018 https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf
- SymPy:
supported
- Seealso:
- exp2r(w)[source]
Create an SO(3) rotation matrix from exponential coordinates
- Parameters:
w (array_like(3)) – exponential coordinate vector
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
- Raises:
ValueError – bad arguments
exp2r(w)
is an SO(3) orthonormal rotation matrix equivalent to a rotation of \(\| w \|\) about the vector \(\hat{w}\).If
w
is zero then result is the identity matrix.>>> from spatialmath.base import exp2r, rotx >>> exp2r([0.3, 0, 0]) array([[ 1. , 0. , 0. ], [ 0. , 0.9553, -0.2955], [ 0. , 0.2955, 0.9553]]) >>> rotx(0.3) array([[ 1. , 0. , 0. ], [ 0. , 0.9553, -0.2955], [ 0. , 0.2955, 0.9553]])
Note
Exponential coordinates are also known as an Euler vector
- Seealso:
- SymPy:
not supported
- exp2tr(w)[source]
Create an SE(3) pure rotation matrix from exponential coordinates
- Parameters:
w (array_like(3)) – exponential coordinate vector
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
- Raises:
ValueError – bad arguments
exp2r(w)
is an SO(3) orthonormal rotation matrix equivalent to a rotation of \(\| w \|\) about the vector \(\hat{w}\).If
w
is zero then result is the identity matrix.>>> from spatialmath.base import exp2tr, trotx >>> exp2tr([0.3, 0, 0]) array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. ], [ 0. , 0.2955, 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]]) >>> trotx(0.3) array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. ], [ 0. , 0.2955, 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]])
Note
Exponential coordinates are also known as an Euler vector
- Seealso:
- SymPy:
not supported
- ishom(T, check=False, tol=20)[source]
Test if matrix belongs to SE(3)
- Parameters:
T (numpy(4,4)) – SE(3) matrix to test
check (
bool
) – check validity of rotation submatrixtol (
float
) – Tolerance in units of eps for rotation submatrix check, defaults to 20
- Return type:
bool
- Returns:
whether matrix is an SE(3) homogeneous transformation matrix
ishom(T)
is True if the argumentT
is of dimension 4x4ishom(T, check=True)
as above, but also checks orthogonality of the rotation sub-matrix and validitity of the bottom row.
>>> from spatialmath.base import ishom >>> import numpy as np >>> T = np.array([[1, 0, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]]) >>> ishom(T) True >>> T = np.array([[1, 1, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]]) # invalid SE(3) >>> ishom(T) # a quick check says it is an SE(3) True >>> ishom(T, check=True) # but if we check more carefully... False >>> R = np.array([[1, 1, 0], [0, 1, 0], [0, 0, 1]]) >>> ishom(R) False
- isrot(R, check=False, tol=20)[source]
Test if matrix belongs to SO(3)
- Parameters:
R (numpy(3,3)) – SO(3) matrix to test
check (
bool
) – check validity of rotation submatrixtol (
float
) – Tolerance in units of eps for rotation matrix test, defaults to 20
- Return type:
bool
- Returns:
whether matrix is an SO(3) rotation matrix
isrot(R)
is True if the argumentR
is of dimension 3x3isrot(R, check=True)
as above, but also checks orthogonality of the rotation matrix.
>>> from spatialmath.base import isrot >>> import numpy as np >>> T = np.array([[1, 0, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]]) >>> isrot(T) False >>> R = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) >>> isrot(R) True >>> R = R = np.array([[1, 1, 0], [0, 1, 0], [0, 0, 1]]) # invalid SO(3) >>> isrot(R) # a quick check says it is an SO(3) True >>> isrot(R, check=True) # but if we check more carefully... False
- oa2r(o, a)[source]
Create SO(3) rotation matrix from two vectors
- Parameters:
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
T = oa2tr(O, A)
is an SO(3) orthonormal rotation matrix for a frame defined in terms of vectors parallel to its Y- and Z-axes with respect to a reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A.Steps:
N’ = O x A
O’ = A x N
normalize N’, O’, A
stack horizontally into rotation matrix
>>> from spatialmath.base import oa2r >>> oa2r([0, 1, 0], [0, 0, -1]) # Y := Y, Z := -Z array([[-1., 0., 0.], [ 0., 1., 0.], [ 0., 0., -1.]])
Note
The A vector is the only guaranteed to have the same direction in the resulting rotation matrix
O and A do not have to be unit-length, they are normalized
O and A do not have to be orthogonal, so long as they are not parallel
The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.
- Seealso:
- SymPy:
not supported
- oa2tr(o, a)[source]
Create SE(3) pure rotation from two vectors
- Parameters:
- Returns:
SE(3) transformation matrix
- Return type:
ndarray(4,4)
T = oa2tr(O, A)
is an SE(3) homogeneous transformation matrix for a frame defined in terms of vectors parallel to its Y- and Z-axes with respect to a reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A.Steps:
N’ = O x A
O’ = A x N
normalize N’, O’, A
stack horizontally into rotation matrix
>>> from spatialmath.base import oa2tr >>> oa2tr([0, 1, 0], [0, 0, -1]) # Y := Y, Z := -Z array([[-1., 0., 0., 0.], [ 0., 1., 0., 0.], [ 0., 0., -1., 0.], [ 0., 0., 0., 1.]])
- Seealso:
- SymPy:
not supported
- r2x(R, representation='rpy/xyz')[source]
Convert SO(3) matrix to angular representation
- Parameters:
R (ndarray(3,3)) – SO(3) rotation matrix
representation (str, optional) – rotational representation, defaults to “rpy/xyz”
- Returns:
angular representation
- Return type:
ndarray(3)
Convert an SO(3) rotation matrix to a minimal rotational representation \(\vec{\Gamma} \in \mathbb{R}^3\).
representation
Rotational representation
"rpy/xyz"
"arm"
RPY angular rates in XYZ order (default)
"rpy/zyx"
"vehicle"
RPY angular rates in XYZ order
"rpy/yxz"
"camera"
RPY angular rates in YXZ order
"eul"
Euler angular rates in ZYZ order
"exp"
exponential coordinate rates
- rodrigues(w, theta=None)[source]
Rodrigues’ formula for 3D rotation
- Parameters:
w (array_like(3)) – rotation vector
theta (float or None) – rotation angle
- Returns:
SO(3) matrix
- Return type:
ndarray(3,3)
Compute Rodrigues’ formula for a rotation matrix given a rotation axis and angle.
\[\mat{R} = \mat{I}_{3 \times 3} + \sin \theta \skx{\hat{\vec{v}}} + (1 - \cos \theta) \skx{\hat{\vec{v}}}^2\]>>> from spatialmath.base import * >>> rodrigues([1, 0, 0], 0.3) array([[ 1. , 0. , 0. ], [ 0. , 0.9553, -0.2955], [ 0. , 0.2955, 0.9553]]) >>> rodrigues([0.3, 0, 0]) array([[ 1. , 0. , 0. ], [ 0. , 0.9553, -0.2955], [ 0. , 0.2955, 0.9553]])
- rot2jac(R, representation='rpy/xyz')[source]
DEPRECATED, use
rotvelxform()
instead
- rotvelxform(Γ, inverse=False, full=False, representation='rpy/xyz')[source]
Rotational velocity transformation
- Parameters:
𝚪 (array_like(3) or ndarray(3,3)) – angular representation or rotation matrix
representation (str, optional) – defaults to ‘rpy/xyz’
inverse (bool) – compute mapping from analytical rates to angular velocity
full (bool) – return 6x6 transform for spatial velocity
- Returns:
rotation rate transformation matrix
- Return type:
ndarray(3,3) or ndarray(6,6)
Computes the transformation from analytical rates \(\dvec{x}\) where the rotational part is expressed as the rate of change in some angular representation to spatial velocity \(\omega\), where rotation rate is expressed as angular velocity.
\[\vec{\omega} = \mat{A}(\Gamma) \dvec{x}\]where \(\mat{A}\) is a 3x3 matrix and \(\Gamma \in \mathbb{R}^3\) is a minimal angular representation.
\(\mat{A}(\Gamma)\) is a function of the rotational representation which can be specified by the parameter
𝚪
as a 1D array, or by an SO(3) rotation matrix which will be converted to therepresentation
.representation
Rotational representation
"rpy/xyz"
"arm"
RPY angular rates in XYZ order (default)
"rpy/zyx"
"vehicle"
RPY angular rates in XYZ order
"rpy/yxz"
"camera"
RPY angular rates in YXZ order
"eul"
Euler angular rates in ZYZ order
"exp"
exponential coordinate rates
If
inverse==True
return \(\mat{A}^{-1}\) computed using a closed-form solution rather than matrix inverse.If
full=True
a block diagonal 6x6 matrix is returned which transforms analytic velocity to spatial velocity.The analytical Jacobian is
\[\mat{J}_a(q) = \mat{A}^{-1}(\Gamma)\, \mat{J}(q)\]where \(\mat{A}\) is computed with
inverse==True
andfull=True
.Reference:
symbolic/angvelxform.ipynb
in this ToolboxRobot Dynamics Lecture Notes, Robotic Systems Lab, ETH Zurich, 2018 https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf
- SymPy:
supported
- Seealso:
- rotvelxform_inv_dot(Γ, Γd, full=False, representation='rpy/xyz')[source]
Derivative of angular velocity transformation
- Parameters:
𝚪 (array_like(3)) – angular representation
𝚪d (array_like(3)) – angular representation rate \(\dvec{\Gamma}\)
representation (
str
) – defaults to ‘rpy/xyz’full (
bool
) – return 6x6 transform for spatial velocity
- Returns:
derivative of inverse angular velocity transformation matrix
- Return type:
ndarray(6,6) or ndarray(3,3)
The angular rate transformation matrix \(\mat{A} \in \mathbb{R}^{6 \times 6}\) is such that
\[\dvec{x} = \mat{A}^{-1}(\Gamma) \vec{\nu}\]where \(\dvec{x} \in \mathbb{R}^6\) is analytic velocity \((\vec{v}, \dvec{\Gamma})\), \(\vec{\nu} \in \mathbb{R}^6\) is spatial velocity \((\vec{v}, \vec{\omega})\), and \(\vec{\Gamma} \in \mathbb{R}^3\) is a minimal rotational representation.
The relationship between spatial and analytic acceleration is
\[\ddvec{x} = \dmat{A}^{-1}(\Gamma, \dot{\Gamma}) \vec{\nu} + \mat{A}^{-1}(\Gamma) \dvec{\nu}\]and \(\dmat{A}^{-1}(\Gamma, \dot{\Gamma})\) is computed by this function.
representation
Rotational representation
"rpy/xyz"
"arm"
RPY angular rates in XYZ order (default)
"rpy/zyx"
"vehicle"
RPY angular rates in XYZ order
"rpy/yxz"
"camera"
RPY angular rates in YXZ order
"eul"
Euler angular rates in ZYZ order
"exp"
exponential coordinate rates
If
full=True
a block diagonal 6x6 matrix is returned which transforms analytic analytic rotational acceleration to angular acceleration.Reference:
symbolic/angvelxform.ipynb
in this Toolboxsymbolic/angvelxform_dot.ipynb
in this Toolbox
- Seealso:
- rotx(theta, unit='rad')[source]
Create SO(3) rotation about X-axis
- Parameters:
theta (
float
) – rotation angle about X-axisunit (
str
) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
rotx(θ)
is an SO(3) rotation matrix (3x3) representing a rotation of θ radians about the x-axisrotx(θ, "deg")
as above but θ is in degrees
>>> from spatialmath.base import rotx >>> rotx(0.3) array([[ 1. , 0. , 0. ], [ 0. , 0.9553, -0.2955], [ 0. , 0.2955, 0.9553]]) >>> rotx(45, 'deg') array([[ 1. , 0. , 0. ], [ 0. , 0.7071, -0.7071], [ 0. , 0.7071, 0.7071]])
- Seealso:
- SymPy:
supported
- roty(theta, unit='rad')[source]
Create SO(3) rotation about Y-axis
- Parameters:
theta (
float
) – rotation angle about Y-axisunit (
str
) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
roty(θ)
is an SO(3) rotation matrix (3x3) representing a rotation of θ radians about the y-axisroty(θ, "deg")
as above but θ is in degrees
>>> from spatialmath.base import roty >>> roty(0.3) array([[ 0.9553, 0. , 0.2955], [ 0. , 1. , 0. ], [-0.2955, 0. , 0.9553]]) >>> roty(45, 'deg') array([[ 0.7071, 0. , 0.7071], [ 0. , 1. , 0. ], [-0.7071, 0. , 0.7071]])
- Seealso:
- SymPy:
supported
- rotz(theta, unit='rad')[source]
Create SO(3) rotation about Z-axis
- Parameters:
theta (
float
) – rotation angle about Z-axisunit (
str
) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
rotz(θ)
is an SO(3) rotation matrix (3x3) representing a rotation of θ radians about the z-axisrotz(θ, "deg")
as above but θ is in degrees
>>> from spatialmath.base import rotz >>> rotz(0.3) array([[ 0.9553, -0.2955, 0. ], [ 0.2955, 0.9553, 0. ], [ 0. , 0. , 1. ]]) >>> rotz(45, 'deg') array([[ 0.7071, -0.7071, 0. ], [ 0.7071, 0.7071, 0. ], [ 0. , 0. , 1. ]])
- Seealso:
- SymPy:
supported
- rpy2jac(angles, order='zyx')[source]
Jacobian from RPY angle rates to angular velocity
- Parameters:
- Return type:
ndarray
[Any
,dtype
[TypeVar
(ScalarType
, bound=generic
, covariant=True)]]- Returns:
Jacobian matrix
rpy2jac(⍺, β, γ)
is a Jacobian matrix (3x3) that maps roll-pitch-yaw angle rates to angular velocity at the operating point (⍺, β, γ). These correspond to successive rotations about the axes specified byorder
:‘zyx’ [default], rotate by γ about the z-axis, then by β about the new y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot with x-axis forward and y-axis sideways.
‘xyz’, rotate by γ about the x-axis, then by β about the new y-axis, then by ⍺ about the new z-axis. Convention for a robot gripper with z-axis forward and y-axis between the gripper fingers.
‘yxz’, rotate by γ about the y-axis, then by β about the new x-axis, then by ⍺ about the new z-axis. Convention for a camera with z-axis parallel to the optic axis and x-axis parallel to the pixel rows.
rpy2jac(𝚪)
as above but the roll, pitch, yaw angles are taken from𝚪
which is a 3-vector with values (⍺, β, γ).
>>> from spatialmath.base import rpy2jac >>> rpy2jac([0.1, 0.2, 0.3]) array([[ 0.9363, -0.2955, 0. ], [ 0.2896, 0.9553, 0. ], [-0.1987, 0. , 1. ]])
Note
Used in the creation of an analytical Jacobian.
Angles in radians, rates in radians/sec.
- Reference:
Robotics, Vision & Control for Python, Section 8.1.3, P. Corke, Springer 2023.
- SymPy:
supported
- Seealso:
- rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx')[source]
Create an SO(3) rotation matrix from roll-pitch-yaw angles
- Parameters:
roll (float or array_like(3)) – roll angle
pitch (float) – pitch angle
yaw (float) – yaw angle
unit (
str
) – angular units: ‘rad’ [default], or ‘deg’order (
str
) – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
- Raises:
ValueError – bad argument
rpy2r(⍺, β, γ)
is an SO(3) orthonormal rotation matrix (3x3) equivalent to the specified roll (⍺), pitch (β), yaw (γ) angles angles. These correspond to successive rotations about the axes specified byorder
:‘zyx’ [default], rotate by γ about the z-axis, then by β about the new y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot with x-axis forward and y-axis sideways.
‘xyz’, rotate by γ about the x-axis, then by β about the new y-axis, then by ⍺ about the new z-axis. Convention for a robot gripper with z-axis forward and y-axis between the gripper fingers.
‘yxz’, rotate by γ about the y-axis, then by β about the new x-axis, then by ⍺ about the new z-axis. Convention for a camera with z-axis parallel to the optic axis and x-axis parallel to the pixel rows.
rpy2r(RPY)
as above but the roll, pitch, yaw angles are taken fromRPY
which is a 3-vector with values (⍺, β, γ).
>>> from spatialmath.base import rpy2r >>> rpy2r(0.1, 0.2, 0.3) array([[ 0.9363, -0.2751, 0.2184], [ 0.2896, 0.9564, -0.037 ], [-0.1987, 0.0978, 0.9752]]) >>> rpy2r([0.1, 0.2, 0.3]) array([[ 0.9363, -0.2751, 0.2184], [ 0.2896, 0.9564, -0.037 ], [-0.1987, 0.0978, 0.9752]]) >>> rpy2r([10, 20, 30], unit='deg') array([[ 0.8138, -0.441 , 0.3785], [ 0.4698, 0.8826, 0.018 ], [-0.342 , 0.1632, 0.9254]])
- rpy2tr(roll, pitch=None, yaw=None, unit='rad', order='zyx')[source]
Create an SE(3) rotation matrix from roll-pitch-yaw angles
- Parameters:
roll (float) – roll angle
pitch (float) – pitch angle
yaw (float) – yaw angle
unit (str) – angular units: ‘rad’ [default], or ‘deg’
order (str) – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns:
SE(3) transformation matrix
- Return type:
ndarray(4,4)
rpy2tr(⍺, β, γ)
is an SE(3) matrix (4x4) equivalent to the specified roll (⍺), pitch (β), yaw (γ) angles angles. These correspond to successive rotations about the axes specified byorder
:‘zyx’ [default], rotate by γ about the z-axis, then by β about the new y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot with x-axis forward and y-axis sideways.
‘xyz’, rotate by γ about the x-axis, then by β about the new y-axis, then by ⍺ about the new z-axis. Convention for a robot gripper with z-axis forward and y-axis between the gripper fingers.
‘yxz’, rotate by γ about the y-axis, then by β about the new x-axis, then by ⍺ about the new z-axis. Convention for a camera with z-axis parallel to the optic axis and x-axis parallel to the pixel rows.
rpy2tr(RPY)
as above but the roll, pitch, yaw angles are taken fromRPY
which is a 3-vector with values (⍺, β, γ).
>>> from spatialmath.base import rpy2tr >>> rpy2tr(0.1, 0.2, 0.3) array([[ 0.9363, -0.2751, 0.2184, 0. ], [ 0.2896, 0.9564, -0.037 , 0. ], [-0.1987, 0.0978, 0.9752, 0. ], [ 0. , 0. , 0. , 1. ]]) >>> rpy2tr([0.1, 0.2, 0.3]) array([[ 0.9363, -0.2751, 0.2184, 0. ], [ 0.2896, 0.9564, -0.037 , 0. ], [-0.1987, 0.0978, 0.9752, 0. ], [ 0. , 0. , 0. , 1. ]]) >>> rpy2tr([10, 20, 30], unit='deg') array([[ 0.8138, -0.441 , 0.3785, 0. ], [ 0.4698, 0.8826, 0.018 , 0. ], [-0.342 , 0.1632, 0.9254, 0. ], [ 0. , 0. , 0. , 1. ]])
Note
By default, the translational component is zero but it can be set to a non-zero value.
- tr2adjoint(T)[source]
Adjoint matrix
- Parameters:
T (ndarray(4,4) or ndarray(3,3)) – SE(3) or SO(3) matrix
- Returns:
adjoint matrix
- Return type:
ndarray(6,6) or ndarray(3,3)
Computes an adjoint matrix that maps the Lie algebra between frames.
where \(\mat{T} \in \SE3\).
tr2jac(T)
is an adjoint matrix (6x6) that maps spatial velocity or differential motion between frame {B} to frame {A} which are attached to the same moving body. The pose of {B} relative to {A} is represented by the homogeneous transform T = \({}^A {\bf T}_B\).>>> from spatialmath.base import tr2adjoint, trotx >>> T = trotx(0.3, t=[4,5,6]) >>> tr2adjoint(T) array([[ 1. , 0. , 0. , 0. , -4.2544, 6.5498], [ 0. , 0.9553, -0.2955, 6. , -1.1821, -3.8213], [ 0. , 0.2955, 0.9553, -5. , 3.8213, -1.1821], [ 0. , 0. , 0. , 1. , 0. , 0. ], [ 0. , 0. , 0. , 0. , 0.9553, -0.2955], [ 0. , 0. , 0. , 0. , 0.2955, 0.9553]])
- Reference:
Robotics, Vision & Control for Python, Section 3, P. Corke, Springer 2023.
- SymPy:
supported
- tr2angvec(T, unit='rad', check=False)[source]
Convert SO(3) or SE(3) to angle and rotation vector
- Parameters:
R (ndarray(4,4) or ndarray(3,3)) – SE(3) or SO(3) matrix
unit (str) – ‘rad’ or ‘deg’
check (bool) – check that rotation matrix is valid
- Returns:
\((\theta, {\bf v})\)
- Return type:
float, ndarray(3)
- Raises:
ValueError – bad arguments
(v, θ) = tr2angvec(R)
is a rotation angle and a vector about which the rotation acts that corresponds to the rotation part ofR
.By default the angle is in radians but can be changed setting unit=’deg’.
>>> from spatialmath.base import troty, tr2angvec >>> T = troty(45, 'deg') >>> v, theta = tr2angvec(T) >>> print(v, theta) 0.7853981633974483 [0. 1. 0.]
Note
If the input is SE(3) the translation component is ignored.
- Seealso:
- tr2delta(T0, T1=None)[source]
Difference of SE(3) matrices as differential motion
- Parameters:
T0 (ndarray(4,4)) – first SE(3) matrix
T1 (ndarray(4,4)) – second SE(3) matrix
- Returns:
Differential motion as a 6-vector
- Return type:
ndarray(6)
- Raises:
ValueError – bad arguments
tr2delta(T0, T1)
is the differential motion Δ (6x1) corresponding to infinitessimal motion (in the T0 frame) from pose T0 to T1 which are SE(3) matrices.tr2delta(T)
as above but the motion is from the world frame to the pose represented by T.
The vector \(\Delta = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]\) represents infinitessimal translation and rotation, and is an approximation to the instantaneous spatial velocity multiplied by time step.
>>> from spatialmath.base import tr2delta, trotx >>> T1 = trotx(0.3, t=[4,5,6]) >>> T2 = trotx(0.31, t=[4,5.02,6]) >>> tr2delta(T1, T2) array([ 0. , 0.0191, -0.0059, 0.01 , 0. , 0. ])
Note
Δ is only an approximation to the motion T, and assumes that T0 ~ T1 or T ~ eye(4,4).
Can be considered as an approximation to the effect of spatial velocity over a a time interval, average spatial velocity multiplied by time.
- Reference:
Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
- Seealso:
- SymPy:
supported
- tr2eul(T, unit='rad', flip=False, check=False, tol=20)[source]
Convert SO(3) or SE(3) to ZYX Euler angles
- Parameters:
R (ndarray(4,4) or ndarray(3,3)) – SE(3) or SO(3) matrix
unit (str) – ‘rad’ or ‘deg’
flip (bool) – choose first Euler angle to be in quadrant 2 or 3
check (bool) – check that rotation matrix is valid
tol (
float
) – Tolerance in units of eps for near-zero checks, defaults to 20
- Type:
float
- Returns:
ZYZ Euler angles
- Return type:
ndarray(3)
tr2eul(R)
are the Euler angles corresponding to the rotation part ofR
.The 3 angles \([\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’.
>>> from spatialmath.base import tr2eul, eul2tr >>> T = eul2tr(0.2, 0.3, 0.5) >>> print(T) [[ 0.7264 -0.6232 0.2896 0. ] [ 0.6364 0.7691 0.0587 0. ] [-0.2593 0.1417 0.9553 0. ] [ 0. 0. 0. 1. ]] >>> tr2eul(T) array([0.2, 0.3, 0.5])
Note
There is a singularity for the case where \(\theta=0\) in which case we arbitrarily set \(\phi = 0\) and \(\phi\) is set to \(\phi+\psi\).
If the input is SE(3) the translation component is ignored.
- Seealso:
- SymPy:
not supported
- tr2jac(T)[source]
SE(3) Jacobian matrix
- Parameters:
T (ndarray(4,4)) – SE(3) matrix
- Returns:
Jacobian matrix
- Return type:
ndarray(6,6)
Computes an Jacobian matrix that maps spatial velocity between two frames defined by an SE(3) matrix.
tr2jac(T)
is a Jacobian matrix (6x6) that maps spatial velocity or differential motion from frame {B} to frame {A} where the pose of {B} elative to {A} is represented by the homogeneous transform T = \({}^A {\bf T}_B\).>>> from spatialmath.base import tr2jac, trotx >>> T = trotx(0.3, t=[4,5,6]) >>> tr2jac(T) array([[ 1. , 0. , 0. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. , 0. , 0. ], [ 0. , 0.2955, 0.9553, 0. , 0. , 0. ], [ 0. , 0. , 0. , 1. , 0. , 0. ], [ 0. , 0. , 0. , 0. , 0.9553, -0.2955], [ 0. , 0. , 0. , 0. , 0.2955, 0.9553]])
- Reference:
Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
- SymPy:
supported
- tr2rpy(T, unit='rad', order='zyx', check=False, tol=20)[source]
Convert SO(3) or SE(3) to roll-pitch-yaw angles
- Parameters:
R (ndarray(4,4) or ndarray(3,3)) – SE(3) or SO(3) matrix
unit (str) – ‘rad’ or ‘deg’
order (str) – ‘xyz’, ‘zyx’ or ‘yxz’ [default ‘zyx’]
check (bool) – check that rotation matrix is valid
tol (
float
) – Tolerance in units of eps, defaults to 20
- Type:
float
- Returns:
Roll-pitch-yaw angles
- Return type:
ndarray(3)
- Raises:
ValueError – bad arguments
tr2rpy(R)
are the roll-pitch-yaw angles corresponding to the rotation part ofR
.The 3 angles RPY = \([\theta_R, \theta_P, \theta_Y]\) correspond to sequential rotations about the Z, Y and X axes respectively. The axis order sequence can be changed by setting:
order='xyz'
for sequential rotations about X, Y, Z axesorder='yxz'
for sequential rotations about Y, X, Z axes
By default the angles are in radians but can be changed setting
unit='deg'
.>>> from spatialmath.base import tr2rpy, rpy2tr >>> T = rpy2tr(0.2, 0.3, 0.5) >>> print(T) [[ 0.8384 -0.4183 0.3494 0. ] [ 0.458 0.8882 -0.0355 0. ] [-0.2955 0.1898 0.9363 0. ] [ 0. 0. 0. 1. ]] >>> tr2rpy(T) array([0.2, 0.3, 0.5])
Note
There is a singularity for the case where \(\theta_P = \pi/2\) in which case we arbitrarily set \(\theta_R=0\) and \(\theta_Y = \theta_R + \theta_Y\).
If the input is SE(3) the translation component is ignored.
- Seealso:
- SymPy:
not supported
- tr2x(T, representation='rpy/xyz')[source]
Convert SE(3) to an analytic representation
- Parameters:
T (ndarray(4,4)) – pose as an SE(3) matrix
representation (str, optional) – angular representation to use, defaults to “rpy/xyz”
- Returns:
analytic vector representation
- Return type:
ndarray(6)
Convert an SE(3) matrix into an equivalent vector representation \(\vec{x} = (\vec{t},\vec{r}) \in \mathbb{R}^6\) where rotation \(\vec{r} \in \mathbb{R}^3\) is encoded in a minimal representation.
representation
Rotational representation
"rpy/xyz"
"arm"
RPY angular rates in XYZ order (default)
"rpy/zyx"
"vehicle"
RPY angular rates in XYZ order
"rpy/yxz"
"camera"
RPY angular rates in YXZ order
"eul"
Euler angular rates in ZYZ order
"exp"
exponential coordinate rates
- SymPy:
supported
- Seealso:
- tranimate(T, **kwargs)[source]
Animate a 3D coordinate frame
- Parameters:
T (ndarray(4,4) or ndarray(3,3) or an iterable returning same) – SE(3) or SO(3) matrix
nframes (int) – number of steps in the animation [default 100]
repeat (bool) – animate in endless loop [default False]
interval (int) – number of milliseconds between frames [default 50]
wait (bool) – wait until animation is complete, default False
movie (str) – name of file to write MP4 movie into
**kwargs –
arguments passed to
trplot
- Return type:
str
tranimate(T)
whereT
is an SO(3) or SE(3) matrix, animates a 3D coordinate frame moving from the world frame to the frameT
innsteps
.tranimate(I)
whereI
is an iterable or generator, animates a 3D coordinate frame representing the pose of each element in the sequence of SO(3) or SE(3) matrices.
Examples:
>>> tranimate(transl(1,2,3)@trotx(1), frame='A', arrow=False, dims=[0, 5]) >>> tranimate(transl(1,2,3)@trotx(1), frame='A', arrow=False, dims=[0, 5], movie='spin.mp4')
Note
For Jupyter this works with the
notebook
andTkAgg
backends.Note
The animation occurs in the background after
tranimate
has returned. Ifblock=True
this blocks after the animation has completed.Note
When saving animation to a file the animation does not appear on screen. A
StopIteration
exception may occur, this seems to be a matplotlib bug #19599- SymPy:
not supported
- Seealso:
trplot, plotvol3
- transl(x, y=None, z=None)[source]
Create SE(3) pure translation, or extract translation from SE(3) matrix
Create a translational SE(3) matrix
- Parameters:
x (float) – translation along X-axis
y (float) – translation along Y-axis
z (float) – translation along Z-axis
- Returns:
SE(3) transformation matrix
- Return type:
numpy(4,4)
- Raises:
ValueError – bad argument
T = transl( X, Y, Z )
is an SE(3) homogeneous transform (4x4) representing a pure translation of X, Y and Z.T = transl( V )
as above but the translation is given by a 3-element list, dict, or a numpy array, row or column vector.
>>> from spatialmath.base import transl >>> import numpy as np >>> transl(3, 4, 5) array([[1., 0., 0., 3.], [0., 1., 0., 4.], [0., 0., 1., 5.], [0., 0., 0., 1.]]) >>> transl([3, 4, 5]) array([[1., 0., 0., 3.], [0., 1., 0., 4.], [0., 0., 1., 5.], [0., 0., 0., 1.]]) >>> transl(np.array([3, 4, 5])) array([[1., 0., 0., 3.], [0., 1., 0., 4.], [0., 0., 1., 5.], [0., 0., 0., 1.]])
Extract the translational part of an SE(3) matrix
- Parameters:
x (numpy(4,4)) – SE(3) transformation matrix
- Returns:
translation elements of SE(2) matrix
- Return type:
ndarray(3)
- Raises:
ValueError – bad argument
t = transl(T)
is the translational part of a homogeneous transform T as a 3-element numpy array.
>>> from spatialmath.base import transl >>> import numpy as np >>> T = np.array([[1, 0, 0, 3], [0, 1, 0, 4], [0, 0, 1, 5], [0, 0, 0, 1]]) >>> transl(T) array([3, 4, 5])
Note
This function is compatible with the MATLAB version of the Toolbox. It is unusual/weird in doing two completely different things inside the one function.
- Seealso:
- SymPy:
supported
- trexp(S, theta=None, check=True)[source]
Exponential of se(3) or so(3) matrix
- Parameters:
S – se(3), so(3) matrix or equivalent twist vector
θ (float) – motion
- Returns:
matrix exponential in SE(3) or SO(3)
- Return type:
ndarray(4,4) or ndarray(3,3)
- Raises:
ValueError – bad arguments
An efficient closed-form solution of the matrix exponential for arguments that are so(3) or se(3).
For so(3) the results is an SO(3) rotation matrix:
trexp(Ω)
is the matrix exponential of the so(3) elementΩ
which is a 3x3 skew-symmetric matrix.trexp(Ω, θ)
as above but for an so(3) motion of Ωθ, whereΩ
is unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude given byθ
.trexp(ω)
is the matrix exponential of the so(3) elementω
expressed as a 3-vector.trexp(ω, θ)
as above but for an so(3) motion of ωθ whereω
is a unit-norm vector representing a rotation axis and a rotation magnitude given byθ
.ω
is expressed as a 3-vector.
>>> from spatialmath.base import trexp, skew >>> trexp(skew([1, 2, 3])) array([[-0.6949, 0.7135, 0.0893], [-0.192 , -0.3038, 0.9332], [ 0.693 , 0.6313, 0.3481]]) >>> trexp(skew([1, 0, 0]), 2) # revolute unit twist array([[ 1. , 0. , 0. ], [ 0. , -0.4161, -0.9093], [ 0. , 0.9093, -0.4161]]) >>> trexp([1, 2, 3]) array([[-0.6949, 0.7135, 0.0893], [-0.192 , -0.3038, 0.9332], [ 0.693 , 0.6313, 0.3481]]) >>> trexp([1, 0, 0], 2) # revolute unit twist array([[ 1. , 0. , 0. ], [ 0. , -0.4161, -0.9093], [ 0. , 0.9093, -0.4161]])
For se(3) the results is an SE(3) homogeneous transformation matrix:
trexp(Σ)
is the matrix exponential of the se(3) elementΣ
which is a 4x4 augmented skew-symmetric matrix.trexp(Σ, θ)
as above but for an se(3) motion of Σθ, whereΣ
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix.trexp(S)
is the matrix exponential of the se(3) elementS
represented as a 6-vector which can be considered a screw motion.trexp(S, θ)
as above but for an se(3) motion of Sθ, whereS
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix.
>>> from spatialmath.base import trexp, skewa >>> trexp(skewa([1, 2, 3, 4, 5, 6])) array([[-0.423 , 0.0528, 0.9046, 1.6867], [ 0.8802, -0.213 , 0.424 , 1.9326], [ 0.2151, 0.9756, 0.0436, 2.5984], [ 0. , 0. , 0. , 1. ]]) >>> trexp(skewa([1, 0, 0, 0, 0, 0]), 2) # prismatic unit twist array([[1., 0., 0., 2.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) >>> trexp([1, 2, 3, 4, 5, 6]) array([[-0.423 , 0.0528, 0.9046, 1.6867], [ 0.8802, -0.213 , 0.424 , 1.9326], [ 0.2151, 0.9756, 0.0436, 2.5984], [ 0. , 0. , 0. , 1. ]]) >>> trexp([1, 0, 0, 0, 0, 0], 2) array([[1., 0., 0., 2.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])
- Seealso:
trexp2()
- trinterp(start, end, s, shortest=True)[source]
Interpolate SE(3) matrices
- Parameters:
start (ndarray(4,4) or ndarray(3,3)) – initial SE(3) or SO(3) matrix value when s=0, if None then identity is used
end (ndarray(4,4) or ndarray(3,3)) – final SE(3) or SO(3) matrix, value when s=1
s (float) – interpolation coefficient, range 0 to 1
shortest (bool, default to True) – take the shortest path along the great circle for the rotation
- Returns:
interpolated SE(3) or SO(3) matrix value
- Return type:
ndarray(4,4) or ndarray(3,3)
- Raises:
ValueError – bad arguments
trinterp(None, T, S)
is a homogeneous transform (4x4) interpolated between identity when S=0 and T (4x4) when S=1.trinterp(T0, T1, S)
as above but interpolated between T0 (4x4) when S=0 and T1 (4x4) when S=1.trinterp(None, R, S)
is a rotation matrix (3x3) interpolated between identity when S=0 and R (3x3) when S=1.trinterp(R0, R1, S)
as above but interpolated between R0 (3x3) when S=0 and R1 (3x3) when S=1.
>>> from spatialmath.base import transl, trinterp >>> T1 = transl(1, 2, 3) >>> T2 = transl(4, 5, 6) >>> trinterp(T1, T2, 0) array([[1., 0., 0., 1.], [0., 1., 0., 2.], [0., 0., 1., 3.], [0., 0., 0., 1.]]) >>> trinterp(T1, T2, 1) array([[1., 0., 0., 4.], [0., 1., 0., 5.], [0., 0., 1., 6.], [0., 0., 0., 1.]]) >>> trinterp(T1, T2, 0.5) array([[1. , 0. , 0. , 2.5], [0. , 1. , 0. , 3.5], [0. , 0. , 1. , 4.5], [0. , 0. , 0. , 1. ]]) >>> trinterp(None, T2, 0) array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) >>> trinterp(None, T2, 1) array([[1., 0., 0., 4.], [0., 1., 0., 5.], [0., 0., 1., 6.], [0., 0., 0., 1.]]) >>> trinterp(None, T2, 0.5) array([[1. , 0. , 0. , 2. ], [0. , 1. , 0. , 2.5], [0. , 0. , 1. , 3. ], [0. , 0. , 0. , 1. ]])
Note
Rotation is interpolated using quaternion spherical linear interpolation (slerp).
- Seealso:
spatialmath.base.quaternions.qlerp()
trinterp2()
- trinv(T)[source]
Invert an SE(3) matrix
- Parameters:
T (ndarray(4,4)) – SE(3) matrix
- Returns:
inverse of SE(3) matrix
- Return type:
ndarray(4,4)
- Raises:
ValueError – bad arguments
Computes an efficient inverse of an SE(3) matrix:
\(\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}\)
>>> from spatialmath.base import trinv, trotx >>> T = trotx(0.3, t=[4,5,6]) >>> trinv(T) array([[ 1. , 0. , 0. , -4. ], [ 0. , 0.9553, 0.2955, -6.5498], [ 0. , -0.2955, 0.9553, -4.2544], [ 0. , 0. , 0. , 1. ]]) >>> T @ trinv(T) array([[ 1., 0., 0., 0.], [ 0., 1., -0., 0.], [ 0., -0., 1., 0.], [ 0., 0., 0., 1.]])
- SymPy:
supported
- trlog(T, check=True, twist=False, tol=20)[source]
Logarithm of SO(3) or SE(3) matrix
- Parameters:
R (ndarray(4,4) or ndarray(3,3)) – SE(3) or SO(3) matrix
check (bool) – check that matrix is valid
twist (bool) – return a twist vector instead of matrix [default]
tol (
float
) – Tolerance in units of eps for zero-rotation case, defaults to 20
- Type:
float
- Returns:
logarithm
- Return type:
ndarray(4,4) or ndarray(3,3)
- Raises:
ValueError – bad argument
An efficient closed-form solution of the matrix logarithm for arguments that are SO(3) or SE(3).
trlog(R)
is the logarithm of the passed rotation matrixR
which will be 3x3 skew-symmetric matrix. The equivalent vector fromvex()
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 matrixT
which will be 4x4 augumented skew-symmetric matrix. The equivalent vector fromvexa()
is the twist vector (6x1) comprising [v w].
>>> from spatialmath.base import trlog, rotx, trotx >>> trlog(trotx(0.3)) array([[ 0. , 0. , 0. , 0. ], [ 0. , 0. , -0.3, 0. ], [ 0. , 0.3, 0. , 0. ], [ 0. , 0. , 0. , 0. ]]) >>> trlog(trotx(0.3), twist=True) array([0. , 0. , 0. , 0.3, 0. , 0. ]) >>> trlog(rotx(0.3)) array([[ 0. , 0. , 0. ], [ 0. , 0. , -0.3], [ 0. , 0.3, 0. ]]) >>> trlog(rotx(0.3), twist=True) array([0.3, 0. , 0. ])
- trnorm(T)[source]
Normalize an SO(3) or SE(3) matrix
- Parameters:
T (ndarray(4,4) or ndarray(3,3)) – SE(3) or SO(3) matrix
- Returns:
normalized SE(3) or SO(3) matrix
- Return type:
ndarray(4,4) or ndarray(3,3)
- Raises:
ValueError – bad arguments
trnorm(R)
is guaranteed to be a proper orthogonal matrix rotation matrix (3x3) which is close to the input matrix R (3x3).trnorm(T)
as above but the rotational submatrix of the homogeneous transformation T (4x4) is normalised while the translational part is unchanged.
The steps in normalization are:
If \(\mathbf{R} = [n, o, a]\)
Form unit vectors \(\hat{o}, \hat{a}\) from \(o, a\) respectively
Form the normal vector \(\hat{n} = \hat{o} \times \hat{a}\)
Recompute \(\hat{o} = \hat{a} \times \hat{n}\) to ensure that \(\hat{o}, \hat{a}\) are orthogonal
Form the normalized SO(3) matrix \(\mathbf{R} = [\hat{n}, \hat{o}, \hat{a}]\)
>>> from spatialmath.base import trnorm, troty >>> from numpy import linalg >>> T = troty(45, 'deg', t=[3, 4, 5]) >>> linalg.det(T[:3,:3]) - 1 # is a valid SO(3) 0.0 >>> T = T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T @ T >>> linalg.det(T[:3,:3]) - 1 # not quite a valid SE(3) anymore -3.3306690738754696e-16 >>> T = trnorm(T) >>> linalg.det(T[:3,:3]) - 1 # once more a valid SE(3) 0.0
Note
Only the direction of a-vector (the z-axis) is unchanged.
Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’, ie. determinant \(\ne 1\).
- trotx(theta, unit='rad', t=None)[source]
Create SE(3) pure rotation about X-axis
- Parameters:
theta (
float
) – rotation angle about X-axisunit (
str
) – angular units: ‘rad’ [default], or ‘deg’t (array_like(3)) – 3D translation vector, defaults to [0,0,0]
- Returns:
SE(3) transformation matrix
- Return type:
ndarray(4,4)
trotx(θ)
is a homogeneous transformation (4x4) representing a rotation of θ radians about the x-axis.trotx(θ, 'deg')
as above but θ is in degreestrotx(θ, 'rad', t=[x,y,z])
as above with translation of [x,y,z]
>>> from spatialmath.base import trotx >>> trotx(0.3) array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. ], [ 0. , 0.2955, 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]]) >>> trotx(45, 'deg', t=[1,2,3]) array([[ 1. , 0. , 0. , 1. ], [ 0. , 0.7071, -0.7071, 2. ], [ 0. , 0.7071, 0.7071, 3. ], [ 0. , 0. , 0. , 1. ]])
- Seealso:
- SymPy:
supported
- troty(theta, unit='rad', t=None)[source]
Create SE(3) pure rotation about Y-axis
- Parameters:
theta (
float
) – rotation angle about Y-axisunit (
str
) – angular units: ‘rad’ [default], or ‘deg’t (array_like(3)) – 3D translation vector, defaults to [0,0,0]
- Returns:
SE(3) transformation matrix
- Return type:
ndarray(4,4)
troty(θ)
is a homogeneous transformation (4x4) representing a rotation of θ radians about the y-axis.troty(θ, 'deg')
as above but θ is in degreestroty(θ, 'rad', t=[x,y,z])
as above with translation of [x,y,z]
>>> from spatialmath.base import troty >>> troty(0.3) array([[ 0.9553, 0. , 0.2955, 0. ], [ 0. , 1. , 0. , 0. ], [-0.2955, 0. , 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]]) >>> troty(45, 'deg', t=[1,2,3]) array([[ 0.7071, 0. , 0.7071, 1. ], [ 0. , 1. , 0. , 2. ], [-0.7071, 0. , 0.7071, 3. ], [ 0. , 0. , 0. , 1. ]])
- Seealso:
- SymPy:
supported
- trotz(theta, unit='rad', t=None)[source]
Create SE(3) pure rotation about Z-axis
- Parameters:
theta (
float
) – rotation angle about Z-axisunit (
str
) – angular units: ‘rad’ [default], or ‘deg’t (array_like(3)) – 3D translation vector, defaults to [0,0,0]
- Returns:
SE(3) transformation matrix
- Return type:
ndarray(4,4)
trotz(θ)
is a homogeneous transformation (4x4) representing a rotation of θ radians about the z-axis.trotz(θ, 'deg')
as above but θ is in degreestrotz(θ, 'rad', t=[x,y,z])
as above with translation of [x,y,z]
>>> from spatialmath.base import trotz >>> trotz(0.3) array([[ 0.9553, -0.2955, 0. , 0. ], [ 0.2955, 0.9553, 0. , 0. ], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]]) >>> trotz(45, 'deg', t=[1,2,3]) array([[ 0.7071, -0.7071, 0. , 1. ], [ 0.7071, 0.7071, 0. , 2. ], [ 0. , 0. , 1. , 3. ], [ 0. , 0. , 0. , 1. ]])
- Seealso:
- SymPy:
supported
- trplot(T, style='arrow', color='blue', frame='', axislabel=True, axissubscript=True, textcolor='', labels=('X', 'Y', 'Z'), length=1, originsize=20, origincolor='', projection='ortho', block=None, anaglyph=None, wtl=0.2, width=None, ax=None, dims=None, d2=1.15, flo=(-0.05, -0.05, -0.05), **kwargs)[source]
Plot a 3D coordinate frame
- Parameters:
T (ndarray(4,4) or ndarray(3,3) or an iterable returning same) – SE(3) or SO(3) matrix
style (str) – axis style: ‘arrow’ [default], ‘line’, ‘rgb’, ‘rviz’ (Rviz style)
color (str or list(3) or tuple(3) of str) – color of the lines defining the frame
textcolor (str) – color of text labels for the frame, default
color
frame (str) – label the frame, name is shown below the frame and as subscripts on the frame axis labels
axislabel (bool) – display labels on axes, default True
axissubscript (bool) – display subscripts on axis labels, default True
labels (3-tuple of strings) – labels for the axes, defaults to X, Y and Z
length (float or array_like(3)) – length of coordinate frame axes, default 1
originsize (int) – size of dot to draw at the origin, 0 for no dot (default 20)
origincolor (str) – color of dot to draw at the origin, default is
color
ax (Axes3D reference) – the axes to plot into, defaults to current axes
block (bool) – run the GUI main loop until all windows are closed, default True
dims (array_like(6) or array_like(2)) – 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.
anaglyph (bool, str or (str, float)) – 3D anaglyph display, if True use use red-cyan glasses. To set the color pass a string like
'gb'
for green-blue glasses. To set the disparity (default 0.1) provide second argument in a tuple, eg.('rc', 0.2)
. Bigger disparity exagerates the 3D “pop out” effect.wtl (float) – width-to-length ratio for arrows, default 0.2
projection (str) – 3D projection: ortho [default] or persp
width (float) – width of lines, default 1
flo (array_like(3)) – frame label offset, a vector for frame label text string relative to frame origin, default (-0.05, -0.05, -0.05)
d2 (float) – distance of frame axis label text from origin, default 1.15
- Returns:
axes containing the frame
- Return type:
Axes3DSubplot
- Raises:
ValueError – bad arguments
Adds a 3D coordinate frame represented by the SO(3) or SE(3) matrix to the current axes. If
T
is iterable then multiple frames will be drawn.The appearance of the coordinate frame depends on many parameters:
- coordinate axes depend on:
color
of axeswidth
of linelength
of linestyle
which is one of:'arrow'
[default], draw line with arrow head incolor
'line'
, draw line with no arrow head incolor
'rgb'
, frame axes are lines with no arrow head and red for X, green for Y, blue for Z; no origin dot'rviz'
, frame axes are thick lines with no arrow head and red for X, green for Y, blue for Z; no origin dot
coordinate axis labels depend on:
axislabel
if True [default] label the axis, default labels are X, Y, Zlabels
3-list of alternative axis labelstextcolor
which defaults tocolor
axissubscript
if True [default] add the frame labelframe
as a subscript for each axis label
coordinate frame label depends on:
frame the label placed inside {} near the origin of the frame
a dot at the origin
originsize
size of the dot, if zero no dotorigincolor
color of the dot, defaults tocolor
Examples:
trplot(T, frame='A') trplot(T, frame='A', color='green') trplot(T1, 'labels', 'UVW');
(
Source code
,png
,hires.png
,pdf
)Note
If
axes
is specified the plot is drawn there, otherwise: - it will draw in the current figure (as given bygca()
) - if no axes in the current figure, it will create a 3D axes - if no current figure, it will create one, and a 3D axesNote
width
can be set in thergb
orrviz
styles to override the defaults which are 1 and 8 respectively.Note
The
anaglyph
effect is induced by drawing two versions of the frame in different colors: one that corresponds to lens over the left eye and one to the lens over the right eye. The view for the right eye is from a view point shifted in the positive x-direction.Note
The origin is normally indicated with a marker of the same color as the frame. The default size is 20. This can be disabled by setting its size to zero by
originsize=0
. For'rgb'
style the default is 0 but it can be set explicitly, and the color is as per thecolor
option.- SymPy:
not supported
- Seealso:
tranimate()
plotvol3()
axes_logic()
- trprint(T, orient='rpy/zyx', label='', file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='utf-8'>, fmt='{:.3g}', degsym=True, unit='deg')[source]
Compact display of SO(3) or SE(3) matrices
trprint(R)
prints the SO(3) rotation matrix to stdout in a compactsingle-line format:
[LABEL:] ORIENTATION UNIT
trprint(T)
prints the SE(3) homogoneous transform to stdout in a compact single-line format:[LABEL:] [t=X, Y, Z;] ORIENTATION UNIT
trprint(X, file=None)
as above but returns the string rather than printing to a file
Orientation is expressed in one of several formats:
‘rpy/zyx’ roll-pitch-yaw angles in ZYX axis order [default]
‘rpy/yxz’ roll-pitch-yaw angles in YXZ axis order
‘rpy/zyx’ roll-pitch-yaw angles in ZYX axis order
‘eul’ Euler angles in ZYZ axis order
‘angvec’ angle and axis
>>> from spatialmath.base import transl, rpy2tr, trprint >>> T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg') >>> trprint(T, file=None) 't = 1, 2, 3; rpy/zyx = 10°, 20°, 30°' >>> trprint(T, file=None, label='T', orient='angvec') 'T: t = 1, 2, 3; angvec = (35.8° | 0.124, 0.616, 0.778)' >>> trprint(T, file=None, label='T', orient='angvec', fmt='{:8.4g}') 'T: t = 1, 2, 3; angvec = ( 35.82° | 0.124, 0.6156, 0.7782)'
Note
If the ‘rpy’ option is selected, then the particular angle sequence can be specified with the options ‘xyz’ or ‘yxz’ which are passed through to
tr2rpy
. ‘zyx’ is the default.Default formatting is for compact display of data
For tabular data set
fmt
to a fixed width format such asfmt='{:.3g}'
- seealso:
- SymPy:
not supported
- Return type:
str
- x2r(r, representation='rpy/xyz')[source]
Convert angular representation to SO(3) matrix
- Parameters:
r (array_like(3)) – angular representation
representation (str, optional) – rotational representation, defaults to “rpy/xyz”
- Returns:
SO(3) rotation matrix
- Return type:
ndarray(3,3)
Convert a minimal rotational representation \(\vec{\Gamma} \in \mathbb{R}^3\) to an SO(3) rotation matrix.
representation
Rotational representation
"rpy/xyz"
"arm"
RPY angular rates in XYZ order (default)
"rpy/zyx"
"vehicle"
RPY angular rates in XYZ order
"rpy/yxz"
"camera"
RPY angular rates in YXZ order
"eul"
Euler angular rates in ZYZ order
"exp"
exponential coordinate rates
- x2tr(x, representation='rpy/xyz')[source]
Convert analytic representation to SE(3)
- Parameters:
x (array_like(6)) – analytic vector representation
representation (str, optional) – angular representation to use, defaults to “rpy/xyz”
- Returns:
pose as an SE(3) matrix
- Return type:
ndarray(4,4)
Convert a vector representation of pose \(\vec{x} = (\vec{t},\vec{r}) \in \mathbb{R}^6\) to SE(3), where rotation \(\vec{r} \in \mathbb{R}^3\) is encoded in a minimal representation to an equivalent SE(3) matrix.
representation
Rotational representation
"rpy/xyz"
"arm"
RPY angular rates in XYZ order (default)
"rpy/zyx"
"vehicle"
RPY angular rates in XYZ order
"rpy/yxz"
"camera"
RPY angular rates in YXZ order
"eul"
Euler angular rates in ZYZ order
"exp"
exponential coordinate rates
- SymPy:
supported
- Seealso: