added interpolation angle
added interpolation angle, changed back to set_joint_positions