Repository: bdaiinstitute/spatialmath-python

Issue:
Fall through error in the binary operator for BasePoseMatrix
Fall through error in the binary operator for BasePoseMatrix:

https://github.com/bdaiinstitute/spatialmath-python/blob/1b89c49395a21b5241e2f0a233e69394f3bc27b1/spatialmath/baseposematrix.py#L1632C7-L1632C7

If you want to compare the rotation of an SE3 to an SO3, it returns none as opposed to throwing an error or returning a valid value. This should probably raise an error as opposed to returning None. 

```
import numpy as np
from spatialmath import SE3, SO3

T0 = SE3()
print(T0.angdist(SO3()))
```

Could be fixed with closing else in `BasePoseMatrix._op2`
```
    else:
        raise ValueError(f'Invalid type ({right.__class__}) for binary operation with {left.__class__}')
```

Hints from issue comments:
@jcao-bdai: Thank you for noticing this, @bokorn-bdaii 

i see you have a PR up with this fix but it also exposed the fact that we did not have test coverage for this logic - do you mind if I push a commit with just a unit test onto your branch?
@bokorn-bdaii: Ya, of course. Feel free to push a unit test in.@jcao-bdai: @bokorn-bdaii pushed.  thanks! :crossed_fingers: 
will approve as soon as the workflow completes. thanks again for fixing this issue.@jcao-bdai: @bokorn-bdaii your PR fixing this issue has been merged: https://github.com/bdaiinstitute/spatialmath-python/pull/117

Solution patch:
diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py
index da1421e..379a543 100644
--- a/spatialmath/baseposematrix.py
+++ b/spatialmath/baseposematrix.py
@@ -1657,7 +1657,7 @@ class BasePoseMatrix(BasePoseList):
         =========   ==========   ====  ================================
 
         """
-        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 @@ class BasePoseMatrix(BasePoseList):
                 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__":


Test patch:
diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py
index 04ed1e1..7132bdd 100755
--- a/tests/test_pose3d.py
+++ b/tests/test_pose3d.py
@@ -1260,6 +1260,45 @@ class TestSE3(unittest.TestCase):
         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