def DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem.DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
self.GetCurrentPlan(context)
t = context.get_time()
q_iiwa = self.iiwa_position_input_port.Eval(context)
v_iiwa = self.iiwa_velocity_input_port.Eval(context)
tau_iiwa = self.iiwa_external_torque_input_port.Eval(context)
q_iiwa_cmd = self.iiwa_position_command_input_port.Eval(context)
t_plan = t - self.current_plan_start_time
new_control_output = discrete_state.get_mutable_vector(
).get_mutable_value()
new_control_output[0:self.nq] = \
self.current_plan.CalcPositionCommand(q_iiwa, q_iiwa_cmd, v_iiwa, tau_iiwa,
t_plan, self.control_period)
new_control_output[self.nq:2 * self.nq] = \
self.current_plan.CalcTorqueCommand(q_iiwa, q_iiwa_cmd, v_iiwa, tau_iiwa,
t_plan, self.control_period)
new_control_output[14] = self.current_gripper_setpoint
# print current simulation time
if self.print_period < np.inf and \
t - self.last_print_time >= self.print_period:
print("t: ", t)
self.last_print_time = t
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem.DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_control_input = discrete_state.get_mutable_vector(
).get_mutable_value()
x = self.EvalVectorInput(context, 0).get_value()
new_u = self.ComputeControlInput(x, context.get_time())
new_control_input[:] = new_u
def DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem.DoCalcDiscreteVariableUpdates(
self, context, events, discrete_state)
self.called_discrete = True
