steps through trajecotry waypoints
records all joint states to file