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])