Implement getSubtrajectory for GlobalTrajectory and use it inside TimedElasticBand planning.
getSubtrajectory
GlobalTrajectory
TimedElasticBand
Also note #25 (closed) for the planToDest variable, as the free goal velocity when not planning to the end does not work yet.
planToDest
Closes #13 (closed)