Fixed fall through error in binary operation (rai-opensource#117)

Fixes rai-opensource#116 and adds more test coverage.

Author: bokorn-bdaii

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__":

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