Add tests for getPositionFK and getPositionIK (all solutions)

[JeroenDM]

These two functions are never tested at the moment.

[JeroenDM]

I added some simple tests that use MoveIt! config package of the Kuka kr6r700 that is configured for this plugin (the kinematics.yaml file is updated as mentioned in the readme.

The calculations are really slow at the moment. Analytical IK should go in 10 ms or something of that order I think.

[==========] Running 4 tests from 2 test cases.
[----------] Global test environment set-up.
[----------] 1 test from testPlugin
[ RUN      ] testPlugin.testInit
ros.moveit_opw_kinematics_plugin.opw: MoveItOPWKinematicsPlugin initializing
[       OK ] testPlugin.testInit (815 ms)
[----------] 1 test from testPlugin (815 ms total)

[----------] 3 tests from LoadPlugin
[ RUN      ] LoadPlugin.positionFK
[       OK ] LoadPlugin.positionFK (907 ms)
[ RUN      ] LoadPlugin.singleSolutionIK
[       OK ] LoadPlugin.singleSolutionIK (904 ms)
[ RUN      ] LoadPlugin.allSolutionsIK
[       OK ] LoadPlugin.allSolutionsIK (824 ms)
[----------] 3 tests from LoadPlugin (2635 ms total)

[----------] Global test environment tear-down
[==========] 4 tests from 2 test cases ran. (3450 ms total)
[  PASSED  ] 4 tests.

Note that the file test_model_from_file is not correct at the moment as there is no constructor that takes a robot model.

[JeroenDM]

After some profiling it seems that a lot of time is spent in the Eigen library. I think this is due to parts like this:

Eigen::Affine3d pose;
tf::poseMsgToEigen(ik_poses[0], pose);

As a reference, here is the output of running 10 ik calcuations for double as wel as float type, whithout using the this MoveIt! wrapper:

$rosrun opw_kinematics kuka_kr6_test 
[==========] Running 2 tests from 1 test case.
[----------] Global test environment set-up.
[----------] 2 tests from kuka_kr6
[ RUN      ] kuka_kr6.random_reachable_poses_double
[       OK ] kuka_kr6.random_reachable_poses_double (7 ms)
[ RUN      ] kuka_kr6.random_reachable_poses_float
[       OK ] kuka_kr6.random_reachable_poses_float (6 ms)
[----------] 2 tests from kuka_kr6 (13 ms total)

[----------] Global test environment tear-down
[==========] 2 tests from 1 test case ran. (13 ms total)
[  PASSED  ] 2 tests.

[gavanderhoorn]

@JeroenDM wrote:

Analytical IK should go in 10 ms or something of that order I think.

I would expect execution time on the order of microseconds for a closed form solution (which iirc this is). Similar to IKFast.

milliseconds range is what KDL and TracIK do.

[JeroenDM]

Completely agree, I should have written microseconds. I was distracted by all the ms in the output :) (As an example reference, the original thesis on IKFast mentions 4-7 microseconds.)

I'm currently learning how c++ profilers work to investigate this. But there is a lot of converting between Eigen and MoveIt! messages, so that will probably part of the cause.

[Jmeyer1292]

Slow? This hurts me deeply. My initial testing (with the ABB2400 example) showed that it was very quick (as fast or faster than the IKFast, mostly because of API choices).

I'm going to ask the dumb question: Are you building the code and tests in release? Is this against the PR you made (adding the tests)? I will look into it.

[Jmeyer1292]

I did some testing where I modified your testing PR to do the random pose generation in a loop and then call IK in a tight loop. I poor-man's profiled the code by putting std::chrono::steady_clock time points at each end of the IK loop only. Built in release and run on my Intel(R) Core(TM) i7-4700MQ CPU @ 2.40GHz via /proc/cpuinfo.

[ RUN      ] kuka_kr6.benchmark_ik_speed_double1
IK Calls (n = 500000) took 1463746 microseconds
Average Duration: 2.92749
[       OK ] kuka_kr6.benchmark_ik_speed_double1 (1746 ms)
[ RUN      ] kuka_kr6.benchmark_ik_speed_float1
IK Calls (n = 500000) took 852672 microseconds
Average Duration: 1.70534
[       OK ] kuka_kr6.benchmark_ik_speed_float1 (1110 ms)
[----------] 4 tests from kuka_kr6 (2856 ms total)

So definitely in the few us range.

[JeroenDM]

@Jmeyer1292 I certainly did not mean that your IK code is slow, just that my implementation of the plugin is slow :) I'm sorry if that was not clear.

I completely forgot to turn on the release option, thanks for pointing that out! I tried to set the release option in the CMakeLists.txt like this:

set(ROS_BUILD_TYPE Release)

and for another test using a command line argument catkin build --cmake-args -CMAKE_BUILD_TYPE=Release. But it does not seem to make a big difference for the tests of this package. I will look further into it.

I feel bad that I made you spend time looking into it. (But the benchmark you did is maybe interesting to add to the readme?)

[gavanderhoorn]

Just a note: ROS_BUILD_TYPE is not something I recognise. The only (and correct) way to configure build time optimisation flags is by setting CMAKE_BUILD_TYPE using the command line you show.

[simonschmeisser]

I hope you found out by now but there is a D missing in your command catkin build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo or catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release

For comfortable profiling on linux have a look at hotspot: https://github.com/KDAB/hotspot you should be able to just download the AppImage, select your binary and profit

This plugin sounds quite nice!

[JeroenDM]

Thanks for the profiler tip! I did find the error, but I never actually used the plugin. I use opw_kinematics directly.

I suspect the driver will not compile at the moment because opw_kinematics switched from Affine3d to Isometry3d to represent a pose. (#8 )

Feel free to suggest any improvements (in addition to #3 ). Since it was my first time writing such a plugin, you could find suspect code or quick fixes in the code.

If you plan on using the plugin for a specific robot, I can add a test for that robot to make sure it keeps working. I should probably also test it in melodic.