From b91dcd8705bd09e734750de8a87cbeeb5008a2e7 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 8 Oct 2026 21:02:49 -0700 Subject: [PATCH 1/3] Fix double transform of animation start pose in Animate.trplot --- spatialmath/base/animate.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py index 45ff705e..0e26ed49 100755 --- a/spatialmath/base/animate.py +++ b/spatialmath/base/animate.py @@ -163,8 +163,9 @@ def trplot( else: self.start = start - # draw axes at the origin - smb.trplot(self.start, ax=self, **kwargs) + # Store frame geometry at the origin so each absolute pose is applied once. + smb.trplot(np.identity(4), ax=self, **kwargs) + self._draw(self.start) def set_proj_type(self, proj_type: str): self.ax.set_proj_type(proj_type) From c08331f68ca51284aaeaa6276a15447135aecea6 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 8 Oct 2026 21:03:16 -0700 Subject: [PATCH 2/3] Add regression test for animation start pose --- tests/base/test_transforms3d_plot.py | 29 ++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/tests/base/test_transforms3d_plot.py b/tests/base/test_transforms3d_plot.py index d18d9300..002598d1 100755 --- a/tests/base/test_transforms3d_plot.py +++ b/tests/base/test_transforms3d_plot.py @@ -22,9 +22,38 @@ from spatialmath.base.transformsNd import isR, t2r, r2t, rt2tr import matplotlib.pyplot as plt +from spatialmath.base.animate import Animate class Test3D(unittest.TestCase): + def test_animate_nonidentity_start(self): + start = transl(-1, 0, 2) @ trotz(0.4) + end = transl(1, 2, 1) @ trotx(0.3) + for style in ("line", "arrow", "rviz"): + fig = plt.figure() + try: + anim = Animate(dim=[-4, 4]) + anim.trplot(end, start=start, style=style, frame="moving") + for pose in (start, end): + anim._draw(pose) + for obj in anim.displaylist: + expected = pose @ obj.p if hasattr(obj, "p") else None + if obj.type == "line": + nt.assert_allclose( + np.asarray(obj.h.get_data_3d()), + expected[:3], + atol=1e-14, + ) + nt.assert_allclose(obj.p[:3, 0], np.zeros(3), atol=1e-14) + elif obj.type == "arrow": + nt.assert_allclose( + np.asarray(obj.h._segments3d), + expected[:3].T.reshape(3, 2, 3), + atol=1e-14, + ) + finally: + plt.close(fig) + @pytest.mark.skipif( os.environ.get("CI") == "true" or (sys.platform.startswith("darwin") and sys.version_info < (3, 11)), From f8298724fe15f100b822d0ddee67039d5f1dad81 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 01:50:22 -0700 Subject: [PATCH 3/3] Strengthen animation start-pose test against static trplot geometry Refactor test_animate_nonidentity_start to improve axis geometry validation and streamline assertions. --- tests/base/test_transforms3d_plot.py | 51 ++++++++++++++++++---------- 1 file changed, 34 insertions(+), 17 deletions(-) diff --git a/tests/base/test_transforms3d_plot.py b/tests/base/test_transforms3d_plot.py index 002598d1..cec724af 100755 --- a/tests/base/test_transforms3d_plot.py +++ b/tests/base/test_transforms3d_plot.py @@ -27,6 +27,30 @@ class Test3D(unittest.TestCase): def test_animate_nonidentity_start(self): + # the axis artists of an Animate must sit where a static trplot puts them, both straight after + # trplot(end, start=start) and after _draw(end); losing either half of the fix breaks one of these + def axis_geometry(artists): + geometry = [] + for h in artists[:3]: # the three axes of the frame + # arrow style: compare the shaft only, the head wings differ + if hasattr(h, "_segments3d"): + geometry.append(np.asarray(h._segments3d[0]).T) + else: + geometry.append(np.asarray(h.get_data_3d())) + return geometry + + def static_geometry(pose, style): + fig = plt.figure() + try: + ax = fig.add_subplot(projection="3d") + trplot(pose, ax=ax, style=style, frame="moving") + kinds = ("Line3DCollection",) if style == "arrow" else ("Line3D",) + return axis_geometry( + [a for a in ax.get_children() if type(a).__name__ in kinds] + ) + finally: + plt.close(fig) + start = transl(-1, 0, 2) @ trotz(0.4) end = transl(1, 2, 1) @ trotx(0.3) for style in ("line", "arrow", "rviz"): @@ -34,23 +58,16 @@ def test_animate_nonidentity_start(self): try: anim = Animate(dim=[-4, 4]) anim.trplot(end, start=start, style=style, frame="moving") - for pose in (start, end): - anim._draw(pose) - for obj in anim.displaylist: - expected = pose @ obj.p if hasattr(obj, "p") else None - if obj.type == "line": - nt.assert_allclose( - np.asarray(obj.h.get_data_3d()), - expected[:3], - atol=1e-14, - ) - nt.assert_allclose(obj.p[:3, 0], np.zeros(3), atol=1e-14) - elif obj.type == "arrow": - nt.assert_allclose( - np.asarray(obj.h._segments3d), - expected[:3].T.reshape(3, 2, 3), - atol=1e-14, - ) + artists = [x.h for x in anim.displaylist] + for actual, expected in zip( + axis_geometry(artists), static_geometry(start, style) + ): + nt.assert_allclose(actual, expected, atol=1e-12) + anim._draw(end) + for actual, expected in zip( + axis_geometry(artists), static_geometry(end, style) + ): + nt.assert_allclose(actual, expected, atol=1e-12) finally: plt.close(fig)