diff --git a/src/roboticstoolbox/blocks/arm.py b/src/roboticstoolbox/blocks/arm.py index 9fd26ae1f..a3e7c2284 100644 --- a/src/roboticstoolbox/blocks/arm.py +++ b/src/roboticstoolbox/blocks/arm.py @@ -1463,13 +1463,13 @@ 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._qdd = np.zeros((robot.n,)) # placeholder until the first deriv() 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 # from last deriv(), or the zero placeholder return [q, qd, qdd] def deriv(self, t: Any, inports: Any, x: Any) -> Any: diff --git a/tests/test_blocks.py b/tests/test_blocks.py index 46576cf03..30d48e274 100644 --- a/tests/test_blocks.py +++ b/tests/test_blocks.py @@ -128,6 +128,16 @@ 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_before_deriv(self): + # bdsim records the type of each output when it compiles a diagram, so qdd + # must be an array of the right shape even before the first deriv() call + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + block = FDyn(robot, q) + + qdd = block.test_output([0] * 6, x=np.r_[q, np.zeros((6,))])[2] + self.assertEqual(np.shape(qdd), (6,)) + @unittest.skip def test_fdyn_x(self): robot = rtb.models.DH.Puma560()