Optimization update, Serial communication functionality, Mechatronic Arm additions

- Speeds up inverse kinematics optimization code greatly. Almost at real-time.
- Adds support for the Serial communication with the mechatronic arm and a demo simulation for controlling the arm through MATLAB.
- Adds the mechatronic arm kinematics file and uses it in the simulation.
- Addition of several utils as well as speed improvements to previous ones (rotations).