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')