Skip to content

Commit de79bb3

Browse files
committed
change to UnitQuaternion constructor,
no longer takes an SE(3) matrix, ambiguous wrt a stack of quaternions
1 parent f05de6f commit de79bb3

3 files changed

Lines changed: 48 additions & 22 deletions

File tree

‎spatialmath/base/quaternions.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -130,6 +130,7 @@ def unit(q, tol=10):
130130
return q
131131
else:
132132
return -q
133+
# return q
133134

134135

135136
def isunit(q, tol=100):

‎spatialmath/quaternion.py‎

Lines changed: 30 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -977,22 +977,34 @@ def __init__(self, s: Any = None, v=None, norm=True, check=True):
977977
# UnitQuaternion(R) R is 3x3 rotation matrix
978978
self.data = [base.r2q(s)]
979979

980-
elif isinstance(s, np.ndarray) and base.ishom(s, check=check):
981-
# UnitQuaternion(T) T is 4x4 homogeneous transformation matrix
982-
self.data = [base.r2q(base.t2r(s))]
983-
984-
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
985-
if norm:
986-
self.data = [base.unit(x) for x in s]
987-
else:
988-
self.data = [base.qpositive(x) for x in s]
980+
elif isinstance(s, np.ndarray):
981+
# passed a NumPy array, it could be:
982+
# an SO(3) or SE(3) matrix
983+
# a quaternion as a 1D array
984+
# an array of quaternions as an nx4 array
985+
986+
if base.isrot(s, check=check):
987+
# UnitQuaternion(R) R is 3x3 rotation matrix
988+
self.data = [base.r2q(s)]
989+
elif s.shape == (4,):
990+
# passed a 4-vector
991+
if norm:
992+
self.data = [base.unit(s)]
993+
else:
994+
self.data = [s]
995+
elif s.ndim == 2 and s.shape[1] == 4:
996+
if norm:
997+
self.data = [base.unit(x) for x in s]
998+
else:
999+
# self.data = [base.qpositive(x) for x in s]
1000+
self.data = [x for x in s]
9891001

9901002
elif isinstance(s, SO3):
991-
# UnitQuaternion(x) x is SO3 or SE3
1003+
# UnitQuaternion(x) x is SO3 or SE3 (since SE3 is subclass of SO3)
9921004
self.data = [base.r2q(x.R) for x in s]
9931005

9941006
elif isinstance(s[0], SO3):
995-
# list of SO3/SE3
1007+
# list of SO3 or SE3
9961008
self.data = [base.r2q(x.R) for x in s]
9971009

9981010
else:
@@ -2258,3 +2270,10 @@ def SE3(self):
22582270
a = UnitQuaternion([0, 1, 0, 0])
22592271

22602272
exec(open(pathlib.Path(__file__).parent.parent.absolute() / "tests" / "test_quaternion.py").read()) # pylint: disable=exec-used
2273+
2274+
2275+
2276+
2277+
2278+
2279+

‎tests/test_quaternion.py‎

Lines changed: 17 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -92,20 +92,26 @@ def test_constructor(self):
9292
qcompare(UnitQuaternion(SO3.Rz(pi)), np.r_[0, 0, 0, 1])
9393

9494
# vector of SO3
95-
qcompare(UnitQuaternion([SO3.Rx(pi / 2), SO3.Ry(pi / 2), SO3.Rz(pi / 2)]), np.array([[1, 1, 0, 0], [1, 0, 1, 0], [1, 0, 0, 1]]) / math.sqrt(2))
95+
qcompare(UnitQuaternion([SO3.Rx(pi / 2), SO3.Ry(pi / 2), SE3.Rz(pi / 2)]), np.array([[1, 1, 0, 0], [1, 0, 1, 0], [1, 0, 0, 1]]) / math.sqrt(2))
9696

97-
# from T
98-
qcompare(UnitQuaternion(trotx(pi / 2)), np.r_[1, 1, 0, 0] / math.sqrt(2))
99-
qcompare(UnitQuaternion(troty(pi / 2)), np.r_[1, 0, 1, 0] / math.sqrt(2))
100-
qcompare(UnitQuaternion(trotz(pi / 2)), np.r_[1, 0, 0, 1] / math.sqrt(2))
97+
# from SE3
10198

102-
qcompare(UnitQuaternion(trotx(-pi / 2)), np.r_[1, -1, 0, 0] / math.sqrt(2))
103-
qcompare(UnitQuaternion(troty(-pi / 2)), np.r_[1, 0, -1, 0] / math.sqrt(2))
104-
qcompare(UnitQuaternion(trotz(-pi / 2)), np.r_[1, 0, 0, -1] / math.sqrt(2))
99+
qcompare(UnitQuaternion(SE3()), np.r_[1, 0, 0, 0])
105100

106-
qcompare(UnitQuaternion(trotx(pi)), np.r_[0, 1, 0, 0])
107-
qcompare(UnitQuaternion(troty(pi)), np.r_[0, 0, 1, 0])
108-
qcompare(UnitQuaternion(trotz(pi)).vec, np.r_[0, 0, 0, 1])
101+
qcompare(UnitQuaternion(SE3.Rx(pi / 2)), np.r_[1, 1, 0, 0] / math.sqrt(2))
102+
qcompare(UnitQuaternion(SE3.Ry(pi / 2)), np.r_[1, 0, 1, 0] / math.sqrt(2))
103+
qcompare(UnitQuaternion(SE3.Rz(pi / 2)), np.r_[1, 0, 0, 1] / math.sqrt(2))
104+
105+
qcompare(UnitQuaternion(SE3.Rx(-pi / 2)), np.r_[1, -1, 0, 0] / math.sqrt(2))
106+
qcompare(UnitQuaternion(SE3.Ry(-pi / 2)), np.r_[1, 0, -1, 0] / math.sqrt(2))
107+
qcompare(UnitQuaternion(SE3.Rz(-pi / 2)), np.r_[1, 0, 0, -1] / math.sqrt(2))
108+
109+
qcompare(UnitQuaternion(SE3.Rx(pi)), np.r_[0, 1, 0, 0])
110+
qcompare(UnitQuaternion(SE3.Ry(pi)), np.r_[0, 0, 1, 0])
111+
qcompare(UnitQuaternion(SE3.Rz(pi)), np.r_[0, 0, 0, 1])
112+
113+
# vector of SE3
114+
qcompare(UnitQuaternion([SE3.Rx(pi / 2), SE3.Ry(pi / 2), SE3.Rz(pi / 2)]), np.array([[1, 1, 0, 0], [1, 0, 1, 0], [1, 0, 0, 1]]) / math.sqrt(2))
109115

110116
# # vectorised forms of R, T
111117
# R = []; T = []

0 commit comments

Comments
 (0)