11from __future__ import annotations
22
3+ import time
4+ from ctypes import c_uint32
35from pathlib import Path
46from typing import NewType
5- from ctypes import c_uint32
7+
8+ from .codes import RobotMode , RuntimeState , SafetyMode
69
710try :
811 from rtde import RTDE as _RTDE
1316
1417u32 = NewType ("u32" , c_uint32 )
1518
19+ _MOVING_VELOCITY_THRESHOLD = 1e-3 # rad/s — below this on all joints = stopped
20+
1621
1722class RTDE :
1823 def __init__ (self , ip : str ):
1924 if _RTDE is None or ConfigFile is None :
2025 raise ImportError (
21- "Universal Robots RTDE support requires additional dependencies."
26+ "Universal Robots RTDE support requires additional dependencies. "
2227 "Install it with: pip install armctl[ur]"
2328 )
2429
2530 config_file = Path (__file__ ).parent / "config.xml"
2631 config = ConfigFile (str (config_file ))
27- state_names , state_types = config .get_recipe ("out" )
32+ out_names , out_types = config .get_recipe ("out" )
33+ in_names , in_types = config .get_recipe ("in" )
2834
2935 self .c = _RTDE (ip )
3036 self .c .connect ()
31- self .c .send_output_setup (state_names , state_types )
37+ self .c .send_output_setup (out_names , out_types )
38+ self ._input = self .c .send_input_setup (in_names , in_types )
3239 self .controller_version = (
3340 self .c .get_controller_version ()
3441 ) # (MAJOR, MINOR, BUGFIX, BUILD)
@@ -40,29 +47,71 @@ def _get_data(self):
4047 return self .c .receive ()
4148
4249 def joint_angles (self ) -> list [float ]:
43- """Return joint angles in radians."""
50+ """Return actual joint angles in radians."""
4451 return list (self ._get_data ().actual_q )
4552
46- def tcp_pose (self ) -> list [float ]:
47- """Return TCP pose [x, y, z, rx, ry, rz]."""
48- return list (self ._get_data ().actual_TCP_pose )
53+ def joint_velocities (self ) -> list [float ]:
54+ """Return actual joint velocities in rad/s."""
55+ return list (self ._get_data ().actual_qd )
56+
57+ def joint_currents (self ) -> list [float ]:
58+ """Return actual joint currents in Amperes."""
59+ return list (self ._get_data ().actual_current )
4960
5061 def joint_torques (self ) -> list [float ]:
51- """Return joint torques in Nm converted from current."""
52- # See: https://www.universal-robots.com/articles/ur/release-notes/release-note-software-version-523x/
62+ """Return joint torques in Nm converted from current.
63+
64+ Requires controller >= 5.23.0.0.
65+ See: https://www.universal-robots.com/articles/ur/release-notes/release-note-software-version-523x/
66+ """
5367 if self .controller_version >= (5 , 23 , 0 , 0 ):
5468 return list (self ._get_data ().actual_current_as_torque )
55- else :
56- raise NotImplementedError (
57- "Joint torques not available for controller versions below 5.23.0.0"
58- )
69+ raise NotImplementedError (
70+ "Joint torques not available for controller versions below 5.23.0.0"
71+ )
72+
73+ def tcp_pose (self ) -> list [float ]:
74+ """Return actual TCP pose [x, y, z, rx, ry, rz] in metres and radians."""
75+ return list (self ._get_data ().actual_TCP_pose )
76+
77+ def tcp_speed (self ) -> list [float ]:
78+ """Return actual TCP speed [vx, vy, vz, wx, wy, wz] in m/s and rad/s."""
79+ return list (self ._get_data ().actual_TCP_speed )
5980
6081 def tcp_force (self ) -> list [float ]:
61- """Return TCP force [Fx, Fy, Fz, Tx, Ty, Tz] in Newton and Newton-meters ."""
82+ """Return TCP force [Fx, Fy, Fz, Tx, Ty, Tz] in Newton and Newton-metres ."""
6283 return list (self ._get_data ().actual_TCP_force )
6384
85+ def target_joint_positions (self ) -> list [float ]:
86+ """Return target joint positions in radians."""
87+ return list (self ._get_data ().target_q )
88+
89+ def target_joint_velocities (self ) -> list [float ]:
90+ """Return target joint velocities in rad/s."""
91+ return list (self ._get_data ().target_qd )
92+
93+ def target_tcp_pose (self ) -> list [float ]:
94+ """Return target TCP pose [x, y, z, rx, ry, rz] in metres and radians."""
95+ return list (self ._get_data ().target_TCP_pose )
96+
97+ def target_tcp_speed (self ) -> list [float ]:
98+ """Return target TCP speed [vx, vy, vz, wx, wy, wz] in m/s and rad/s."""
99+ return list (self ._get_data ().target_TCP_speed )
100+
101+ def robot_mode (self ) -> RobotMode :
102+ """Return robot mode as a RobotMode enum."""
103+ return RobotMode (self ._get_data ().robot_mode )
104+
105+ def safety_mode (self ) -> SafetyMode :
106+ """Return safety mode as a SafetyMode enum."""
107+ return SafetyMode (self ._get_data ().safety_mode )
108+
109+ def runtime_state (self ) -> RuntimeState :
110+ """Return runtime state as a RuntimeState enum."""
111+ return RuntimeState (self ._get_data ().runtime_state )
112+
64113 def robot_status (self ) -> dict [str , bool ]:
65- """Return robot status.
114+ """Return robot and safety status bit fields .
66115
67116 Robot status bits (UINT32):
68117 - **`Bit 0`**: Is power on
@@ -108,3 +157,148 @@ def robot_status(self) -> dict[str, bool]:
108157 "Fault" : bit (ssb , 9 ),
109158 "Stopped Due to Safety" : bit (ssb , 10 ),
110159 }
160+
161+ def speed_scaling (self ) -> float :
162+ """Return current speed scaling factor in [0, 1]."""
163+ return float (self ._get_data ().speed_scaling )
164+
165+ def payload (self ) -> dict :
166+ """Return payload mass (kg) and centre of gravity (m)."""
167+ data = self ._get_data ()
168+ return {
169+ "mass_kg" : float (data .payload ),
170+ "cog" : list (data .payload_cog ),
171+ }
172+
173+ def analog_inputs (self ) -> dict [str , float ]:
174+ """Return standard and tool analog input values (V or mA)."""
175+ data = self ._get_data ()
176+ return {
177+ "standard_0" : float (data .standard_analog_input0 ),
178+ "standard_1" : float (data .standard_analog_input1 ),
179+ "tool_0" : float (data .tool_analog_input0 ),
180+ "tool_1" : float (data .tool_analog_input1 ),
181+ }
182+
183+ def analog_outputs (self ) -> dict [str , float ]:
184+ """Return standard analog output values (V or mA)."""
185+ data = self ._get_data ()
186+ return {
187+ "standard_0" : float (data .standard_analog_output0 ),
188+ "standard_1" : float (data .standard_analog_output1 ),
189+ }
190+
191+ def digital_inputs (self ) -> dict [str , bool ]:
192+ """Return digital input states.
193+
194+ Bits 0-7: Standard DI 0-7
195+ Bits 8-15: Configurable DI 0-7
196+ Bits 16-17: Tool DI 0-1
197+ """
198+ bits = self ._get_data ().standard_digital_input_bits
199+ bit = lambda n : bool (bits & (1 << n ))
200+ return {
201+ ** {f"DI{ i } : bit (i ) for i in range (8 )},
202+ ** {f"CDI{ i } : bit (i + 8 ) for i in range (8 )},
203+ "Tool_DI0" : bit (16 ),
204+ "Tool_DI1" : bit (17 ),
205+ }
206+
207+ def digital_outputs (self ) -> dict [str , bool ]:
208+ """Return digital output states.
209+
210+ Bits 0-7: Standard DO 0-7
211+ Bits 8-15: Configurable DO 0-7
212+ Bits 16-17: Tool DO 0-1
213+ """
214+ bits = self ._get_data ().standard_digital_output_bits
215+ bit = lambda n : bool (bits & (1 << n ))
216+ return {
217+ ** {f"DO{ i } : bit (i ) for i in range (8 )},
218+ ** {f"CDO{ i } : bit (i + 8 ) for i in range (8 )},
219+ "Tool_DO0" : bit (16 ),
220+ "Tool_DO1" : bit (17 ),
221+ }
222+
223+ def tool_io (self ) -> dict :
224+ """Return tool I/O state."""
225+ data = self ._get_data ()
226+ return {
227+ "analog_input_0" : float (data .tool_analog_input0 ),
228+ "analog_input_1" : float (data .tool_analog_input1 ),
229+ "output_voltage" : int (data .tool_output_voltage ),
230+ "output_current" : float (data .tool_output_current ),
231+ "temperature" : float (data .tool_temperature ),
232+ }
233+
234+ def is_moving (self , threshold : float = _MOVING_VELOCITY_THRESHOLD ) -> bool :
235+ """Return True if any joint velocity exceeds threshold (rad/s)."""
236+ return any (abs (v ) > threshold for v in self ._get_data ().actual_qd )
237+
238+ def wait_until_stopped (
239+ self , timeout : float = 120.0 , poll_interval : float = 0.05
240+ ) -> None :
241+ """Block until all joints are stopped or timeout expires.
242+
243+ Waits for motion to begin, then polls joint velocities via RTDE
244+ until all fall below _MOVING_VELOCITY_THRESHOLD.
245+
246+ Parameters
247+ ----------
248+ timeout : float
249+ Maximum seconds to wait after motion starts.
250+ poll_interval : float
251+ RTDE poll interval in seconds.
252+
253+ Raises
254+ ------
255+ TimeoutError
256+ If the robot does not stop within timeout.
257+ """
258+ deadline = time .time () + timeout
259+ while time .time () < deadline :
260+ if self ._get_data ().output_int_registers_0 == 1 :
261+ return
262+ time .sleep (poll_interval )
263+ raise TimeoutError (f"Robot did not stop within { timeout } )
264+
265+ def set_speed_slider (self , fraction : float ) -> None :
266+ """Set speed override slider fraction in [0.0, 1.0]."""
267+ if not 0.0 <= fraction <= 1.0 :
268+ raise ValueError (
269+ f"Speed fraction must be in [0, 1], got { fraction }
270+ )
271+ self ._input .speed_slider_mask = 1
272+ self ._input .speed_slider_fraction = fraction
273+ self .c .send (self ._input )
274+
275+ def set_digital_output (self , pin : int , value : bool ) -> None :
276+ """Set a standard digital output pin (0-7) high or low."""
277+ if not 0 <= pin <= 7 :
278+ raise ValueError (f"Digital output pin must be 0-7, got { pin } )
279+ mask = 1 << pin
280+ self ._input .standard_digital_output_mask = mask
281+ self ._input .standard_digital_output = mask if value else 0
282+ self .c .send (self ._input )
283+
284+ def set_analog_output (self , channel : int , value : float ) -> None :
285+ """Set standard analog output voltage (channel 0 or 1).
286+
287+ Parameters
288+ ----------
289+ channel : int
290+ Output channel — 0 or 1.
291+ value : float
292+ Output value in Volts [0, 10] (voltage mode).
293+ """
294+ if channel not in 0 , 1 ):
295+ raise ValueError (
296+ f"Analog output channel must be 0 or 1, got { channel }
297+ )
298+ self ._input .standard_analog_output_mask = 1 << channel
299+ self ._input .standard_analog_output_type = 0 # 0 = voltage mode
300+ if channel == 0 :
301+ self ._input .standard_analog_output_0 = value
302+ else :
303+ self ._input .standard_analog_output_1 = value
304+ self .c .send (self ._input )
0 commit comments