def exp(self, theta=None, units='rad'): """ Exponentiate a twist :param theta: DESCRIPTION, defaults to None :type theta: TYPE, optional :param units: DESCRIPTION, defaults to 'rad' :type units: TYPE, optional :return: DESCRIPTION :rtype: TYPE TW.exp is the homogeneous transformation equivalent to the twist (SE2 or SE3). TW.exp(THETA) as above but with a rotation of THETA about the Twist3. Notes:: - For the second form the twist must, if rotational, have a unit rotational component. See also Twist3.T, trexp, trexp2. """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): return SE3(base.trexp(self.S * theta)) else: return SE3([base.trexp(self.S * t) for t in theta])
def exp(self, theta=None, units='rad'): """ Exponentiate a 3D twist :param theta: rotation magnitude, defaults to None :type theta: float, optional :param units: rotational units, defaults to 'rad' :type units: str, optional :return: SE(3) matrix :rtype: SE3 instance - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` Example: .. runblock:: pycon >>> from spatialmath import SE3, Twist3 >>> T = SE3(1, 2, 3) * SE3.Rx(0.3) >>> S = Twist3(T) >>> S.exp(0) >>> S.exp(1) .. notes:: - For the second form, the twist must, if rotational, have a unit rotational component. :seealso: :func:`spatialmath.base.trexp` """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): # theta is a scalar return SE3(base.trexp(self.S * theta)) else: # theta is a vector if len(self) == 1: return SE3([base.trexp(self.S * t) for t in theta]) elif len(self) == len(theta): return SE3( [base.trexp(S * t) for S, t in zip(self.data, theta)]) else: raise ValueError('length of twist and theta not consistent')
def exp(self, theta=None, units='rad'): """ Exponentiate a 3D twist :param theta: DESCRIPTION, defaults to None :type theta: TYPE, optional :param units: DESCRIPTION, defaults to 'rad' :type units: TYPE, optional :return: homogeneous transformation :rtype: SE3 -``X.exp()`` is the homogeneous transformation equivalent to the twist. -``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis. .. notes:: - For the second form, the twist must, if rotational, have a unit rotational component. See also Twist3.T, trexp, trexp2. """ if units != 'rad' and self.isprismatic: print('Twist3.exp: using degree mode for a prismatic twist') if theta is None: theta = 1 else: theta = base.getunit(theta, units) if base.isscalar(theta): # theta is a scalar return SE3(base.trexp(self.S * theta)) else: # theta is a vector if len(self) == 1: return SE3([base.trexp(self.S * t) for t in theta]) elif len(self) == len(theta): return SE3( [base.trexp(S * t) for S, t in zip(self.data, theta)]) else: raise ValueError('length of twist and theta not consistent')
def SE3(self): """ Convert twist to SE(3) :return: an SE(3) representation :rtype: SE3 instance - ``X.SE3()`` is an SE3 object representing the homogeneous transformation equivalent to the Twist3. """ return SE3(self.exp())
def SE3(self): """ Convert 3D twist to SE(3) matrix :return: an SE(3) representation :rtype: SE3 instance ``X.SE3()`` is an SE3 object representing the homogeneous transformation equivalent to the Twist3. This is the exponentiation of the twist. """ return SE3(self.exp())
def SE3(self): """ Convert 3D twist to SE(3) matrix :return: an SE(3) representation :rtype: SE3 instance ``S.SE3()`` is an SE3 object representing the homogeneous transformation equivalent to the Twist3. This is the exponentiation of the twist vector. Example: .. runblock:: pycon >>> from spatialmath import Twist3 >>> S = Twist3.Rx(0.3) >>> S.SE3() :seealso: :func:`Twist3.exp` """ return SE3(self.exp())
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')
return "Twist2([\n" + \ ',\n'.join([" [{:.5g}, {:.5g}, {:.5g}}]".format(*list(tw.S)) for tw in self]) +\ "\n])" def _repr_pretty_(self, p, cycle): """ Pretty string for IPython :param p: pretty printer handle (ignored) :param cycle: pretty printer flag (ignored) Print colorized output when variable is displayed in IPython, ie. on a line by itself. """ if len(self) == 1: p.text(str(self)) else: for i, x in enumerate(self): if i > 0: p.break_() p.text(f"{i:3d}: {str(x)}") if __name__ == '__main__': # pragma: no cover tw = Twist3(SE3.Rx(0)) # import pathlib # exec(open(pathlib.Path(__file__).parent.parent.absolute() / "tests" / "test_twist.py").read()) # pylint: disable=exec-used