Glad to hear you have a handle on it. I would still believe that a good trajectory planner in Mach should be able to join tangential line segments at a smooth speed, but unfortunately it doesn't.