Refactor Kinematics interface class and added IK solver using ikfast
Some more tests