Improved the introduction of holonomic motion and tests, cleanup and tests.