Control system refactor
Recovery trajectory generation working