def test_rotation(self):
        # rotation matrix to quaternion
        nt.assert_array_almost_equal(r2q(tr.rotx(180, 'deg')), np.r_[0, 1, 0,
                                                                     0])
        nt.assert_array_almost_equal(r2q(tr.roty(180, 'deg')), np.r_[0, 0, 1,
                                                                     0])
        nt.assert_array_almost_equal(r2q(tr.rotz(180, 'deg')), np.r_[0, 0, 0,
                                                                     1])

        # quaternion to rotation matrix
        nt.assert_array_almost_equal(q2r(np.r_[0, 1, 0, 0]),
                                     tr.rotx(180, 'deg'))
        nt.assert_array_almost_equal(q2r(np.r_[0, 0, 1, 0]),
                                     tr.roty(180, 'deg'))
        nt.assert_array_almost_equal(q2r(np.r_[0, 0, 0, 1]),
                                     tr.rotz(180, 'deg'))

        nt.assert_array_almost_equal(q2r([0, 1, 0, 0]), tr.rotx(180, 'deg'))
        nt.assert_array_almost_equal(q2r([0, 0, 1, 0]), tr.roty(180, 'deg'))
        nt.assert_array_almost_equal(q2r([0, 0, 0, 1]), tr.rotz(180, 'deg'))

        # quaternion - vector product
        nt.assert_array_almost_equal(qvmul(np.r_[0, 1, 0, 0], np.r_[0, 0, 1]),
                                     np.r_[0, 0, -1])
        nt.assert_array_almost_equal(qvmul([0, 1, 0, 0], [0, 0, 1]),
                                     np.r_[0, 0, -1])
Esempio n. 2
0
    def Rz(cls, theta, unit='rad'):
        """
        Construct a new SO(3) from Z-axis rotation

        :param θ: rotation angle about Z-axis
        :type θ: float or array_like
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: SO(3) rotation
        :rtype: SO3 instance

        - ``SO3.Rz(θ)`` is an SO(3) rotation of ``θ`` radians about the z-axis
        - ``SO3.Rz(θ, "deg")`` as above but ``θ`` is in degrees

        If ``θ`` is an array then the result is a sequence of rotations defined by consecutive
        elements.

        Example:

        .. runblock:: pycon

            >>> from spatialmath import SE3
            >>> x = SE3.Rz(np.linspace(0, math.pi, 20))
            >>> len(x)
            >>> x[7]

        """
        return cls([base.rotz(x, unit=unit) for x in base.getvector(theta)],
                   check=False)
Esempio n. 3
0
    def Rz(cls, theta, unit='rad'):
        """
        Construct a new SO(3) from Z-axis rotation

        :param theta: rotation angle about Z-axis
        :type theta: float or array_like
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: SO(3) rotation
        :rtype: SO3 instance
        
        - ``SO3.Rz(theta)`` is an SO(3) rotation of ``theta`` radians about the z-axis
        - ``SO3.Rz(theta, "deg")`` as above but ``theta`` is in degrees
        
        If ``theta`` is an array then the result is a sequence of rotations defined by consecutive
        elements.

        Example::
            
            >>> x = SE3.Rz(np.linspace(0, math.pi, 20))
            >>> len(x)
            20
            SO3(array([[ 0.40169542, -0.91577333,  0.        ],
                       [ 0.91577333,  0.40169542,  0.        ],
                       [ 0.        ,  0.        ,  1.        ]]))
        """
        return cls([tr.rotz(x, unit=unit) for x in argcheck.getvector(theta)],
                   check=False)
Esempio n. 4
0
 def Rz(cls, theta, unit='rad'):
     """
     Create SO(3) rotation about the Z-axis
 
     :param theta: rotation angle about Z-axis
     :type theta: float or array_like
     :param unit: angular units: 'rad' [default], or 'deg'
     :type unit: str
     :return: 3x3 rotation matrix
     :rtype: SO3 instance
 
     - ``SO3.Rz(THETA)`` is an SO(3) rotation of THETA radians about the z-axis
     - ``SO3.Rz(THETA, "deg")`` as above but THETA is in degrees
     """
     return cls([tr.rotz(x, unit=unit) for x in argcheck.getvector(theta)],
                check=False)
Esempio n. 5
0
    def Rz(cls, angle, unit='rad'):
        """
        Construct a UnitQuaternion object representing rotation about Z-axis
        
        :arg angle: rotation angle
        :type norm: float
        :arg unit: rotation unit 'rad' [default] or 'deg'
        :type unit: str
        :return: new unit-quaternion
        :rtype: UnitQuaternion

        - ``UnitQuaternion(theta)`` constructs a unit quaternion representing a 
          rotation of `theta` radians about the Z-axis.
        - ``UnitQuaternion(theta, 'deg')`` constructs a unit quaternion representing a 
          rotation of `theta` degrees about the Z-axis.

        """
        return cls(tr.rotz(angle, unit=unit), check=False)