def Exp(cls, S, check=True, so3=True):
r"""
Create an SO(3) rotation matrix from so(3)
:param S: Lie algebra so(3)
:type S: numpy ndarray
:param check: check that passed matrix is valid so(3), default True
:type check: bool
:return: SO(3) rotation
:rtype: SO3 instance
- ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra
which is a 3x3 so(3) matrix (skew symmetric)
- ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist
vector (the unique elements of the so(3) skew-symmetric matrix)
- ``SO3.Exp(T)`` is a sequence of SO(3) rotations defined by an Nx3 matrix
of twist vectors, one per row.
Note:
- if :math:`\theta \eq 0` the result in an identity matrix
- an input 3x3 matrix is ambiguous, it could be the first or third case above. In this
case the parameter `so3` is the decider.
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if base.ismatrix(S, (-1, 3)) and not so3:
return cls([base.trexp(s, check=check) for s in S], check=False)
else:
return cls(base.trexp(S, check=check), check=False)
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(cls, S, so3=False):
"""
Create an SO(3) rotation matrix from so(3)
:param S: Lie algebra so(3)
:type S: numpy ndarray
:param so3: accept input as an so(3) matrix [default False]
:type so3: bool
:return: 3x3 rotation matrix
:rtype: SO3 instance
- ``SO3.Exp(S)`` is an SO(3) rotation defined by its Lie algebra
which is a 3x3 so(3) matrix (skew symmetric)
- ``SO3.Exp(t)`` is an SO(3) rotation defined by a 3-element twist
vector (the unique elements of the so(3) skew-symmetric matrix)
- ``SO3.Exp(T)`` is a sequence of SO(3) rotations 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 parameter `so3` is the decider.
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if argcheck.ismatrix(S, (-1, 3)) and not so3:
return cls([tr.trexp(s) for s in S])
else:
return cls(tr.trexp(S), check=False)
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 prod(self):
r"""
Product of 3D twists
:return: Product of elements
:rtype: Twist3
For a twist instance with N values return the matrix product of those
elements :math:`\prod_i^N S_i`.
"""
twprod = base.trexp(self.data[0])
for tw in self.data[1:]:
twprod = twprod @ base.trexp(tw)
return Twist3(base.trlog(twprod))
def Exp(cls, S, check=True):
"""
Create an SE(3) matrix from se(3)
:param S: Lie algebra se(3) matrix
:type S: numpy ndarray
:return: SE(3) matrix
:rtype: SE3 instance
- ``SE3.Exp(S)`` is an SE(3) rotation defined by its Lie algebra
which is a 4x4 se(3) matrix (skew symmetric)
- ``SE3.Exp(t)`` is an SE(3) rotation defined by a 6-element twist
vector (the unique elements of the se(3) skew-symmetric matrix)
:seealso: :func:`~spatialmath.base.transforms3d.trexp`, :func:`~spatialmath.base.transformsNd.skew`
"""
if base.isvector(S, 6):
return cls(base.trexp(base.getvector(S)), check=False)
else:
return cls([base.trexp(s) for s in S], check=False)
def Exp(cls, S):
"""
Create an SE(3) rotation matrix from se(3)
:param S: Lie algebra se(3)
:type S: numpy ndarray
:return: 3x3 rotation matrix
:rtype: SO3 instance
- ``SE3.Exp(S)`` is an SE(3) rotation defined by its Lie algebra
which is a 3x3 se(3) matrix (skew symmetric)
- ``SE3.Exp(t)`` is an SE(3) rotation defined by a 6-element twist
vector (the unique elements of the se(3) skew-symmetric matrix)
:seealso: :func:`spatialmath.base.transforms3d.trexp`, :func:`spatialmath.base.transformsNd.skew`
"""
if isinstance(S, np.ndarray) and S.shape[1] == 6:
return cls([tr.trexp(s) for s in S], check=False)
else:
return cls(tr.trexp(S), check=False)
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 __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')
def prod(self):
twprod = base.trexp(self.data[0])
for tw in self.data[1:]:
twprod = twprod @ base.trexp(tw)
return Twist3(base.trlog(twprod))
