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)