Trajectory controller working, test file for it added.