diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 482e5d2..07990e1 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -39,7 +39,7 @@ jobs: - name: Install dependencies run: | - uv sync --locked --all-groups --extra zaber + uv sync --locked --all-groups --all-extras - name: Clear all caches and compiled files run: | diff --git a/README.md b/README.md index 1f10f09..0978ab8 100644 --- a/README.md +++ b/README.md @@ -78,9 +78,8 @@ The `armctl` library currently supports the following manufacturers and robot mo pip install armctl ``` -> **Note:** Universal Robots (UR) and Zaber support require optional extras: +> **Note:** Zaber support requires the optional extra: > ```bash -> pip install armctl[ur] > pip install armctl[zaber] > ``` @@ -342,8 +341,3 @@ Please feel free to submit a pull request or open an issue for any enhancements ## License This project is licensed under the MIT License. See the [LICENSE](https://github.com/MGross21/armctl/blob/main/LICENSE) file for more details. - -### Notice - -> This package automatically installs the [Universal Robots RTDE Python Client Library](https://github.com/UniversalRobots/RTDE_Python_Client_Library) when needed. -> The RTDE library is provided by Universal Robots and is subject to their licensing terms. diff --git a/armctl/universal_robots/README.md b/armctl/universal_robots/README.md index fc1eae1..97596de 100644 --- a/armctl/universal_robots/README.md +++ b/armctl/universal_robots/README.md @@ -18,7 +18,8 @@ RTDE is a proprietary protocol for real-time data synchronization with Universal - [Complete I/O Documentation](https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/) - [RTDE Python Client Documentation](https://docs.universal-robots.com/tutorials/communication-protocol-tutorials/rtde-python-client-guide.html) - [RTDE Python Client Library (GitHub)](https://github.com/UniversalRobots/RTDE_Python_Client_Library) - _This project uses the official RTDE Python Client Library._ + _Protocol reference only; armctl bundles its own RTDE client in `protocols/`._ +- [RTDE Guide (docs site)](https://docs.universal-robots.com/tutorials/communication-protocol-tutorials/rtde-guide.html): input recipe ownership (`IN_USE`) and recipe limits. **Note:** The following third-party libraries are not currently used in this project but are provided for reference: diff --git a/armctl/universal_robots/protocols/config.py b/armctl/universal_robots/protocols/config.py new file mode 100644 index 0000000..4252be9 --- /dev/null +++ b/armctl/universal_robots/protocols/config.py @@ -0,0 +1,114 @@ +from __future__ import annotations + +import ctypes +from enum import Enum + + +class RTDEType(Enum): + BOOL = ctypes.c_bool + UINT8 = ctypes.c_uint8 + UINT32 = ctypes.c_uint32 + UINT64 = ctypes.c_uint64 + INT32 = ctypes.c_int32 + DOUBLE = ctypes.c_double + VECTOR3D = ctypes.c_double * 3 + VECTOR6D = ctypes.c_double * 6 + VECTOR6INT32 = ctypes.c_int32 * 6 + VECTOR6UINT32 = ctypes.c_uint32 * 6 + + +class Recipe(ctypes.BigEndianStructure): + """RTDE data package: recipe id followed by the recipe's fields in order.""" + + _pack_ = 1 + _layout_ = "ms" + _fields_ = [("recipe_id", ctypes.c_uint8)] + names: tuple[str, ...] = () + rtde_types: tuple[str, ...] = () + + def __init_subclass__(cls, **kwargs): + super().__init_subclass__(**kwargs) + own = cls.__dict__.get("_fields_", ()) + cls.names = (*cls.names, *(name for name, _ in own)) + cls.rtde_types = (*cls.rtde_types, *(RTDEType(ctype).name for _, ctype in own)) + + def __init__(self, **values): + unknown = values.keys() - {"recipe_id", *self.names} + if unknown: + raise TypeError(f"{type(self).__name__} has no fields {sorted(unknown)}") + super().__init__(**values) + + @classmethod + def parse_setup(cls, payload: bytes) -> int | None: + """Return the recipe id from a setup reply; None if any field is IN_USE.""" + types = tuple(payload[1:].decode().split(",")) + if "IN_USE" in types: + return None + if types != cls.rtde_types: + bad = [f"{n}: {a}" for n, e, a in zip(cls.names, cls.rtde_types, types) if a != e] + raise RuntimeError(f"RTDE {cls.__name__} mismatch (name: controller type): {bad}") + return payload[0] + + +class OutputData(Recipe): + _fields_ = [ + ("actual_q", ctypes.c_double * 6), + ("actual_qd", ctypes.c_double * 6), + ("actual_current", ctypes.c_double * 6), + ("actual_TCP_pose", ctypes.c_double * 6), + ("actual_TCP_speed", ctypes.c_double * 6), + ("actual_TCP_force", ctypes.c_double * 6), + ("target_q", ctypes.c_double * 6), + ("target_qd", ctypes.c_double * 6), + ("target_TCP_pose", ctypes.c_double * 6), + ("target_TCP_speed", ctypes.c_double * 6), + ("robot_mode", ctypes.c_int32), + ("safety_mode", ctypes.c_int32), + ("runtime_state", ctypes.c_uint32), + ("robot_status_bits", ctypes.c_uint32), + ("safety_status_bits", ctypes.c_uint32), + ("speed_scaling", ctypes.c_double), + ("payload", ctypes.c_double), + ("payload_cog", ctypes.c_double * 3), + ("standard_analog_input0", ctypes.c_double), + ("standard_analog_input1", ctypes.c_double), + ("standard_analog_output0", ctypes.c_double), + ("standard_analog_output1", ctypes.c_double), + ("actual_digital_input_bits", ctypes.c_uint64), + ("actual_digital_output_bits", ctypes.c_uint64), + ("output_int_register_0", ctypes.c_int32), + ("tool_analog_input0", ctypes.c_double), + ("tool_analog_input1", ctypes.c_double), + ("tool_output_voltage", ctypes.c_int32), + ("tool_output_current", ctypes.c_double), + ("tool_temperature", ctypes.c_double), + ] + + +class TorqueOutputData(OutputData): + """OutputData plus fields that require controller >= 5.23.""" + + _fields_ = [("actual_current_as_torque", ctypes.c_double * 6)] + + +class SpeedSliderInput(Recipe): + _fields_ = [ + ("speed_slider_mask", ctypes.c_uint32), + ("speed_slider_fraction", ctypes.c_double), + ] + + +class DigitalOutputInput(Recipe): + _fields_ = [ + ("standard_digital_output_mask", ctypes.c_uint8), + ("standard_digital_output", ctypes.c_uint8), + ] + + +class AnalogOutputInput(Recipe): + _fields_ = [ + ("standard_analog_output_mask", ctypes.c_uint8), + ("standard_analog_output_type", ctypes.c_uint8), + ("standard_analog_output_0", ctypes.c_double), + ("standard_analog_output_1", ctypes.c_double), + ] diff --git a/armctl/universal_robots/protocols/config.xml b/armctl/universal_robots/protocols/config.xml deleted file mode 100644 index 2a901ad..0000000 --- a/armctl/universal_robots/protocols/config.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/armctl/universal_robots/protocols/rtde.py b/armctl/universal_robots/protocols/rtde.py index 53d8fa2..e94a34a 100644 --- a/armctl/universal_robots/protocols/rtde.py +++ b/armctl/universal_robots/protocols/rtde.py @@ -1,48 +1,39 @@ from __future__ import annotations import time -from ctypes import c_uint32 -from pathlib import Path -from typing import NewType from .codes import RobotMode, RuntimeState, SafetyMode - -try: - from rtde import RTDE as _RTDE - from rtde.rtde_config import ConfigFile -except ImportError: - _RTDE = None - ConfigFile = None - -u32 = NewType("u32", c_uint32) - -_MOVING_VELOCITY_THRESHOLD = 1e-3 # rad/s — below this on all joints = stopped +from .config import ( + AnalogOutputInput, + DigitalOutputInput, + OutputData, + Recipe, + SpeedSliderInput, + TorqueOutputData, +) +from .rtde_core import RTDEClient class RTDE: def __init__(self, ip: str): - if _RTDE is None or ConfigFile is None: - raise ImportError( - "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)) - 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(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) - self.c.send_start() + self._client = RTDEClient(ip) + self._client.connect() + # (MAJOR, MINOR, BUGFIX, BUILD) + self.controller_version = self._client.get_controller_version() + self._client.send_output_setup( + TorqueOutputData if self.controller_version >= (5, 23, 0, 0) else OutputData + ) + self._input_ids = { + recipe: self._client.send_input_setup(recipe) + for recipe in (SpeedSliderInput, DigitalOutputInput, AnalogOutputInput) + } + self._client.send_start() + + def disconnect(self) -> None: + self._client.disconnect() def _get_data(self): - if not self.c.is_connected(): - raise ConnectionError("RTDE connection has been lost.") - return self.c.receive() + return self._client.receive() def joint_angles(self) -> list[float]: """Return actual joint angles in radians.""" @@ -131,8 +122,8 @@ def robot_status(self) -> dict[str, bool]: - **`Bit 10`**: Is stopped due to safety """ data = self._get_data() - rsb: u32 = data.robot_status_bits - ssb: u32 = data.safety_status_bits + rsb = data.robot_status_bits + ssb = data.safety_status_bits def bit(data, n): return bool(data & (1 << n)) @@ -194,7 +185,7 @@ def digital_inputs(self) -> dict[str, bool]: Bits 8-15: Configurable DI 0-7 Bits 16-17: Tool DI 0-1 """ - bits = self._get_data().standard_digital_input_bits + bits = self._get_data().actual_digital_input_bits def bit(n): return bool(bits & (1 << n)) @@ -213,7 +204,7 @@ def digital_outputs(self) -> dict[str, bool]: Bits 8-15: Configurable DO 0-7 Bits 16-17: Tool DO 0-1 """ - bits = self._get_data().standard_digital_output_bits + bits = self._get_data().actual_digital_output_bits def bit(n): return bool(bits & (1 << n)) @@ -236,51 +227,56 @@ def tool_io(self) -> dict: "temperature": float(data.tool_temperature), } - def is_moving(self, threshold: float = _MOVING_VELOCITY_THRESHOLD) -> bool: + def is_moving(self, threshold: float = 1e-3) -> 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. + def get_last_move_seqno(self) -> int: + """Return the sequence number of the last completed move program.""" + return self._get_data().output_int_register_0 - Waits for motion to begin, then polls joint velocities via RTDE - until all fall below _MOVING_VELOCITY_THRESHOLD. + def wait_for_move( + self, seqno: int, timeout: float = 120.0, poll_interval: float = 0.05 + ) -> None: + """Block until the move program writes seqno to output int register 0. Parameters ---------- + seqno : int + Sequence number written by the move program on completion. timeout : float - Maximum seconds to wait after motion starts. + Maximum seconds to wait. poll_interval : float RTDE poll interval in seconds. Raises ------ TimeoutError - If the robot does not stop within timeout. + If the move does not finish within timeout. """ - deadline = time.time() + timeout - while time.time() < deadline: - if self._get_data().output_int_registers_0 == 1: + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if self.get_last_move_seqno() == seqno: return time.sleep(poll_interval) - raise TimeoutError(f"Robot did not stop within {timeout}s") + raise TimeoutError(f"Move {seqno} did not finish 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) + self._send(SpeedSliderInput, speed_slider_mask=1, speed_slider_fraction=fraction) 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) + self._send( + DigitalOutputInput, + standard_digital_output_mask=mask, + standard_digital_output=mask if value else 0, + ) def set_analog_output(self, channel: int, value: float) -> None: """Set standard analog output voltage (channel 0 or 1). @@ -294,10 +290,17 @@ def set_analog_output(self, channel: int, value: float) -> None: """ 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) + if not 0.0 <= value <= 10.0: + raise ValueError(f"Analog output voltage must be in [0, 10], got {value}") + self._send( + AnalogOutputInput, + standard_analog_output_mask=1 << channel, + standard_analog_output_type=1 << channel, # set bit = voltage mode + **{f"standard_analog_output_{channel}": value / 10.0}, # 0..1 ratio + ) + + def _send(self, recipe: type[Recipe], **fields: float) -> None: + recipe_id = self._input_ids[recipe] + if recipe_id is None: + raise RuntimeError(f"RTDE {recipe.__name__} fields are in use by another client") + self._client.send(recipe(recipe_id=recipe_id, **fields)) diff --git a/armctl/universal_robots/protocols/rtde_core.py b/armctl/universal_robots/protocols/rtde_core.py new file mode 100644 index 0000000..885c713 --- /dev/null +++ b/armctl/universal_robots/protocols/rtde_core.py @@ -0,0 +1,117 @@ +from __future__ import annotations + +import select +import socket +import struct +from enum import IntEnum + +from .config import OutputData, Recipe + + +class Command(IntEnum): + REQUEST_PROTOCOL_VERSION = ord("V") + GET_URCONTROL_VERSION = ord("v") + TEXT_MESSAGE = ord("M") + DATA_PACKAGE = ord("U") + SETUP_OUTPUTS = ord("O") + SETUP_INPUTS = ord("I") + START = ord("S") + + +class RTDEClient: + _HEADER = struct.Struct(">HB") # packet size incl. header, command + + def __init__(self, hostname: str, port: int = 30004, timeout: float = 1.0): + self.address = (hostname, port) + self.timeout = timeout + self._sock: socket.socket | None = None + self._buffer = bytearray() + self._output: type[OutputData] = OutputData + + def connect(self) -> None: + if self._sock is not None: + return + self._sock = socket.create_connection(self.address, self.timeout) + self._sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + if self._request(Command.REQUEST_PROTOCOL_VERSION, struct.pack(">H", 2)) != b"\x01": + raise RuntimeError("Unable to negotiate RTDE protocol version") + + def disconnect(self) -> None: + if self._sock is not None: + self._sock.close() + self._sock = None + self._buffer.clear() + + def get_controller_version(self) -> tuple[int, int, int, int]: + return struct.unpack(">IIII", self._request(Command.GET_URCONTROL_VERSION)) + + def send_output_setup( + self, recipe: type[OutputData] = OutputData, frequency: float = 125 + ) -> None: + request = struct.pack(">d", frequency) + ",".join(recipe.names).encode() + recipe.parse_setup(self._request(Command.SETUP_OUTPUTS, request)) + self._output = recipe + + def send_input_setup(self, recipe: type[Recipe]) -> int | None: + """Register an input recipe; return its id, or None if another client owns a field.""" + return recipe.parse_setup( + self._request(Command.SETUP_INPUTS, ",".join(recipe.names).encode()) + ) + + def send_start(self) -> None: + if self._request(Command.START) != b"\x01": + raise RuntimeError("RTDE controller refused to start synchronization") + + def send(self, data: Recipe) -> None: + self._send(Command.DATA_PACKAGE, bytes(data)) + + def receive(self) -> OutputData: + """Return the newest buffered data package, blocking until one arrives.""" + while select.select([self._socket], [], [], 0)[0]: + self._recv() + payload = self._read_packet(Command.DATA_PACKAGE) + while self._packet_ready(): + payload = self._read_packet(Command.DATA_PACKAGE) + return self._output.from_buffer_copy(payload) + + @property + def _socket(self) -> socket.socket: + if self._sock is None: + raise ConnectionError("RTDE socket not connected") + return self._sock + + def _request(self, command: Command, payload: bytes = b"") -> bytes: + self._send(command, payload) + return self._read_packet(command) + + def _send(self, command: Command, payload: bytes) -> None: + self._socket.sendall(self._HEADER.pack(self._HEADER.size + len(payload), command) + payload) + + def _packet_ready(self) -> bool: + return ( + len(self._buffer) >= self._HEADER.size + and len(self._buffer) >= self._HEADER.unpack_from(self._buffer)[0] + ) + + def _read_packet(self, expected: Command) -> bytes: + """Pop the next packet's payload, skipping text messages.""" + while True: + while not self._packet_ready(): + self._recv() + size, command = self._HEADER.unpack_from(self._buffer) + payload = bytes(self._buffer[self._HEADER.size : size]) + del self._buffer[:size] + if command == expected: + return payload + if command != Command.TEXT_MESSAGE: + raise RuntimeError(f"Unexpected RTDE command {command}, expected {expected!r}") + + def _recv(self) -> None: + try: + chunk = self._socket.recv(65536) + except socket.timeout as exc: + raise TimeoutError("Timed out waiting for RTDE data") from exc + if not chunk: + self.disconnect() + raise ConnectionError("RTDE socket disconnected") + self._buffer.extend(chunk) diff --git a/armctl/universal_robots/universal_robots.py b/armctl/universal_robots/universal_robots.py index 3d6a624..250bc3e 100644 --- a/armctl/universal_robots/universal_robots.py +++ b/armctl/universal_robots/universal_robots.py @@ -1,5 +1,6 @@ from __future__ import annotations +import ctypes import math import time as _time @@ -34,11 +35,13 @@ def __init__(self, ip: str, port: int | tuple[int, int] = 30_002): # Source: https://forum.universal-robots.com/t/maximum-axis-speed-acceleration/13338/4 self.MAX_JOINT_ACCELERATION = 10.0 # rad/s^2 self.rtde: RTDE | None = None + self._seqno = ctypes.c_int32() @override def connect(self): super().connect() self.rtde = RTDE(self.ip) + self._seqno.value = self.rtde.get_last_move_seqno() @override def disconnect(self): @@ -47,7 +50,7 @@ def disconnect(self): except Exception: pass if self.rtde is not None: - self.rtde.c.disconnect() + self.rtde.disconnect() self.rtde = None super().disconnect() @@ -70,7 +73,6 @@ def move_joints( speed: float = 0.1, acceleration: float = 0.05, t: float = 0.0, - radius: float = 0.0, ) -> None: """MoveJ — move to joint positions. @@ -84,20 +86,10 @@ def move_joints( Acceleration in rad/s^2. t : float Move duration in seconds (overrides speed/acceleration when > 0). - radius : float - Blend radius in metres. Non-zero disables blocking at this waypoint. """ cc.move_joints(self, pos, speed, acceleration) - command = ( - f"write_output_integer_register(0,0)\n" - f"movej([{','.join(map(str, pos))}]," - f" a={acceleration}, v={speed}, t={t}, r={radius})\n" - f"write_output_integer_register(0,1)\n" - ) - self.send_command(command, timeout=t + 10, suppress_output=True) - if radius == 0.0: - timeout = (t + 15.0) if t > 0 else 120.0 - self._rtde.wait_until_stopped(timeout=timeout) + joints = ",".join(map(str, pos)) + self._run_move(f"movej([{joints}], a={acceleration}, v={speed}, t={t})", t) @override def move_cartesian( @@ -107,7 +99,6 @@ def move_cartesian( speed: float = 0.1, acceleration: float = 0.1, time: float = 0.0, - radius: float = 0.0, ) -> None: """Move to Cartesian pose. @@ -123,32 +114,24 @@ def move_cartesian( Acceleration in m/s^2. time : float Move duration in seconds (overrides speed/acceleration when > 0). - radius : float - Blend radius in metres. Non-zero disables blocking at this waypoint. """ if move_type not in {"movel", "movep", "movej"}: raise ValueError("Unsupported move type. Use 'movel', 'movep', or 'movej'.") cc.move_cartesian(self, pose) - base = f"{move_type}(p[{','.join(map(str, pose))}], a={acceleration}, v={speed}" - if move_type in {"movel", "movej"}: - command = ( - "write_output_integer_register(0,0)\n" - f"{base}, t={time}, r={radius})\n" - "write_output_integer_register(0,1)\n" - ) - else: # movep - command = ( - "write_output_integer_register(0,0)\n" - f"{base}, r={radius})\n" - "write_output_integer_register(0,1)\n" - ) - - self.send_command(command, suppress_output=True) - if radius == 0.0: - timeout = (time + 15.0) if time > 0 else 120.0 - self._rtde.wait_until_stopped(timeout=timeout) + duration = "" if move_type == "movep" else f", t={time}" + target = ",".join(map(str, pose)) + self._run_move(f"{move_type}(p[{target}], a={acceleration}, v={speed}{duration})", time) + + def _run_move(self, motion: str, duration: float) -> None: + """Run ``motion`` as one program and block until done.""" + self._seqno.value += 1 + seqno = self._seqno.value + program = f"def armctl_move():\n{motion}\nwrite_output_integer_register(0,{seqno})\nend\n" + self.send_command(program, timeout=duration + 10, suppress_output=True) + timeout = (duration + 15.0) if duration > 0 else 120.0 + self._rtde.wait_for_move(seqno, timeout=timeout) @override def stop_motion(self) -> None: diff --git a/pyproject.toml b/pyproject.toml index b74e08c..5657dab 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -6,12 +6,9 @@ build-backend = "uv_build" module-name = "armctl" module-root = "" -[tool.uv.sources] -urrtde = { git = "https://github.com/UniversalRobots/RTDE_Python_Client_Library.git", rev = "main" } - [project] name = "armctl" -version = "0.4.0" +version = "0.5.0" description = "Agnostic Robotic Manipulation Controller (armctl)" authors = [{ name = "Michael Gross", email = "MGross21@users.noreply.github.com" }] readme = {file = "README.md", content-type = "text/markdown"} @@ -35,9 +32,6 @@ dependencies = [ zaber = [ "zaber-motion>=2.13.1", ] -ur = [ - "urrtde @ git+https://github.com/UniversalRobots/RTDE_Python_Client_Library.git@main" -] [project.urls] Homepage = "https://github.com/MGross21/armctl" @@ -80,11 +74,10 @@ format.indent-style = "space" [tool.pytest.ini_options] testpaths = ["tests"] addopts = ["-ra", "--strict-markers"] -markers = ["integration: requires Docker and the ur extra (URSim)"] +markers = ["integration: requires Docker (URSim)"] [tool.pyright] pythonVersion = "3.9" venvPath = "." venv = ".venv" reportImplicitOverride = "error" -ignore = ["armctl/universal_robots/protocols/rtde.py"] diff --git a/tests/test_drivers.py b/tests/test_drivers.py index 1b0c23c..5988e50 100644 --- a/tests/test_drivers.py +++ b/tests/test_drivers.py @@ -41,22 +41,31 @@ def test_ur_move_joints_script(ur): ur.move_joints(HOME, speed=0.5, acceleration=0.2) ur.send_command.assert_called_once() assert ur.send_command.call_args.args[0] == ( - "write_output_integer_register(0,0)\n" - "movej([0.0,-1.57,1.57,0.0,0.0,0.0], a=0.2, v=0.5, t=0.0, r=0.0)\n" + "def armctl_move():\n" + "movej([0.0,-1.57,1.57,0.0,0.0,0.0], a=0.2, v=0.5, t=0.0)\n" "write_output_integer_register(0,1)\n" + "end\n" ) - ur.rtde.wait_until_stopped.assert_called_once_with(timeout=120.0) + ur.rtde.wait_for_move.assert_called_once_with(1, timeout=120.0) -def test_ur_blended_move_does_not_block(ur): - ur.move_joints(HOME, radius=0.05) - ur.rtde.wait_until_stopped.assert_not_called() +def test_ur_sequential_moves_wait_on_new_seqno(ur): + ur.move_joints(HOME) + ur.move_cartesian([0.1, 0.2, 0.3, 0.0, 3.14, 0.0]) + assert "write_output_integer_register(0,2)" in ur.send_command.call_args.args[0] + assert [c.args[0] for c in ur.rtde.wait_for_move.call_args_list] == [1, 2] + + +def test_ur_seqno_wraps_within_int32(ur): + ur._seqno.value = 2**31 - 1 + ur.move_joints(HOME) + ur.rtde.wait_for_move.assert_called_once_with(-(2**31), timeout=120.0) def test_ur_movep_omits_time(ur): ur.move_cartesian([0.1, 0.2, 0.3, 0.0, 3.14, 0.0], move_type="movep") script = ur.send_command.call_args.args[0] - assert "movep(p[0.1,0.2,0.3,0.0,3.14,0.0], a=0.1, v=0.1, r=0.0)" in script + assert "movep(p[0.1,0.2,0.3,0.0,3.14,0.0], a=0.1, v=0.1)" in script assert "t=" not in script diff --git a/tests/test_rtde_packets.py b/tests/test_rtde_packets.py new file mode 100644 index 0000000..28b11d3 --- /dev/null +++ b/tests/test_rtde_packets.py @@ -0,0 +1,240 @@ +"""Tests for RTDE packet structure and wire-format correctness.""" + +import ctypes +import socket +import struct + +import pytest + +from armctl.universal_robots.protocols.config import ( + AnalogOutputInput, + DigitalOutputInput, + OutputData, + SpeedSliderInput, + TorqueOutputData, +) +from armctl.universal_robots.protocols.rtde_core import Command, RTDEClient + + +class TestStructSizes: + def test_output_data_size(self): + assert ctypes.sizeof(OutputData) == 629 + + def test_torque_output_data_appends_torque(self): + assert ctypes.sizeof(TorqueOutputData) == 677 + assert TorqueOutputData.names == (*OutputData.names, "actual_current_as_torque") + + def test_input_data_sizes(self): + assert ctypes.sizeof(SpeedSliderInput) == 13 + assert ctypes.sizeof(DigitalOutputInput) == 3 + assert ctypes.sizeof(AnalogOutputInput) == 19 + + +class TestFieldNames: + def test_output_names_count(self): + assert len(OutputData.names) == 30 + + def test_output_names_order(self): + assert list(OutputData.names[:10]) == [ + "actual_q", + "actual_qd", + "actual_current", + "actual_TCP_pose", + "actual_TCP_speed", + "actual_TCP_force", + "target_q", + "target_qd", + "target_TCP_pose", + "target_TCP_speed", + ] + + def test_input_names_order(self): + assert list(SpeedSliderInput.names) == ["speed_slider_mask", "speed_slider_fraction"] + assert list(DigitalOutputInput.names) == [ + "standard_digital_output_mask", + "standard_digital_output", + ] + assert list(AnalogOutputInput.names) == [ + "standard_analog_output_mask", + "standard_analog_output_type", + "standard_analog_output_0", + "standard_analog_output_1", + ] + + +class TestOutputDataRoundtrip: + def test_from_buffer_copy_zero(self): + payload = bytes(ctypes.sizeof(OutputData)) + data = OutputData.from_buffer_copy(payload) + assert data.recipe_id == 0 + assert list(data.actual_q) == [0.0] * 6 + assert data.robot_mode == 0 + + def test_recipe_id_preserved(self): + raw = bytearray(ctypes.sizeof(OutputData)) + raw[0] = 42 + data = OutputData.from_buffer_copy(bytes(raw)) + assert data.recipe_id == 42 + + def test_double_vector_roundtrip(self): + raw = bytearray(ctypes.sizeof(OutputData)) + raw[0] = 1 + joints = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] + struct.pack_into(">6d", raw, 1, *joints) + data = OutputData.from_buffer_copy(bytes(raw)) + assert [round(v, 9) for v in data.actual_q] == [round(v, 9) for v in joints] + + def test_int32_field_roundtrip(self): + raw = bytearray(ctypes.sizeof(OutputData)) + # robot_mode is at offset 1 + 10*6*8 = 481 + struct.pack_into(">i", raw, 481, -7) + data = OutputData.from_buffer_copy(bytes(raw)) + assert data.robot_mode == -7 + + def test_uint32_field_roundtrip(self): + raw = bytearray(ctypes.sizeof(OutputData)) + # safety_status_bits at offset 481 + 4 + 4 + 4 + 4 = 497 + struct.pack_into(">I", raw, 497, 0b10110) + data = OutputData.from_buffer_copy(bytes(raw)) + assert data.safety_status_bits == 0b10110 + + +class TestInputDataRoundtrip: + def test_recipe_id_in_first_byte(self): + inp = SpeedSliderInput() + inp.recipe_id = 3 + raw = bytes(inp) + assert raw[0] == 3 + + def test_speed_slider_mask_big_endian(self): + inp = SpeedSliderInput() + inp.recipe_id = 1 + inp.speed_slider_mask = 0x12345678 + raw = bytes(inp) + assert raw[1:5] == b"\x12\x34\x56\x78" + + def test_speed_slider_fraction_big_endian(self): + inp = SpeedSliderInput() + inp.recipe_id = 1 + inp.speed_slider_fraction = 0.5 + raw = bytes(inp) + expected = struct.pack(">d", 0.5) + assert raw[5:13] == expected + + def test_roundtrip_via_from_buffer_copy(self): + inp = SpeedSliderInput() + inp.recipe_id = 7 + inp.speed_slider_mask = 1 + inp.speed_slider_fraction = 0.75 + raw = bytes(inp) + out = SpeedSliderInput.from_buffer_copy(raw) + assert out.recipe_id == 7 + assert out.speed_slider_mask == 1 + assert abs(out.speed_slider_fraction - 0.75) < 1e-12 + + +class TestRecipeTypes: + def test_input_types_match_spec(self): + assert list(SpeedSliderInput.rtde_types) == ["UINT32", "DOUBLE"] + assert list(DigitalOutputInput.rtde_types) == ["UINT8", "UINT8"] + assert list(AnalogOutputInput.rtde_types) == ["UINT8", "UINT8", "DOUBLE", "DOUBLE"] + + def test_torque_output_types_include_inherited_fields(self): + types = TorqueOutputData.rtde_types + assert len(types) == 31 + assert types[0] == types[-1] == "VECTOR6D" + + def test_parse_setup_returns_recipe_id(self): + payload = b"\x04" + ",".join(DigitalOutputInput.rtde_types).encode() + assert DigitalOutputInput.parse_setup(payload) == 4 + + def test_parse_setup_returns_none_when_in_use(self): + assert DigitalOutputInput.parse_setup(b"\x00IN_USE,UINT8") is None + + @pytest.mark.parametrize("bad", ["NOT_FOUND", "UINT32"]) + def test_parse_setup_rejects_mismatched_types(self, bad): + payload = b"\x00" + f"{bad},UINT8".encode() + with pytest.raises(RuntimeError, match="standard_digital_output_mask"): + DigitalOutputInput.parse_setup(payload) + + +class TestRecipeInit: + def test_keyword_fields(self): + data = DigitalOutputInput(recipe_id=3, standard_digital_output_mask=2) + assert bytes(data) == b"\x03\x02\x00" + + def test_rejects_unknown_field(self): + with pytest.raises(TypeError, match="standard_digital_ouput"): + DigitalOutputInput(recipe_id=1, standard_digital_ouput=1) + + def test_accepts_inherited_field(self): + assert TorqueOutputData(robot_mode=7).robot_mode == 7 + + +class TestReceive: + @staticmethod + def packet(command: Command, payload: bytes) -> bytes: + return struct.pack(">HB", len(payload) + 3, command) + payload + + @classmethod + def data_package(cls, robot_mode: int) -> bytes: + data = OutputData(recipe_id=1) + data.robot_mode = robot_mode + return cls.packet(Command.DATA_PACKAGE, bytes(data)) + + @pytest.fixture + def pair(self): + client = RTDEClient("unused") + client._sock, server = socket.socketpair() + client._sock.settimeout(1.0) + yield client, server + client.disconnect() + server.close() + + def test_returns_newest_package_and_skips_text(self, pair): + client, server = pair + server.sendall( + self.data_package(1) + + self.packet(Command.TEXT_MESSAGE, b"hi") + + self.data_package(2) + + self.data_package(3) + ) + assert client.receive().robot_mode == 3 + server.sendall(self.data_package(4)) + assert client.receive().robot_mode == 4 + + def test_rejects_unexpected_command(self, pair): + client, server = pair + server.sendall(self.packet(Command.START, b"\x01")) + with pytest.raises(RuntimeError, match="Unexpected RTDE command"): + client.receive() + + def test_input_setup_returns_none_when_in_use(self, pair): + client, server = pair + server.sendall(self.packet(Command.SETUP_INPUTS, b"\x00IN_USE,DOUBLE")) + assert client.send_input_setup(SpeedSliderInput) is None + + def test_input_setup_returns_recipe_id(self, pair): + client, server = pair + server.sendall(self.packet(Command.SETUP_INPUTS, b"\x02UINT8,UINT8")) + assert client.send_input_setup(DigitalOutputInput) == 2 + + def test_receive_uses_torque_recipe(self, pair): + client, server = pair + types = ",".join(TorqueOutputData.rtde_types).encode() + server.sendall(self.packet(Command.SETUP_OUTPUTS, b"\x01" + types)) + client.send_output_setup(TorqueOutputData) + data = TorqueOutputData(recipe_id=1) + data.actual_current_as_torque[5] = 1.5 + server.sendall(self.packet(Command.DATA_PACKAGE, bytes(data))) + assert client.receive().actual_current_as_torque[5] == 1.5 + + def test_disconnect_raises(self, pair): + client, server = pair + server.close() + with pytest.raises(ConnectionError): + client.receive() + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_ursim_docker.py b/tests/test_ursim_docker.py index fe2629a..f7728c1 100644 --- a/tests/test_ursim_docker.py +++ b/tests/test_ursim_docker.py @@ -30,7 +30,6 @@ import pytest pytestmark = pytest.mark.integration -pytest.importorskip("rtde", reason="requires the ur extra") if TYPE_CHECKING: from armctl import UR5 @@ -42,6 +41,7 @@ # Configuration constants URSIM_PORT = 30004 URSIM_PRIMARY_PORT = 30002 +URSIM_DASHBOARD_PORT = 29999 URSIM_CONTAINER_NAME = "ursim_test_container" URSIM_IMAGE = "universalrobots/ursim_e-series:latest" @@ -243,6 +243,7 @@ def get_ursim_port_mappings() -> dict[str, str]: return { str(URSIM_PORT): str(URSIM_PORT), # RTDE port str(URSIM_PRIMARY_PORT): str(URSIM_PRIMARY_PORT), # Primary socket port + str(URSIM_DASHBOARD_PORT): str(URSIM_DASHBOARD_PORT), "5900": "5900", # VNC "6080": "6080", # Web VNC } @@ -457,6 +458,31 @@ def test_ursim_error_handling(ursim_container, ur5_robot): logger.info("Error handling test passed") +def test_ursim_sequential_moves_block_until_arrival(ursim_container, ur5_robot): + from armctl.universal_robots.protocols.codes import RobotMode + + with socket.create_connection(("localhost", URSIM_DASHBOARD_PORT), timeout=5) as dash: + dash.recv(1024) + for cmd in (b"power on\n", b"brake release\n"): + dash.sendall(cmd) + dash.recv(1024) + deadline = time.monotonic() + 30 + while ur5_robot.rtde.robot_mode() != RobotMode.RUNNING: + assert time.monotonic() < deadline, "URSim did not reach RUNNING" + time.sleep(0.5) + + targets = [ + [0.0, -1.57, 1.57, 0.0, 0.0, 0.0], + [0.5, -1.2, 1.2, -0.3, 0.4, 0.2], + [0.0, -1.57, 1.57, 0.0, 0.0, 0.0], + ] + for target in targets: + ur5_robot.move_joints(target, speed=1.0, acceleration=1.0) + actual = ur5_robot.get_joint_positions() + assert max(abs(a - t) for a, t in zip(actual, target)) < 1e-3 + assert not ur5_robot.rtde.is_moving() + + def _create_robot_connection() -> UR5: """Helper function to create and verify robot connection for manual testing. diff --git a/uv.lock b/uv.lock index 181a8d6..faa7d3a 100644 --- a/uv.lock +++ b/uv.lock @@ -18,7 +18,7 @@ wheels = [ [[package]] name = "armctl" -version = "0.4.0" +version = "0.5.0" source = { editable = "." } dependencies = [ { name = "pyserial" }, @@ -28,9 +28,6 @@ dependencies = [ ] [package.optional-dependencies] -ur = [ - { name = "urrtde" }, -] zaber = [ { name = "zaber-motion" }, ] @@ -56,10 +53,9 @@ requires-dist = [ { name = "pyserial", specifier = ">=3.5" }, { name = "typer", specifier = ">=0.9.0" }, { name = "typing-extensions", specifier = ">=4.4.0" }, - { name = "urrtde", marker = "extra == 'ur'", git = "https://github.com/UniversalRobots/RTDE_Python_Client_Library.git?rev=main" }, { name = "zaber-motion", marker = "extra == 'zaber'", specifier = ">=2.13.1" }, ] -provides-extras = ["zaber", "ur"] +provides-extras = ["zaber"] [package.metadata.requires-dev] dev = [ @@ -686,11 +682,6 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/18/67/36e9267722cc04a6b9f15c7f3441c2363321a3ea07da7ae0c0707beb2a9c/typing_extensions-4.15.0-py3-none-any.whl", hash = "sha256:f0fa19c6845758ab08074a0cfa8b7aecb71c999ca73d62883bc25cc018c4e548", size = 44614, upload-time = "2025-08-25T13:49:24.86Z" }, ] -[[package]] -name = "urrtde" -version = "2.7.12" -source = { git = "https://github.com/UniversalRobots/RTDE_Python_Client_Library.git?rev=main#3b1a2da3e7291910952ca590269bcee49dc90909" } - [[package]] name = "zaber-motion" version = "10.1.0"