diff --git a/src/roboticstoolbox/blocks/arm.py b/src/roboticstoolbox/blocks/arm.py index 8668b196b..cd25f0992 100644 --- a/src/roboticstoolbox/blocks/arm.py +++ b/src/roboticstoolbox/blocks/arm.py @@ -1478,13 +1478,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: @@ -1607,7 +1617,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 13924daec..889e0c4d8 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()