Skip to content

Commit d4e9d47

Browse files
petercorkepetercorke
committed
polish
more unit tests tidy up doco add autorun examples
1 parent 291f337 commit d4e9d47

4 files changed

Lines changed: 544 additions & 140 deletions

File tree

‎spatialmath/base/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,7 @@
102102
'e2h',
103103
'homtrans',
104104
# spatialmath.base.vectors
105+
'colvec',
105106
'unitvec',
106107
'norm',
107108
'isunitvec',

‎spatialmath/base/test_transforms.py‎

Lines changed: 287 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@
4848
from scipy.linalg import logm, expm
4949

5050
from spatialmath.base import *
51+
from spatialmath.base import sym
5152

5253
import matplotlib.pyplot as plt
5354

@@ -80,6 +81,13 @@ def test_unit(self):
8081
self.assertIsNone(unitvec([0]))
8182
self.assertIsNone(unitvec(0))
8283

84+
def test_colvec(self):
85+
86+
t = np.r_[1,2,3]
87+
cv = colvec(t)
88+
self.assertEqual(cv.shape, (3,1))
89+
nt.assert_array_almost_equal(cv.flatten(), t)
90+
8391
def test_isunitvec(self):
8492
self.assertTrue(isunitvec([1, 0, 0]))
8593
self.assertTrue(isunitvec((1, 0, 0)))
@@ -104,6 +112,7 @@ def test_norm(self):
104112
nt.assert_array_almost_equal(norm([1, 2, 3]), math.sqrt(14))
105113

106114
def test_isunittwist(self):
115+
# 3D
107116
# unit rotational twist
108117
self.assertTrue(isunittwist([1, 2, 3, 1, 0, 0]))
109118
self.assertTrue(isunittwist((1, 2, 3, 1, 0, 0)))
@@ -118,6 +127,25 @@ def test_isunittwist(self):
118127
# not a unit translation twist
119128
self.assertFalse(isunittwist([2, 0, 0, 0, 0, 0]))
120129

130+
# 2D
131+
# unit rotational twist
132+
self.assertTrue(isunittwist2([1, 2, 1]))
133+
134+
# not a unit rotational twist
135+
self.assertFalse(isunittwist2([1, 2, 3]))
136+
137+
# unit translation twist
138+
self.assertTrue(isunittwist2([1, 0, 0]))
139+
140+
# not a unit translation twist
141+
self.assertFalse(isunittwist2([2, 0, 0]))
142+
143+
with self.assertRaises(ValueError):
144+
isunittwist([3,4])
145+
146+
with self.assertRaises(ValueError):
147+
isunittwist2([3,4])
148+
121149
def test_unittwist(self):
122150
nt.assert_array_almost_equal(unittwist([0, 0, 0, 1, 0, 0]), np.r_[0, 0, 0, 1, 0, 0])
123151
nt.assert_array_almost_equal(unittwist([0, 0, 0, 0, 2, 0]), np.r_[0, 0, 0, 0, 1, 0])
@@ -182,6 +210,29 @@ def test_iszerovec(self):
182210
self.assertFalse(iszerovec([0, 1]), False)
183211
self.assertFalse(iszerovec([0, 1, 0]), False)
184212

213+
def test_angdiff(self):
214+
215+
self.assertEqual(angdiff(0, 0), 0)
216+
self.assertEqual(angdiff(np.pi, 0), -np.pi)
217+
self.assertEqual(angdiff(-np.pi, np.pi), 0)
218+
219+
def test_removesmall(self):
220+
221+
v = np.r_[1, 2, 3]
222+
nt.assert_array_almost_equal(removesmall(v), v)
223+
224+
v = np.r_[1, 2, 3, 1e-6, -1e-6]
225+
nt.assert_array_almost_equal(removesmall(v), v)
226+
227+
v = np.r_[1, 2, 3, 1e-15, -1e-15]
228+
nt.assert_array_almost_equal(removesmall(v), [1,2,3,0, 0])
229+
230+
v = np.r_[1, 2, 3, 1e-10, -1e-10]
231+
nt.assert_array_almost_equal(removesmall(v), [1,2,3,1e-10,-1e-10])
232+
233+
v = np.r_[1, 2, 3, 1e-10, -1e-10]
234+
nt.assert_array_almost_equal(removesmall(v, tol=1e8), [1,2,3,0,0])
235+
185236

186237
class TestND(unittest.TestCase):
187238
def test_iseye(self):
@@ -195,16 +246,109 @@ def test_iseye(self):
195246
self.assertFalse(iseye(np.array([[1, 0, 0], [0, 1, 0]])))
196247
self.assertFalse(iseye(np.array([1, 0, 0])))
197248

198-
def test_Rt(self):
199-
nt.assert_array_almost_equal(rotx(0.3), t2r(trotx(0.3)))
200-
nt.assert_array_almost_equal(trotx(0.3), r2t(rotx(0.3)))
249+
def test_r2t(self):
250+
# 3D
251+
R = rotx(0.3)
252+
T = r2t(R)
253+
nt.assert_array_almost_equal(T[0:3,3], np.r_[0,0,0])
254+
nt.assert_array_almost_equal(T[:3,:3], R)
255+
256+
theta = sym.symbol('theta')
257+
R = rotx(theta)
258+
T = r2t(R)
259+
self.assertEqual(r2t(R).dtype, 'O')
260+
nt.assert_array_almost_equal(T[0:3,3], np.r_[0,0,0])
261+
# nt.assert_array_almost_equal(T[:3,:3], R)
262+
self.assertTrue((T[:3,:3] == R).all())
263+
264+
# 2D
265+
R = rot2(0.3)
266+
T = r2t(R)
267+
nt.assert_array_almost_equal(T[0:2,2], np.r_[0,0])
268+
nt.assert_array_almost_equal(T[:2,:2], R)
269+
270+
theta = sym.symbol('theta')
271+
R = rot2(theta)
272+
T = r2t(R)
273+
self.assertEqual(r2t(R).dtype, 'O')
274+
nt.assert_array_almost_equal(T[0:2,2], np.r_[0,0])
275+
nt.assert_array_almost_equal(T[:2,:2], R)
276+
277+
with self.assertRaises(ValueError):
278+
r2t(3)
279+
280+
with self.assertRaises(ValueError):
281+
r2t(np.eye(3,4))
282+
283+
def test_t2r(self):
284+
# 3D
285+
t=[1,2,3]
286+
T = trotx(0.3, t=t)
287+
R = t2r(T)
288+
nt.assert_array_almost_equal(T[:3,:3], R)
289+
nt.assert_array_almost_equal(transl(T), np.array(t))
290+
291+
# 2D
292+
t=[1,2]
293+
T = trot2(0.3, t=t)
294+
R = t2r(T)
295+
nt.assert_array_almost_equal(T[:2,:2], R)
296+
nt.assert_array_almost_equal(transl2(T), np.array(t))
297+
298+
with self.assertRaises(ValueError):
299+
t2r(3)
201300

301+
with self.assertRaises(ValueError):
302+
r2t(np.eye(3,4))
303+
304+
def test_rt2tr(self):
305+
# 3D
202306
R = rotx(0.2)
203307
t = [3, 4, 5]
204308
T = rt2tr(R, t)
205309
nt.assert_array_almost_equal(t2r(T), R)
206310
nt.assert_array_almost_equal(transl(T), np.array(t))
207311

312+
theta = sym.symbol('theta')
313+
R = rotx(theta)
314+
self.assertEqual(r2t(R).dtype, 'O')
315+
316+
# 2D
317+
R = rot2(0.2)
318+
t = [3, 4]
319+
T = rt2tr(R, t)
320+
nt.assert_array_almost_equal(t2r(T), R)
321+
nt.assert_array_almost_equal(transl2(T), np.array(t))
322+
323+
theta = sym.symbol('theta')
324+
R = rot2(theta)
325+
self.assertEqual(r2t(R).dtype, 'O')
326+
327+
with self.assertRaises(ValueError):
328+
rt2tr(3, 4)
329+
330+
with self.assertRaises(ValueError):
331+
rt2tr(np.eye(3,4), [1,2,3,4])
332+
333+
def test_tr2rt(self):
334+
# 3D
335+
T = trotx(0.3, t=[1,2,3])
336+
R, t = tr2rt(T)
337+
nt.assert_array_almost_equal(T[:3,:3], R)
338+
nt.assert_array_almost_equal(T[:3,3], t)
339+
340+
# 2D
341+
T = trot2(0.3, t=[1,2])
342+
R, t = tr2rt(T)
343+
nt.assert_array_almost_equal(T[:2,:2], R)
344+
nt.assert_array_almost_equal(T[:2,2], t)
345+
346+
with self.assertRaises(ValueError):
347+
R, t = tr2rt(3)
348+
349+
with self.assertRaises(ValueError):
350+
R, t = tr2rt(np.eye(3,4))
351+
208352
def test_checks(self):
209353

210354
# 3D case, with rotation matrix
@@ -284,23 +428,152 @@ def test_checks(self):
284428
S[0, 0] = 1
285429
nt.assert_equal(isskew(S), False)
286430

287-
# augmented skew matrices
288-
S = np.array([
289-
[0, 2, 3],
290-
[-2, 0, 4],
291-
[0, 0, 0]])
292-
nt.assert_equal(isskewa(S), True)
293-
S[0, 0] = 1
294-
nt.assert_equal(isskew(S), False)
295-
S[0, 0] = 0
296-
S[2, 0] = 1
297-
nt.assert_equal(isskew(S), False)
298431

299432
def test_homog(self):
300433
nt.assert_almost_equal(e2h([1, 2, 3]), np.r_[1, 2, 3, 1])
301434

302435
nt.assert_almost_equal(h2e([2, 4, 6, 2]), np.r_[1, 2, 3])
303436

437+
def test_homtrans(self):
438+
439+
#3D
440+
T = trotx(pi/2, t=[1,2,3])
441+
v = [10,12,14]
442+
v2 = homtrans(T, v)
443+
nt.assert_almost_equal(v2, [11, -12, 15])
444+
v = np.c_[[10,12,14], [-3,-4,-5]]
445+
v2 = homtrans(T, v)
446+
nt.assert_almost_equal(v2, np.c_[[11, -12, 15], [-2,7,-1]])
447+
448+
T = trotx(pi/2, t=[1,2,3])
449+
v = [10,12,14,1]
450+
v2 = homtrans(T, v)
451+
nt.assert_almost_equal(v2, [11, -12, 15,1])
452+
v = np.c_[[10,12,14,1], [-3,-4,-5,1]]
453+
v2 = homtrans(T, v)
454+
nt.assert_almost_equal(v2, np.c_[[11, -12, 15,1], [-2,7,-1,1]])
455+
456+
#2D
457+
T = trot2(pi/2, t=[1,2])
458+
v = [10,12]
459+
v2 = homtrans(T, v)
460+
nt.assert_almost_equal(v2, [-11, 12])
461+
v = np.c_[[10,12], [-3,-4]]
462+
v2 = homtrans(T, v)
463+
nt.assert_almost_equal(v2, np.c_[[-11, 12], [5, -1]])
464+
465+
T = trot2(pi/2, t=[1,2])
466+
v = [10,12,1]
467+
v2 = homtrans(T, v)
468+
nt.assert_almost_equal(v2, [-11, 12, 1])
469+
v = [20,24,2]
470+
v2 = homtrans(T, v)
471+
nt.assert_almost_equal(v2, [-22, 24, 2])
472+
v = np.c_[[10,12, 1], [-3,-4, 1]]
473+
v2 = homtrans(T, v)
474+
nt.assert_almost_equal(v2, np.c_[[-11, 12, 1], [5, -1, 1]])
475+
476+
with self.assertRaises(ValueError):
477+
T = trotx(pi/2, t=[1,2,3])
478+
v = [10,12]
479+
v2 = homtrans(T, v)
480+
481+
def test_skew(self):
482+
# 3D
483+
sk = skew([1, 2, 3])
484+
self.assertEqual(sk.shape, (3,3))
485+
nt.assert_almost_equal(sk + sk.T, np.zeros((3,3)))
486+
self.assertEqual(sk[2,1], 1)
487+
self.assertEqual(sk[0,2], 2)
488+
self.assertEqual(sk[1,0], 3)
489+
nt.assert_almost_equal(sk.diagonal(), np.r_[0,0,0])
490+
491+
# 2D
492+
sk = skew([1])
493+
self.assertEqual(sk.shape, (2,2))
494+
nt.assert_almost_equal(sk + sk.T, np.zeros((2,2)))
495+
self.assertEqual(sk[1,0], 1)
496+
nt.assert_almost_equal(sk.diagonal(), np.r_[0,0])
497+
498+
with self.assertRaises(ValueError):
499+
sk = skew([1,2])
500+
501+
def test_vex(self):
502+
# 3D
503+
t = [3, 4, 5]
504+
sk = skew(t)
505+
nt.assert_almost_equal(vex(sk), t)
506+
507+
# 2D
508+
t = [3]
509+
sk = skew(t)
510+
nt.assert_almost_equal(vex(sk), t)
511+
512+
def test_isskew(self):
513+
t = [3, 4, 5]
514+
sk = skew(t)
515+
self.assertTrue(isskew(sk))
516+
sk[0,0] = 3
517+
self.assertFalse(isskew(sk))
518+
519+
# 2D
520+
t = [3]
521+
sk = skew(t)
522+
self.assertTrue(isskew(sk))
523+
sk[0,0] = 3
524+
self.assertFalse(isskew(sk))
525+
526+
def test_isskewa(self):
527+
# 3D
528+
t = [3, 4, 5, 6, 7, 8]
529+
sk = skewa(t)
530+
self.assertTrue(isskewa(sk))
531+
sk[0,0] = 3
532+
self.assertFalse(isskew(sk))
533+
sk = skewa(t)
534+
sk[3,3] = 3
535+
self.assertFalse(isskew(sk))
536+
537+
# 2D
538+
t = [3, 4, 5]
539+
sk = skew(t)
540+
self.assertTrue(isskew(sk))
541+
sk[0,0] = 3
542+
self.assertFalse(isskew(sk))
543+
sk = skewa(t)
544+
sk[2,2] = 3
545+
self.assertFalse(isskew(sk))
546+
547+
def test_skewa(self):
548+
# 3D
549+
sk = skewa([1, 2, 3, 4, 5, 6])
550+
self.assertEqual(sk.shape, (4,4))
551+
nt.assert_almost_equal(sk.diagonal(), np.r_[0,0,0,0])
552+
nt.assert_almost_equal(sk[-1,:], np.r_[0,0,0,0])
553+
nt.assert_almost_equal(sk[:3,3], [1, 2, 3])
554+
nt.assert_almost_equal(vex(sk[:3,:3]), [4,5,6])
555+
556+
# 2D
557+
sk = skewa([1, 2, 3])
558+
self.assertEqual(sk.shape, (3,3))
559+
nt.assert_almost_equal(sk.diagonal(), np.r_[0,0,0])
560+
nt.assert_almost_equal(sk[-1,:], np.r_[0,0,0])
561+
nt.assert_almost_equal(sk[:2,2], [1, 2])
562+
nt.assert_almost_equal(vex(sk[:2,:2]), [3])
563+
564+
with self.assertRaises(ValueError):
565+
sk = skew([1,2])
566+
567+
def test_vexa(self):
568+
# 3D
569+
t = [1, 2, 3, 4, 5, 6]
570+
sk = skewa(t)
571+
nt.assert_almost_equal(vexa(sk), t)
572+
573+
# 2D
574+
t = [1, 2, 3]
575+
sk = skewa(t)
576+
nt.assert_almost_equal(vexa(sk), t)
304577

305578
class Test2D(unittest.TestCase):
306579
def test_rot2(self):

0 commit comments

Comments
 (0)