Add gravity compensation filter

[guihomework]

This is a rework of PR https://github.com/ros-controls/control_toolbox/pull/115 for the gravity-compensation filter part, now using generate_parameter_library.

[guihomework]

The new changes to this PR stem from using the filter in admittance control and realizing mistakes.

One major mistake was the switch to wrench transforms (as suggested in a TODO in previous version of this code), because it (correctly) applies an additional torque, which should not happen due to gravity being a field and not a force exerted at a certain point than generates a torque when moved. So one should not transform the wrench to base or what ever other computation frame, just rotate it, and remove the gravity force on the correct axes.

Additionally, since there are lookupTransforms done, I offered the possibility to have a different output frame than the input frame. A transform back was pre-existing anyway, so might as well be one that is useful if desired.

I refactored variable names and comments to better explain what happens, and simplified the frames (no need to compute in world frame, sensor_frame=measurement frame is completely fine in my opinion). I generalized the force field to a vector, to permit having a tilted world in which gravity is not along -z.

I would still like to add a test using a param.yaml + maybe some transforms to fully evaluate the filter in real conditions. The current tests were not sufficient to spot the wrench "transform" errors, because they set up identity transforms which are a special case.

[guihomework]

Could we add a short documentation here please? Some (pseudo-) formulas to describe the mathematics would be great, otherwise it is very hard to review. Maybe in a separate readme.[md,rst] together with some high-level description of what the filter is good for.

Sure, this could permit to avoid others not understanding immediately what was going on either, redoing all the maths.

[guihomework]

The calculations look too complex to me – at least to the older version as I can remember. Is everything necessary?

Also, tests should not declare parameters if possible, but generate_parameter_library should take care of it

I think I will clarify with a tiny drawing in a readme. I would say the code I started from was a special case, and did not work in all cases, especially if the force torque measurement does not happen in a frame that the kinematics knows about (not talking about lookup here). One example was in case the force torque sensor is mounted at the robot flange, but the measurement happens 5 cm further (thickness of the sensor). So one needs to consider this transform, which adds one transform. But I simplified (removed one transform from Cog to world) the original code to not have to move the computation in world frame and back. World frame is just where gravity is defined. Computation do not have to be there I believe.

[guihomework]

Could we add a short documentation here please? Some (pseudo-) formulas to describe the mathematics would be great, otherwise it is very hard to review. Maybe in a separate readme.[md,rst] together with some high-level description of what the filter is good for.

I added a README.md inside the source folder, as the general README does not even describe the PID etc...

For differenet cases simply use different *.yaml and test files

I will try address this in some near future (also for #152)

[christophfroehlich]

I added a README.md inside the source folder, as the general README does not even describe the PID etc...

Great thx, I already realized the lack of any docs here. -> #154 ;)

I'll have a look the next days.

[guihomework]

Thanks a lot @christophfroehlich for the review and sorry for fixing spell-checking, I had forgotten to install a spell checker in VS Code, my former editor had one integrated.

Now, only the issue about removing clock access is open I believe and is not straight forward to solve. @destogl can we live with this ?

[guihomework]

I just added one export that solves an issue when building in symlink-install

  ament_cmake_symlink_install_targets()
  'gravity_compensation_filter_parameters' is an interface library - there's
  nothing to symlink install.  Perhaps you forgot to add EXPORT or INCLUDES
  DESTINATION?