8000 test for dual quaternion · zfxu/spatialmath-python@4bdb4b4 · GitHub
[go: up one dir, main page]
Skip to content

Commit 4bdb4b4

Browse files
petercorkepetercorke
committed
test for dual quaternion
1 parent 37e1f8b commit 4bdb4b4

File tree

2 files changed

+121
-18
lines changed

2 files changed

+121
-18
lines changed

‎spatialmath/DualQuaternion.py

Lines changed: 5 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -113,8 +113,8 @@ def norm(self):
113113
>>> d.norm() # norm is a dual number
114114
"""
115115
a = self.real * self.real.conj()
116-
b= self.real * self.dual.conj() + self.dual * self.real.conj()
117-
return (a.s, b.s)
116+
b = self.real * self.dual.conj() + self.dual * self.real.conj()
117+
return (base.sqrt(a.s), base.sqrt(b.s))
118118

119119
def 10000 conj(self):
120120
r"""
@@ -345,21 +345,8 @@ def SE3(self):
345345
# v = self.dual.v
346346
# theta = base.norm(w)
347347

348-
if __name__ == "__main__":
348+
if __name__ == "__main__": # pragma: no cover
349349

350-
a = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
351-
print(a)
352-
print(a.vec)
353-
print(a.conj())
354-
print(a.norm())
355-
print(a+a)
356-
print(a*a)
357-
print(a.matrix())
350+
import pathlib
358351

359-
T = SE3.Rand()
360-
print(T)
361-
362-
aa = UnitDualQuaternion(T)
363-
print(aa)
364-
print(aa.norm())
365-
print(aa.T())
352+
exec(open(pathlib.Path(__file__).parent.parent.absolute() / "tests" / "test_dualquaternion.py").read()) # pylint: disable=exec-used

‎tests/test_dualquaternion.py

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
import math
2+
from math import pi
3+
import numpy as np
4+
5+
import numpy.testing as nt
6+
import unittest
7+
8+
from spatialmath import DualQuaternion, UnitDualQuaternion, Quaternion, SE3
9+
from spatialmath import base
10+
11+
12+
def qcompare(x, y):
13+
if isinstance(x, Quaternion):
14+
x = x.vec
15+
elif isinstance(x, SMPose):
16+
x = x.A
17+
if isinstance(y, Quaternion):
18+
y = y.vec
19+
elif isinstance(y, SMPose):
20+
y = y.A
21+
nt.assert_array_almost_equal(x, y)
22+
23+
class TestDualQuaternion(unittest.TestCase):
24+
25+
def test_init(self):
26+
27+
dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8]))
28+
nt.assert_array_almost_equal(dq.vec, np.r_[1,2,3,4,5,6,7,8])
29+
30+
dq = DualQuaternion([1.,2,3,4,5,6,7,8])
31+
nt.assert_array_almost_equal(dq.vec, np.r_[1,2,3,4,5,6,7,8])
32+
dq = DualQuaternion(np.r_[1,2,3,4,5,6,7,8])
33+
nt.assert_array_almost_equal(dq.vec, np.r_[1,2,3,4,5,6,7,8])
34+
35+
def test_pure(self):
36+
37+
dq = DualQuaternion.Pure([1.,2,3])
38+
nt.assert_array_almost_equal(dq.vec, np.r_[1,0,0,0, 0,1,2,3])
39+
40+
def test_strings(self):
41+
42+
dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8]))
43+
self.assertIsInstance(str(dq), str)
44+
self.assertIsInstance(repr(dq), str)
45+
46+
def test_conj(self):
47+
dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8]))
48+
nt.assert_array_almost_equal(dq.conj().vec, np.r_[1,-2,-3,-4, 5,-6,-7,-8])
49+
50+
# def test_norm(self):
51+
# q1 = Quaternion([1.,2,3,4])
52+
# q2 = Quaternion([5.,6,7,8])
53+
54+
# dq = DualQuaternion(q1, q2)
55+
# nt.assert_array_almost_equal(dq.norm(), (q1.norm(), q2.norm()))
56+
57+
def test_plus(self):
58+
dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8]))
59+
s = dq + dq
60+
nt.assert_array_almost_equal(s.vec, 2*np.r_[1,2,3,4,5,6,7,8])
61+
62+
def test_minus(self):
63+
dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8]))
64+
s = dq - dq
65+
nt.assert_array_almost_equal(s.vec, np.zeros((8,)))
66+
67+
def test_matrix(self):
68+
69+
dq1 = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8]))
70+
71+
M = dq1.matrix()
72+
self.assertIsInstance(M, np.ndarray)
73+
self.assertEqual(M.shape, (8,8))
74+
75+
def test_multiply(self):
76+
dq1 = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8]))
77+
dq2 = DualQuaternion(Quaternion([4,3,2,1]), Quaternion([5,6,7,8]))
78+
79+
M = dq1.matrix()
80+
v = dq2.vec
81+
nt.assert_array_almost_equal(M @ v, (dq1 * dq2).vec)
82+
83+
def test_unit(self):
84+
pass
85+
86+
87+
class TestUnitDualQuaternion(unittest.TestCase):
88+
89+
def test_init(self):
90+
91+
T = SE3.Rx(pi/4)
92+
dq = UnitDualQuaternion(T)
93+
nt.assert_array_almost_equal(dq.SE3().A, T.A)
94+
95+
def test_norm(self):
96+
T = SE3.Rx(pi/4)
97+
dq = UnitDualQuaternion(T)
98+
nt.assert_array_almost_equal(dq.norm(), (1,0))
99+
100+
def test_multiply(self):
101+
T1 = SE3.Rx(pi/4)
102+
T2 = SE3.Rz(-pi/3)
103+
104+
T = T1 * T2
105+
106+
d1 = UnitDualQuaternion(T1)
107+
d2 = UnitDualQuaternion(T2)
108+
109+
d = d1 * d2
110+
nt.assert_array_almost_equal(d.SE3().A, T.A)
111+
112+
113+
# ---------------------------------------------------------------------------------------#
114+
if __name__ == '__main__': # pragma: no cover
115+
116+
unittest.main()

0 commit comments

Comments
 (0)
0