Fix animation start pose being applied twice in Animate.trplot (#238)

Fixes #171.

`Animate.trplot` built the frame geometry already placed at the
requested start pose. The animation then applied the absolute pose again
when drawing, so the frame showed up at the wrong place: asking for a
start of `(-1, 0, 2)` displayed it at `(-2, 0, 4)`.

The geometry is now built at the identity and the start pose is applied
once through `_draw(self.start)`, which matches how the end pose and the
intermediate frames are already drawn.

Test: added a case to `tests/base/test_transforms3d_plot.py` that checks
the drawn frame at the start and end poses for the line, arrow and rviz
styles. It fails on `master` and passes with this change. Full suite:
348 passed, 3 skipped. The interactive animation loop itself is not
exercised in the headless tests; the endpoint poses are.

Author: RoboX2020

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)

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)),