Start doing optimal control stuff for AStar