Add force mode controller #1049

URJala · 2024-07-08T14:16:57Z

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

…ller

URJala · 2024-07-09T06:06:48Z

Also this is dependent on PR for the service definitiion.

Also add example of using force mode controller

fmauch · 2024-10-10T11:03:12Z

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 · 2024-10-14T17:14:38Z

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 · 2024-10-14T20:17:53Z

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 · 2024-10-16T16:37:51Z

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...

Currently, this isn't working as expected and we don't really have a method to counteract the robot "jumping" back to the joint configuration without force mode.

Since it isn't compatible with streaming controllers, we deactivate it for now. It can be activated at runtime.

This should be more thread-safe, as the buffers are filled from the same thread as the write function is called.

domire8 · 2024-10-17T05:33:47Z

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.

Also added tests for that

Since the force_mode controller should be able to run with that, we need to update resource checking.

Since those are now in the service interface, we can delete them.

…is stopped

VinDp

It looks good and works as intended, I just have few suggestions/questions :)

ur_controllers/src/force_mode_controller.cpp

ur_robot_driver/src/hardware_interface.cpp

ur_robot_driver/examples/force_mode.py

Co-authored-by: Vincenzo Di Pentima <DiPentima@fzi.de>

…ode controller

This enables using the robot's force_mode in a standalone controller. Force mode can be enabled and parametrized using a service call provided by the controller. --------- Co-authored-by: urmarp <marp@universal-robots.com> Co-authored-by: Felix Exner <feex@universal-robots.com> (cherry picked from commit e949995) # Conflicts: # ur_controllers/controller_plugins.xml # ur_controllers/doc/index.rst # ur_robot_driver/CMakeLists.txt # ur_robot_driver/config/ur_controllers.yaml # ur_robot_driver/doc/usage/controllers.rst # ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp # ur_robot_driver/launch/ur_control.launch.py # ur_robot_driver/src/hardware_interface.cpp # ur_robot_driver/test/integration_test_controller_switch.py # ur_robot_driver/test/robot_driver.py # ur_robot_driver/urdf/ur.ros2_control.xacro

This enables using the robot's force_mode in a standalone controller. Force mode can be enabled and parametrized using a service call provided by the controller. --------- Co-authored-by: urmarp <marp@universal-robots.com> Co-authored-by: Felix Exner <feex@universal-robots.com>

This enables using the robot's force_mode in a standalone controller. Force mode can be enabled and parametrized using a service call provided by the controller. --------- Co-authored-by: URJala <159417921+URJala@users.noreply.github.com> Co-authored-by: urmarp <marp@universal-robots.com>

This enables using the robot's force_mode in a standalone controller. Force mode can be enabled and parametrized using a service call provided by the controller. --------- Co-authored-by: urmarp <marp@universal-robots.com> Co-authored-by: Felix Exner <feex@universal-robots.com>

urmarp and others added 7 commits July 4, 2024 10:34

Added service to set force mode

dedc846

Use tf_prefix for force_mode handles

f372025

Fix stuff

040c76a

More bug fixing

3c52986

Add service to disable force mode

f0c5ea6

Use an own controller for force_mode

949610c

Add force mode arguments: damping and gain scale to force mode contro…

62f02ea

…ller

Update controller to match updated service definition

b454a76

Also add example of using force mode controller

fmauch mentioned this pull request Jul 16, 2024

Added service to control force mode from ROS2 #579

Closed

6 tasks

fmauch added 3 commits October 9, 2024 11:48

Merge remote-tracking branch 'origin/main' into force_mode_arguments

3e758b1

Make communication thread-safe

d96b096

Add a service to disable force mode again

ca2ecc1

fmauch changed the title ~~Force mode arguments~~ Add force mode controller Oct 10, 2024

Formatting controller param file

0e91444

fmauch added 3 commits October 16, 2024 15:03

Remove superflous cmd interface write

6f77e23

Merge remote-tracking branch 'origin/main' into force_mode_arguments

577e9fc

Add required command interfaces to xacro

5b07594

fmauch added 3 commits October 16, 2024 19:18

Disable starting force_mode_controller with joint streaming

8fdf95d

Currently, this isn't working as expected and we don't really have a method to counteract the robot "jumping" back to the joint configuration without force mode.

Do not activate force_mode_controller by default

950629d

Since it isn't compatible with streaming controllers, we deactivate it for now. It can be activated at runtime.

Move force_mode communication from AsyncIO to write

b30fb33

This should be more thread-safe, as the buffers are filled from the same thread as the write function is called.

fmauch marked this pull request as draft October 16, 2024 17:48

urfeex added 3 commits November 7, 2024 13:09

Update to changed message definition

51ddaa6

Added first test for force_mode_controller

d8b533c

Fix interface compatibility check in prepare_switch

efbb144

Also added tests for that

urfeex added 9 commits November 8, 2024 17:13

Move force_mode_controller integration test to separate file

255b192

Merge upstream changes with passthrough controller

2a0b387

Since the force_mode controller should be able to run with that, we need to update resource checking.

Remove beta feature warning

3bb779b

Code formatting

84903de

Verify inputs

849873d

Remove unused parameters

c4f050e

Since those are now in the service interface, we can delete them.

Make sure (and test) that the robot stops when force_mode controller …

c34c7d1

…is stopped

Added check (with tests) for gain_scaling and damping_factor limits

a4930f9

Add basic documentation about force_mode controller

875b3ee

urfeex added the backport-humble label Nov 12, 2024

urfeex marked this pull request as ready for review November 12, 2024 14:24

urfeex requested a review from VinDp November 12, 2024 14:24

urfeex self-assigned this Nov 14, 2024

urfeex linked an issue Nov 14, 2024 that may be closed by this pull request

Added service to control force mode from ROS2 #771

Closed

5 tasks

Make sure tests use floating point numbers

6256577

VinDp approved these changes Nov 23, 2024

View reviewed changes

ur_controllers/src/force_mode_controller.cpp Outdated Show resolved Hide resolved

ur_robot_driver/src/hardware_interface.cpp Outdated Show resolved Hide resolved

ur_robot_driver/examples/force_mode.py Outdated Show resolved Hide resolved

urfeex and others added 5 commits November 25, 2024 09:16

Apply suggestions from code review

9ec6565

Co-authored-by: Vincenzo Di Pentima <DiPentima@fzi.de>

Code formatting

ed53ddc

Do not require force_mode being inactive when trying to start force m…

a325f18

…ode controller

Update force_mode example

53e9351

Merge branch 'main' into force_mode_arguments

fce2702

urfeex merged commit e949995 into UniversalRobots:main Nov 27, 2024
7 of 12 checks passed

mergify bot mentioned this pull request Nov 27, 2024

Add force mode controller (backport #1049) #1193

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add force mode controller #1049

Add force mode controller #1049

URJala commented Jul 8, 2024

URJala commented Jul 9, 2024

fmauch commented Oct 10, 2024 •

edited by urfeex

Loading

domire8 commented Oct 14, 2024 •

edited

Loading

fmauch commented Oct 14, 2024

fmauch commented Oct 16, 2024

domire8 commented Oct 17, 2024

VinDp left a comment

Add force mode controller #1049

Add force mode controller #1049

Conversation

URJala commented Jul 8, 2024

URJala commented Jul 9, 2024

fmauch commented Oct 10, 2024 • edited by urfeex Loading

Current ToDos:

domire8 commented Oct 14, 2024 • edited Loading

fmauch commented Oct 14, 2024

fmauch commented Oct 16, 2024

domire8 commented Oct 17, 2024

VinDp left a comment

Choose a reason for hiding this comment

fmauch commented Oct 10, 2024 •

edited by urfeex

Loading

domire8 commented Oct 14, 2024 •

edited

Loading