fix bug with SE2 to SE3 conversion

petercorke · petercorke · commit 651f7c5bffb1 · 2021-03-28T18:41:05.000+10:00
diff --git a/docs/source/intro.rst b/docs/source/intro.rst
@@ -263,7 +263,7 @@ The classes ``SE3``, ``SO3``, ``SE2`` and ``SO2`` can provide colorized text out
 with rotational elements in red, translational elements in blue and constants in grey.
 
 The foreground and background colors can be controlled using the following
-class variables for the ``SMPose`` subclasses
+class variables for the ``BasePoseMatrix`` subclasses
 
 ===============  ===================  ============================================
 Variable         Default              Description
diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py
@@ -738,7 +738,7 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
                 return
             elif isinstance(x, SO3):
                 self.data = [base.r2t(_x) for _x in x.data]
-            elif isinstance(x, SE2):
+            elif type(x).__name__ == 'SE2':
                 def convert(x):
                     # convert SE(2) to SE(3)
                     out = np.identity(4, dtype=x.dtype)
diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py
@@ -6,7 +6,8 @@
 we will assume that the primitives rotx,trotx, etc. all work
 """
 from math import pi
-from spatialmath.pose3d import *
+from spatialmath import SE3, SO3, SE2
+import numpy as np
 # from spatialmath import super_pose as sp
 from spatialmath.base import *
 from spatialmath.base import argcheck
@@ -777,12 +778,37 @@ def test_constructor(self):
         nt.assert_equal(len(R), 1)
         self.assertIsInstance(R, SE3)
 
+        # random
+        T = SE3.Rand()
+        R = T.R
+        t = T.t
+        T = SE3.Rt(R, t)
+        self.assertIsInstance(T, SE3)
+        self.assertEqual(T.A.shape, (4,4))
+
+        nt.assert_equal(T.R, R)
+        nt.assert_equal(T.t, t)
+
+
         # copy constructor
         R = SE3.Rx(pi / 2)
         R2 = SE3(R)
         R = SE3.Ry(pi / 2)
         array_compare(R2, trotx(pi / 2))
 
+        # SO3
+        T = SE3(SO3())
+        nt.assert_equal(len(T), 1)
+        self.assertIsInstance(T, SE3)
+        nt.assert_equal(T.A, np.eye(4))
+
+        # SE2
+        T = SE3(SE2(1, 2, 0.4))
+        nt.assert_equal(len(T), 1)
+        self.assertIsInstance(T, SE3)
+        self.assertEqual(T.A.shape, (4,4))
+        nt.assert_equal(T.t, [1, 2, 0])
+
     def test_shape(self):
         a = SE3()
         self.assertEqual(a._A.shape, a.shape)
