Add indexes of parts of the kinematics state vector  as constants of the project
Use the new constants to index parts of the kinematics state vector
Correct the derivatives of the rotation to translation transformation
Update the unit testing, now the test passes.
Update the indexation of the kinematics state vector to comply with the new constants
Change the indexes to sort the vector components by derivation order