Merge pull request #1 from ipa-bnm/master

Upstream changes
Changed trajectory to take from the front and not the back
Merge pull request #1 from abubeck/master

Changed Trajectory from back to front