tested working inverse dynamics into forward simulation; tested working triangluar velocity profile with fine discretization of waypoints, with scaled timing information