Skip to content

Commit 641f876

Browse files
urfeexurmahp
urfeex
and
urmahp
authored
Rtde external ft (#388)
Add support for sending external force-torque measurements through RTDE. This can be especially useful if you want to inject fake measurements into a URSim simulated robot. There's also an example with a simple TUI doing exactly that. --------- Co-authored-by: Mads Holm Peters <[email protected]>
1 parent 173e3c3 commit 641f876

18 files changed

+925
-26
lines changed

‎doc/architecture/script_command_interface.rst‎

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
2121
<https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/tool_contact_example.cpp>`_
2222
for more information.
2323
- ``setFrictionCompensation()``: Set friction compensation for torque command.
24+
- ``ftRtdeInputEnable()``: Enable/disable FT RTDE input processing.
2425

2526
Communication protocol
2627
----------------------
@@ -50,6 +51,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
5051
- 5: startToolContact
5152
- 6: endToolContact
5253
- 7: setFrictionCompensation
54+
- 8: ftRtdeInputEnable
5355
1-27 data fields specific to the command
5456
===== =====
5557

@@ -132,11 +134,25 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
132134
1 friction_compensation_enabled enable/disable friction compensation for torque command.
133135
===== =====
134136

137+
.. table:: With ftRtdeInputEnable command. See script manual for details.
138+
:widths: auto
139+
140+
===== =====
141+
index meaning
142+
===== =====
143+
1 ft_rtde_input_enabled enable/disable FT RTDE input processing.
144+
2 sensor_mass in kg (floating point)
145+
3-5 sensor_mesurement_offset in m, displacement from the tool flange (3d floating point)
146+
6-9 sensor_cog in m, displacement from the tool flange (3d floating point)
147+
===== =====
148+
135149
.. note::
136150
In URScript the ``socket_read_binary_integer()`` function is used to read the data from the
137151
script command socket. The first index in that function's return value is the number of integers read,
138-
so the actual data starts at index 1. The indices in the table above are shifted by one when
139-
accessing the result array of the URScript function.
152+
so the actual data starts at index 1. The first data entry is always the command type, hence the
153+
indices in the table above are shifted by one when accessing the result array of the URScript
154+
function. E.g. reading the boolean value for friction compensation in the
155+
``setFrictionCompensation`` command would be done by accessing index 2 of the result array.
140156

141157
All floating point data is encoded into an integer representation and has to be divided by the
142158
``MULT_JOINTSTATE`` constant to get the actual floating point value. This constant is defined in

‎doc/examples.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ may be running forever until manually stopped.
2323
examples/primary_pipeline
2424
examples/primary_pipeline_calibration
2525
examples/rtde_client
26+
examples/external_fts_through_rtde
2627
examples/script_command_interface
2728
examples/script_sender
2829
examples/spline_example

‎doc/examples/external_fts_through_rtde.rst‎

Lines changed: 165 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/external_fts_through_rtde.rst
2+
3+
External FTS example
4+
====================
5+
6+
It is possible to feed data of an external Force-Torque-Sensor (FTS) into the robot controller
7+
through RTDE. This feature has to be explicitly enabled by calling the URScript function
8+
`ft_rtde_input_enable
9+
<https://www.universal-robots.com/manuals/EN/HTML/SW10_10/Content/prod-scriptmanual/all_scripts/ft_rtde_input_enable.htm?Highlight=ft_rtde_input_enable>`_.
10+
11+
Usually, that functionality would be used with a URCap running on the robot but data can also be
12+
routed through an external computer with a matching driver for that FTS. Keep in mind that the
13+
latency introduced by the network connection may have a negative impact on the performance.
14+
15+
Another use case could be to inject force-torque measurements into a simulated robot for testing
16+
purposes.
17+
18+
This example allows setting force values that are sent to the robot as external ft measurements.
19+
The terminal user interface (TUI) is pretty simple:
20+
21+
- Pressing the keys ``x``, ``y``, ``z``, ``a``, ``b``, or ``c`` will increase the respective
22+
force/torque component by 10 (N or Nm). (``a``, ``b``, and ``c`` correspond to torques around the
23+
x, y, and z axis respectively.)
24+
- Pressing the keys ``X``, ``Y``, ``Z``, ``A``, ``B``, or ``C`` will decrease the respective
25+
force/torque component by 10 (N or Nm).
26+
- Pressing ``0`` will reset all force/torque components to zero.
27+
- Pressing ``q`` will exit the program.
28+
29+
The example's source code can be found in `external_fts_through_rtde.cpp
30+
<https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/external_fts_through_rtde.cpp>`_.
31+
32+
33+
.. note:: This example requires the robot to be in *remote control mode*.
34+
35+
Robot initialization
36+
--------------------
37+
38+
The robot initialization is handed off to the ``ExampleRobotWrapper`` class:
39+
40+
.. literalinclude:: ../../examples/external_fts_through_rtde.cpp
41+
:language: c++
42+
:caption: external_fts_through_rtde/external_fts_through_rtde.cpp
43+
:linenos:
44+
:lineno-match:
45+
:start-at: g_my_robot = std::make_unique
46+
:end-before: // Enable using the force-torque
47+
48+
In order to use the RTDE input for the FTS, we have to enable it first by calling the
49+
``ft_rtde_input_enable`` function. This is done by sending a script to the robot through the
50+
``ftRtdeInputEnable()`` method of the ``UrDriver`` class.
51+
52+
.. literalinclude:: ../../examples/external_fts_through_rtde.cpp
53+
:language: c++
54+
:caption: external_fts_through_rtde/external_fts_through_rtde.cpp
55+
:linenos:
56+
:lineno-match:
57+
:start-at: // Enable using the force-torque
58+
:end-at: g_my_robot->getUrDriver()->ftRtdeInputEnable(true);
59+
60+
RTDE communication is moved to a separate thread in order to use the main thread for handling
61+
keyboard input commands.
62+
63+
Input key handling
64+
------------------
65+
66+
For handling the keyboard input, define a ``getChar()`` function that reads a single character from
67+
the terminal without waiting for a newline character. This has to be done differently on Windows
68+
and Linux (and other unix-like systems), therefore the code is split using preprocessor directives.
69+
70+
.. literalinclude:: ../../examples/external_fts_through_rtde.cpp
71+
:language: c++
72+
:caption: external_fts_through_rtde/external_fts_through_rtde.cpp
73+
:linenos:
74+
:lineno-match:
75+
:start-at: // Platform-specific implementation of getChar()
76+
:end-at: #endif
77+
78+
Setting the external force-torque values
79+
----------------------------------------
80+
81+
The result from the tui function is synchonized to the RTDE communication thread using a buffer ``g_FT_VEC`` and a mutex.
82+
83+
.. literalinclude:: ../../examples/external_fts_through_rtde.cpp
84+
:language: c++
85+
:caption: external_fts_through_rtde/external_fts_through_rtde.cpp
86+
:linenos:
87+
:lineno-match:
88+
:start-at: std::scoped_lock<std::mutex> lock(g_FT_VEC_MUTEX);
89+
:end-at: g_FT_VEC = local_ft_vec;
90+
91+
.. literalinclude:: ../../examples/external_fts_through_rtde.cpp
92+
:language: c++
93+
:caption: external_fts_through_rtde/external_fts_through_rtde.cpp
94+
:linenos:
95+
:lineno-match:
96+
:start-at: if (g_FT_VEC_MUTEX.try_lock())
97+
:end-at: }
98+
99+
Note that the ``try_lock()`` method is used in the RTDE thread to avoid blocking the thread if the
100+
tui thread is currently holding the lock. The new values will be available in a later cycle then.
101+
102+
.. note::
103+
The values reported from the robot are rotated to the base coordinate system of the robot.
104+
Hence, they will not be the same as the ones sent to the robot. For them to be the same, align
105+
the TCP frame with robots base frame.
106+
107+
Example output
108+
--------------
109+
The following shows an example output.
110+
.. code::
111+
112+
[1760084283.005655] INFO ur_client_library/src/primary/primary_client.cpp 67: Starting primary client pipeline
113+
[1760084283.056577] INFO ur_client_library/src/ur/dashboard_client.cpp 76: Connected: Universal Robots Dashboard Server
114+
115+
[1760084283.060021] INFO ur_client_library/src/example_robot_wrapper.cpp 201: Robot ready to start a program
116+
[1760084283.060652] INFO ur_client_library/src/helpers.cpp 95: SCHED_FIFO OK, priority 99
117+
[1760084283.061806] INFO ur_client_library/src/rtde/rtde_client.cpp 139: Negotiated RTDE protocol version to 2.
118+
[1760084283.061944] INFO ur_client_library/src/rtde/rtde_client.cpp 291: Setting up RTDE communication with frequency 500.000000
119+
Program running: false
120+
121+
[1760084284.095662] INFO ur_client_library/src/primary/primary_client.cpp 67: Starting primary client pipeline
122+
[1760084284.225584] INFO ur_client_library/src/control/reverse_interface.cpp 225: Robot connected to reverse interface. Ready to receive control commands.
123+
Program running: true
124+
125+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
126+
[1760084284.225944] INFO ur_client_library/src/helpers.cpp 95: SCHED_FIFO OK, priority 99
127+
[1760084284.227630] INFO ur_client_library/src/control/reverse_interface.cpp 238: Connection to reverse interface dropped.
128+
Program running: false
129+
130+
Force-torque reported by robot: [0, 0, 0, 0, 0, 0]
131+
Force-torque reported by robot: [0, 0, 0, 0, 0, 0]
132+
<'x' pressed>
133+
Artificial FT input: [10, 0, 0, 0, 0, 0]
134+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
135+
<'y' pressed>
136+
Artificial FT input: [10, 10, 0, 0, 0, 0]
137+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
138+
<'y' pressed>
139+
Artificial FT input: [10, 20, 0, 0, 0, 0]
140+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
141+
Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0]
142+
Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0]
143+
Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0]
144+
Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0]
145+
<'Y' pressed>
146+
Artificial FT input: [10, 10, 0, 0, 0, 0]
147+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
148+
Force-torque reported by robot: [10, 10, -3.67436e-15, 0, 0, 0]
149+
Force-torque reported by robot: [10, 10, -3.67436e-15, 0, 0, 0]
150+
<'c' pressed>
151+
Artificial FT input: [10, 10, 0, 0, 0, 10]
152+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
153+
Force-torque reported by robot: [10, 10, -3.67436e-15, 2.05104e-09, -2.05103e-09, 10]
154+
<'X' pressed>
155+
Artificial FT input: [0, 10, 0, 0, 0, 10]
156+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
157+
Force-torque reported by robot: [2.05103e-09, 10, 2.05103e-09, 2.05104e-09, -2.05103e-09, 10]
158+
<'0' pressed>
159+
Artificial FT input: [0, 0, 0, 0, 0, 0]
160+
Press x,y,z to increase the respective axes, 0 for reset, q for quit.
161+
Force-torque reported by robot: [0, 0, 0, 0, 0, 0]
162+
Force-torque reported by robot: [0, 0, 0, 0, 0, 0]
163+
<'q' pressed>
164+
Artificial FT input: [0, 0, 0, 0, 0, 0]
165+
[1760084296.656255] INFO ur_client_library/src/primary/primary_client.cpp 61: Stopping primary client pipeline

‎examples/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,3 +59,7 @@ target_link_libraries(trajectory_point_interface_example ur_client_library::urcl
5959
add_executable(instruction_executor
6060
instruction_executor.cpp)
6161
target_link_libraries(instruction_executor ur_client_library::urcl)
62+
63+
add_executable(external_fts_through_rtde
64+
external_fts_through_rtde.cpp)
65+
target_link_libraries(external_fts_through_rtde ur_client_library::urcl)

0 commit comments

Comments
 (0)