def _import(self, value, check=True):
if isinstance(value, np.ndarray) and self.isvalid(value, check=check):
if value.shape == (4, 4):
# it's an se(3)
return base.vexa(value)
elif value.shape == (6, ):
# it's a twist vector
return value
elif base.ishom(value, check=check):
return base.trlog(value, twist=True, check=False)
raise TypeError('bad type passed')
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 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 __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))
def _twist(x, y, z, r):
T = base.transl(x, y, z) @ base.r2t(r.A)
return base.trlog(T, twist=True)
def angdist(self, other, metric=6):
r"""
Angular distance metric between rotations
:param other: second rotation
:type other: SO3 instance
:param metric: metric, default is 6
:type metric: int
:raises TypeError: if other is not an SO3
:return: angle in radians
:rtype: float or ndarray
``R1.angdist(R2)`` is the geodesic norm, or geodesic distance between two
rotations.
Several metrics are supported, the first 5 are computed after conversion
to unit quaternions.
====== ===============================================================
Metric Details
====== ===============================================================
0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
5 :math:`\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]`
6 :math:`\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]`
====== ===============================================================
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion
>>> R1 = SO3.Rx(0.3)
>>> R2 = SO3.Ry(0.3)
>>> print(R1.angdist(R1))
>>> print(R1.angdist(R2))
.. note::
- metrics 1, 2, 4 can throw ValueError "math domain error" due to
numeric errors which push the argument of ``acos()`` marginally
outside its domain [0, 1].
- metrics 2 and 3 are equivalent, but 3 is more robust
:seealso: :func:`UnitQuaternion.angdist`
"""
if metric < 5:
from spatialmath.quaternion import UnitQuaternion
return UnitQuaternion(self).angdist(UnitQuaternion(other),
metric=metric)
elif metric == 5:
op = lambda R1, R2: np.linalg.norm(np.eye(3) - R1 @ R2.T)
elif metric == 6:
op = lambda R1, R2: base.norm(base.trlog(R1 @ R2.T, twist=True))
else:
raise ValueError('unknown metric')
ad = self._op2(other, op)
if isinstance(ad, list):
return np.array(ad)
else:
return ad
def __init__(self, arg=None, w=None, check=True):
"""
Construct a new Twist object
TW = Twist(T) is a Twist object representing the SE(2) or SE(3)
homogeneous transformation matrix T (3x3 or 4x4).
TW = Twist(V) is a twist object where the vector is specified directly.
3D CASE:
TW = Twist('R', A, Q) is a Twist object representing rotation about the
axis of direction A (3x1) and passing through the point Q (3x1).
%
TW = Twist('R', A, Q, P) as above but with a pitch of P (distance/angle).
TW = Twist('T', A) is a Twist object representing translation in the
direction of A (3x1).
Notes:
- The argument 'P' for prismatic is synonymous with 'T'.
"""
super().__init__() # enable UserList superpowers
if arg is None:
self.data = [np.r_[0, 0, 0, 0, 0, 0]]
elif isinstance(arg, Twist):
# clone it
self.data = [np.r_[arg.v, arg.w]]
elif argcheck.isvector(arg, 6):
s = argcheck.getvector(arg)
self.data = [s]
elif argcheck.isvector(arg, 3) and argcheck.isvector(w, 3):
v = argcheck.getvector(arg)
w = argcheck.getvector(w)
self.data = [np.r_[v, w]]
elif isinstance(arg, SE3):
S = tr.trlog(arg.A) # use closed form for SE(3)
skw, v = tr.tr2rt(S)
w = tr.vex(skw)
self.data = [np.r_[v, w]]
elif Twist.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: Twist.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 6-lists
assert all(
map(lambda x: isinstance(x, list) and len(x) == 6,
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')
