Updated Kinematics functions

Created header for kinematics functions
Included lookup table for inverse kinematics.