def __init__(self, m=None, c=None, I=None):
"""
Create a new spatial inertia
:param m: mass
:type m: float
:param c: centre of mass relative to link frame
:type c: 3-element array_like
:param I: inertia about the centre of mass, axes aligned with link frame
:type I: numpy.array, shape=(6,6)
- ``SpatialInertia(M, C, I)`` is a spatial inertia object for a rigid-body
with mass ``M``, centre of mass at ``C`` relative to the link frame, and an
inertia matrix ``I`` (3x3) about the centre of mass.
- ``SpatialInertia(I)`` is a spatial inertia object with a value equal
to ``I`` (6x6).
"""
if m is not None and c is not None:
assert arg.isvector(c, 3), 'c must be 3-vector'
if I is None:
I = np.zeros((3,3))
else:
assert arg.ismatrix(I, (3,3)), 'I must be 3x3 matrix'
C = tr.skew(c)
self.I = np.array([
[m * np.eye(3), m @ C.T],
[m @ C, I + m * C @ C.T]
])
elif m is None and c is None and I is not None:
assert arg.ismatrix(I, (6, 6)), 'I must be 6x6 matrix'
def __init__(self, value):
"""
Create a new spatial vector (abstract superclass)
:param value: Value of the
- ``SpatialVector(vec)`` is a spatial vector constructed from the 6-element array-like ``vec``
- ``SpatialVector([V1, V2, ... VN])`` is a spatial vector array with N elements, constructed from the 6-element
array-like values ``Vi``
- ``SpatialVector(A)`` is a spatial vector array with N elements, constructed from the columns of the 6xN
array ``A``.
:seealso: :func:`SpatialVelocity`, :func:`SpatialAcceleration`, :func:`SpatialForce`, :func:`SpatialMomentum`.
"""
print('spatialVec6 init')
super().__init__()
if value is None:
self.data = [np.zeros((6,))]
elif arg.isvector(value, 6):
self.data = [np.array(value)]
elif isinstance(value, list):
assert all(map(lambda x: arg.isvector(x, 6), value)), 'all elements of list must have valid shape and value for the class'
self.data = [np.array(x) for x in value]
elif arg.ismatrix(value, (6, None)):
self.data = [x for x in value.T]
else:
raise ValueError('bad arguments to constructor')
def Exp(cls, S, so3=False):
"""
Create an SO(3) rotation matrix from so(3)
:param S: Lie algebra so(3)
:type S: numpy ndarray
:param so3: accept input as an so(3) matrix [default False]
:type so3: bool
:return: 3x3 rotation matrix
:rtype: SO3 instance
- ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra
which is a 3x3 so(3) matrix (skew symmetric)
- ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist
vector (the unique elements of the so(3) skew-symmetric matrix)
- ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix
of twist vectors, one per row.
Note:
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this
case the parameter `so3` is the decider.
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if argcheck.ismatrix(S, (-1, 3)) and not so3:
return cls([tr.trexp(s) for s in S])
else:
return cls(tr.trexp(S), check=False)
def I(self, I_new): # noqa
if ismatrix(I_new, (3, 3)):
# 3x3 matrix passed
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError('3x3 matrix is not symmetric')
elif isvector(I_new, 9):
# 3x3 matrix passed as a 1d vector
I_new = I_new.reshape(3, 3)
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError('3x3 matrix is not symmetric')
elif isvector(I_new, 6):
# 6-vector passed, moments and products of inertia,
# [Ixx Iyy Izz Ixy Iyz Ixz]
I_new = np.array([
[I_new[0], I_new[3], I_new[5]],
[I_new[3], I_new[1], I_new[4]],
[I_new[5], I_new[4], I_new[2]]
])
elif isvector(I_new, 3):
# 3-vector passed, moments of inertia [Ixx Iyy Izz]
I_new = np.diag(I_new)
else:
raise ValueError('invalid shape passed: must be (3,3), (6,), (3,)')
self._I = I_new
def Exp(cls, S, check=True, so3=True):
r"""
Create an SO(3) rotation matrix from so(3)
:param S: Lie algebra so(3)
:type S: numpy ndarray
:param check: check that passed matrix is valid so(3), default True
:type check: bool
:param so3: input is an so(3) matrix (default True)
:type so3: bool
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra
which is a 3x3 so(3) matrix (skew symmetric)
- ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist
vector (the unique elements of the so(3) skew-symmetric matrix)
- ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix
of twist vectors, one per row.
Note:
- if :math:`\theta \eq 0` the result in an identity matrix
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this
case the parameter `so3` is the decider.
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if argcheck.ismatrix(S, (-1, 3)) and not so3:
return cls([tr.trexp(s, check=check) for s in S], check=False)
else:
return cls(tr.trexp(S, check=check), check=False)
def contains(self, x, tol=50 * _eps):
"""
Test if points are on the line
:param x: 3D point
:type x: 3-element array_like, or numpy.ndarray, shape=(3,N)
:param tol: Tolerance, defaults to 50*_eps
:type tol: float, optional
:raises ValueError: Bad argument
:return: Whether point is on the line
:rtype: bool or numpy.ndarray(N) of bool
``line.contains(X)`` is true if the point ``X`` lies on the line defined by
the Plucker object self.
If ``X`` is an array with 3 rows, the test is performed on every column and
an array of booleans is returned.
"""
if arg.isvector(x, 3):
x = arg.getvector(x)
return np.linalg.norm(np.cross(x - self.pp, self.w)) < tol
elif arg.ismatrix(x, (3, None)):
return [
np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol
for _ in x.T
]
else:
raise ValueError('bad argument')
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
def isvalid(v, check=True):
if argcheck.isvector(v, 3):
return True
elif argcheck.ismatrix(v, (3, 3)):
# maybe be an se(2)
if not all(v.diagonal() == 0): # check diagonal is zero
return False
if not all(v[2, :] == 0): # check bottom row is zero
return False
if not tr.isskew(v[:2, :2]):
# top left 2x2is skew symmetric
return False
return True
return False
def contains(self, x, tol=50 * _eps):
"""
Plucker.contains Test if point is on the line
``line.contains(X)`` is true if the point X (3x1) lies on the line defined by
the Plucker object self.
"""
if arg.isvector(x, 3):
return np.linalg.norm(np.cross(x - self.pp, self.w)) < tol
elif arg.ismatrix(x, (3, None)):
return [
np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol
for _ in x.T
]
else:
raise ValueError('bad argument')
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)
def P3(cls, p):
"""
Create a plane object from three points
:param p: Three points in the plane
:type p: numpy.ndarray, shape=(3,3)
:return: a Plane object
:rtype: Plane
"""
p = arg.ismatrix((3, 3))
v1 = p[:, 0]
v2 = p[:, 1]
v3 = p[:, 2]
# compute a normal
n = np.cross(v2 - v1, v3 - v1)
return cls(n, v1)
def __rmul__(right, left):
"""
Plucker.mtimes Plucker multiplication
M * PL is the product of M (Nx4) and the Plucker skew matrix (4x4).
Notes::
- The * operator is overloaded for convenience.
- Multiplication or composition of Plucker lines is not defined.
- Premultiplying by an SE3 will transform the line with respect to the world
coordinate frame.
See also Plucker.skew, SE3.mtimes.
"""
if arg.ismatrix(left, (None, 4)):
return left @ right.skew # premultiply by Nx4
elif isinstance(left, SE3):
return left.A @ right.skew # premultiply by 4x4
def Exp(cls, S, check=True):
"""
Construct new SO(2) rotation matrix from so(2) Lie algebra
:param S: element of Lie algebra so(2)
:type S: numpy ndarray
:param check: check that passed matrix is valid so(2), default True
:type check: bool
:return: SO(2) rotation matrix
:rtype: SO2 instance
- ``SO2.Exp(S)`` is an SO(2) rotation defined by its Lie algebra
which is a 2x2 so(2) matrix (skew symmetric)
:seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if argcheck.ismatrix(S, (-1, 2)) and not so2:
return cls([tr.trexp2(s, check=check) for s in S])
else:
return cls(tr.trexp2(S, check=check), check=False)
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')
def __mul__(left, right):
"""
Plucker.mtimes Plucker multiplication
PL1 * PL2 is the scalar reciprocal product.
PL * M is the product of the Plucker skew matrix and M (4xN).
Notes::
- The * operator is overloaded for convenience.
- Multiplication or composition of Plucker lines is not defined.
- Premultiplying by an SE3 will transform the line with respect to the world
coordinate frame.
See also Plucker.skew, SE3.mtimes.
"""
if isinstance(left, Plucker) and isinstance(right, Plucker):
# reciprocal product
return np.dot(left.uw, right.v) + np.dot(right.uw, left.v)
elif isinstance(left, Plucker) and arg.ismatrix(right, (4, None)):
return left.skew @ right
def isvalid(v, check=True):
"""
Test if matrix is valid twist
:param x: array to test
:type x: numpy.ndarray
:return: true of the matrix is a 6-vector or a 4x4 se(3) element
:rtype: bool
"""
if argcheck.isvector(v, 6):
return True
elif argcheck.ismatrix(v, (4, 4)):
# maybe be an se(3)
if not all(v.diagonal() == 0): # check diagonal is zero
return False
if not all(v[3, :] == 0): # check bottom row is zero
return False
if not tr.isskew(v[:3, :3]):
# top left 3x3 is skew symmetric
return False
return True
return False
def transl2(x, y=None):
"""
Create SE(2) pure translation, or extract translation from SE(2) matrix
:param x: translation along X-axis
:type x: float
:param y: translation along Y-axis
:type y: float
:return: homogeneous transform matrix or the translation elements of a homogeneous transform
:rtype: numpy.ndarray, shape=(3,3)
Create a translational SE(2) matrix:
- ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3) representing a
pure translation.
- ``T = transl2( V )`` as above but the translation is given by a 2-element
list, dict, or a numpy array, row or column vector.
Extract the translational part of an SE(2) matrix:
P = TRANSL2(T) is the translational part of a homogeneous transform as a
2-element numpy array.
"""
if np.isscalar(x):
T = np.identity(3)
T[:2, 2] = [x, y]
return T
elif argcheck.isvector(x, 2):
T = np.identity(3)
T[:2, 2] = argcheck.getvector(x, 2)
return T
elif argcheck.ismatrix(x, (3, 3)):
return x[:2, 2]
else:
ValueError('bad argument')
def trinterp2(start, end, s=None):
"""
Interpolate SE(2) matrices
:param start: initial SO(2) or SE(2) matrix value when s=0, if None then identity is used
:type T1: np.ndarray, shape=(2,2), (3,3) or None
:param end: final SO(2) or SE(2) matrix, value when s=1
:type T0: np.ndarray, shape=(2,2), (3,3)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: SO(2) or SE(2) matrix
:rtype: np.ndarray, shape=(2,2), (3,3)
- ``trinterp2(None, T, S)`` is a homogeneous transform (3x3) interpolated
between identity when S=0 and T (3x3) when S=1.
- ``trinterp2(T0, T1, S)`` as above but interpolated
between T0 (3x3) when S=0 and T1 (3x3) when S=1.
- ``trinterp2(None, R, S)`` is a rotation matrix (2x2) interpolated
between identity when S=0 and R (2x2) when S=1.
- ``trinterp2(R0, R1, S)`` as above but interpolated
between R0 (2x2) when S=0 and R1 (2x2) when S=1.
Notes:
- Rotation angle is linearly interpolated.
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
%## 2d homogeneous trajectory
"""
if argcheck.ismatrix(end, (2, 2)):
# SO(2) case
if start is None:
# TRINTERP2(T, s)
th0 = math.atan2(end[1, 0], end[0, 0])
th = s * th0
else:
# TRINTERP2(T1, start= s)
assert start.shape == end.shape, 'start and end matrices must be same shape'
th0 = math.atan2(start[1, 0], start[0, 0])
th1 = math.atan2(end[1, 0], end[0, 0])
th = th0 * (1 - s) + s * th1
return rot2(th)
elif argcheck.ismatrix(end, (3, 3)):
if start is None:
# TRINTERP2(T, s)
th0 = math.atan2(end[1, 0], end[0, 0])
p0 = transl2(end)
th = s * th0
pr = s * p0
else:
# TRINTERP2(T0, T1, s)
assert start.shape == end.shape, 'both matrices must be same shape'
th0 = math.atan2(start[1, 0], start[0, 0])
th1 = math.atan2(end[1, 0], end[0, 0])
p0 = transl2(start)
p1 = transl2(end)
pr = p0 * (1 - s) + s * p1
th = th0 * (1 - s) + s * th1
return trn.rt2tr(rot2(th), pr)
else:
return ValueError('Argument must be SO(2) or SE(2)')
def trinterp2(end, start=None, s=None):
"""
Interpolate SE(2) matrices
:param end: final SE(3) matrix, value when s=1
:type T0: np.ndarray, shape=(3,3)
:param start: initial SE(3) matrix, value when s=0, optional, defaults to identity
:type T1: np.ndarray, shape=(3,3)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: SE(2) matrix
:rtype: np.ndarray, shape=(3,3)
- ``trinterp2(T1, s=S)`` is a homogeneous transform (3x3) interpolated
between identity when S=0 and T1 when S=1.
- ``trinterp2(T1, start=T0, s=S)`` as above but interpolated
between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous
transforms (3x3).
Notes:
- Rotation angle is linearly interpolated.
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
%## 2d homogeneous trajectory
"""
if argcheck.ismatrix(start, (2,2)):
# SO(2) case
if T1 is None:
# TRINTERP2(T, s)
th0 = math.atan2(start[1,0], start[0,0])
th = s * th0
else:
# TRINTERP2(T0, T1, s)
assert start.shape == end.shape, 'both matrices must be same shape'
th0 = math.atan2(start[1,0], start[0,0])
th1 = math.atan2(end[1,0], end[0,0])
th = th0 * (1 - s) + s * th1
return rot2(th)
elif argcheck.ismatrix(start, (3,3)):
if end is None:
# TRINTERP2(T, s)
th0 = math.atan2(start[1,0], start[0,0])
p0 = transl2(start)
th = s * th0
pr = s * p0
else:
# TRINTERP2(T0, T1, s)
assert start.shape == end.shape, 'both matrices must be same shape'
th0 = math.atan2(start[1,0], start[0,0])
th1 = math.atan2(end[1,0], end[0,0])
p0 = transl2(start)
p1 = transl2(end)
pr = p0 * (1 - s) + s * p1;
th = th0 * (1 - s) + s * th1
return trn.rt2tr(rot2(th), pr)
else:
return ValueError('Argument must be SO(2) or SE(2)')
def trexp2(S, theta=None):
"""
Exponential of so(2) or se(2) matrix
:param S: so(2), se(2) matrix or equivalent velctor
:type T: numpy.ndarray, shape=(2,2) or (3,3); array_like
:param theta: motion
:type theta: float
:return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2)
:rtype: numpy.ndarray, shape=(2,2) or (3,3)
An efficient closed-form solution of the matrix exponential for arguments
that are so(2) or se(2).
For so(2) the results is an SO(2) rotation matrix:
- ``trexp2(S)`` is the matrix exponential of the so(3) element ``S`` which is a 2x2
skew-symmetric matrix.
- ``trexp2(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``.
- ``trexp2(W)`` is the matrix exponential of the so(2) element ``W`` expressed as
a 1-vector (array_like).
- ``trexp2(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 1-vector (array_like).
For se(2) the results is an SE(2) homogeneous transformation matrix:
- ``trexp2(SIGMA)`` is the matrix exponential of the se(2) element ``SIGMA`` which is
a 3x3 augmented skew-symmetric matrix.
- ``trexp2(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.
- ``trexp2(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as
a 3-vector which can be considered a screw motion.
- ``trexp2(TW, THETA)`` as above but for an se(2) motion of TW*THETA, where ``TW``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
:seealso: trlog, trexp2
"""
if argcheck.ismatrix(S, (3, 3)) or argcheck.isvector(S, 3):
# se(2) case
if argcheck.ismatrix(S, (3, 3)):
# augmentented skew matrix
tw = trn.vexa(S)
else:
# 3 vector
tw = argcheck.getvector(S)
if theta is None:
(tw, theta) = vec.unittwist2(tw)
else:
assert vec.isunittwist2(
tw), 'If theta is specified S must be a unit twist'
t = tw[0:2]
w = tw[2]
R = trn._rodrigues(w, theta)
skw = trn.skew(w)
V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (
theta - math.sin(theta)) * skw @ skw
return trn.rt2tr(R, V @ t)
elif argcheck.ismatrix(S, (2, 2)) or argcheck.isvector(S, 1):
# so(2) case
if argcheck.ismatrix(S, (2, 2)):
# skew symmetric matrix
w = trn.vex(S)
else:
# 1 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(2), 1-vector, SE(2) or 3-vector")
def trinterp2(T0, T1=None, s=None):
"""
Interpolate SE(2) matrices
:param T0: first SE(2) matrix
:type T0: np.ndarray, shape=(3,3)
:param T1: second SE(2) matrix
:type T1: np.ndarray, shape=(3,3)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: SE(2) matrix
:rtype: np.ndarray, shape=(3,3)
- ``trinterp2(T0, T1, S)`` is a homogeneous transform (3x3) interpolated
between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous
transforms (3x3).
- ``trinterp2(T1, S)`` as above but interpolated between the identity matrix
when S=0 to T1 when S=1.
Notes:
- Rotation angle is linearly interpolated.
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
%## 2d homogeneous trajectory
"""
if argcheck.ismatrix(T0, (2, 2)):
# SO(2) case
if T1 is None:
# TRINTERP2(T, s)
th0 = math.atan2(T0[1, 0], T0[0, 0])
th = s * th0
else:
# TRINTERP2(T0, T1, s)
assert T0.shape == T1.shape, 'both matrices must be same shape'
th0 = math.atan2(T0[1, 0], T0[0, 0])
th1 = math.atan2(T1[1, 0], T1[0, 0])
th = th0 * (1 - s) + s * th1
return rot2(th)
elif argcheck.ismatrix(T0, (3, 3)):
if T1 is None:
# TRINTERP2(T, s)
th0 = math.atan2(T0[1, 0], T0[0, 0])
p0 = transl2(T0)
th = s * th0
pr = s * p0
else:
# TRINTERP2(T0, T1, s)
assert T0.shape == T1.shape, 'both matrices must be same shape'
th0 = math.atan2(T0[1, 0], T0[0, 0])
th1 = math.atan2(T1[1, 0], T1[0, 0])
p0 = transl2(T0)
p1 = transl2(T1)
pr = p0 * (1 - s) + s * p1
th = th0 * (1 - s) + s * th1
return trn.rt2tr(rot2(th), pr)
else:
return ValueError('Argument must be SO(2) or SE(2)')
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
