feat(ur): complete RTDE driver with blocking motion and full telemetry

armctl/universal_robots/protocols/codes.py

@@ -0,0 +1,36 @@
+from __future__ import annotations
+
+from enum import IntEnum
+
+
+class RobotMode(IntEnum):
+    NO_CONTROLLER = -1
+    DISCONNECTED = 0
+    CONFIRM_SAFETY = 1
+    BOOTING = 2
+    POWER_OFF = 3
+    POWER_ON = 4
+    IDLE = 5
+    BACKDRIVE = 6
+    RUNNING = 7
+    UPDATING_FIRMWARE = 8
+
+
+class SafetyMode(IntEnum):
+    NORMAL = 1
+    REDUCED = 2
+    PROTECTIVE_STOP = 3
+    RECOVERY = 4
+    SAFEGUARD_STOP = 5
+    SYSTEM_EMERGENCY_STOP = 6
+    ROBOT_EMERGENCY_STOP = 7
+    VIOLATION = 8
+    FAULT = 9
+
+
+class RuntimeState(IntEnum):
+    STOPPED = 0
+    PLAYING = 1
+    PAUSED = 2
+    PAUSING = 3
+    RESUMING = 4

armctl/universal_robots/protocols/config.xml

@@ -1,11 +1,51 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <rtde_config>
     <recipe key="out">
-        <field name="actual_TCP_pose" type="VECTOR6D" />
+        <!-- Pose / kinematics -->
         <field name="actual_q" type="VECTOR6D" />
+        <field name="actual_qd" type="VECTOR6D" />
+        <field name="actual_current" type="VECTOR6D" />
         <field name="actual_current_as_torque" type="VECTOR6D" />
+        <field name="actual_TCP_pose" type="VECTOR6D" />
+        <field name="actual_TCP_speed" type="VECTOR6D" />
         <field name="actual_TCP_force" type="VECTOR6D" />
+        <field name="target_q" type="VECTOR6D" />
+        <field name="target_qd" type="VECTOR6D" />
+        <field name="target_TCP_pose" type="VECTOR6D" />
+        <field name="target_TCP_speed" type="VECTOR6D" />
+        <!-- Controller state -->
+        <field name="robot_mode" type="INT32" />
+        <field name="safety_mode" type="INT32" />
+        <field name="runtime_state" type="UINT32" />
         <field name="robot_status_bits" type="UINT32" />
         <field name="safety_status_bits" type="UINT32" />
+        <field name="speed_scaling" type="DOUBLE" />
+        <field name="payload" type="DOUBLE" />
+        <field name="payload_cog" type="VECTOR3D" />
+        <!-- Standard I/O -->
+        <field name="standard_analog_input0" type="DOUBLE" />
+        <field name="standard_analog_input1" type="DOUBLE" />
+        <field name="standard_analog_output0" type="DOUBLE" />
+        <field name="standard_analog_output1" type="DOUBLE" />
+        <field name="standard_digital_input_bits" type="UINT32" />
+        <field name="standard_digital_output_bits" type="UINT32" />
+        <!-- Completion register -->
+        <field name="output_int_registers_0" type="INT32" />
+        <!-- Tool I/O -->
+        <field name="tool_analog_input0" type="DOUBLE" />
+        <field name="tool_analog_input1" type="DOUBLE" />
+        <field name="tool_output_voltage" type="INT32" />
+        <field name="tool_output_current" type="DOUBLE" />
+        <field name="tool_temperature" type="DOUBLE" />
+    </recipe>
+    <recipe key="in">
+        <field name="speed_slider_mask" type="UINT32" />
+        <field name="speed_slider_fraction" type="DOUBLE" />
+        <field name="standard_digital_output_mask" type="UINT32" />
+        <field name="standard_digital_output" type="UINT32" />
+        <field name="standard_analog_output_mask" type="UINT32" />
+        <field name="standard_analog_output_type" type="UINT32" />
+        <field name="standard_analog_output_0" type="DOUBLE" />
+        <field name="standard_analog_output_1" type="DOUBLE" />
     </recipe>
-</rtde_config>
+</rtde_config>

armctl/universal_robots/protocols/rtde.py

@@ -1,8 +1,11 @@
 from __future__ import annotations
 
+import time
+from ctypes import c_uint32
 from pathlib import Path
 from typing import NewType
-from ctypes import c_uint32
+
+from .codes import RobotMode, RuntimeState, SafetyMode
 
 try:
     from rtde import RTDE as _RTDE
@@ -13,22 +16,26 @@
 
 u32 = NewType("u32", c_uint32)
 
+_MOVING_VELOCITY_THRESHOLD = 1e-3  # rad/s — below this on all joints = stopped
+
 
 class RTDE:
     def __init__(self, ip: str):
         if _RTDE is None or ConfigFile is None:
             raise ImportError(
-                "Universal Robots RTDE support requires additional dependencies."
+                "Universal Robots RTDE support requires additional dependencies. "
                 "Install it with: pip install armctl[ur]"
             )
 
         config_file = Path(__file__).parent / "config.xml"
         config = ConfigFile(str(config_file))
-        state_names, state_types = config.get_recipe("out")
+        out_names, out_types = config.get_recipe("out")
+        in_names, in_types = config.get_recipe("in")
 
         self.c = _RTDE(ip)
         self.c.connect()
-        self.c.send_output_setup(state_names, state_types)
+        self.c.send_output_setup(out_names, out_types)
+        self._input = self.c.send_input_setup(in_names, in_types)
         self.controller_version = (
             self.c.get_controller_version()
         )  # (MAJOR, MINOR, BUGFIX, BUILD)
@@ -40,29 +47,71 @@ def _get_data(self):
         return self.c.receive()
 
     def joint_angles(self) -> list[float]:
-        """Return joint angles in radians."""
+        """Return actual joint angles in radians."""
         return list(self._get_data().actual_q)
 
-    def tcp_pose(self) -> list[float]:
-        """Return TCP pose [x, y, z, rx, ry, rz]."""
-        return list(self._get_data().actual_TCP_pose)
+    def joint_velocities(self) -> list[float]:
+        """Return actual joint velocities in rad/s."""
+        return list(self._get_data().actual_qd)
+
+    def joint_currents(self) -> list[float]:
+        """Return actual joint currents in Amperes."""
+        return list(self._get_data().actual_current)
 
     def joint_torques(self) -> list[float]:
-        """Return joint torques in Nm converted from current."""
-        # See: https://www.universal-robots.com/articles/ur/release-notes/release-note-software-version-523x/
+        """Return joint torques in Nm converted from current.
+
+        Requires controller >= 5.23.0.0.
+        See: https://www.universal-robots.com/articles/ur/release-notes/release-note-software-version-523x/
+        """
         if self.controller_version >= (5, 23, 0, 0):
             return list(self._get_data().actual_current_as_torque)
-        else:
-            raise NotImplementedError(
-                "Joint torques not available for controller versions below 5.23.0.0"
-            )
+        raise NotImplementedError(
+            "Joint torques not available for controller versions below 5.23.0.0"
+        )
+
+    def tcp_pose(self) -> list[float]:
+        """Return actual TCP pose [x, y, z, rx, ry, rz] in metres and radians."""
+        return list(self._get_data().actual_TCP_pose)
+
+    def tcp_speed(self) -> list[float]:
+        """Return actual TCP speed [vx, vy, vz, wx, wy, wz] in m/s and rad/s."""
+        return list(self._get_data().actual_TCP_speed)
 
     def tcp_force(self) -> list[float]:
-        """Return TCP force [Fx, Fy, Fz, Tx, Ty, Tz] in Newton and Newton-meters."""
+        """Return TCP force [Fx, Fy, Fz, Tx, Ty, Tz] in Newton and Newton-metres."""
         return list(self._get_data().actual_TCP_force)
 
+    def target_joint_positions(self) -> list[float]:
+        """Return target joint positions in radians."""
+        return list(self._get_data().target_q)
+
+    def target_joint_velocities(self) -> list[float]:
+        """Return target joint velocities in rad/s."""
+        return list(self._get_data().target_qd)
+
+    def target_tcp_pose(self) -> list[float]:
+        """Return target TCP pose [x, y, z, rx, ry, rz] in metres and radians."""
+        return list(self._get_data().target_TCP_pose)
+
+    def target_tcp_speed(self) -> list[float]:
+        """Return target TCP speed [vx, vy, vz, wx, wy, wz] in m/s and rad/s."""
+        return list(self._get_data().target_TCP_speed)
+
+    def robot_mode(self) -> RobotMode:
+        """Return robot mode as a RobotMode enum."""
+        return RobotMode(self._get_data().robot_mode)
+
+    def safety_mode(self) -> SafetyMode:
+        """Return safety mode as a SafetyMode enum."""
+        return SafetyMode(self._get_data().safety_mode)
+
+    def runtime_state(self) -> RuntimeState:
+        """Return runtime state as a RuntimeState enum."""
+        return RuntimeState(self._get_data().runtime_state)
+
     def robot_status(self) -> dict[str, bool]:
-        """Return robot status.
+        """Return robot and safety status bit fields.
 
         Robot status bits (UINT32):
         - **`Bit 0`**: Is power on
@@ -108,3 +157,148 @@ def robot_status(self) -> dict[str, bool]:
             "Fault": bit(ssb, 9),
             "Stopped Due to Safety": bit(ssb, 10),
         }
+
+    def speed_scaling(self) -> float:
+        """Return current speed scaling factor in [0, 1]."""
+        return float(self._get_data().speed_scaling)
+
+    def payload(self) -> dict:
+        """Return payload mass (kg) and centre of gravity (m)."""
+        data = self._get_data()
+        return {
+            "mass_kg": float(data.payload),
+            "cog": list(data.payload_cog),
+        }
+
+    def analog_inputs(self) -> dict[str, float]:
+        """Return standard and tool analog input values (V or mA)."""
+        data = self._get_data()
+        return {
+            "standard_0": float(data.standard_analog_input0),
+            "standard_1": float(data.standard_analog_input1),
+            "tool_0": float(data.tool_analog_input0),
+            "tool_1": float(data.tool_analog_input1),
+        }
+
+    def analog_outputs(self) -> dict[str, float]:
+        """Return standard analog output values (V or mA)."""
+        data = self._get_data()
+        return {
+            "standard_0": float(data.standard_analog_output0),
+            "standard_1": float(data.standard_analog_output1),
+        }
+
+    def digital_inputs(self) -> dict[str, bool]:
+        """Return digital input states.
+
+        Bits 0-7:   Standard DI 0-7
+        Bits 8-15:  Configurable DI 0-7
+        Bits 16-17: Tool DI 0-1
+        """
+        bits = self._get_data().standard_digital_input_bits
+        bit = lambda n: bool(bits & (1 << n))
+        return {
+            **{f"DI{i}": bit(i) for i in range(8)},
+            **{f"CDI{i}": bit(i + 8) for i in range(8)},
+            "Tool_DI0": bit(16),
+            "Tool_DI1": bit(17),
+        }
+
+    def digital_outputs(self) -> dict[str, bool]:
+        """Return digital output states.
+
+        Bits 0-7:   Standard DO 0-7
+        Bits 8-15:  Configurable DO 0-7
+        Bits 16-17: Tool DO 0-1
+        """
+        bits = self._get_data().standard_digital_output_bits
+        bit = lambda n: bool(bits & (1 << n))
+        return {
+            **{f"DO{i}": bit(i) for i in range(8)},
+            **{f"CDO{i}": bit(i + 8) for i in range(8)},
+            "Tool_DO0": bit(16),
+            "Tool_DO1": bit(17),
+        }
+
+    def tool_io(self) -> dict:
+        """Return tool I/O state."""
+        data = self._get_data()
+        return {
+            "analog_input_0": float(data.tool_analog_input0),
+            "analog_input_1": float(data.tool_analog_input1),
+            "output_voltage": int(data.tool_output_voltage),
+            "output_current": float(data.tool_output_current),
+            "temperature": float(data.tool_temperature),
+        }
+
+    def is_moving(self, threshold: float = _MOVING_VELOCITY_THRESHOLD) -> bool:
+        """Return True if any joint velocity exceeds threshold (rad/s)."""
+        return any(abs(v) > threshold for v in self._get_data().actual_qd)
+
+    def wait_until_stopped(
+        self, timeout: float = 120.0, poll_interval: float = 0.05
+    ) -> None:
+        """Block until all joints are stopped or timeout expires.
+
+        Waits for motion to begin, then polls joint velocities via RTDE
+        until all fall below _MOVING_VELOCITY_THRESHOLD.
+
+        Parameters
+        ----------
+        timeout : float
+            Maximum seconds to wait after motion starts.
+        poll_interval : float
+            RTDE poll interval in seconds.
+
+        Raises
+        ------
+        TimeoutError
+            If the robot does not stop within timeout.
+        """
+        deadline = time.time() + timeout
+        while time.time() < deadline:
+            if self._get_data().output_int_registers_0 == 1:
+                return
+            time.sleep(poll_interval)
+        raise TimeoutError(f"Robot did not stop within {timeout}s")
+
+    def set_speed_slider(self, fraction: float) -> None:
+        """Set speed override slider fraction in [0.0, 1.0]."""
+        if not 0.0 <= fraction <= 1.0:
+            raise ValueError(
+                f"Speed fraction must be in [0, 1], got {fraction}"
+            )
+        self._input.speed_slider_mask = 1
+        self._input.speed_slider_fraction = fraction
+        self.c.send(self._input)
+
+    def set_digital_output(self, pin: int, value: bool) -> None:
+        """Set a standard digital output pin (0-7) high or low."""
+        if not 0 <= pin <= 7:
+            raise ValueError(f"Digital output pin must be 0-7, got {pin}")
+        mask = 1 << pin
+        self._input.standard_digital_output_mask = mask
+        self._input.standard_digital_output = mask if value else 0
+        self.c.send(self._input)
+
+    def set_analog_output(self, channel: int, value: float) -> None:
+        """Set standard analog output voltage (channel 0 or 1).
+
+        Parameters
+        ----------
+        channel : int
+            Output channel — 0 or 1.
+        value : float
+            Output value in Volts [0, 10] (voltage mode).
+        """
+        if channel not in (0, 1):
+            raise ValueError(
+                f"Analog output channel must be 0 or 1, got {channel}"
+            )
+        self._input.standard_analog_output_mask = 1 << channel
+        self._input.standard_analog_output_type = 0  # 0 = voltage mode
+        if channel == 0:
+            self._input.standard_analog_output_0 = value
+        else:
+            self._input.standard_analog_output_1 = value
+        self.c.send(self._input)