def exp(self, theta=None, units='rad'):
"""
Twist3.exp Convert twist to homogeneous transformation
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 SE2(base.trexp2(self.S * theta))
else:
return SE2([base.trexp2(self.S * t) for t in theta])
def Exp(cls, S, check=True): # pylint: disable=arguments-differ
"""
Construct a new SE(2) from se(2) Lie algebra
:param S: element of Lie algebra se(2)
:type S: numpy ndarray
:param check: check that passed matrix is valid se(2), default True
:type check: bool
:return: homogeneous transform matrix
:rtype: SE2 instance
- ``SE2.Exp(S)`` is an SE(2) rotation defined by its Lie algebra
which is a 3x3 se(2) matrix (skew symmetric)
- ``SE2.Exp(t)`` is an SE(2) rotation defined by a 3-element twist
vector array_like (the unique elements of the se(2) skew-symmetric matrix)
- ``SE2.Exp(T)`` is a sequence of SE(2) rigid-body motions defined by an Nx3 matrix of twist vectors, one per row.
Note:
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this case the argument ``se2`` is the decider.
:seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if isinstance(S, (list, tuple)):
return cls([tr.trexp2(s) for s in S])
else:
return cls(tr.trexp2(S), check=False)
def exp(self, theta=None, units='rad'):
"""
Twist.exp Convert twist to homogeneous transformation
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 twist.
Notes::
- For the second form the twist must, if rotational, have a unit rotational component.
See also Twist.T, trexp, trexp2.
"""
if units != 'rad' and self.isprismatic:
print('Twist.exp: using degree mode for a prismatic twist')
if theta is None:
theta = 1
else:
theta = argcheck.getunit(theta, units)
if isinstance(theta, (int, np.int64, float, np.float64)):
return SE2(tr.trexp2(self.S * theta))
else:
return SE2([tr.trexp2(self.S * t) for t in theta])
def __mul__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``*`` operator
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
- ``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
======== ==================== =================== ========================
Twist2 Twist2 Twist2 product of exponentials
Twist2 scalar Twist2 element-wise product
scalar Twist2 Twist2 element-wise product
Twist2 SE2 Twist2 exponential x SE2
======== ==================== =================== ========================
.. note::
#. scalar x Twist 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]``
========= ========== ==== ================================
"""
if isinstance(right, Twist2):
# twist composition -> Twist
return Twist2(
left.binop(
right,
lambda x, y: base.trlog2(base.trexp2(x) @ base.trexp2(y),
twist=True)))
elif isinstance(right, SE2):
# twist * SE2 -> SE2
return SE2(left.binop(right, lambda x, y: base.trexp2(x) @ y),
check=False)
elif base.isscalar(right):
# return Twist(left.S * right)
return Twist2(left.binop(right, lambda x, y: x * y))
else:
raise ValueError('Twist2 *, incorrect right operand')
def prod(self):
"""
%Twist3.prod Compound array of twists
%
TW.prod is a twist representing the product (composition) of the
successive elements of TW (1xN), an array of Twists.
%
%
See also RTBPose.prod, Twist3.mtimes.
"""
twprod = base.trexp2(self.data[0])
for tw in self.data[1:]:
twprod = twprod @ base.trexp2(tw)
return Twist2(base.trlog2(twprod, twist=True))
def exp(self, theta=None, units='rad'):
r"""
Exponentiate a 2D 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(2) matrix
:rtype: SE2 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 SE2, Twist2
>>> T = SE2(1, 2, 0.3)
>>> S = Twist2(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.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 SE2(base.trexp2(self.S * theta))
else:
return SE2([base.trexp2(self.S * t) for t in theta])
def Exp(cls, S, check=True):
"""
Construct new SO(2) rotation matrix from so(2) Lie algebra
:param S: element of Lie algebra so(2)
:type S: numpy ndarray
:param check: check that passed matrix is valid so(2), default True
:type check: bool
:return: SO(2) rotation matrix
:rtype: SO2 instance
- ``SO2.Exp(S)`` is an SO(2) rotation defined by its Lie algebra
which is a 2x2 so(2) matrix (skew symmetric)
:seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if isinstance(S, (list, tuple)):
return cls([tr.trexp2(s, check=check) for s in S])
else:
return cls(tr.trexp2(S, check=check), check=False)
