Add force mode controller

[URJala]

Added force mode arguments: damping and gain scaling to be usable with the force mode controller. damping and gain scaling will be changeable every time you start force mode when PR is implemented.
This is an update to PR

[URJala]

Also this is dependent on PR for the service definitiion.

[fmauch]

Current ToDos:

• Make sure the controller is only able to activate, when no conflicting controllers are running.
• Add documentation
• Decide on the interface
  • Wrench / WrenchStamped?
  • Split limits vector into two? (Speed and deviation)
• Add interfaces to ros2_control xacro

[domire8]

Hello, exciting to see this work in progress, we from AICA have already achieved force mode through ros2_control. My question is why you are implementing this through the async IOs and not as a full command mode in the write function? Is your goal to be able to have force mode active while another joint velocity controller is running, for instance?

Also, this PR is missing the updates in the xacro macros for the relevant command interface of the force_mode.

Happy to have a discussion, if there is interest from your side

[fmauch]

Hello, exciting to see this work in progress, we from AICA have already achieved force mode through ros2_control. My question is why you are implementing this through the async IOs and not as a full command mode in the write function? Is your goal to be able to have force mode active while another joint velocity controller is running, for instance?

Yes, exactly.

Also, this PR is missing the updates in the xacro macros for the relevant command interface of the force_mode.

Yes, this is still active work. But thanks for the hint I'll add it to the list.

Happy to have a discussion, if there is interest from your side

You don't happen to be at ROSCon next week?

[fmauch]

My question is why you are implementing this through the async IOs and not as a full command mode in the write function? Is your goal to be able to have force mode active while another joint velocity controller is running, for instance?

I've been thinking about this comment and in all honesty, I don't think there is a valid reason why communication is managed outside of write. Especially, since we are doing the communication from the controller to the HW interface in the update loop, it seems that this is even a problem in terms of thread safety...

[domire8]

I've been thinking about this comment and in all honesty, I don't think there is a valid reason why communication is managed outside of write. Especially, since we are doing the communication from the controller to the HW interface in the update loop, it seems that this is even a problem in terms of thread safety...

I can't say about thread safety, but I guess at the end the question is what you want to achieve. Our main goal for a force controller was to run force mode with an impedance controller as a primary control mode, while the robot has no other setpoints. For that, definitely the write function is the right place since the AsyncIO runs at a fixed (?) lower frequency. If however, you have applications in mind where one just enables force mode in one axis once and then uses joint velocity setpoints to drive the robot, the asyncio approach could work as well.