Feature/subtrajectory

Implement getSubtrajectory for GlobalTrajectory and use it inside TimedElasticBand planning.

Also note #25 (closed) for the planToDest variable, as the free goal velocity when not planning to the end does not work yet.

Closes #13 (closed)