Drafted FK code. Testing/Compiling to come
Compiled and Tested FK
Added a heuristic function based on euclidean distance in the work space and a matching cost function that just calculates euclidean distance as well. Easy to modify to make a fancier cost funtion. Possibly some #define or input to the code to select between them?