Unit Tests for Kuka and Motoman Robots are Nearly Identical

[geoffreychiou]

We currently run unit tests on two separate robots: the kuka kr210 and motoman mh5. The tests are essentially the same except for some minor differences.

Instead of separating the tests into two different cpp files with essentially the same code, we should pass the constants in to a single file. This will make it easier to maintain the tests.

[geoffreychiou]

Just a reminder to myself to load the kinematics solver from a parameter and run tests for the MH5 on ikfast and LMA.

[geoffreychiou]

There is a bug in the UserRobotTest::copy_current_pose test where we get the joint state of the robot right after an execution and compare it to another vector. We need to add a function to compare two vectors with some allowable error (or find one in g_test that does the same).