Converting motion optimization tests to unit tests. Moved Eigen nearness checks to amd_utilities/test_utils
Finished porting motion optimization tests to unit tests.