* The kinematics solver is finally working
* Refactored a bit
* Extracted utility functions to separate classes
* Removed unused class
* Removed unused code
* Refactored
* Refactored