[C++] Fix bug in joint integration

Most of the joints have bugs inside their methods - integration,
differentiate, interpolate.
A small remind that the joint velocity vector is generally expressed in
the joint frame while the translations are expressed in the joint
parent frame.