merged, in the update function now included two vectors that saves both position and velocity of the joint_handles_[i]