Skip to content

Commit 6940542

Browse files
committed
Use the scalar zero tolerance for angle-axis quaternions
1 parent 54fb3c8 commit 6940542

2 files changed

Lines changed: 34 additions & 3 deletions

File tree

‎spatialmath/quaternion.py‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1544,14 +1544,15 @@ def AngVec(
15441544
>>> print(UQ.AngVec(0, [1,0,0]))
15451545
>>> print(UQ.AngVec(90, [1,0,0], unit='deg'))
15461546
1547-
.. note:: :math:`\theta = 0` the result in an identity quaternion, otherwise
1548-
``V`` must have a finite length, ie. :math:`|V| > 0`.
1547+
.. note:: If :math:`\theta` is zero within the default tolerance of
1548+
:func:`~spatialmath.base.vectors.iszero`, the result is an identity
1549+
quaternion. Otherwise, ``v`` must have a nonzero norm.
15491550
15501551
:seealso: :meth:`UnitQuaternion.angvec` :meth:`UnitQuaternion.exp` :func:`~spatialmath.base.transforms3d.angvec2r`
15511552
"""
15521553
v = smb.getvector(v, 3)
15531554
theta = smb.getunit(theta, unit, vector=False)
1554-
if theta == 0:
1555+
if smb.iszero(theta):
15551556
return cls()
15561557
v = smb.unitvec(v)
15571558
return cls(

‎tests/test_angvec_nonunit_axis.py‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,3 +50,33 @@ def test_zero_degrees_zero_axis(self):
5050
def test_nonzero_angle_requires_nonzero_axis(self):
5151
with self.assertRaises(ValueError):
5252
UnitQuaternion.AngVec(0.5, [0, 0, 0])
53+
54+
def test_near_zero_angle_zero_axis(self):
55+
for theta in [1e-20, -1e-20]:
56+
with self.subTest(theta=theta):
57+
assert_allclose(
58+
UnitQuaternion.AngVec(theta, [0, 0, 0]).vec, [1, 0, 0, 0]
59+
)
60+
61+
def test_near_zero_degrees_zero_axis(self):
62+
for theta in [1e-20, -1e-20]:
63+
with self.subTest(theta=theta):
64+
assert_allclose(
65+
UnitQuaternion.AngVec(theta, [0, 0, 0], unit="deg").vec,
66+
[1, 0, 0, 0],
67+
)
68+
69+
def test_nonzero_small_angle_requires_nonzero_axis(self):
70+
for theta in [1e-10, -1e-10]:
71+
with self.subTest(theta=theta):
72+
with self.assertRaises(ValueError):
73+
UnitQuaternion.AngVec(theta, [0, 0, 0])
74+
75+
def test_nonzero_small_angle_preserves_rotation(self):
76+
for theta in [1e-10, -1e-10]:
77+
with self.subTest(theta=theta):
78+
assert_allclose(
79+
UnitQuaternion.AngVec(theta, [2, 0, 0]).vec,
80+
[math.cos(theta / 2), math.sin(theta / 2), 0, 0],
81+
atol=0,
82+
)

0 commit comments

Comments
 (0)