def finished_state(self, q, q_dot, time):
command = self.commands[self.state]
angles = []
for cmd in [command[MOTOR_1_KEY], command[MOTOR_2_KEY], command[MOTOR_3_KEY]]:
try:
angles.append(cmd[THETA_KEY])
except:
angles.append(None)
return at_angles(angles, q, q_dot)
def is_finished(self, q, q_dot):
return at_angles((self.hold_to.controller.theta_1,
self.hold_to.controller.theta_2,
self.hold_to.controller.theta_3),
q, q_dot, .2, .01)
