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)
