Skip to content

Commit facfb53

Browse files
committed
unit quaternion has positive scalar part
1 parent b105a4c commit facfb53

3 files changed

Lines changed: 31 additions & 6 deletions

File tree

‎spatialmath/base/quaternions.py‎

Lines changed: 27 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,21 @@ def pure(v):
5353
v = base.getvector(v, 3)
5454
return np.r_[0, v]
5555

56+
def qpositive(q):
57+
"""
58+
Quaternion with positive scalar part
59+
60+
:arg q: quaternion
61+
:type v: : ndarray(4)
62+
:return: pure quaternion
63+
:rtype: ndarray(4)
64+
65+
If the scalar part is negative return -q.
66+
"""
67+
if q[0] < 0:
68+
return -q
69+
else:
70+
return q
5671

5772
def qnorm(q):
5873
r"""
@@ -97,6 +112,8 @@ def unit(q, tol=10):
97112
>>> q = unit([1, 2, 3, 4])
98113
>>> qprint(q)
99114
115+
.. note:: Scalar part is always positive.
116+
100117
.. note:: If the quaternion norm is less than ``tol * eps`` an exception is
101118
raised.
102119
@@ -106,7 +123,13 @@ def unit(q, tol=10):
106123
nm = np.linalg.norm(q)
107124
if abs(nm) < tol * _eps:
108125
raise ValueError("cannot normalize (near) zero length quaternion")
109-
return q / nm
126+
else:
127+
q /= nm
128+
129+
if q[0] >= 0:
130+
return q
131+
else:
132+
return -q
110133

111134

112135
def isunit(q, tol=100):
@@ -495,12 +518,14 @@ def r2q(R, check=False, tol=100):
495518
496519
.. warning:: There is no check that the passed matrix is a valid rotation matrix.
497520
521+
.. note:: Scalar part is always positive.
522+
498523
:seealso: :func:`q2r`
499524
"""
500525
if not base.isrot(R, check=check, tol=tol):
501526
raise ValueError("Argument must be a valid SO(3) matrix")
502527

503-
qs = math.sqrt(max(0, np.trace(R) + 1)) / 2.0
528+
qs = math.sqrt(max(0, np.trace(R) + 1)) / 2.0 # scalar part
504529
kx = R[2, 1] - R[1, 2] # Oz - Ay
505530
ky = R[0, 2] - R[2, 0] # Ax - Nz
506531
kz = R[1, 0] - R[0, 1] # Ny - Ox

‎spatialmath/quaternion.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -983,9 +983,9 @@ def __init__(self, s: Any = None, v=None, norm=True, check=True):
983983

984984
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
985985
if norm:
986-
self.data = [base.qnorm(x) for x in s]
986+
self.data = [base.unit(x) for x in s]
987987
else:
988-
self.data = [x for x in s]
988+
self.data = [base.qpositive(x) for x in s]
989989

990990
elif isinstance(s, SO3):
991991
# UnitQuaternion(x) x is SO3 or SE3

‎tests/test_quaternion.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ def test_constructor(self):
4848
qcompare(UnitQuaternion([0, 0, 0, 1]), np.r_[0, 0, 0, 1])
4949

5050
qcompare(UnitQuaternion([2, 0, 0, 0]), np.r_[1, 0, 0, 0])
51-
qcompare(UnitQuaternion([-2, 0, 0, 0]), np.r_[-1, 0, 0, 0])
51+
qcompare(UnitQuaternion([-2, 0, 0, 0]), np.r_[1, 0, 0, 0])
5252

5353
# from [S,V]
5454
qcompare(UnitQuaternion(1, [0, 0, 0]), np.r_[1, 0, 0, 0])
@@ -57,7 +57,7 @@ def test_constructor(self):
5757
qcompare(UnitQuaternion(0, [0, 0, 1]), np.r_[0, 0, 0, 1])
5858

5959
qcompare(UnitQuaternion(2, [0, 0, 0]), np.r_[1, 0, 0, 0])
60-
qcompare(UnitQuaternion(-2, [0, 0, 0]), np.r_[-1, 0, 0, 0])
60+
qcompare(UnitQuaternion(-2, [0, 0, 0]), np.r_[1, 0, 0, 0])
6161

6262
# from R
6363

0 commit comments

Comments
 (0)