def _import(self, value, check=True): if isinstance(value, np.ndarray) and self.isvalid(value, check=check): if value.shape == (4, 4): # it's an se(3) return base.vexa(value) elif value.shape == (6, ): # it's a twist vector return value elif base.ishom(value, check=check): return base.trlog(value, twist=True, check=False) raise TypeError('bad type passed')
def prod(self): r""" Product of 3D twists :return: Product of elements :rtype: Twist3 For a twist instance with N values return the matrix product of those elements :math:`\prod_i^N S_i`. """ twprod = base.trexp(self.data[0]) for tw in self.data[1:]: twprod = twprod @ base.trexp(tw) return Twist3(base.trlog(twprod))
def log(self, twist=False): """ Logarithm of pose (superclass method) :return: logarithm :rtype: numpy.ndarray :raises: ValueError An efficient closed-form solution of the matrix logarithm. ===== ====== =============================== Input Output ----- --------------------------------------- Pose Shape Structure ===== ====== =============================== SO2 (2,2) skew-symmetric SE2 (3,3) augmented skew-symmetric SO3 (3,3) skew-symmetric SE3 (4,4) augmented skew-symmetric ===== ====== =============================== Example:: >>> x = SE3.Rx(0.3) >>> y = x.log() >>> y array([[ 0. , -0. , 0. , 0. ], [ 0. , 0. , -0.3, 0. ], [-0. , 0.3, 0. , 0. ], [ 0. , 0. , 0. , 0. ]]) :seealso: :func:`~spatialmath.base.transforms2d.trlog2`, :func:`~spatialmath.base.transforms3d.trlog` :SymPy: not supported """ if self.N == 2: log = [tr.trlog2(x, twist=twist) for x in self.data] else: log = [tr.trlog(x, twist=twist) for x in self.data] if len(log) == 1: return log[0] else: return log
def __mul__(left, right): # pylint: disable=no-self-argument """ Overloaded ``*`` operator :arg left: left multiplicand :arg right: right multiplicand :return: product :raises: ValueError Twist composition or scaling: - ``X * Y`` compounds the twists ``X`` and ``Y`` - ``X * s`` performs elementwise multiplication of the elements of ``X`` by ``s`` - ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s`` ======== ==================== =================== ======================== Multiplicands Product ------------------------------- ----------------------------------- left right type operation ======== ==================== =================== ======================== Twist Twist Twist product of exponentials Twist scalar Twist element-wise product scalar Twist Twist element-wise product Twist SE3 Twist exponential x SE3 Twist SpatialVelocity SpatialVelocity adjoint product Twist SpatialAcceleration SpatialAcceleration adjoint product Twist SpatialForce SpatialForce adjoint product ======== ==================== =================== ======================== Notes: #. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance #. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3`` #. scalar x Pose is handled by ``__rmul__`` #. scalar multiplication is commutative but the result is not a group operation so the result will be a matrix #. Any other input combinations result in a ValueError. For pose composition the ``left`` and ``right`` operands may be a sequence ========= ========== ==== ================================ len(left) len(right) len operation ========= ========== ==== ================================ 1 1 1 ``prod = left * right`` 1 M M ``prod[i] = left * right[i]`` N 1 M ``prod[i] = left[i] * right`` M M M ``prod[i] = left[i] * right[i]`` ========= ========== ==== ================================ """ # TODO TW * T compounds a twist with an SE2/3 transformation if isinstance(right, Twist3): # twist composition -> Twist return Twist3( left.binop( right, lambda x, y: base.trlog(base.trexp(x) @ base.trexp(y), twist=True))) elif isinstance(right, SE3): # twist * SE3 -> SE3 return SE3(left.binop(right, lambda x, y: base.trexp(x) @ y), check=False) elif base.isscalar(right): # return Twist(left.S * right) return Twist3(left.binop(right, lambda x, y: x * y)) elif isinstance(right, SpatialVelocity): return SpatialVelocity(left.Ad @ right.V) elif isinstance(right, SpatialAcceleration): return SpatialAcceleration(left.Ad @ right.V) elif isinstance(right, SpatialForce): return SpatialForce(left.Ad @ right.V) else: raise ValueError('twist *, incorrect right operand')
def prod(self): twprod = base.trexp(self.data[0]) for tw in self.data[1:]: twprod = twprod @ base.trexp(tw) return Twist3(base.trlog(twprod))
def _twist(x, y, z, r): T = base.transl(x, y, z) @ base.r2t(r.A) return base.trlog(T, twist=True)
def angdist(self, other, metric=6): r""" Angular distance metric between rotations :param other: second rotation :type other: SO3 instance :param metric: metric, default is 6 :type metric: int :raises TypeError: if other is not an SO3 :return: angle in radians :rtype: float or ndarray ``R1.angdist(R2)`` is the geodesic norm, or geodesic distance between two rotations. Several metrics are supported, the first 5 are computed after conversion to unit quaternions. ====== =============================================================== Metric Details ====== =============================================================== 0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]` 1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]` 2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]` 3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]` 4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]` 5 :math:`\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]` 6 :math:`\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]` ====== =============================================================== Example: .. runblock:: pycon >>> from spatialmath import UnitQuaternion >>> R1 = SO3.Rx(0.3) >>> R2 = SO3.Ry(0.3) >>> print(R1.angdist(R1)) >>> print(R1.angdist(R2)) .. note:: - metrics 1, 2, 4 can throw ValueError "math domain error" due to numeric errors which push the argument of ``acos()`` marginally outside its domain [0, 1]. - metrics 2 and 3 are equivalent, but 3 is more robust :seealso: :func:`UnitQuaternion.angdist` """ if metric < 5: from spatialmath.quaternion import UnitQuaternion return UnitQuaternion(self).angdist(UnitQuaternion(other), metric=metric) elif metric == 5: op = lambda R1, R2: np.linalg.norm(np.eye(3) - R1 @ R2.T) elif metric == 6: op = lambda R1, R2: base.norm(base.trlog(R1 @ R2.T, twist=True)) else: raise ValueError('unknown metric') ad = self._op2(other, op) if isinstance(ad, list): return np.array(ad) else: return ad
def __init__(self, arg=None, w=None, check=True): """ Construct a new Twist object TW = Twist(T) is a Twist object representing the SE(2) or SE(3) homogeneous transformation matrix T (3x3 or 4x4). TW = Twist(V) is a twist object where the vector is specified directly. 3D CASE: TW = Twist('R', A, Q) is a Twist object representing rotation about the axis of direction A (3x1) and passing through the point Q (3x1). % TW = Twist('R', A, Q, P) as above but with a pitch of P (distance/angle). TW = Twist('T', A) is a Twist object representing translation in the direction of A (3x1). Notes: - The argument 'P' for prismatic is synonymous with 'T'. """ super().__init__() # enable UserList superpowers if arg is None: self.data = [np.r_[0, 0, 0, 0, 0, 0]] elif isinstance(arg, Twist): # clone it self.data = [np.r_[arg.v, arg.w]] elif argcheck.isvector(arg, 6): s = argcheck.getvector(arg) self.data = [s] elif argcheck.isvector(arg, 3) and argcheck.isvector(w, 3): v = argcheck.getvector(arg) w = argcheck.getvector(w) self.data = [np.r_[v, w]] elif isinstance(arg, SE3): S = tr.trlog(arg.A) # use closed form for SE(3) skw, v = tr.tr2rt(S) w = tr.vex(skw) self.data = [np.r_[v, w]] elif Twist.isvalid(arg): # it's an augmented skew matrix, unpack it skw, v = tr.tr2rt(arg) w = tr.vex(skw) self.data = [np.r_[v, w]] elif isinstance(arg, list): # construct from a list if isinstance(arg[0], np.ndarray): # possibly a list of numpy arrays if check: assert all( map(lambda x: Twist.isvalid(x), arg) ), 'all elements of list must have valid shape and value for the class' self.data = arg elif type(arg[0]) == type(self): # possibly a list of objects of same type assert all( map(lambda x: type(x) == type(self), arg)), 'all elements of list must have same type' self.data = [x.S for x in arg] elif type(arg[0]) == list: # possibly a list of 6-lists assert all( map(lambda x: isinstance(x, list) and len(x) == 6, arg)), 'all elements of list must have same type' self.data = [np.r_[x] for x in arg] else: raise ValueError('bad list argument to constructor') else: raise ValueError('bad argument to constructor')