fixed some amp bugs. beginning to debug performance
added option for ikfast, fixed bugs in reconstruct path
added loading map from file. fixed bugs in path reconstruct.