Hack to get final velocity doesn't really work yet
removed kludge negative index feature
Fixed following pre-optimized motion
Minor refactoring of TC initialization
rescoped INDETITY_QUAT
Merge branch 'feature/pre-optimized-path-follow' into develop