figure out why the trajectory is not smooth
a bug that the dif-v is always 0