Support various sensor measurement conventions

[stefanscherzinger]

Problem

Currently, we assume force-torque sensor measurements to be given in

• the robot's base frame (robot_base_link in the controller's .yaml) (for foxy)
• a user-specified frame (ft_sensor_ref_link in the controller's .yaml) (for galactic)

The behavior of foxy is a somewhat Universal Robots ROS2 driver (e-Series) specific adjustment during development.

Goal

Rethink this again and try to support both variants in both foxy and galactic. It should be possible for users to paramterize this in configuration files.

[stefanscherzinger]

Is related to #54

[ksotebeer]

Hi, really cool software package you have here! I've been experimenting with it a bit and came across this issue as I'm running foxy and my force torque sensor data is relative to the end effector frame rather than the base frame. I could transform it to the base frame if needed, but figured I'd open the discussion about handling it here in the controller.

Since the force torque data is a WrenchStamped message, I wanted to get your thoughts on just interpreting the sensor data based on the message header.frame_id. This could be done for both foxy and galactic and the ft_sensor_ref_link parameter could be removed.

I've implemented this in my fork of the repo so I'm happy to submit a PR if you think it could be a potential solution.

[stefanscherzinger]

Hi @ksotebeer

Thanks for contributing your thoughts! I Looked at your fork and it seems very promising. It would be awesome if you could open a PR :)

This could be done for both foxy and galactic and the ft_sensor_ref_link parameter could be removed.

I like this approach. It seems reasonable to use the header.frame_id for this purpose. However, we should make sure to emit some warning when this frame_id is not part of the kinematics chain and would hence fail when transforming the sensor quantities to the base frame.

Where you able to test your changes on real hardware?
I'm tempted to add some integration tests to check if the behavior is as expected.

[ksotebeer]

Hey sorry for the delayed response, I've been travelling for the past week. I will clean up my branch and add some warnings then open a PR.

So far I've only been testing in simulation but I plan to test on hardware within the next month or two. I have a custom 7DOF arm I'll be testing on but currently having some issues with the force torque sensor that I need to work out.