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(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 __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 SE2(self):
"""
%Twist3.SE Convert twist to SE2 or SE3 object
%
TW.SE is an SE2 or SE3 object representing the homogeneous transformation equivalent to the Twist3.
%
See also Twist3.T, SE2, SE3.
"""
return SE2(self.exp())
def __init__(self, occ_grid=None, goal=None, inflate=0,
reset=False, verbose=None, seed=None,
transform=SE2()):
self._occ_grid_nav = None
self._start = None
self._verbose = verbose
self._seed = seed
self._spin_count = None
self._rand_stream = None
self._seed_0 = None
self._w2g = None
self._inflate = inflate
self._reset = reset
self._transform = transform
self._goal = None
# if occ_grid is not None:
# # this is the inverse of the matlab code
# if type(occ_grid) is dict:
# self._occ_grid = occ_grid["map"]
# # self._w2g = self._T # TODO
# else:
# self._occ_grid = occ_grid
# # self._w2g = SE2(0, 0, 0)
self._occ_grid = occ_grid
if inflate > 0:
# Generate a circular structuring element
r = inflate
y, x = np.ogrid[-r: r+1, -r: r+1]
SE = np.square(x) + np.square(y) <= np.square(r)
SE = SE.astype(int)
# do the inflation using SciPy
self._occ_grid_nav = binary_dilation(self._occ_grid, SE)
else:
self._occ_grid_nav = self._occ_grid
if goal is not None:
self._goal = np.transpose(goal)
# Simplification of matlab code
self._privaterandom = np.random.default_rng(seed=seed)
rs = np.random.RandomState()
if seed is not None:
rs = np.random.RandomState(seed)
self._seed_0 = rs.get_state()
self._rand_stream = rs
self._w2g = transform
self._spin_count = 0
def SE2(self):
"""
Convert 2D twist to SE(2) matrix
:return: an SE(2) representation
:rtype: SE3 instance
``S.SE2()`` is an SE2 object representing the homogeneous transformation
equivalent to the Twist2. This is the exponentiation of the twist vector.
Example:
.. runblock:: pycon
>>> from spatialmath import Twist2
>>> S = Twist2.Prismatic([1,2])
>>> S.SE2()
:seealso: :func:`Twist3.exp`
"""
return SE2(self.exp())