Keep track of time penalty in dtw
Fix rotation alignment bug. Fix using jacobian at unwarped position. Fix timestep mismatch. Put force and torque together in aug_traj.