def simulate(self, step): for model in self.models: fwdyn_ab(model) for joint in model.joints: joint.qd += joint.qdd * step joint.integrate_q(joint.qd, step)