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.EvalVectorInput(
context, self.iiwa_position_input_port.get_index()).get_value()
v_iiwa = self.EvalVectorInput(
context, self.iiwa_velocity_input_port.get_index()).get_value()
tau_iiwa = self.EvalVectorInput(
context, self.iiwa_external_torque_input_port.get_index()).get_value()
t_plan = t - self.current_plan_start_time
new_control_output = discrete_state.get_mutable_vector().get_mutable_value()
new_control_output[0:self.nu] = \
self.current_plan.CalcPositionCommand(q_iiwa, v_iiwa, tau_iiwa, t_plan, self.control_period)
new_control_output[self.nu:2*self.nu] = \
self.current_plan.CalcTorqueCommand(q_iiwa, 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 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
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
