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
