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
