Add first pass of code adaptation from matlab. TODO: Bugs appearing in Test_Kinematics when calling c2j after RS function.