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 _import(self, value, check=True):
if isinstance(value, np.ndarray) and self.isvalid(value, check=check):
if value.shape == (3, 3):
# it's an se(2)
return base.vexa(value)
elif value.shape == (3, ):
# it's a twist vector
return value
elif base.ishom2(value, check=check):
return base.trlog2(value, twist=True, check=False)
raise TypeError('bad type passed')
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 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 __init__(self, arg=None, w=None, check=True):
"""
Construct a new 2D Twist object
:type a: 2-element array-like
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as
a 2x2 matrix.
- ``Twist2(T)`` is a 2D Twist object representing the SE(2) rigid-body motion expressed as
a 3x3 matrix.
- ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation,
and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion
- ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the
moment vector (1 element) and direction vector (2 elements).
"""
super().__init__() # enable UserList superpowers
if arg is None:
self.data = [np.r_[0.0, 0.0, 0.0, ]]
elif isinstance(arg, Twist2):
# clone it
self.data = [np.r_[arg.v, arg.w]]
elif argcheck.isvector(arg, 3):
s = argcheck.getvector(arg)
self.data = [s]
elif argcheck.isvector(arg, 2) and argcheck.isvector(w, 1):
v = argcheck.getvector(arg)
w = argcheck.getvector(w)
self.data = [np.r_[v, w]]
elif isinstance(arg, SE2):
S = tr.trlog2(arg.A) # use closed form for SE(2)
skw, v = tr.tr2rt(S)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif Twist2.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: Twist2.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 3-lists
assert all(
map(lambda x: isinstance(x, list) and len(x) == 3,
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')