Add gravity compensation filter

[guihomework]

This is a rework of PR #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.

[destogl]

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

[destogl]

Are you sure this is correct?

[guihomework]

for the rotation, well, if I use the full transform, the -mg force along z world gets transformed from the world to sensor frame, generating a force matching -mg in the sensor frame, but also the cross product of the relative distance between sensor and world with the -mg, creating a torque. Gravity is not creating such a torque at the CoG. The only cross product is the distance of CoG to center of measurement with the simple gravity force at CoG (but oriented correctly).
The same calculation is done there https://github.com/ros-controls/ros2_controllers/blob/master/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp#L318

[christophfroehlich]

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.

[codecov-commenter]

Codecov Report

Attention: Patch coverage is 96.33028% with 4 lines in your changes missing coverage. Please review.

Project coverage is 77.31%. Comparing base (3ed348a) to head (9c01c3a).

Files with missing lines	Patch %	Lines
...olbox/src/control_filters/gravity_compensation.cpp	91.42%	2 Missing and 1 partial ⚠️
...test/control_filters/test_gravity_compensation.hpp	92.30%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@               Coverage Diff               @@
##           ros2-master     #153      +/-   ##
===============================================
+ Coverage        75.55%   77.31%   +1.76%     
===============================================
  Files               24       29       +5     
  Lines             1174     1283     +109     
  Branches            92       97       +5     
===============================================
+ Hits               887      992     +105     
- Misses             238      240       +2     
- Partials            49       51       +2     

Flag	Coverage Δ
unittests	77.31% <96.33%> (+1.76%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...x/include/control_filters/gravity_compensation.hpp	100.00% <100.00%> (ø)
...test/control_filters/test_gravity_compensation.cpp	100.00% <100.00%> (ø)
...control_filters/test_load_gravity_compensation.cpp	100.00% <100.00%> (ø)
...test/control_filters/test_gravity_compensation.hpp	92.30% <92.30%> (ø)
...olbox/src/control_filters/gravity_compensation.cpp	91.42% <91.42%> (ø)

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[saikishor]

Looks good. I haven't gone through the math

[saikishor]

The changes look good to me.

I'm just not sure about using lookup transform directly in the RT loop. We should take a look at it and improve it.

[guihomework]

I'm just not sure about using lookup transform directly in the RT loop. We should take a look at it and improve it.

There are thoughts and some explanation on the reason of using lookup transform in some out-dated/hidden remarks in the PR, but the main issue was that the computation requires to know something the kinematics/controllers did not know in the use case we had, especially if the real life sensor had a thickness, and the 'sensor_frame' was not at the flange (end-effector frame).
The second issue was probably that the filter just received a wrench, in some frame, and had to deal with it and convert the gravity field in that frame correctly to compensate for it.

I think it was decided to let it as is, but I agree it is not nice to have lookup transforms in an RT loop. I think it requires passing more information to the update loop if the lookup must be removed, so that the controller itself, having access to the kinematics can compute it, but it might also require the sensor position and CoG position of the mass to pre-compute those transforms for the filter. Not so trivial.

[saikishor]

@guihomework yes, I understood that it was used due to the end effector transformation. Maybe, it would be nice to have an update method with this extra transformation arg, so if a controller can compute it, then it can use this method to avoid it. This can be a future work

[christophfroehlich]

Thanks for the initial work and the discussion, I think we can merge this now!