def angvec(cls, theta, v, unit='rad'):
v = argcheck.getvector(v, 3)
argcheck.isscalar(theta)
theta = argcheck.getunit(theta, unit)
return UnitQuaternion(s=math.cos(theta / 2),
v=math.sin(theta / 2) * tr.unit(v),
norm=False)
def kcircle(self, r, hw=None):
"""
Circular structuring element
:param r: radius of circle structuring element, or 2-vector (see below)
:type r: float, 2-tuple or 2-element vector of floats
:param hw: half-width of kernel
:type hw: integer
:return k: kernel
:rtype: numpy array (2 * 3 * sigma + 1, 2 * 3 * sigma + 1)
- ``IM.kcircle(r)`` is a square matrix ``(w,w)`` where ``w=2r+1`` of
zeros with a maximal centred circular region of radius ``r`` pixels
set to one.
- ``IM.kcircle(r,w)`` as above but the dimension of the kernel is
explicitly specified.
Example:
.. autorun:: pycon
.. note::
- If ``r`` is a 2-element vector the result is an annulus of ones,
and the two numbers are interpretted as inner and outer radii.
"""
# check valid input:
if not argcheck.isscalar(r): # r.shape[1] > 1:
r = argcheck.getvector(r)
rmax = r.max()
rmin = r.min()
else:
rmax = r
if hw is not None:
w = hw * 2 + 1
elif hw is None:
w = 2 * rmax + 1
s = np.zeros((np.int(w), np.int(w)))
c = np.floor(w / 2.0)
if not argcheck.isscalar(r):
s = self.kcircle(rmax, w) - self.kcircle(rmin, w)
else:
x, y = self.meshgrid(s)
x = x - c
y = y - c
ll = np.where(np.round((x**2 + y**2 - r**2) <= 0))
s[ll] = 1
return s
def __init__(self, s=None, v=None, check=True, norm=True):
"""
A unit quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
A quaternion can be considered as a rotation about a vector in space where
q = cos (theta/2) sin(theta/2) <vx vy vz>
where <vx vy vz> is a unit vector.
:param s: scalar
:param v: vector
"""
if s is None and v is None:
self.data = [ quat.qone() ]
elif argcheck.isscalar(s) and argcheck.isvector(v,3):
self.data = [ np.r_[s, argcheck.getvector(v)] ]
elif argcheck.isvector(s,4):
self.data = [ argcheck.getvector(s) ]
elif type(s) is list:
if check:
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
self.data = [x for x in s]
else:
raise ValueError('bad argument to Quaternion constructor')
def lspbfunc(t):
p = []
pd = []
pdd = []
if isscalar(t):
t = [t]
for tk in t:
if tk <= tb:
# initial blend
pk = q0 + a / 2 * tk**2
pdk = a * tk
pddk = a
elif tk <= (tf - tb):
# linear motion
pk = (qf + q0 - V * tf) / 2 + V * tk
pdk = V
pddk = 0
else:
# final blend
pk = qf - a / 2 * tf**2 + a * tf * tk - a / 2 * tk**2
pdk = a * tf - a * tk
pddk = -a
p.append(pk)
pd.append(pdk)
pdd.append(pddk)
return (np.array(p), np.array(pd), np.array(pdd))
def __init__(self, arg=None, *, unit='rad', check=True):
"""
Construct new SO(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable
:type unit: str, optional
:param check: check for valid SO(2) elements if applicable, default to True
:type check: bool
:return: SO(2) rotation
:rtype: SO2 instance
- ``SO2()`` is an SO2 instance representing a null rotation -- the identity matrix.
- ``SO2(θ)`` is an SO2 instance representing a rotation by ``θ`` radians. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(θ, unit='deg')`` is an SO2 instance representing a rotation by ``θ`` degrees. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(R)`` is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array. If ``check``
is ``True`` check the matrix belongs to SO(2).
- ``SO2([R1, R2, ... RN])`` is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix
Ri which is a 2x2 numpy array. If ``check`` is ``True`` then check each matrix belongs to SO(2).
- ``SO2([X1, X2, ... XN])`` is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.
"""
if super().arghandler(arg, check=check):
return
elif argcheck.isscalar(arg):
self.data = [tr.rot2(arg, unit=unit)]
elif argcheck.isvector(arg):
self.data = [tr.rot2(x, unit=unit) for x in argcheck.getvector(arg)]
else:
raise ValueError('bad argument to constructor')
def __mul__(left, right):
"""
multiply quaternion
:arg left: left multiplicand
:type left: Quaternion, UnitQuaternion
:arg right: right multiplicand
:type left: Quaternion, UnitQuaternion, 3-vector, float
:return: product
:rtype: Quaternion, UnitQuaternion
:raises: ValueError
============== ============== ============== ================
Multiplicands Product
------------------------------- --------------------------------
left right type result
============== ============== ============== ================
Quaternion Quaternion Quaternion Hamilton product
Quaternion UnitQuaternion Quaternion Hamilton product
Quaternion scalar Quaternion scalar product
UnitQuaternion Quaternion Quaternion Hamilton product
UnitQuaternion UnitQuaternion UnitQuaternion Hamilton product
UnitQuaternion scalar Quaternion scalar product
UnitQuaternion 3-vector 3-vector vector rotation
============== ============== ============== ================
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
==== ===== ==== ================================
left right len operation
==== ===== ==== ================================
1 1 1 ``prod = left * right``
1 N N ``prod[i] = left * right[i]``
N 1 N ``prod[i] = left[i] * right``
N N N ``prod[i] = left[i] * right[i]``
N M - ``ValueError``
==== ===== ==== ================================
A scalar of length N is a list, tuple or numpy array.
A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
"""
if type(left) == type(right):
# quaternion * quaternion case (same class)
return left.__class__( left._op2(right, lambda x, y: quat.qqmul(x, y) ) )
elif isinstance(other, Quaternion):
# quaternion * quaternion case (different class)
return Quaternion( left._op2(right, lambda x, y: quat.qqmul(x, y) ) )
elif argcheck.isscalar(right):
# quaternion * scalar case
print('scalar * quat')
return Quaternion([right*q._A for q in left])
elif isinstance(self, UnitQuaternion) and argcheck.isvector(right,3):
# scalar * vector case
return quat.qvmul(left._A, argcheck.getvector(right,3))
else:
raise ValueError('operands to * are of different types')
return left._op2(right, lambda x, y: x @ y )
def _op2(left, right, op):
"""
Perform binary operation
:param left: left side of comparison
:type self: SO2, SE2, SO3, SE3
:param right: right side of comparison
:type self: SO2, SE2, SO3, SE3
:param op: binary operation
:type op: callable
:raises ValueError: arguments are not compatible
:return: list of matrices
:rtype: list
Peform a binary operation on a pair of operands. If either operand
contains a sequence the results is a sequence accordinging to this
truth table.
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``ret = op(left, right)``
1 M M ``ret[i] = op(left, right[i])``
N 1 M ``ret[i] = op(left[i], right)``
M M M ``ret[i] = op(left[i], right[i])``
========= ========== ==== ================================
"""
if isinstance(right, left.__class__):
# class by class
if len(left) == 1:
if len(right) == 1:
#print('== 1x1')
return op(left.A, right.A)
else:
#print('== 1xN')
return [op(left.A, x) for x in right.A]
else:
if len(right) == 1:
#print('== Nx1')
return [op(x, right.A) for x in left.A]
elif len(left) == len(right):
#print('== NxN')
return [op(x, y) for (x, y) in zip(left.A, right.A)]
else:
raise ValueError(
'length of lists to == must be same length')
elif argcheck.isscalar(right) or (isinstance(right, np.ndarray)
and right.shape == left.shape):
# class by matrix
if len(left) == 1:
return op(left.A, right)
else:
return [op(x, right) for x in left.A]
def __truediv__(left, right):
"""
Overloaded ``/`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises ValueError: for incompatible arguments
:return: matrix
:rtype: numpy ndarray
Pose composition or scaling:
- ``X / Y`` compounds the poses ``X`` and ``Y.inv()``
- ``X / s`` performs elementwise multiplication of the elements of ``X`` by ``s``
============== ============== =========== =========================
Multiplicands Quotient
------------------------------- --------------------------------------
left right type operation
============== ============== =========== =========================
Pose Pose Pose matrix product by inverse
Pose scalar NxN matrix element-wise division
============== ============== =========== =========================
Notes:
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. scalar multiplication 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.inv()``
1 M M ``prod[i] = left * right[i].inv()``
N 1 M ``prod[i] = left[i] * right.inv()``
M M M ``prod[i] = left[i] * right[i].inv()``
========= ========== ==== ================================
"""
if isinstance(left, right.__class__):
return left.__class__(left._op2(right.inv(), lambda x, y: x @ y),
check=False)
elif argcheck.isscalar(right):
return left._op2(right, lambda x, y: x / y)
else:
raise ValueError('bad operands')
def __mul__(left, right):
"""
multiply quaternion
:arg left: left multiplicand
:type left: Quaternion
:arg right: right multiplicand
:type left: Quaternion, UnitQuaternion, float
:return: product
:rtype: Quaternion
:raises: ValueError
============== ============== ============== ================
Multiplicands Product
------------------------------- --------------------------------
left right type result
============== ============== ============== ================
Quaternion Quaternion Quaternion Hamilton product
Quaternion UnitQuaternion Quaternion Hamilton product
Quaternion scalar Quaternion scalar product
============== ============== ============== ================
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
==== ===== ==== ================================
left right len operation
==== ===== ==== ================================
1 1 1 ``prod = left * right``
1 N N ``prod[i] = left * right[i]``
N 1 N ``prod[i] = left[i] * right``
N N N ``prod[i] = left[i] * right[i]``
N M - ``ValueError``
==== ===== ==== ================================
"""
if isinstance(right, left.__class__):
# quaternion * [unit]quaternion case
return Quaternion(left._op2(right, lambda x, y: quat.qqmul(x, y)))
elif argcheck.isscalar(right):
# quaternion * scalar case
#print('scalar * quat')
return Quaternion([right * q._A for q in left])
else:
raise ValueError('operands to * are of different types')
return left._op2(right, lambda x, y: x @ y)
def __truediv__(left, right): # pylint: disable=no-self-argument
"""
Overloaded ``/`` operator (superclass method)
:return: Product of right operand and inverse of left operand
:rtype: pose instance or NumPy array
:raises ValueError: for incompatible arguments
Pose composition or scaling:
- ``X / Y`` compounds the poses ``X`` and ``Y.inv()``
- ``X / s`` performs elementwise multiplication of the elements of ``X`` by ``s``
============== ============== =========== =========================
Multiplicands Quotient
------------------------------- --------------------------------------
left right type operation
============== ============== =========== =========================
Pose Pose Pose matrix product by inverse
Pose scalar NxN matrix element-wise division
============== ============== =========== =========================
.. notes::
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. Scalar multiplication is not a group operation so the result will
be a matrix
#. Any other input combinations result in a ValueError.
For pose composition either or both operands may hold more than one value which
results in the composition holding more than one value according to:
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``quo = left * right.inv()``
1 M M ``quo[i] = left * right[i].inv()``
N 1 M ``quo[i] = left[i] * right.inv()``
M M M ``quo[i] = left[i] * right[i].inv()``
========= ========== ==== ================================
"""
if isinstance(left, right.__class__):
return left.__class__(left._op2(right.inv(), lambda x, y: x @ y),
check=False)
elif argcheck.isscalar(right):
return left._op2(right, lambda x, y: x / y)
else:
raise ValueError('bad operands')
def _format(self, l, name, ignorevalue=0, indices=None): # noqa # pragma nocover
v = getattr(self, name)
s = None
if v is None:
return
if isscalar(v) and v != ignorevalue:
s = f"{name}={v}"
elif isinstance(v, np.ndarray):
if np.linalg.norm(v, ord=np.inf) > 0:
if indices is not None:
flat = v.flatten()
v = np.r_[[flat[k] for k in indices]]
s = f"{name}=[" + ", ".join([str(x) for x in v]) + "]"
if s is not None:
l.append(s)
def _checkimage(self, im, mask):
"""
Check image and mask for pixelswitch
:param im: image, possibly a color vector or identifier
:type im: numpy array or scalar or string
:param mask: mask
:type mask: numpy array
:return: out
:rtype: Image instance
- ``_checkimage(im, mask)`` is an image the same shape as ``mask``, and
might be an image of all one color, depending on the value of ``im``
"""
if isinstance(im, str):
# image is a string color name
col = color.colorname(im)
if col is []:
raise ValueError(im, 'unknown color')
out = self.__class__(np.ones(mask.shape))
out = out.colorise(col)
elif argcheck.isscalar(im):
# image is a scalar, create a greyscale image the same size
# as mask
# TODO not certain if im.dtype works if im is scalar
out = np.ones(mask.shape, dtype=im.dtype) * im
elif im.ndim < 3 and max(im.shape) == 3:
# or (3,) or (3,1)
# image is a (1,3), create a color image the same size as mask
out = self.__class__(np.ones(mask.shape, dtype=im.dtype))
out = out.colorise(im)
elif isinstance(im, self.__class__):
# image class, check dimensions: (NOTE: im.size, not im.shape)
# here, we are assuming mask is a 2D matrix
if not np.any(im.size == mask.shape):
raise ValueError(
im, 'input image size does not confirm with mask')
out = im.image
else:
# actual image, check the dimensions
if not np.any(im.shape == mask.shape):
raise ValueError(
im, 'input image sizes (im or mask) do not conform')
return out
def __init__(self, s: Any = None, v=None, check=True, norm=True):
"""
A zero quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
A quaternion can be considered as a rotation about a vector in space where
q = cos (theta/2) sin(theta/2) <vx vy vz>
where <vx vy vz> is a unit vector.
:param s: scalar
:param v: vector
"""
super().__init__()
if s is None and v is None:
self.data = [np.array([0, 0, 0, 0])]
elif argcheck.isscalar(s) and argcheck.isvector(v, 3):
self.data = [np.r_[s, argcheck.getvector(v)]]
elif argcheck.isvector(s, 4):
self.data = [argcheck.getvector(s)]
elif isinstance(s, list):
if isinstance(s[0], np.ndarray):
if check:
assert argcheck.isvectorlist(
s, 4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s[0], self.__class__):
# possibly a list of objects of same type
assert all(map(lambda x: isinstance(x, self.__class__),
s)), 'all elements of list must have same type'
self.data = [x._A for x in s]
else:
raise ValueError('incorrect list')
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
self.data = [x for x in s]
elif isinstance(s, Quaternion):
self.data = s.data
else:
raise ValueError('bad argument to Quaternion constructor')
def __rmul__(right, left): # pylint: disable=no-self-argument
"""
Overloaded ``*`` operator (superclass method)
:return: Product of two operands
:rtype: pose instance
:raises NotImplemented: for incompatible arguments
Left-multiplication by a scalar
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
Notes:
#. For other left-operands return ``NotImplemented``. Other classes
such as ``Plucker`` and ``Twist`` implement left-multiplication by
an ``SE3`` using their own ``__rmul__`` methods.
"""
if argcheck.isscalar(left):
return right.__mul__(left)
else:
return NotImplemented
def __rmul__(right, left):
"""
Overloaded ``*`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: NotImplemented
Left-multiplication by a scalar
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
Notes:
#. For other left-operands return ``NotImplemented``. Other classes
such as ``Plucker`` and ``Twist`` implement left-multiplication by
an ``SE33`` using their own ``__rmul__`` methods.
"""
if argcheck.isscalar(left):
return right.__mul__(left)
else:
return NotImplemented
def __init__(self, s: Any = None, v=None, norm=True, check=True):
"""
Construct a UnitQuaternion object
:arg norm: explicitly normalize the quaternion [default True]
:type norm: bool
:arg check: explicitly check dimension of passed lists [default True]
:type check: bool
:return: new unit uaternion
:rtype: UnitQuaternion
:raises: ValueError
Single element quaternion:
- ``UnitQuaternion()`` constructs the identity quaternion 1<0,0,0>
- ``UnitQuaternion(s, v)`` constructs a unit quaternion with specified
real ``s`` and ``v`` vector parts. ``v`` is a 3-vector given as a
list, tuple, numpy.ndarray
- ``UnitQuaternion(v)`` constructs a unit quaternion with specified
elements from ``v`` which is a 4-vector given as a list, tuple, numpy.ndarray
- ``UnitQuaternion(R)`` constructs a unit quaternion from an orthonormal
rotation matrix given as a 3x3 numpy.ndarray. If ``check`` is True
test the matrix for orthogonality.
- ``UnitQuaternion(X)`` constructs a unit quaternion from the rotational
part of ``X`` which is SO3 or SE3 instance. If len(X) > 1 then
the resulting unit quaternion is of the same length.
Multi-element quaternion:
- ``UnitQuaternion(V)`` constructs a unit quaternion list with specified
elements from ``V`` which is an Nx4 numpy.ndarray, each row is a
quaternion. If ``norm`` is True explicitly normalize each row.
- ``UnitQuaternion(L)`` constructs a unit quaternion list from a list
of 4-element numpy.ndarrays. If ``check`` is True test each element
of the list is a 4-vector. If ``norm`` is True explicitly normalize
each vector.
"""
super().__init__()
if s is None and v is None:
self.data = [quat.eye()]
elif argcheck.isscalar(s) and argcheck.isvector(v, 3):
# UnitQuaternion(s, v) s is scalar, v is 3-vector
q = np.r_[s, argcheck.getvector(v)]
if norm:
q = quat.unit(q)
self.data = [q]
elif argcheck.isvector(s, 4):
# UnitQuaternion(q) q is 4-vector
q = argcheck.getvector(s)
if norm:
s = quat.unit(s)
self.data = [s]
elif isinstance(s, list):
# UnitQuaternion(list)
if isinstance(s[0], np.ndarray):
# list of 4-vectors
if check:
assert argcheck.isvectorlist(
s, 4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s[0], p3d.SO3):
# list of SO3/SE3
self.data = [quat.r2q(x.R) for x in s]
elif isinstance(s[0], self.__class__):
# possibly a list of objects of same type
assert all(map(lambda x: isinstance(x, type(self)),
s)), 'all elements of list must have same type'
self.data = [x._A for x in s]
else:
raise ValueError('incorrect list')
elif isinstance(s, p3d.SO3):
# UnitQuaternion(x) x is SO3 or SE3
self.data = [quat.r2q(x.R) for x in s]
elif isinstance(s, np.ndarray) and tr.isrot(s, check=check):
# UnitQuaternion(R) R is 3x3 rotation matrix
self.data = [quat.r2q(s)]
elif isinstance(s, np.ndarray) and tr.ishom(s, check=check):
# UnitQuaternion(T) T is 4x4 homogeneous transformation matrix
self.data = [quat.r2q(tr.t2r(s))]
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
if norm:
self.data = [quat.qnorm(x) for x in s]
else:
self.data = [x for x in s]
elif isinstance(s, UnitQuaternion):
# UnitQuaternion(Q) Q is a UnitQuaternion instance, clone it
self.data = s.data
else:
raise ValueError('bad argument to UnitQuaternion constructor')
def __init__(self, x=None, y=None, theta=None, *, unit='rad', check=True):
"""
Construct new SE(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable :type
unit: str, optional :param check: check for valid SE(2) elements if
applicable, default to True :type check: bool :return: homogeneous
rigid-body transformation matrix :rtype: SE2 instance
- ``SE2()`` is an SE2 instance representing a null motion -- the
identity matrix
- ``SE2(θ)`` is an SE2 instance representing a pure rotation of
``θ`` radians
- ``SE2(θ, unit='deg')`` as above but ``θ`` in degrees
- ``SE2(x, y)`` is an SE2 instance representing a pure translation of
(``x``, ``y``)
- ``SE2(t)`` is an SE2 instance representing a pure translation of
(``x``, ``y``) where``t``=[x,y] is a 2-element array_like
- ``SE2(x, y, θ)`` is an SE2 instance representing a translation of
(``x``, ``y``) and a rotation of ``θ`` radians
- ``SE2(x, y, θ, unit='deg')`` as above but ``θ`` in degrees
- ``SE2(t)`` where ``t``=[x,y] is a 2-element array_like, is an SE2
instance representing a pure translation of (``x``, ``y``)
- ``SE2(q)`` where ``q``=[x,y,θ] is a 3-element array_like, is an SE2
instance representing a translation of (``x``, ``y``) and a rotation
of ``θ`` radians
- ``SE2(t, unit='deg')`` as above but ``θ`` in degrees
- ``SE2(T)`` is an SE2 instance with rigid-body motion described by the
SE(2) matrix T which is a 3x3 numpy array. If ``check`` is ``True``
check the matrix belongs to SE(2).
- ``SE2([T1, T2, ... TN])`` is an SE2 instance containing a sequence of
N rigid-body motions, each described by an SE(2) matrix Ti which is a
3x3 numpy array. If ``check`` is ``True`` then check each matrix
belongs to SE(2).
- ``SE2([X1, X2, ... XN])`` is an SE2 instance containing a sequence of
N rigid-body motions, where each Xi is an SE2 instance.
"""
if y is None and theta is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif argcheck.isscalar(x):
self.data = [tr.trot2(x, unit=unit)]
elif len(x) == 2:
# SE2([x,y])
self.data = [tr.transl2(x)]
elif len(x) == 3:
# SE2([x,y,theta])
self.data = [tr.trot2(x[2], t=x[:2], unit=unit)]
else:
raise ValueError('bad argument to constructor')
elif x is not None:
if y is not None and theta is None:
# SE2(x, y)
self.data = [tr.transl2(x, y)]
elif y is not None and theta is not None:
# SE2(x, y, theta)
self.data = [tr.trot2(theta, t=[x, y], unit=unit)]
else:
raise ValueError('bad arguments to constructor')
def __mul__(left, right):
"""
Multiply unit quaternion
:arg left: left multiplicand
:type left: UnitQuaternion
:arg right: right multiplicand
:type left: UnitQuaternion, Quaternion, 3-vector, 3xN array, float
:return: product
:rtype: Quaternion, UnitQuaternion
:raises: ValueError
============== ============== ============== ================
Multiplicands Product
------------------------------- --------------------------------
left right type result
============== ============== ============== ================
UnitQuaternion Quaternion Quaternion Hamilton product
UnitQuaternion UnitQuaternion UnitQuaternion Hamilton product
UnitQuaternion scalar Quaternion scalar product
UnitQuaternion 3-vector 3-vector vector rotation
UnitQuaternion 3xN array 3xN array vector rotations
============== ============== ============== ================
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
==== ===== ==== ================================
left right len operation
==== ===== ==== ================================
1 1 1 ``prod = left * right``
1 N N ``prod[i] = left * right[i]``
N 1 N ``prod[i] = left[i] * right``
N N N ``prod[i] = left[i] * right[i]``
N M - ``ValueError``
==== ===== ==== ================================
A scalar of length N is a list, tuple or numpy array.
A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
:seealso: :func:`~spatialmath.Quaternion.__mul__`
"""
if isinstance(left, right.__class__):
# quaternion * quaternion case (same class)
return right.__class__(
left._op2(right, lambda x, y: quat.qqmul(x, y)))
elif argcheck.isscalar(right):
# quaternion * scalar case
#print('scalar * quat')
return Quaternion([right * q._A for q in left])
elif isinstance(right, (list, tuple, np.ndarray)):
#print('*: pose x array')
if argcheck.isvector(right, 3):
v = argcheck.getvector(right)
if len(left) == 1:
# pose x vector
#print('*: pose x vector')
return quat.qvmul(left._A, argcheck.getvector(right, 3))
elif len(left) > 1 and argcheck.isvector(right, 3):
# pose array x vector
#print('*: pose array x vector')
return np.array([tr.qvmul(x, v) for x in left._A]).T
elif len(left) == 1 and isinstance(
right, np.ndarray) and right.shape[0] == 3:
return np.array([tr.qvmul(left._A, x) for x in right.T]).T
else:
raise ValueError('bad operands')
else:
raise ValueError(
'UnitQuaternion: operands to * are of different types')
return left._op2(right, lambda x, y: x @ y)
return right.__mul__(left)
def smooth(self, sigma, hw=None, optmode='same', optboundary='fill'):
"""
Smooth image
:param sigma: standard deviation of the Gaussian kernel
:type sigma: float
:param hw: half-width of the kernel
:type hw: float
:param opt: convolution options np.convolve (see below)
:type opt: string
:return out: Image with smoothed image pixels
:rtype: Image instance
- ``IM.smooth(sigma)`` is the image after convolution with a Gaussian
kernel of standard deviation ``sigma``
- ``IM.smooth(sigma, hw)`` as above with kernel half-width ``hw``.
- ``IM.smooth(sigma, opt)`` as above with options passed to np.convolve
:options:
- 'full' returns the full 2-D convolution (default)
- 'same' returns OUT the same size as IM
- 'valid' returns the valid pixels only, those where the kernel
does not exceed the bounds of the image.
Example:
.. runblock:: pycon
.. note::
- By default (option 'full') the returned image is larger than the
passed image.
- Smooths all planes of the input image.
- The Gaussian kernel has a unit volume.
- If input image is integer it is converted to float, convolved,
then converted back to integer.
"""
if not argcheck.isscalar(sigma):
raise ValueError(sigma, 'sigma must be a scalar')
modeopt = {
'full': 'full',
'valid': 'valid',
'same': 'same'
}
if optmode not in modeopt:
raise ValueError(optmode, 'opt is not a valid option')
boundaryopt = {
'fill': 'fill',
'wrap': 'wrap',
'reflect': 'symm'
}
if optboundary not in boundaryopt:
raise ValueError(optboundary, 'opt is not a valid option')
is_int = False
if np.issubdtype(self.dtype, np.integer):
is_int = True
img = self.float()
else:
img = self
# make the smoothing kernel
K = self.kgauss(sigma, hw)
if img.iscolor:
# could replace this with a nested list comprehension
ims = []
for im in img:
o = np.dstack([signal.convolve2d(np.squeeze(im.image[:, :, i]),
K,
mode=modeopt[optmode],
boundary=boundaryopt[
optboundary])
for i in range(im.numchannels)])
ims.append(o)
elif not img.iscolor:
ims = []
for im in img:
ims.append(signal.convolve2d(im.image,
K,
mode=modeopt[optmode],
boundary=boundaryopt[
optboundary]))
else:
raise ValueError(self.iscolor, 'bad value for iscolor')
if is_int:
return self.__class__(ims).int()
else:
return self.__class__(ims)
def pyramid(self, sigma=1, N=None):
"""
Pyramidal image decomposition
:param sigma: standard deviation of Gaussian kernel
:type sigma: float
:param N: number of pyramid levels to be computed
:type N: int
:return pyrimlist: list of Images for each pyramid level computed
:rtype pyrimlist: list
- ``IM.pyramid()`` is a pyramid decomposition of image using Gaussian
smoothing with standard deviation of 1. The return is a list array of
images each one having dimensions half that of the previous image.
The pyramid is computed down to a non-halvable image size.
- ``IM.pyramid(sigma)`` as above but the Gaussian standard deviation is
``sigma``.
- ``IM.pyramid(sigma, N)`` as above but only ``N`` levels of the
pyramid are computed.
Example:
.. runblock:: pycon
.. note::
- Converts a color image to greyscale.
- Works for greyscale images only.
"""
# check inputs, greyscale only
im = self.mono()
if not argcheck.isscalar(sigma):
raise ValueError(sigma, 'sigma must be a scalar')
if N is None:
N = max(im.shape)
else:
if (not argcheck.isscalar(N)) and (N >= 0) and \
(N <= max(im.shape)):
raise ValueError(N, 'N must be a scalar and \
0 <= N <= max(im.shape)')
# TODO options to accept different border types,
# note that the Matlab implementation is hard-coded to 'same'
# return cv.buildPyramid(im, N, borderType=cv.BORDER_REPLICATE)
# Python version does not seem to be implemented
# list comprehension approach
# TODO pyr = [cv.pyrdown(inputs(i)) for i in range(N) if conditional]
impyr = im.image
pyr = [impyr]
for i in range(N):
if impyr.shape[0] == 1 or impyr.shape[1] == 1:
break
impyr = cv.pyrDown(impyr, borderType=cv.BORDER_REPLICATE)
pyr.append(impyr)
# output list of Image objects
pyrimlist = [self.__class__(p) for p in pyr]
return pyrimlist
def thresh(self, t=None, opt='binary'):
"""
Image threshold
:param t: threshold
:type t: scalar
:param opt: threshold option (see below)
:type opt: string
:return imt: Image thresholded binary image
:rtype imt: Image instance
:return: threshold if opt is otsu or triangle
:rtype: list of scalars
- ``IM.thresh()`` uses Otsu's method for thresholding a greyscale
image.
- ``IM.thresh(t)`` as above but the threshold ``t`` is specified.
- ``IM.thresh(t, opt)`` as above but the threshold option is specified.
See opencv threshold types for threshold options
https://docs.opencv.org/4.2.0/d7/d1b/group__imgproc__
misc.html#gaa9e58d2860d4afa658ef70a9b1115576
Example:
.. runblock:: pycon
:options:
- 'binary' # TODO consider the LaTeX formatting of equations
- 'binary_inv'
- 'trunc'
- 'tozero'
- 'tozero_inv'
- 'otsu'
- 'triangle'
.. note::
- Converts a color image to greyscale.
- For a uint8 class image the slider range is 0 to 255.
- For a floating point class image the slider range is 0 to 1.0
"""
# dictionary of threshold options from OpenCV
threshopt = {
'binary': cv.THRESH_BINARY,
'binary_inv': cv.THRESH_BINARY_INV,
'trunc': cv.THRESH_TRUNC,
'tozero': cv.THRESH_TOZERO,
'tozero_inv': cv.THRESH_TOZERO_INV,
'otsu': cv.THRESH_OTSU,
'triangle': cv.THRESH_TRIANGLE
}
if t is not None:
if not argcheck.isscalar(t):
raise ValueError(t, 't must be a scalar')
else:
# if no threshold is specified, we assume to use Otsu's method
print('No threshold specified. Applying Otsu' 's method.')
opt = 'otsu'
# ensure mono images
if self.iscolor:
imono = self.mono()
else:
imono = self
out_t = []
out_imt = []
for im in [img.image for img in imono]:
# for image int class, maxval = max of int class
# for image float class, maxval = 1
if np.issubdtype(im.dtype, np.integer):
maxval = np.iinfo(im.dtype).max
else:
# float image, [0, 1] range
maxval = 1.0
threshvalue, imt = cv.threshold(im, t, maxval, threshopt[opt])
out_t.append(threshvalue)
out_imt.append(imt)
if opt == 'otsu' or opt == 'triangle':
return self.__class__(out_imt), out_t
else:
return self.__class__(out_imt)
def binop(self, right, op, op2=None, list1=True):
"""
Perform binary operation
:param left: left operand
:type left: BasePoseList subclass
:param right: right operand
:type right: BasePoseList subclass, scalar or array
:param op: binary operation
:type op: callable
:param op2: binary operation
:type op2: callable
:param list1: return single array as a list, default True
:type list1: bool
:raises ValueError: arguments are not compatible
:return: list of values
:rtype: list
The is a helper method for implementing binary operation with overloaded
operators such as ``X * Y`` where ``X`` and ``Y`` are both subclasses
of ``BasePoseList``. Each operand has a list of one or more
values and this methods computes a list of result values according to:
========= ========== ==== ===================================
Inputs Output
---------------------- -----------------------------------------
len(left) len(right) len operation
========= ========== ==== ===================================
1 1 1 ``ret = op(left, right)``
1 M M ``ret[i] = op(left, right[i])``
M 1 M ``ret[i] = op(left[i], right)``
M M M ``ret[i] = op(left[i], right[i])``
========= ========== ==== ===================================
The arguments to ``op`` are the internal numeric values, ie. as returned
by the ``._A`` property.
The result is always a list, except for the first case above and
``list1`` is ``False``.
If the right operand is not a ``BasePoseList`` subclass, but is a numeric
scalar or array then then ``op2`` is invoked
For example::
X._binop(Y, lambda x, y: x + y)
========= ==== ===================================
Input Output
--------- -----------------------------------------
len(left) len operation
========= ==== ===================================
1 1 ``ret = op2(left, right)``
M M ``ret[i] = op2(left[i], right)``
========= ==== ===================================
There is no check on the shape of ``right`` if it is an array.
The result is always a list, except for the first case above and
``list1`` is ``False``.
"""
left = self
# class * class
if len(left) == 1:
# singleton *
if argcheck.isscalar(right):
if list1:
return [op(left._A, right)]
else:
return op(left.A, right)
elif len(right) == 1:
# singleton * singleton
if list1:
return [op(left._A, right._A)]
else:
return op(left.A, right.A)
else:
# singleton * non-singleton
return [op(left.A, x) for x in right.A]
else:
# non-singleton *
if argcheck.isscalar(right):
return [op(x, right) for x in left.A]
elif len(right) == 1:
# non-singleton * singleton
return [op(x, right.A) for x in left.A]
elif len(left) == len(right):
# non-singleton * non-singleton
return [op(x, y) for (x, y) in zip(left.A, right.A)]
else:
raise ValueError('length of lists to == must be same length')
def fdyn(self,
T,
q0,
torqfun=None,
targs=None,
qd0=None,
solver='RK45',
sargs=None,
dt=None,
progress=False):
"""
Integrate forward dynamics
:param T: integration time
:type T: float
:param q0: initial joint coordinates
:type q0: array_like
:param qd0: initial joint velocities, assumed zero if not given
:type qd0: array_like
:param torqfun: a function that computes torque as a function of time
and/or state
:type torqfun: callable
:param targs: argumments passed to ``torqfun``
:type targs: dict
:type solver: name of scipy solver to use, RK45 is the default
:param solver: str
:type sargs: arguments passed to the solver
:param sargs: dict
:type dt: time step for results
:param dt: float
:param progress: show progress bar, default False
:type progress: bool
:return: robot trajectory
:rtype: namedtuple
- ``tg = R.fdyn(T, q)`` integrates the dynamics of the robot with zero
input torques over the time interval 0 to ``T`` and returns the
trajectory as a namedtuple with elements:
- ``t`` the time vector (M,)
- ``q`` the joint coordinates (M,n)
- ``qd`` the joint velocities (M,n)
- ``tg = R.fdyn(T, q, torqfun)`` as above but the torque applied to the
joints is given by the provided function::
tau = function(robot, t, q, qd, **args)
where the inputs are:
- the robot object
- current time
- current joint coordinates (n,)
- current joint velocity (n,)
- args, optional keyword arguments can be specified, these are
passed in from the ``targs`` kewyword argument.
The function must return a Numpy array (n,) of joint forces/torques.
Examples:
#. to apply zero joint torque to the robot without Coulomb
friction::
def myfunc(robot, t, q, qd):
return np.zeros((robot.n,))
tg = robot.nofriction().fdyn(5, q0, myfunc)
plt.figure()
plt.plot(tg.t, tg.q)
plt.show()
We could also use a lambda function::
tg = robot.nofriction().fdyn(
5, q0, lambda r, t, q, qd: np.zeros((r.n,)))
#. the robot is controlled by a PD controller. We first define a
function to compute the control which has additional parameters for
the setpoint and control gains (qstar, P, D)::
def myfunc(robot, t, q, qd, qstar, P, D):
return (qstar - q) * P + qd * D # P, D are (6,)
targs = {'qstar': VALUE, 'P': VALUE, 'D': VALUE}
tg = robot.fdyn(10, q0, myfunc, targs=targs) )
Many integrators have variable step length which is problematic if we
want to animate the result. If ``dt`` is specified then the solver
results are interpolated in time steps of ``dt``.
:notes:
- This function performs poorly with non-linear joint friction, such as
Coulomb friction. The R.nofriction() method can be used to set this
friction to zero.
- If the function is not specified then zero force/torque is
applied to the manipulator joints.
- Interpolation is performed using `ScipY integrate.ode
<https://docs.scipy.org/doc/scipy/reference/generated/scipy.integrate.ode.html>`
- The SciPy RK45 integrator is used by default
- Interpolation is performed using `SciPy interp1
<https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.interp1d.html>`
:seealso: :func:`DHRobot.accel`, :func:`DHRobot.nofriction`,
:func:`DHRobot.rne`.
"""
n = self.n
if not isscalar(T):
raise ValueError('T must be a scalar')
q0 = getvector(q0, n)
if qd0 is None:
qd0 = np.zeros((n, ))
else:
qd0 = getvector(qd0, n)
if torqfun is not None:
if not callable(torqfun):
raise ValueError('torque function must be callable')
if sargs is None:
sargs = {}
if targs is None:
targs = {}
# concatenate q and qd into the initial state vector
x0 = np.r_[q0, qd0]
# get user specified integrator
scipy_integrator = integrate.__dict__[solver]
integrator = scipy_integrator(
lambda t, y: self._fdyn(t, y, torqfun, targs),
t0=0.0,
y0=x0,
t_bound=T,
**sargs)
# initialize list of time and states
tlist = [0]
xlist = [np.r_[q0, qd0]]
if progress:
_printProgressBar(0,
prefix='Progress:',
suffix='complete',
length=60)
while integrator.status == 'running':
# step the integrator, calls _fdyn multiple times
integrator.step()
if integrator.status == 'failed':
raise RuntimeError('integration completed with failed status ')
# stash the results
tlist.append(integrator.t)
xlist.append(integrator.y)
# update the progress bar
if progress:
_printProgressBar(integrator.t / T,
prefix='Progress:',
suffix='complete',
length=60)
# cleanup the progress bar
if progress:
print('\r' + ' ' * 90 + '\r')
tarray = np.array(tlist)
xarray = np.array(xlist)
if dt is not None:
# interpolate data to equal time steps of dt
interp = interpolate.interp1d(tarray, xarray, axis=0)
tnew = np.arange(0, T, dt)
xnew = interp(tnew)
return namedtuple('fdyn', 't q qd')(tnew, xnew[:, :n], xnew[:, n:])
else:
return namedtuple('fdyn', 't q qd')(tarray, xarray[:, :n],
xarray[:, n:])
def B(self, B_new):
if isscalar(B_new):
self._B = B_new
else:
raise TypeError("B must be a scalar")
def __mul__(left, right):
"""
Overloaded ``*`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
Pose composition, scaling or vector transformation:
- ``X * Y`` compounds the poses ``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``
- ``X * v`` linear transform of the vector ``v``
============== ============== =========== ======================
Multiplicands Product
------------------------------- -----------------------------------
left right type operation
============== ============== =========== ======================
Pose Pose Pose matrix product
Pose scalar NxN matrix element-wise product
scalar Pose NxN matrix element-wise product
Pose N-vector N-vector vector transform
Pose NxM matrix NxM matrix transform each column
============== ============== =========== ======================
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]``
========= ========== ==== ================================
For vector transformation there are three cases
========= =========== ===== ==========================
Multiplicands Product
---------------------- ---------------------------------
len(left) right.shape shape operation
========= =========== ===== ==========================
1 (N,) (N,) vector transformation
M (N,) (N,M) vector transformations
1 (N,M) (N,M) column transformation
========= =========== ===== ==========================
Notes:
#. for the ``SE2`` and ``SE3`` case the vectors are converted to homogeneous
form, transformed, then converted back to Euclidean form.
"""
if isinstance(left, right.__class__):
#print('*: pose x pose')
return left.__class__(left._op2(right, lambda x, y: x @ y),
check=False)
elif isinstance(right, (list, tuple, np.ndarray)):
#print('*: pose x array')
if len(left) == 1 and argcheck.isvector(right, left.N):
# pose x vector
#print('*: pose x vector')
v = argcheck.getvector(right, out='col')
if left.isSE:
# SE(n) x vector
return tr.h2e(left.A @ tr.e2h(v))
else:
# SO(n) x vector
return left.A @ v
elif len(left) > 1 and argcheck.isvector(right, left.N):
# pose array x vector
#print('*: pose array x vector')
v = argcheck.getvector(right)
if left.isSE:
# SE(n) x vector
v = tr.e2h(v)
return np.array([tr.h2e(x @ v).flatten()
for x in left.A]).T
else:
# SO(n) x vector
return np.array([(x @ v).flatten() for x in left.A]).T
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSO and right.shape[0] == left.N:
# SO(n) x matrix
return left.A @ right
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSE and right.shape[0] == left.N:
# SE(n) x matrix
return tr.h2e(left.A @ tr.e2h(right))
elif isinstance(right, np.ndarray) and left.isSO and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SO(n) x matrix
return np.c_[[x.A @ y for x, y in zip(right, left.T)]].T
elif isinstance(right, np.ndarray) and left.isSE and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SE(n) x matrix
return np.c_[[
tr.h2e(x.A @ tr.e2h(y)) for x, y in zip(right, left.T)
]].T
else:
raise ValueError('bad operands')
elif argcheck.isscalar(right):
return left._op2(right, lambda x, y: x * y)
else:
return NotImplemented
def __mul__(left, right): # pylint: disable=no-self-argument
"""
Overloaded ``*`` operator (superclass method)
:return: Product of two operands
:rtype: pose instance
:raises NotImplemented: for incompatible arguments
Pose composition, scaling or vector transformation:
- ``X * Y`` compounds the poses ``X`` and ``Y``
- ``X * s`` performs element-wise multiplication of the elements of ``X`` by ``s``
- ``s * X`` performs element-wise multiplication of the elements of ``X`` by ``s``
- ``X * v`` linear transformation of the vector ``v`` where ``v`` is array-like
============== ============== =========== ======================
Multiplicands Product
------------------------------- -----------------------------------
left right type operation
============== ============== =========== ======================
Pose Pose Pose matrix product
Pose scalar NxN matrix element-wise product
scalar Pose NxN matrix element-wise product
Pose N-vector N-vector vector transform
Pose NxM matrix NxM matrix transform each column
============== ============== =========== ======================
.. note::
#. Pose is an ``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 either or both operands may hold more than one value which
results in the composition holding more than one value according to:
========= ========== ==== ================================
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]``
========= ========== ==== ================================
Example::
>>> SE3.Rx(pi/2) * SE3.Ry(pi/2)
SE3(array([[0., 0., 1., 0.],
[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 0., 1.]]))
>>> SE3.Rx(pi/2) * 2
array([[ 2.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, 1.2246468e-16, -2.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, 2.0000000e+00, 1.2246468e-16, 0.0000000e+00],
[ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 2.0000000e+00]])
For vector transformation there are three cases:
========= =========== ===== ==========================
Multiplicands Product
---------------------- ---------------------------------
len(left) right.shape shape operation
========= =========== ===== ==========================
1 (N,) (N,) vector transformation
M (N,) (N,M) vector transformations
1 (N,M) (N,M) column transformation
========= =========== ===== ==========================
.. note:: For the ``SE2`` and ``SE3`` case the vectors are converted to homogeneous
form, transformed, then converted back to Euclidean form.
Example::
>>> SE3.Rx(pi/2) * [0, 1, 0]
array([0.000000e+00, 6.123234e-17, 1.000000e+00])
>>> SE3.Rx(pi/2) * np.r_[0, 0, 1]
array([ 0.000000e+00, -1.000000e+00, 6.123234e-17])
"""
if isinstance(left, right.__class__):
#print('*: pose x pose')
return left.__class__(left._op2(right, lambda x, y: x @ y),
check=False)
elif isinstance(right, (list, tuple, np.ndarray)):
#print('*: pose x array')
if len(left) == 1 and argcheck.isvector(right, left.N):
# pose x vector
#print('*: pose x vector')
v = argcheck.getvector(right, out='col')
if left.isSE:
# SE(n) x vector
return tr.h2e(left.A @ tr.e2h(v))
else:
# SO(n) x vector
return left.A @ v
elif len(left) > 1 and argcheck.isvector(right, left.N):
# pose array x vector
#print('*: pose array x vector')
v = argcheck.getvector(right)
if left.isSE:
# SE(n) x vector
v = tr.e2h(v)
return np.array([tr.h2e(x @ v).flatten()
for x in left.A]).T
else:
# SO(n) x vector
return np.array([(x @ v).flatten() for x in left.A]).T
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSO and right.shape[0] == left.N:
# SO(n) x matrix
return left.A @ right
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSE and right.shape[0] == left.N:
# SE(n) x matrix
return tr.h2e(left.A @ tr.e2h(right))
elif isinstance(right, np.ndarray) and left.isSO and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SO(n) x matrix
return np.c_[[x.A @ y for x, y in zip(right, left.T)]].T
elif isinstance(right, np.ndarray) and left.isSE and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SE(n) x matrix
return np.c_[[
tr.h2e(x.A @ tr.e2h(y)) for x, y in zip(right, left.T)
]].T
else:
raise ValueError('bad operands')
elif argcheck.isscalar(right):
return left._op2(right, lambda x, y: x * y)
else:
return NotImplemented
def __init__(self, s=None, v=None, norm=True, check=True):
"""
Construct a UnitQuaternion object
:arg norm: explicitly normalize the quaternion [default True]
:type norm: bool
:arg check: explicitly check dimension of passed lists [default True]
:type check: bool
:return: new unit uaternion
:rtype: UnitQuaternion
:raises: ValueError
Single element quaternion:
- ``UnitQuaternion()`` constructs the identity quaternion 1<0,0,0>
- ``UnitQuaternion(s, v)`` constructs a unit quaternion with specified
real ``s`` and ``v`` vector parts. ``v`` is a 3-vector given as a
list, tuple, numpy.ndarray
- ``UnitQuaternion(v)`` constructs a unit quaternion with specified
elements from ``v`` which is a 4-vector given as a list, tuple, numpy.ndarray
- ``UnitQuaternion(R)`` constructs a unit quaternion from an orthonormal
rotation matrix given as a 3x3 numpy.ndarray. If ``check`` is True
test the matrix for orthogonality.
Multi-element quaternion:
- ``UnitQuaternion(V)`` constructs a unit quaternion list with specified
elements from ``V`` which is an Nx4 numpy.ndarray, each row is a
quaternion. If ``norm`` is True explicitly normalize each row.
- ``UnitQuaternion(L)`` constructs a unit quaternion list from a list
of 4-element numpy.ndarrays. If ``check`` is True test each element
of the list is a 4-vector. If ``norm`` is True explicitly normalize
each vector.
"""
if s is None and v is None:
self.data = [ quat.eye() ]
elif argcheck.isscalar(s) and argcheck.isvector(v,3):
q = np.r_[ s, argcheck.getvector(v) ]
if norm:
q = quat.unit(q)
self.data = [q]
elif argcheck.isvector(s,4):
print('uq constructor 4vec')
q = argcheck.getvector(s)
# if norm:
# q = quat.unit(q)
print(q)
self.data = [q]
elif type(s) is list:
if check:
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
if norm:
s = [quat.unit(q) for q in s]
self.data = s
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
if norm:
self.data = [quat.norm(x) for x in s]
else:
self.data = [x for x in s]
elif tr.isrot(s, check=check):
self.data = [ quat.r2q(s) ]
else:
raise ValueError('bad argument to UnitQuaternion constructor')
def __mul__(left, right):
"""
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, Twist):
# twist composition
return Twist(left.exp() * right.exp())
elif isinstanve(right, SE3):
return Twist(left.exp() * right)
elif argcheck.isscalar(right):
return Twist(left.S * right)
elif isinstance(right, SpatialVelocity):
return SpatialVelocity(a.Ad @ b.vw)
elif isinstance(right, SpatialAcceleration):
return SpatialAcceleration(a.Ad @ b.vw)
elif isinstance(right, SpatialForce):
return SpatialForce(a.Ad @ b.vw)
else:
raise ValueError('twist *, incorrect right operand')