diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index da1421e9..379a5439 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -1657,7 +1657,7 @@ def _op2(left, right: Self, op: Callable): # pylint: disable=no-self-argument ========= ========== ==== ================================ """ - if isinstance(right, left.__class__): + if isinstance(right, left.__class__) or isinstance(left, right.__class__): # class by class if len(left) == 1: if len(right) == 1: @@ -1683,6 +1683,10 @@ def _op2(left, right: Self, op: Callable): # pylint: disable=no-self-argument return op(left.A, right) else: return [op(x, right) for x in left.A] + else: + raise TypeError( + f"Invalid type ({right.__class__}) for binary operation with {left.__class__}" + ) if __name__ == "__main__": diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 04ed1e12..7132bddc 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -1260,6 +1260,45 @@ def test_arith_vect(self): array_compare(a[1], ry - 1) array_compare(a[2], rz - 1) + def test_angle(self): + # angle between SO3's + r1 = SO3.Rx(0.1) + r2 = SO3.Rx(0.2) + for metric in range(6): + self.assertAlmostEqual(r1.angdist(other=r1, metric=metric), 0.0) + self.assertGreater(r1.angdist(other=r2, metric=metric), 0.0) + self.assertAlmostEqual( + r1.angdist(other=r2, metric=metric), r2.angdist(other=r1, metric=metric) + ) + # angle between SE3's + p1a, p1b = SE3.Rx(0.1), SE3.Rx(0.1, t=(1, 2, 3)) + p2a, p2b = SE3.Rx(0.2), SE3.Rx(0.2, t=(3, 2, 1)) + for metric in range(6): + self.assertAlmostEqual(p1a.angdist(other=p1a, metric=metric), 0.0) + self.assertGreater(p1a.angdist(other=p2a, metric=metric), 0.0) + self.assertAlmostEqual(p1a.angdist(other=p1b, metric=metric), 0.0) + self.assertAlmostEqual( + p1a.angdist(other=p2a, metric=metric), + p2a.angdist(other=p1a, metric=metric), + ) + self.assertAlmostEqual( + p1a.angdist(other=p2a, metric=metric), + p1a.angdist(other=p2b, metric=metric), + ) + # angdist is not implemented for mismatched types + with self.assertRaises(ValueError): + _ = r1.angdist(p1a) + + with self.assertRaises(ValueError): + _ = r1._op2(right=p1a, op=r1.angdist) + + with self.assertRaises(ValueError): + _ = p1a._op2(right=r1, op=p1a.angdist) + + # in general, the _op2 interface enforces an isinstance check. + with self.assertRaises(TypeError): + _ = r1._op2(right=(1, 0, 0), op=r1.angdist) + def test_functions(self): # inv # .T