Fixed state_history to record all robots, fixed time arg in control law, fixed typos.