[Kinematics Update]: Forward kinematics

- Added mouse interaction
- Code clean-up