Clean-up and CMake improvements

Hi Arne,

I also came up with the Eigen problem and further improved the cmake files. Now it nicely uses KDL stuff if available, looks for Eigen3 and finally for Eigen2 if not found...

Best, Robert