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])
def Rx(cls, theta, unit='rad'): """ Construct a new SO(3) from X-axis rotation :param θ: rotation angle about the X-axis :type θ: float or array_like :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: SO(3) rotation :rtype: SO3 instance - ``SE3.Rx(θ)`` is an SO(3) rotation of ``θ`` radians about the x-axis - ``SE3.Rx(θ, "deg")`` as above but ``θ`` is in degrees If ``theta`` is an array then the result is a sequence of rotations defined by consecutive elements. Example: .. runblock:: pycon >>> from spatialmath import SO3 >>> x = SO3.Rx(np.linspace(0, math.pi, 20)) >>> len(x) >>> x[7] """ return cls([base.rotx(x, unit=unit) for x in base.getvector(theta)], check=False)
def Rx(cls, theta, unit='rad'): """ Construct a new SO(3) from X-axis rotation :param theta: rotation angle about the X-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 - ``SE3.Rx(theta)`` is an SO(3) rotation of ``theta`` radians about the x-axis - ``SE3.Rx(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 = SO3.Rx(np.linspace(0, math.pi, 20)) >>> len(x) 20 >>> x[7] SO3(array([[ 1. , 0. , 0. ], [ 0. , 0.40169542, -0.91577333], [ 0. , 0.91577333, 0.40169542]])) """ return cls([tr.rotx(x, unit=unit) for x in argcheck.getvector(theta)], check=False)
def test_slerp(self): q1 = np.r_[0, 1, 0, 0] q2 = np.r_[0, 0, 1, 0] nt.assert_array_almost_equal(slerp(q1, q2, 0), q1) nt.assert_array_almost_equal(slerp(q1, q2, 1), q2) nt.assert_array_almost_equal(slerp(q1, q2, 0.5), np.r_[0, 1, 1, 0] / math.sqrt(2)) q1 = [0, 1, 0, 0] q2 = [0, 0, 1, 0] nt.assert_array_almost_equal(slerp(q1, q2, 0), q1) nt.assert_array_almost_equal(slerp(q1, q2, 1), q2) nt.assert_array_almost_equal(slerp(q1, q2, 0.5), np.r_[0, 1, 1, 0] / math.sqrt(2)) nt.assert_array_almost_equal( slerp(r2q(tr.rotx(-0.3)), r2q(tr.rotx(0.3)), 0.5), np.r_[1, 0, 0, 0]) nt.assert_array_almost_equal( slerp(r2q(tr.roty(0.3)), r2q(tr.roty(0.5)), 0.5), r2q(tr.roty(0.4)))
def Rx(cls, theta, unit='rad'): """ Create SO(3) rotation about X-axis :param theta: rotation angle about the X-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 - ``SE3.Rx(THETA)`` is an SO(3) rotation of THETA radians about the x-axis - ``SE3.Rx(THETA, "deg")`` as above but THETA is in degrees """ return cls([tr.rotx(x, unit=unit) for x in argcheck.getvector(theta)], check=False)
def Rx(cls, angle, unit='rad'): """ Construct a UnitQuaternion object representing rotation about X-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 X-axis. - ``UnitQuaternion(theta, 'deg')`` constructs a unit quaternion representing a rotation of `theta` degrees about the X-axis. """ return cls(tr.rotx(angle, unit=unit), check=False)