From d258a59a5ab1ae5735041a6138cf9ebea9a4f824 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 15 Nov 2026 10:56:02 +1000 Subject: [PATCH] fix(blocks): FDyn qdd output is an array from the first evaluation, and reset each run FDyn initialised its cached joint acceleration to None, so port 2 returned None until the first deriv() and an ndarray afterwards. bdsim now asserts that an output keeps its type, so any run with FDyn failed at the first step. Start from a zero array instead, and clear the cache in reset() so a run does not see the last acceleration of the previous one. FDyn_X had the same stale cache across runs and gets the same reset(). Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/blocks/arm.py | 22 +++++++++++++++--- tests/test_blocks.py | 38 +++++++++++++++++++++++++++++++ 2 files changed, 57 insertions(+), 3 deletions(-) diff --git a/src/roboticstoolbox/blocks/arm.py b/src/roboticstoolbox/blocks/arm.py index 9fd26ae1f..a606b25c2 100644 --- a/src/roboticstoolbox/blocks/arm.py +++ b/src/roboticstoolbox/blocks/arm.py @@ -1463,13 +1463,23 @@ def __init__(self, robot: Any, q0: Any = None, **blockargs: Any) -> None: else: q0 = smb.getvector(q0, robot.n) self._x0 = np.r_[q0, np.zeros((robot.n,))] - self._qdd = None + self.reset() + + def reset(self) -> None: + # Joint acceleration cache. output() cannot compute qdd itself, since + # it needs the torque input and bdsim may call output() before that + # input is resolved (petercorke/bdsim#81), so it returns the value from + # the last deriv(). Until deriv() has run, and at the start of every + # run, that is a zero placeholder: it must be an ndarray, never None, + # since bdsim checks that an output keeps the same type. + super().reset() + self._qdd = np.zeros((self.robot.n,)) def output(self, t: Any, inports: Any, x: Any) -> Any: n = self.robot.n q = x[:n] qd = x[n:] - qdd = self._qdd # from last deriv + qdd = self._qdd # cached from the last deriv(), or the zero placeholder return [q, qd, qdd] def deriv(self, t: Any, inports: Any, x: Any) -> Any: @@ -1592,7 +1602,13 @@ def __init__( # doesn't have that problem: the integrator only calls it with a # genuinely resolved input. See petercorke/bdsim#81 for the general # case (stateful blocks whose output() needs a resolved input). - self._qdd = np.zeros((robot.n,)) + self.reset() + + def reset(self) -> None: + # the cache is cleared at the start of every run, so a run never sees + # the last acceleration of the previous one + super().reset() + self._qdd = np.zeros((self.robot.n,)) def output(self, t: Any, inports: Any, x: Any) -> Any: n = self.robot.n diff --git a/tests/test_blocks.py b/tests/test_blocks.py index 46576cf03..6bac07946 100644 --- a/tests/test_blocks.py +++ b/tests/test_blocks.py @@ -128,6 +128,44 @@ def test_fdyn(self): nt.assert_equal(block.test_output(tau, x=x)[0], q) nt.assert_equal(block.getstate0(), x) + def test_fdyn_qdd_is_always_an_array(self): + # bdsim checks that an output keeps its type; qdd used to be None until + # the first deriv() + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + block = FDyn(robot, q) + x = np.r_[q, np.zeros((6,))] + tau = [6, 5, 4, 3, 2, 1] + + qdd = block.test_output(tau, x=x)[2] + self.assertIsInstance(qdd, np.ndarray) + nt.assert_equal(qdd, np.zeros((6,))) + + block.test_deriv(tau, x=x) + nt.assert_array_almost_equal( + block.test_output(tau, x=x)[2], robot.accel(q, np.zeros((6,)), tau) + ) + + # a new run starts again from the placeholder + block.reset() + nt.assert_equal(block.test_output(tau, x=x)[2], np.zeros((6,))) + + def test_fdyn_runs_twice(self): + # a diagram with FDyn simulates, twice, with its qdd output watched + import bdsim + + robot = rtb.models.DH.Puma560() + sim = bdsim.BDSim(graphics=False, animation=False, quiet=True) + bd = sim.blockdiagram() + tau = bd.CONSTANT([6, 5, 4, 3, 2, 1]) + fdyn = bd.FDYN(robot, robot.configs["qn"]) + bd.connect(tau, fdyn) + bd.compile() + + first = sim.run(bd, T=0.01, dt=0.001, watch=[fdyn[2]]) + second = sim.run(bd, T=0.01, dt=0.001, watch=[fdyn[2]]) + nt.assert_array_almost_equal(first.y, second.y) + @unittest.skip def test_fdyn_x(self): robot = rtb.models.DH.Puma560()