Added KinematicInteface library to unified the kinematics operations in 1 place
Finished the KinematicInterface integration. Testing to be done