def read_data(): servo_state = None for obs in iterate_robot_observations(robot, sleep=0): timestamp = obs.timestamp observations = obs.observations commands = obs.commands if servo_state != STATE_SERVOING and np.linalg.norm(commands) == 0: servo_state = STATE_WAIT else: servo_state = STATE_SERVOING controller.process_observations(observations) goal = controller.get_current_goal() yield 'y', timestamp, observations yield 'servo_state', timestamp, servo_state yield 'commands', timestamp, commands yield 'y_goal', timestamp, goal
def read_data(): goal = None t0 = None servo_state = STATE_WAIT for obs in iterate_robot_observations(robot, sleep=0): timestamp = obs.timestamp if t0 is None: t0 = timestamp observations = obs.observations if goal is None and timestamp - t0 > goal_at: goal = observations commands = obs.commands # if still have transitions if transitions: next_transition = transitions[0][0] if timestamp > next_transition: servo_state = transitions[0][1] transitions.pop(0) # if np.linalg.norm(commands) == 0: # servo_state = STATE_WAIT # else: # servo_state = STATE_SERVOING # # n = np.sum(commands != 0) # print "%20s" % servo_state + "n: %5s " % n + ",".join('%+10.6f' % s for s in commands) # goal = controller.get_current_goal() yield 'y', timestamp, observations yield 'servo_state', timestamp, servo_state yield 'commands', timestamp, commands if goal is None: yield 'y_goal', timestamp, observations else: yield 'y_goal', timestamp, goal