def trplot(self, end, start=None, **kwargs): """ Define the transform to animate :param end: the final pose SO(3) or SE(3) to display as a coordinate frame :type end: numpy.ndarray, shape=(3,3) or (4,4) :param start: the initial pose SO(3) or SE(3) to display as a coordinate frame, defaults to null :type start: numpy.ndarray, shape=(3,3) or (4,4) :param start: an Is polymorphic with ``base.trplot`` and accepts the same parameters. This sets up the animation but doesn't execute it. :seealso: :func:`run` """ # stash the final value if tr.isrot(end): self.end = tr.r2t(end) else: self.end = end if start is None: self.start = np.identity(4) else: if tr.isrot(start): self.start = tr.r2t(start) else: self.start = start # draw axes at the origin tr.trplot(self.start, axes=self, block=None, **kwargs)
def SO3(cls, R, check=True): if isinstance(R, SO3): R = R.A elif base.isrot(R, check=check): pass else: raise ValueError('expecting SO3 or rotation matrix') return cls(base.r2t(R))
def trplot(self, end, start=None, **kwargs): """ Define the transform to animate :param end: the final pose SE(3) or SO(3) to display as a coordinate frame :type end: ndarray(4,4) or ndarray(3,3) :param start: the initial pose SE(3) or SO(3) to display as a coordinate frame, defaults to null :type start: ndarray(4,4) or ndarray(3,3) :param start: an Is polymorphic with ``base.trplot`` and accepts the same parameters. This sets up the animation but doesn't execute it. :seealso: :func:`run` """ if not isinstance(end, (np.ndarray, np.generic)) and isinstance( end, Iterable): try: if len(end) == 1: end = end[0] elif len(end) >= 2: self.trajectory = end except TypeError: # a generator has no len() self.trajectory = end # stash the final value if base.isrot(end): self.end = base.r2t(end) else: self.end = end if start is None: self.start = np.identity(4) else: if base.isrot(start): self.start = base.r2t(start) else: self.start = start # draw axes at the origin base.trplot(self.start, axes=self, **kwargs)
def isvalid(x, check=True): """ Test if matrix is valid SO(3) :param x: matrix to test :type x: numpy.ndarray :return: ``True`` if the matrix is a valid element of SO(3), ie. it is a 3x3 orthonormal matrix with determinant of +1. :rtype: bool :seealso: :func:`~spatialmath.base.transform3d.isrot` """ return base.isrot(x, check=True)
def trplot(self, T, **kwargs): """ Define the transform to animate :param T: an SO(3) or SE(3) pose to be displayed as coordinate frame :type: numpy.ndarray, shape=(3,3) or (4,4) Is polymorphic with ``base.trplot`` and accepts the same parameters. This sets up the animation but doesn't execute it. :seealso: :func:`run` """ # stash the final value if tr.isrot(T): self.T = tr.r2t(T) else: self.T = T # draw axes at the origin tr.trplot(np.eye(4), axes=self, **kwargs)
def Rt(cls, R, t, check=True): """ Create an SE(3) from rotation and translation :param R: rotation :type R: SO3 or ndarray(3,3) :param t: translation :type t: array_like(3) :param check: check rotation validity, defaults to True :type check: bool, optional :raises ValueError: bad rotation matrix :return: SE(3) matrix :rtype: SE3 instance """ if isinstance(R, SO3): R = R.A elif base.isrot(R, check=check): pass else: raise ValueError('expecting SO3 or rotation matrix') return cls(base.rt2tr(R, t))
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 __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 r2q(R, check=False, tol=100): """ Convert SO(3) rotation matrix to unit-quaternion :arg R: SO(3) rotation matrix :type R: ndarray(3,3) :param check: check validity of rotation matrix, default False :type check: bool :param tol: tolerance in units of eps :type tol: float :return: unit-quaternion :rtype: ndarray(4) :raises ValueError: for non SO(3) argument Returns a unit-quaternion corresponding to the input SO(3) rotation matrix. .. runblock:: pycon >>> from spatialmath.base import r2q, qprint, rotx >>> R = rotx(90, 'deg') # rotation of 90deg about x-axis >>> print(R) >>> qprint(r2q(R)) .. warning:: There is no check that the passed matrix is a valid rotation matrix. .. note:: Scalar part is always positive. :seealso: :func:`q2r` """ if not base.isrot(R, check=check, tol=tol): raise ValueError("Argument must be a valid SO(3) matrix") qs = math.sqrt(max(0, np.trace(R) + 1)) / 2.0 # scalar part kx = R[2, 1] - R[1, 2] # Oz - Ay ky = R[0, 2] - R[2, 0] # Ax - Nz kz = R[1, 0] - R[0, 1] # Ny - Ox if (R[0, 0] >= R[1, 1]) and (R[0, 0] >= R[2, 2]): kx1 = R[0, 0] - R[1, 1] - R[2, 2] + 1 # Nx - Oy - Az + 1 ky1 = R[1, 0] + R[0, 1] # Ny + Ox kz1 = R[2, 0] + R[0, 2] # Nz + Ax add = (kx >= 0) elif R[1, 1] >= R[2, 2]: kx1 = R[1, 0] + R[0, 1] # Ny + Ox ky1 = R[1, 1] - R[0, 0] - R[2, 2] + 1 # Oy - Nx - Az + 1 kz1 = R[2, 1] + R[1, 2] # Oz + Ay add = (ky >= 0) else: kx1 = R[2, 0] + R[0, 2] # Nz + Ax ky1 = R[2, 1] + R[1, 2] # Oz + Ay kz1 = R[2, 2] - R[0, 0] - R[1, 1] + 1 # Az - Nx - Oy + 1 add = (kz >= 0) if add: kx = kx + kx1 ky = ky + ky1 kz = kz + kz1 else: kx = kx - kx1 ky = ky - ky1 kz = kz - kz1 kv = np.r_[kx, ky, kz] nm = np.linalg.norm(kv) if abs(nm) < tol * _eps: return eye() else: return np.r_[qs, (math.sqrt(1.0 - qs**2) / nm) * kv]
def r2q(R, check=False, tol=100, order='sxyz'): """ Convert SO(3) rotation matrix to unit-quaternion :arg R: SO(3) rotation matrix :type R: ndarray(3,3) :param check: check validity of rotation matrix, default False :type check: bool :param tol: tolerance in units of eps :type tol: float :param order: the order of the returned quaternion. Must be 'sxyz' or 'xyzs'. Defaults to 'sxyz'. :type order: str :return: unit-quaternion as Euler parameters :rtype: ndarray(4) :raises ValueError: for non SO(3) argument Returns a unit-quaternion corresponding to the input SO(3) rotation matrix. .. runblock:: pycon >>> from spatialmath.base import r2q, qprint, rotx >>> R = rotx(90, 'deg') # rotation of 90deg about x-axis >>> print(R) >>> qprint(r2q(R)) .. warning:: There is no check that the passed matrix is a valid rotation matrix. .. note:: - Scalar part is always positive - implements Cayley's method :reference: - Sarabandi, S., and Thomas, F. (March 1, 2019). "A Survey on the Computation of Quaternions From Rotation Matrices." ASME. J. Mechanisms Robotics. April 2019; 11(2): 021006. `doi.org/10.1115/1.4041889 <https://doi.org/10.1115/1.4041889>`_ :seealso: :func:`q2r` """ if not base.isrot(R, check=check, tol=tol): raise ValueError("Argument must be a valid SO(3) matrix") t12p = (R[0, 1] + R[1, 0])**2 t13p = (R[0, 2] + R[2, 0])**2 t23p = (R[1, 2] + R[2, 1])**2 t12m = (R[0, 1] - R[1, 0])**2 t13m = (R[0, 2] - R[2, 0])**2 t23m = (R[1, 2] - R[2, 1])**2 d1 = (R[0, 0] + R[1, 1] + R[2, 2] + 1)**2 d2 = (R[0, 0] - R[1, 1] - R[2, 2] + 1)**2 d3 = (-R[0, 0] + R[1, 1] - R[2, 2] + 1)**2 d4 = (-R[0, 0] - R[1, 1] + R[2, 2] + 1)**2 e0 = math.sqrt(d1 + t23m + t13m + t12m) / 4.0 e1 = math.sqrt(t23m + d2 + t12p + t13p) / 4.0 e2 = math.sqrt(t13m + t12p + d3 + t23p) / 4.0 e3 = math.sqrt(t12m + t13p + t23p + d4) / 4.0 # transfer sign from rotation element differences if R[2, 1] < R[1, 2]: e1 = -e1 if R[0, 2] < R[2, 0]: e2 = -e2 if R[1, 0] < R[0, 1]: e3 = -e3 if order == 'sxyz': return np.r_[e0, e1, e2, e3] elif order == 'xyzs': return np.r_[e1, e2, e3, e0] else: raise ValueError("order is invalid, must be 'sxyz' or 'xyzs'")