Add "flattening" from SE3 to SE2.

myeatman-bdai · myeatman-bdai · commit 258a55176a8f · 2023-06-09T09:30:36.000-04:00
diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py
@@ -961,15 +961,7 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
             elif isinstance(x, SO3):
                 self.data = [smb.r2t(_x) for _x in x.data]
             elif isinstance(x, SE2):  # type(x).__name__ == "SE2":
-
-                def convert(x):
-                    # convert SE(2) to SE(3)
-                    out = np.identity(4, dtype=x.dtype)
-                    out[:2, :2] = x[:2, :2]
-                    out[:2, 3] = x[:2, 2]
-                    return out
-
-                self.data = [convert(_x) for _x in x.data]
+                self.data = x.SE3().data
             elif smb.isvector(x, 3):
                 # SE3( [x, y, z] )
                 self.data = [smb.transl(x)]
@@ -1170,6 +1162,9 @@ def inv(self) -> SE3:
         else:
             return SE3([smb.trinv(x) for x in self.A], check=False)
 
+    def SE2(self) -> SE2:
+        return SE2(self.x, self.y, self.rpy()[2])
+
     def delta(self, X2: Optional[SE3] = None) -> R6:
         r"""
         Infinitesimal difference of SE(3) values
diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py
@@ -659,7 +659,14 @@ def test_arith_vect(self):
     def test_functions(self):
         # inv
         # .T
-        pass
+
+        # conversion to SE2
+        poseSE3 = SE3.Tx(3.3) * SE3.Rz(1.5)
+        poseSE2 = poseSE3.SE2()
+        nt.assert_almost_equal(poseSE3.R[0:1,0:1], poseSE2.R[0:1,0:1])
+        nt.assert_equal(poseSE3.x , poseSE2.x)
+        nt.assert_equal(poseSE3.y , poseSE2.y)
+
 
     def test_functions_vect(self):
         # inv
