diff --git a/spatialmath/baseposelist.py b/spatialmath/baseposelist.py index b102b4bb..bdf3ac64 100644 --- a/spatialmath/baseposelist.py +++ b/spatialmath/baseposelist.py @@ -154,6 +154,8 @@ def arghandler( #. None, an identity value is created #. a numpy.ndarray of the appropriate shape and value which is valid for the subclass + #. a stacked numpy.ndarray of shape (N, r, c), where (r, c) is the shape of one value + of the subclass, which is a sequence of N values #. a list whose elements all meet the criteria above #. an instance of the subclass #. a list whose elements are all singelton instances of the subclass @@ -187,6 +189,19 @@ def arghandler( elif isinstance(arg, np.ndarray): # it's a numpy array + shape = tuple(self.shape) + if len(shape) == 2 and arg.ndim == 3 and arg.shape[1:] == shape: + # a stacked (N, r, c) array is a sequence of N values + values = [self._import(x, check=check) for x in arg] + if any(x is None for x in values): + return False + self.data = values + return True + + if not check and arg.shape != shape: + # check=False skips the value checks, but not the shape check + return False + x = self._import(arg, check=check) if x is not None: self.data = [x] diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index dfc91a86..117f03a8 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -1096,6 +1096,9 @@ def __init__(self, x=None, y=None, z=None, *, check=True): - ``SE3([X1, X2, ... XN])`` has ``N`` values given by the elements ``Xi`` each of which is an SE3 instance. + - ``SE3(A)`` where ``A`` is an ``(N, 4, 4)`` NumPy array has ``N`` values, + equivalent to ``SE3(list(A))``. + :SymPy: supported """ if y is None and z is None: @@ -1115,7 +1118,10 @@ def __init__(self, x=None, y=None, z=None, *, check=True): self.data = [smb.transl(T) for T in x] else: - raise ValueError("bad argument to constructor") + raise ValueError( + "bad argument to constructor: expected an SE3, SO3 or SE2 instance, a 4x4 or " + "(N, 4, 4) array, a list of those, a 3-vector or an (N, 3) array of translations" + ) elif y is not None and z is not None: # SE3(x, y, z) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 589c0ec1..8e401920 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -784,6 +784,26 @@ class TestSE3(unittest.TestCase): def tearDownClass(cls): plt.close("all") + def test_constructor_stacked_array(self): + # an (N, 4, 4) ndarray is a sequence of N poses, like a list of 4x4 arrays + poses = [SE3.Rx(0.3), SE3(1, 2, 3) * SE3.Ry(0.4), SE3.Rz(-0.2)] + stacked = np.array([T.A for T in poses]) + for check in (True, False): + T = SE3(stacked, check=check) + self.assertEqual(len(T), 3) + for k in range(3): + nt.assert_array_almost_equal(T[k].A, poses[k].A) + self.assertEqual(len(SE3(np.empty((0, 4, 4)))), 0) + self.assertEqual(len(SO3(np.array([SO3.Rx(0.1).A, SO3.Ry(0.2).A]))), 2) + + # stacked matrices that are not valid poses are still rejected by check=True + with self.assertRaises(ValueError): + SE3(2 * stacked) + # and a stack of matrices of the wrong size is rejected whatever the value of check + for check in (True, False): + with self.assertRaises(ValueError): + SE3(np.ones((3, 3, 3)), check=check) + def test_constructor(self): # null constructor R = SE3()