def OA(cls, o, a): """ Create SO(3) rotation from two vectors :param o: 3-vector parallel to Y- axis :type o: array_like :param a: 3-vector parallel to the Z-axis :type o: array_like :return: 3x3 rotation matrix :rtype: SO3 instance ``SO3.OA(O, A)`` is an SO(3) rotation defined in terms of vectors parallel to the Y- and Z-axes of its reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A. Notes: - The A vector is the only guaranteed to have the same direction in the resulting rotation matrix - O and A do not have to be unit-length, they are normalized - O and A do not have to be orthogonal, so long as they are not parallel - The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame. :seealso: :func:`spatialmath.base.transforms3d.oa2r` """ return cls(quat.r2q(tr.oa2r(angles, unit=unit)), check=False)
def RPY(cls, angles, *, order='zyx', unit='rad'): """ Create an SO(3) rotation from roll-pitch-yaw angles :param angles: 3-vector of roll-pitch-yaw angles :type angles: array_like :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz' :type unit: str :return: 3x3 rotation matrix :rtype: SO3 instance ``SO3.RPY(ANGLES)`` is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles :math:`(r, p, y)` which correspond to successive rotations about the axes specified by ``order``: - 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis, then by roll about the new x-axis. Convention for a mobile robot with x-axis forward and y-axis sideways. - 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis, then by roll about the new z-axis. Covention for a robot gripper with z-axis forward and y-axis between the gripper fingers. - 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis, then by roll about the new z-axis. Convention for a camera with z-axis parallel to the optic axis and x-axis parallel to the pixel rows. :seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r` """ return cls(quat.r2q(tr.rpy2r(angles, unit=unit, order=order)), check=False)
def Eul(cls, angles, *, unit='rad'): """ Create an SO(3) rotation from Euler angles :param angles: 3-vector of Euler angles :type angles: array_like :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 3x3 rotation matrix :rtype: SO3 instance ``SO3.Eul(ANGLES)`` is an SO(3) rotation defined by a 3-vector of Euler angles :math:`(\phi, \theta, \psi)` which correspond to consecutive rotations about the Z, Y, Z axes respectively. :seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`spatialmath.base.transforms3d.eul2r` """ return cls(quat.r2q(tr.eul2r(angles, unit=unit)), check=False)
def AngVec(cls, theta, v, *, unit='rad'): """ Create an SO(3) rotation matrix from rotation angle and axis :param theta: rotation :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :param v: rotation axis, 3-vector :type v: array_like :return: 3x3 rotation matrix :rtype: SO3 instance ``SO3.AngVec(THETA, V)`` is an SO(3) rotation defined by a rotation of ``THETA`` about the vector ``V``. Notes: - If ``THETA == 0`` then return identity matrix. - If ``THETA ~= 0`` then ``V`` must have a finite length. :seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`spatialmath.base.transforms3d.angvec2r` """ return cls(quat.r2q(tr.angvec2r(theta, v, unit=unit)), check=False)
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 Omega(cls, w): return cls(quat.r2q(tr.angvec2r(tr.norm(w), tr.unitvec(w))), check=False)
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')