Added function approximation code; working on inverse kinematics model