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.

[christophfroehlich]

Hi! Have you further worked on that topic?
There is already some work in this direction,
#1215
and more, but haven't been finished/merged yet
#1215
#560
ros-controls/control_toolbox#153

[ndunkelb]

Interesting! Glad to see that there is some work here.

I ended up going the route of making a custom force torque sensor broadcaster controller, with a lot of the same parameters as the admittance controller. This meant that I could do gravity compensation in the controller based on the COG information and the joint states of the robot paired with the KDL tree. With that information, it also means that I can call a custom zero service command that incorporates the expected contribution of the weight of the EE at that time.

I also made the controller chainable so that it could hook into a slightly modified admittance controller as well. The main changes I made to the admittance controller were to make it also accept chained FTS data rather than hooking straight into state interfaces, and I did some general cleaning because the out of the box implementation of the admittance controller has some bugs and some weird naming that I cleaned up (some of which looks like it is addressed in #560 .

Although one thing I did find and fix which I think is a relatively serious issue - I believe the force torque data used for the admittance computation is not projected to the control frame, and the calculations instead assume it is at the origin of the FT sensor. I can probably make another issue w.r.t. that if that is useful.

It looks like the zeroing is behavior is kind of achievable with the new parameter implementation you linked in #1215 , but requires an outside node to compute the parameters and then fill those in when relevant, which I guess is fine. But it also does not account for the gravity compensation. It looks like that is trying to be solved by #560, but it looks like that hasn't been touched in a while. Also, because that isn't chained with the FTS that has zeroing through parameters, I still don't think there is the ability to zero it for use with admittance.

tldr - I did find a solution that works for me, but it isn't trivial to get the code out. If you think that my strategy is useful for ros2_control though, I could probably find a way to get it out.