def inv(self):
r"""
Inverse of SE(3)
:param self: pose
:type self: SE3 instance
:return: inverse
:rtype: SE3
Returns the inverse taking into account its structure
:math:`T = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]`
"""
if len(self) == 1:
return SE3(tr.rt2tr(self.R.T, -self.R.T @ self.t))
else:
return SE3([SE3(tr.rt2tr(x.R.T, -x.R.T @ x.t)) for x in self])
def SE2(self):
"""
Create SE(2) from SO(2)
:return: SE(2) with same rotation but zero translation
:rtype: SE2 instance
"""
return SE2(tr.rt2tr(self.A, [0, 0]))
def inv(self):
r"""
Inverse of SE(2)
:param self: pose
:type self: SE2 instance
:return: inverse
:rtype: SE2
Notes:
- for elements of SE(2) this takes into account the matrix structure :math:`T^{-1} = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]`
- if `x` contains a sequence, returns an `SE2` with a sequence of inverses
"""
if len(self) == 1:
return SE2(tr.rt2tr(self.R.T, -self.R.T @ self.t))
else:
return SE2([tr.rt2tr(x.R.T, -x.R.T @ x.t) for x in self])
def SO3(cls, R, t=None, check=True):
if isinstance(R, SO3):
R = R.A
elif base.isrot(R, check=check):
pass
else:
raise ValueError('expecting SO3 or rotation matrix')
if t is None:
return cls(base.r2t(R))
else:
return cls(base.rt2tr(R, t))
def SE3(self):
"""
Convert unit dual quaternion to SE(3) matrix
:return: SE(3) matrix
:rtype: SE3
Example:
.. runblock:: pycon
>>> from spatialmath import DualQuaternion, SE3
>>> T = SE3.Rand()
>>> print(T)
>>> d = UnitDualQuaternion(T)
>>> print(d)
>>> print(d.T)
"""
R = base.q2r(self.real.A)
t = 2 * self.dual * self.real.conj()
return SE3(base.rt2tr(R, t.v))
def Rt(cls, R, t, check=True):
"""
Create an SE(3) from rotation and translation
:param R: rotation
:type R: SO3 or ndarray(3,3)
:param t: translation
:type t: array_like(3)
:param check: check rotation validity, defaults to True
:type check: bool, optional
:raises ValueError: bad rotation matrix
:return: SE(3) matrix
:rtype: SE3 instance
"""
if isinstance(R, SO3):
R = R.A
elif base.isrot(R, check=check):
pass
else:
raise ValueError('expecting SO3 or rotation matrix')
return cls(base.rt2tr(R, t))
def trinterp2(start, end, s=None):
"""
Interpolate SE(2) or SO(2) matrices
:param start: initial SE(2) or SO(2) matrix value when s=0, if None then identity is used
:type start: ndarray(3,3) or ndarray(2,2) or None
:param end: final SE(2) or SO(2) matrix, value when s=1
:type end: ndarray(3,3) or ndarray(2,2)
:param s: interpolation coefficient, range 0 to 1
:type s: float
:return: interpolated SE(2) or SO(2) matrix value
:rtype: ndarray(3,3) or ndarray(2,2)
:raises ValueError: bad arguments
- ``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.
.. note:: Rotation angle is linearly interpolated.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> T1 = transl2(1, 2)
>>> T2 = transl2(3, 4)
>>> trinterp2(T1, T2, 0)
>>> trinterp2(T1, T2, 1)
>>> trinterp2(T1, T2, 0.5)
>>> trinterp2(None, T2, 0)
>>> trinterp2(None, T2, 1)
>>> trinterp2(None, T2, 0.5)
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`
"""
if base.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)
if start.shape != end.shape:
raise ValueError("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 base.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)
if start.shape != end.shape:
raise ValueError("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 base.rt2tr(rot2(th), pr)
else:
return ValueError('Argument must be SO(2) or SE(2)')
def trexp2(S, theta=None, check=True):
"""
Exponential of so(2) or se(2) matrix
:param S: se(2), so(2) matrix or equivalent velctor
:type T: ndarray(3,3) or ndarray(2,2)
:param theta: motion
:type theta: float
:return: matrix exponential in SE(2) or SO(2)
:rtype: ndarray(3,3) or ndarray(2,2)
:raises ValueError: bad argument
An efficient closed-form solution of the matrix exponential for arguments
that are se(2) or so(2).
For se(2) the results is an SE(2) homogeneous transformation matrix:
- ``trexp2(Σ)`` is the matrix exponential of the se(2) element ``Σ`` which is
a 3x3 augmented skew-symmetric matrix.
- ``trexp2(Σ, θ)`` 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.
- ``trexp2(S)`` is the matrix exponential of the se(3) element ``S`` represented as
a 3-vector which can be considered a screw motion.
- ``trexp2(S, θ)`` as above but for an se(2) motion of Sθ, where ``S``
must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
matrix.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skew(1))
>>> trexp2(skew(1), 2) # revolute unit twist
>>> trexp2(1)
>>> trexp2(1, 2) # revolute unit twist
For so(2) the results is an SO(2) rotation matrix:
- ``trexp2(Ω)`` is the matrix exponential of the so(3) element ``Ω`` which is a 2x2
skew-symmetric matrix.
- ``trexp2(Ω, θ)`` 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 ``θ``.
- ``trexp2(ω)`` is the matrix exponential of the so(2) element ``ω`` expressed as
a 1-vector.
- ``trexp2(ω, θ)`` 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 1-vector.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> trexp2(skewa([1, 2, 3]))
>>> trexp2(skewa([1, 0, 0]), 2) # prismatic unit twist
>>> trexp2([1, 2, 3])
>>> trexp2([1, 0, 0], 2)
:seealso: trlog, trexp2
"""
if base.ismatrix(S, (3, 3)) or base.isvector(S, 3):
# se(2) case
if base.ismatrix(S, (3, 3)):
# augmentented skew matrix
if check and not base.isskewa(S):
raise ValueError("argument must be a valid se(2) element")
tw = base.vexa(S)
else:
# 3 vector
tw = base.getvector(S)
if base.iszerovec(tw):
return np.eye(3)
if theta is None:
(tw, theta) = base.unittwist2_norm(tw)
elif not base.isunittwist2(tw):
raise ValueError("If theta is specified S must be a unit twist")
t = tw[0:2]
w = tw[2]
R = base.rodrigues(w, theta)
skw = base.skew(w)
V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (
theta - math.sin(theta)) * skw @ skw
return base.rt2tr(R, V @ t)
elif base.ismatrix(S, (2, 2)) or base.isvector(S, 1):
# so(2) case
if base.ismatrix(S, (2, 2)):
# skew symmetric matrix
if check and not base.isskew(S):
raise ValueError("argument must be a valid so(2) element")
w = base.vex(S)
else:
# 1 vector
w = base.getvector(S)
if theta is not None and not base.isunitvec(w):
raise ValueError("If theta is specified S must be a unit twist")
# do Rodrigues' formula for rotation
return base.rodrigues(w, theta)
else:
raise ValueError(
" First argument must be SO(2), 1-vector, SE(2) or 3-vector")
def inv(self):
if len(self) == 1:
return SE2(tr.rt2tr(self.R.T, -self.R.T @ self.t))
else:
return SE2([tr.rt2tr(x.R.T, -x.R.T @ x.t) for x in self])
