3rd Party Force Torque Sensor Integration with Gravity Compensation and Zeroing

[ndunkelb]

TLDR: The main controller that ros2 control provides out of the box that integrates with a FTS, the AdmittanceController, does not have a good way of handling gravity compensation of an EE if the HW interface does not provide a way to zero the FTS while accounting for orientation of the robot. As far as I can tell, the structure of ros2 control makes it hard to account for the orientation of the robot in a simple ros-y way given the current toolset.

Is your feature request related to a problem? Please describe.

This is potentially more a discussion than a specific request, but I have found myself in a use case where the architecture of ros2 control is making it a bit tricky to find a reasonable implementation. I am looking for a discussion on if I am missing a better idea, or if there are changes or contributions that might benefit the state of ros2 control/controllers in general.

My group is working on several different robots that have a mixture between integrated force torque sensors (like on UR e series), and 3rd party force torque sensors that we have written our own hardware interfaces for on robots that don't have integrated FTSs. We are trying to set up these combinations of hardware in a robot-agnostic way so that we can implement application-level software across our robots.

We are using ros2 control's AdmittanceController, and the current implementation works great on robots that have integrated FTSs. Robots with integrated FTSs work well because they can use the weight and COM of the EE stackup which has been provided by the user in combination with the orientation of the robot to do proper gravity compensation dependent on the orientation of the robot. Importantly, when you zero the FTS, it also accounts for the weight of the EE in that process.

The AdmittanceController implementation does provide EE gravity compensation for a provided FTS, but does not provide a way to zero the FTS. Because of this, it seems like this would be the responsibility of the FTS Hardware Interface to implement zeroing capabilities, but that would require a significant amount of information about the kinematics of the robot, or an external controller providing extra information to the FTS HWI through command interfaces about how to zero it given the weight of the EE in the current robot configuration.

Describe the solution you'd like

My solution to this was to create a ForceTorqueSensorBroadcasterAdvanced controller which implements the gravity compensation and zeroing capabilities at the controller level, and can connect to the robot position state interfaces along with loading a kinematics plugin (similar to how the AdmittanceController does this) to do the gravity compensation math. My hope with this is it would generalize to pretty much any FTS implementation given the appropriate controller config, and that you could basically just swap it in for a standard ForceTorqueSensorBroadcaster if you want this extra capability without changing the FTS hardware interface. This is implemented and working well, but I realized afterwards that the AdmittanceController is connecting directly to the FTS state interfaces, rather than the wrench topic provided by a broadcaster. As of now, my solution to this is to add force.x.correction, force.y.correction, etc command interfaces into the FTS hardware interfaces that I can connect my controller to so that it's state interface output of the standard force.x, force.y, etc are corrected and can be used by the AdmittanceController. This would work, but seems like a hacky way to accomplish the goal rather than a clean generalized solution.

Describe alternatives you've considered

There are a couple of other solutions that I have thought of, but none of them seem like the ideal scenario, and I would be interested to see if anyone else has ideas.

One solution could be to add an parameter which allows you to use a topic input (which could be provided by my custom ForceTorqueSensorBroadcasterAdvanced controller) from the FTS in the AdmittanceController, rather than basically reimplementing the functionality of the ForceTorqueSensorBroadcaster in the AdmittanceController like is currently done now. This would have the negative impact of adding another subscriber which could make performance slower.

Another solution could be to implement the zeroing capability in the AdmittanceController, which already has gravity compensation available, but it seems more appropriate to do this at a FTS level (either hardware interface or controller) so that anything else that might want to utilize the FTS data doesn't have to re-implement this functionality.

I feel like the most ros2_control-y way to solve this would be to allow chaining to the AdmittanceController from a fts broadcaster. I haven't implemented any chaining before, so I don't know exactly what that implementation looks like or if it is the right solution in this case.

Additional context

If any of these implementations seem like a good and general solution that could be useful to others, hopefully it could be contributed to the ros2 control infrastructure.