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) diff --git a/tests/base/test_transforms3d_plot.py b/tests/base/test_transforms3d_plot.py index d18d9300..cec724af 100755 --- a/tests/base/test_transforms3d_plot.py +++ b/tests/base/test_transforms3d_plot.py @@ -22,9 +22,55 @@ 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): + # 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"): + fig = plt.figure() + try: + anim = Animate(dim=[-4, 4]) + anim.trplot(end, start=start, style=style, frame="moving") + 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) + @pytest.mark.skipif( os.environ.get("CI") == "true" or (sys.platform.startswith("darwin") and sys.version_info < (3, 11)),