Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 19 additions & 3 deletions src/roboticstoolbox/blocks/arm.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down
38 changes: 38 additions & 0 deletions tests/test_blocks.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
Loading