add interpolation function that takes joint velocities into account, generalize setDiffFromIK
resolve merge conflict