Add wrench offset for admittance controller

[firesurfer]

This PR adds an additional WrenchStamped subscriber to the AdmittanceController in order to allow specifying a Wrench offset.

The basic idea behind this PR is to simply add the offset to the measured force values which has the controller to produce an offset into the desired direction. I tested it on an UR16e where it seems to work quite well.

Is this a desired feature? What do you think of it? Perhaps there are better ways to implement this feature in this controller.

[saikishor]

In general, the changes look good to me. I've left some minor comments. Please take a look

[saikishor]

@firesurfer can you remove the draft status on the title?

[firesurfer]

@saikishor I removed the draft status.

[firesurfer]

Any of the other reviewers willing to take a look at this ?

[bmagyar]

Do you have any ideas for a test for this by any chance?

[firesurfer]

The way I tested it on hardware was: Load the controller and then command 1N into a single direction. One will then see a motion of the arm in that direction. I guess it would be possible to implement something similar in an unit test.

[firesurfer]

@destogl When browsing through the tests I found this:

    // TODO(destogl): comment in when using unified mode
    //     if (controller_->admittance_->unified_mode_) {
    //       force_command_publisher_->publish(force_msg);
    //     }
    //    pose_command_publisher_->publish(pose_msg);

Apparently there was some support for a force command planned but not implemented? Is this right?

[destogl]

Generally, great!

What I would like to know is, do we want to enable this input even admittance controller is in a chain? As you did right now. I am more on a yes side, but would like to hear opinions.

[destogl]

Why don't we transform the wrench if it is in another frame to the sensor frame?

[firesurfer]

I had some very weird results in the past when transforming wrenches with TF (e.g. non zero torques afterwards when only forces where given). Additionally I don't think we have a tf buffer/listener in the controller at the moment and why add this additional overhead?