Fix DTW for rotational joints and add joint centers trajectories