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