Added trajectory control, with functions for tests
Merge https://github.com/Jochi-Labs/J3CA-main