From 73222bf69553c3ffda085ce6832b12841f9d6a6d Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 15:49:52 -0700 Subject: [PATCH 1/3] Accept a stacked (N, r, c) array as a sequence of poses Added support for stacked numpy arrays in baseposelist.py. --- spatialmath/baseposelist.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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] From 1057abf2564d5dbad59ddd8723c12fd6815bea11 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 15:50:25 -0700 Subject: [PATCH 2/3] SE3: accept a stacked (N, 4, 4) array and say what is accepted in the error Added support for SE3 constructor with (N, 4, 4) NumPy array. --- spatialmath/pose3d.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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) From 3f06d5e5dc465ee936c8c7e8cb577fa502c2721a Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 15:50:48 -0700 Subject: [PATCH 3/3] Test SE3 and SO3 construction from a stacked array Add unit test for SE3 constructor with stacked array input. --- tests/test_pose3d.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) 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()