Numerical mass matrix from FRI

[xEnVrE]

As reported in the KUKA.FastResearchInterface 1.0 manual it is possible to obtain the numerical mass matrix, i.e. joint space inertia matrix.

In order to obtain the matrix in controllers implementing the class KinematicChainControllerBase

• a function getMsrMassMatrix() is added in lwr_hw/include/fri/friremote.h that returns a pointer to the numerical mass matrix
• the function getMsrMassMatrix() is called in the method read() in lwr_hw/include/lwr_hw/lwr_hw_fri.hpp
• 7x7 handles are registered in the already existent effortinterface in lwr_hw/src/lwr_hw.cpp so that handles can be obtained back in KinematicChainControllerBase
• a new handle inertia_matrix_handles is created in lwr_controllers/include/lwr_controllers/KinematicChainControllerBase.h and populated in lwr_controllers/src/KinematicChainControllerBase.cpp

[manuelbonilla]

Tested and working correctly on real robot. Read to for merge

[manuelbonilla]

I agree about improving the URDF however the Inertial Matrix will be neither equal not close to the one provider by kuka, I think. This fact is a problem for people using model based controllers.

We can take a look to realtime_tools for now we can merge this solution and tag it for enhancement.