Bugfixes
Finished correct_kinematics.