Testing out Odrive's new trajectory planner HD

Final test of the trajectory planner for Odrive. Big thanks to Wetmelon for putting this together.