8000 Fixed fall through error in binary operation (#117) · petercorke/spatialmath-python@ad98b0e · GitHub
[go: up one dir, main page]
Skip to content

Commit ad98b0e

Browse files
bokorn-bdaiibokorn-bdaii
authored
Fixed fall through error in binary operation (bdaiinstitute#117)
Fixes bdaiinstitute#116 and adds more test coverage.
1 parent 1b89c49 commit ad98b0e

File tree

2 files changed

+44
-1
lines changed

2 files changed

+44
-1
lines changed

‎spatialmath/baseposematrix.py

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1657,7 +1657,7 @@ def _op2(left, right: Self, op: Callable): # pylint: disable=no-self-argument
16571657
========= ========== ==== ================================
16581658
16591659
"""
1660-
if isinstance(right, left.__class__):
1660+
if isinstance(right, left.__class__) or isinstance(left, right.__class__):
16611661
# class by class
16621662
if len(left) == 1:
16631663
if len(right) == 1:
@@ -1683,6 +1683,10 @@ def _op2(left, right: Self, op: Callable): # pylint: disable=no-self-argument
16831683
return op(left.A, right)
16841684
else:
16851685
return [op(x, right) for x in left.A]
1686+
else:
1687+
raise TypeError(
1688+
f"Invalid type ({right.__class__}) for binary operation with {left.__class__}"
1689+
)
16861690

16871691

16881692
if __name__ == "__main__":

‎tests/test_pose3d.py

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1260,6 +1260,45 @@ def test_arith_vect(self):
12601260
array_compare(a[1], ry - 1)
12611261
array_compare(a[2], rz - 1)
12621262

1263+
def test_angle(self):
1264+
# angle between SO3's
1265+
r1 = SO3.Rx(0.1)
1266+
r2 = SO3.Rx(0.2)
1267+
for metric in range(6):
1268+
self.assertAlmostEqual(r1.angdist(other=r1, metric=metric), 0.0)
1269+
self.assertGreater(r1.angdist(other=r2, metric=metric), 0.0)
1270+
self.assertAlmostEqual(
1271+
r1.angdist(other=r2, metric=metric), r2.angdist(other=r1, metric=metric)
1272+
)
1273+
# angle between SE3's
1274+
p1a, p1b = SE3.Rx(0.1), SE3.Rx(0.1, t=(1, 2, 3))
1275+
p2a, p2b = SE3.Rx(0.2), SE3.Rx(0.2, t=(3, 2, 1))
1276+
for metric in range(6):
1277+
self.assertAlmostEqual(p1a.angdist(other=p1a, metric=metric), 0.0)
1278+
self.assertGreater(p1a.angdist(other=p2a, metric=metric), 0.0)
1279+
self.assertAlmostEqual(p1a.angdist(other=p1b, metric=metric), 0.0)
1280+
self.assertAlmostEqual(
1281+
p1a.angdist(other=p2a, metric=metric),
1282+
p2a.angdist(other=p1a, metric=metric),
1283+
)
1284+
self.assertAlmostEqual(
1285+
p1a.angdist(other=p2a, metric=metric),
1286+
p1a.angdist(other=p2b, metric=metric),
1287+
)
1288+
# angdist is not implemented for mismatched types
1289+
with self.assertRaises(ValueError):
1290+
_ = r1.angdist(p1a)
1291+
1292+
with self.assertRaises(ValueError):
1293+
_ = r1._op2(right=p1a, op=r1.angdist)
1294+
1295+
with self.assertRaises(ValueError):
1296+
_ = p1a._op2(right=r1, op=p1a.angdist)
1297+
1298+
# in general, the _op2 interface enforces an isinstance check.
1299+
with self.assertRaises(TypeError):
1300+
_ = r1._op2(right=(1, 0, 0), op=r1.angdist)
1301+
12631302
def test_functions(self):
12641303
# inv
12651304
# .T

0 commit comments

Comments
 (0)
2918
0