Added functionality and got simple trajectories running