From 4a9fc50ff3d6d16aea7381817d7f1021d6ea844c Mon Sep 17 00:00:00 2001 From: Adam Date: Sun, 26 Apr 2026 00:31:26 +0100 Subject: [PATCH] refactor: revamp core API, network handler, XML processing, and examples --- .gitignore | 8 + README.md | 760 ++++++++++++++---- RSI_EthernetConfig_Full.xml | 139 ++++ TestServer.exe | Bin 0 -> 323584 bytes .../advanced_motion/01_velocity_profiles.py | 2 + .../02_geometric_primitives.py | 2 + examples/advanced_motion/03_path_blending.py | 2 + .../04_coordinate_transforms.py | 2 + .../advanced_motion/05_combined_motion.py | 2 + examples/coordination/01_basic_handshake.py | 2 + examples/coordination/02_parameter_passing.py | 2 + examples/coordination/03_state_machine.py | 2 + examples/example_01_start_stop.py | 26 +- examples/example_02_send_cartesian.py | 22 +- examples/example_03_send_joint.py | 22 +- examples/example_04_external_axes.py | 22 +- examples/example_05_digital_io.py | 22 +- examples/example_06_logging_csv.py | 26 +- examples/example_07_graphing_live.py | 26 +- examples/example_08_safety_limits.py | 32 +- examples/example_09_trajectory_cartesian.py | 44 +- examples/example_10_shutdown_safe.py | 32 +- main.py | 112 ++- rsi_config/RSIPI_Full.rsi | 517 ++++++++++++ rsi_config/RSIPI_Full.rsi.diagram | 256 ++++++ rsi_config/RSIPI_Full.rsi.xml | 203 +++++ rsi_config/RSIPI_Test.src | 207 +++++ rsi_config/RSI_EthernetConfig_Full.xml | 159 ++++ rsi_config/rsipi_test.py | 196 +++++ src/RSIPI.egg-info/PKG-INFO | 2 + src/RSIPI.egg-info/SOURCES.txt | 16 +- src/RSIPI.egg-info/requires.txt | 3 + src/RSIPI/io_api.py | 26 +- src/RSIPI/monitoring_api.py | 2 +- src/RSIPI/motion_api.py | 162 ++-- src/RSIPI/network_handler.py | 307 ++++--- src/RSIPI/rsi_api.py | 390 ++++----- src/RSIPI/rsi_client.py | 635 ++++++++------- src/RSIPI/rsi_echo_server.py | 94 ++- src/RSIPI/safety_api.py | 4 +- src/RSIPI/tools_api.py | 12 +- src/RSIPI/xml_handler.py | 256 ++++-- src/__pycache__/__init__.cpython-313.pyc | Bin 254 -> 167 bytes test_server.py | 615 ++++++++++++++ ...afety_manager.cpython-313-pytest-8.4.1.pyc | Bin 23600 -> 23682 bytes tests/test_config_parser.py | 261 ++++++ tests/test_io_api.py | 184 +++++ tests/test_motion_api.py | 439 ++++++++++ tests/test_safety_manager.py | 13 +- 49 files changed, 5057 insertions(+), 1211 deletions(-) create mode 100644 .gitignore create mode 100644 RSI_EthernetConfig_Full.xml create mode 100644 TestServer.exe create mode 100644 rsi_config/RSIPI_Full.rsi create mode 100644 rsi_config/RSIPI_Full.rsi.diagram create mode 100644 rsi_config/RSIPI_Full.rsi.xml create mode 100644 rsi_config/RSIPI_Test.src create mode 100644 rsi_config/RSI_EthernetConfig_Full.xml create mode 100644 rsi_config/rsipi_test.py create mode 100644 test_server.py create mode 100644 tests/test_config_parser.py create mode 100644 tests/test_io_api.py create mode 100644 tests/test_motion_api.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..82618a4 --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +# Python +__pycache__/ +*.pyc + +# Claude Code +.claude/ +CLAUDE.md +.claudeignore diff --git a/README.md b/README.md index 6bebca3..b191e4e 100644 --- a/README.md +++ b/README.md @@ -1,178 +1,582 @@ -# RSIPI: Robot Sensor Interface - Python Integration - -RSIPI is a high-performance, Python-based communication and control system designed for real-time interfacing with KUKA robots using the Robot Sensor Interface (RSI) protocol. It provides both a robust **API** for developers and a powerful **Command Line Interface (CLI)** for researchers and engineers who need to monitor, control, and analyse robotic movements in real time. - ---- - -🛡️ Safety Notice -RSIPI is a powerful tool that directly interfaces with industrial robotic systems. Improper use can lead to dangerous movements, property damage, or personal injury. - -⚠️ Safety Guidelines -- **Test in Simulation First:** Always verify your RSI communication and trajectories using simulation tools before deploying to a live robot. -- **Enable Emergency Stops:** Ensure all safety hardware (E-Stop, fencing, light curtains) is active and functioning correctly. -- **Supervised Operation Only:** Run RSIPI only in supervised environments with trained personnel present. -- **Limit Movement Ranges:** Use KUKA Workspaces or software limits to constrain movement, especially when testing new code. -- **Use Logging for Debugging:** Avoid debugging while RSI is active; instead, enable CSV logging and review logs post-run. -- **Secure Network Configuration:** Ensure your RSI network is on a closed, isolated interface to avoid external interference or spoofing. -- **Never Rely on RSIPI for Safety:** RSIPI is not a safety-rated system. Do not use it in applications where failure could result in harm. - ---- - -## 📄 Description - -RSIPI allows users to: -- Communicate with KUKA robots using the RSI XML-based protocol. -- Dynamically update control variables (TCP position, joint angles, I/O, external axes, etc.). -- Log and visualise robot movements with live graphs and static plots. -- Analyse motion data and compare planned vs actual trajectories. -- Parse and inject RSI into KRL programs. -- Simulate robot behaviour using a realistic Echo Server. -- Enforce safety limits and manage emergency stops. - -### Target Audience -- **Researchers** working on advanced robotic applications, control algorithms, and feedback systems. -- **Engineers** developing robotic workflows or automated processes. -- **Educators** using real robots in coursework or lab environments. -- **Students** learning about robot control systems and data-driven motion planning. - ---- - -## 📊 Features -- Real-time network communication with KUKA RSI over UDP. -- Structured logging to CSV with British date formatting. -- Background execution and live variable updates. -- Fully-featured Python API for scripting or external integration. -- CLI for interactive control, trajectory planning, and live monitoring. -- Real-time and post-analysis graphing (live TCP, joints, force, acceleration). -- Safety management: emergency stop, limit enforcement, safety override. -- KUKA KRL `.src/.dat` parsing and RSI injection tools. -- Echo Server and GUI for offline simulation and testing. -- Deviation and force spike alerts during live operation. - ---- - -## 📊 API Overview (`rsi_api.py`) - -### Initialization -```python -from src.RSIPI import rsi_api -api = rsi_api.RSIAPI(config_file='examples/RSI_EthernetConfig.xml') -``` - -### Selected Methods -| Method | CLI | API | Description | -|--------|-----|-----|-------------| -| `start_rsi()` | ✅ | ✅ | Starts RSI communication (non-blocking). | -| `stop_rsi()` | ✅ | ✅ | Stops RSI communication. | -| `update_variable(path, value)` | ✅ | ✅ | Dynamically updates a send variable (e.g. `RKorr.X`). | -| `get_variable(path)` | ✅ | ✅ | Retrieves the latest value of any variable. | -| `plan_linear_cartesian(start, end, steps)` | ✅ | ✅ | Create Cartesian paths. | -| `plan_linear_joint(start, end, steps)` | ✅ | ✅ | Create Joint-space paths. | -| `execute_trajectory(traj, rate)` | ✅ | ✅ | Execute planned trajectory live. | -| `enable_alerts(True/False)` | ✅ | ✅ | Enable or disable deviation/force alerts. | -| `start_live_plot(mode)` | ✅ | ✅ | Live graph position, velocity, force, etc. | -| `generate_plot(csv, type)` | ✅ | ✅ | Static graphing from CSV files. | -| `export_movement_data(filename)` | ✅ | ✅ | Export recorded motion as CSV. | -| `parse_krl_to_csv(src, dat, output)` | ✅ | ✅ | Extract TCP points from KRL programs. | -| `inject_rsi(input, output, config)` | ✅ | ✅ | Add RSI startup code to a KRL file. | - -_(Full API details available in `rsi_api.py`.)_ - ---- - -## 🔧 CLI Overview (`rsi_cli.py`) - -Start the CLI: -```bash -python main.py --cli -``` - -### Selected Commands -| Command | Description | -|---------|-------------| -| `start` / `stop` | Start or stop RSI client. | -| `set ` | Update send variable. | -| `get ` | Get latest receive variable. | -| `move_cartesian`, `move_joint` | Move robot using planned trajectories. | -| `queue_cartesian`, `queue_joint` | Queue trajectory steps. | -| `execute_queue` | Run queued trajectories. | -| `alerts on/off` | Enable or disable alerts. | -| `graph show/compare` | Plot or compare test runs. | -| `log start/stop/status` | Manage CSV logging. | -| `plot ` | Static plotting (position, velocity, deviation, etc.). | -| `safety-stop`, `safety-reset`, `safety-status` | Emergency stop and limit management. | -| `krlparse ` | Parse KRL to CSV. | -| `inject_rsi [out] [config]` | Inject RSI code into KRL file. | - ---- - -## 📃 Example Usage - -### Update TCP position live -```python -api.start_rsi() -api.update_variable('RKorr.X', 100.0) -api.update_variable('RKorr.Y', 50.0) -``` - -### Plan and execute a Cartesian move -```python -start_pose = {'X': 0, 'Y': 0, 'Z': 500} -end_pose = {'X': 200, 'Y': 0, 'Z': 500} -traj = api.plan_linear_cartesian(start_pose, end_pose, steps=100) -api.execute_trajectory(traj, rate=0.012) -``` - -### CLI session sample -```bash -> start -> set RKorr.X 150 -> move_cartesian X=0,Y=0,Z=500 X=200,Y=0,Z=500 steps=100 rate=0.012 -> graph show my_log.csv -> log start -> stop -``` - ---- - -## 📅 Output and Logs -- CSV logs saved to `logs/` folder. -- Each log includes British timestamp, sent/received variables. -- Static plots exportable as PNG/PDF. -- Live plots include alert messages and deviation tracking. - ---- - -## 🚀 Getting Started -1. Connect robot and PC via Ethernet. -2. Deploy KUKA RSI program with matching config. -3. Install Python dependencies: -```bash -pip install -r requirements.txt -``` -4. Run `main.py` or import `RSIAPI` in your Python scripts. - ---- - -## 🔖 Citation -If you use RSIPI in your research, please cite: -```bibtex -@software{rsipi2025, - author = {RSIPI Development Team}, - title = {RSIPI: Robot Sensor Interface - Python Integration}, - year = {2025}, - url = {https://github.com/your-org/rsipi}, - note = {Accessed: [insert date]} -} -``` - ---- - -## ⚖️ License -RSIPI is licensed under the MIT License. - ---- - -## 🚧 Disclaimer -RSIPI is intended for research and experimental purposes only. Always ensure safe operation with appropriate safety measures in place. - +# RSIPI: Robot Sensor Interface for Python + +[![Python 3.10+](https://img.shields.io/badge/python-3.10+-blue.svg)](https://www.python.org/downloads/) +[![License: MIT](https://img.shields.io/badge/License-MIT-green.svg)](LICENSE) + +RSIPI is a Python library for real-time control of KUKA industrial robots via the Robot Sensor Interface (RSI) protocol. The robot controller sends its state over UDP at a configurable cycle rate (4ms at 250Hz or 12ms at 83Hz), and RSIPI sends back position corrections, I/O commands, and Tech parameters. Communication uses XML packets over a dedicated Ethernet link, managed in a separate process so your control logic never blocks the real-time loop. + +--- + +## Safety Notice + +RSIPI directly controls industrial robot motion. Misuse can cause damage or injury. + +- **Test offline first** using the built-in echo server before connecting to a real robot. +- **Hardware E-stops** must be present and functional. RSIPI's software E-stop is not safety-rated. +- **Limit correction ranges** via `api.safety.set_limit()` and KUKA Workspaces. +- **Isolate the RSI network** -- use a dedicated Ethernet interface with no external access. +- **Never run unattended** without proper risk assessment and safety measures. + +--- + +## Installation + +Requires Python 3.10+. + +```bash +# Development install (editable) +pip install -e . + +# Or install dependencies directly +pip install pandas>=2.0 numpy>=1.22 matplotlib>=3.5 lxml>=4.9 scipy>=1.8 +``` + +--- + +## Quick Start + +```python +from RSIPI import RSIAPI + +# Context manager handles cleanup on exit +with RSIAPI("RSI_EthernetConfig.xml") as api: + api.start() + + if api.wait_for_connection(timeout=10.0): + # Send a 10mm correction in X + api.motion.update_cartesian(X=10.0) + + # Read current TCP position + pose = api.motion.get_current_pose() + print(f"TCP: X={pose['X']}, Y={pose['Y']}, Z={pose['Z']}") + else: + print("No robot connection within 10s") + +# api.stop() called automatically +``` + +Without the context manager: + +```python +api = RSIAPI("RSI_EthernetConfig.xml", rsi_mode="relative") +api.start() +api.wait_for_connection() + +api.motion.update_cartesian(X=5.0, Y=-3.0) + +api.stop() +``` + +--- + +## Configuration + +RSIPI reads `RSI_EthernetConfig.xml` to determine network settings and which variables are exchanged with the robot. + +```xml + + + 10.10.10.10 + 64000 + ImFree + FALSE + + + + + + + + + + + + + + + + + + + + + + + + + +``` + +Key points: +- `DEF_` prefixed tags are expanded internally (e.g., `DEF_RIst` becomes `RIst: {X, Y, Z, A, B, C}`). +- `HOLDON="1"` means the last value is held if no new value is sent. +- SEND variables are read via `api.monitoring`, RECEIVE variables are written via `api.motion`, `api.io`, etc. +- The config must match the RSI object configuration on the KUKA controller. + +--- + +## API Reference + +### Core Lifecycle + +```python +api = RSIAPI( + config_file="RSI_EthernetConfig.xml", + rsi_mode="relative", # "absolute" or "relative" -- must match KRL + max_cartesian_rate=0.5, # Max mm/cycle for RKorr (0 = unlimited) + max_joint_rate=0.1, # Max deg/cycle for AKorr (0 = unlimited) + cycle_time=0.004 # 0.004 = 4ms/250Hz, 0.012 = 12ms/83Hz +) + +api.start() # Start UDP listener in background thread +api.wait_for_connection(10.0) # Block until first robot packet (returns bool) +api.is_running() # Check if communication is active +api.stop() # Graceful shutdown +api.reconnect() # Restart network with fresh resources +``` + +### `api.motion` -- Motion Control + +```python +# Cartesian corrections (RKorr) -- mm for XYZ, degrees for ABC +api.motion.update_cartesian(X=10.0, Y=-5.0, Z=0.0) +api.motion.update_cartesian(A=2.5, B=0.0, C=0.0) + +# Joint corrections (AKorr) -- degrees +api.motion.update_joints(A1=5.0, A2=-3.0) + +# Read current state +pose = api.motion.get_current_pose() # {X, Y, Z, A, B, C} +joints = api.motion.get_current_joints() # {A1, A2, A3, A4, A5, A6} + +# External axes +api.motion.move_external_axis("E1", 500.0) + +# Tech parameters (runtime motion adjustment) +api.motion.adjust_speed("Tech.T21", 0.5) +``` + +#### Trajectories + +```python +# Generate linear trajectory +traj = api.motion.generate_trajectory( + start={"X": 0, "Y": 0, "Z": 500}, + end={"X": 100, "Y": 0, "Z": 500}, + steps=50, + space="cartesian" +) + +# Execute (blocking) +api.motion.execute_trajectory(traj, space="cartesian", rate=0.012) + +# Or generate + execute in one call +api.motion.move_cartesian_trajectory( + {"X": 0, "Y": 0, "Z": 500}, + {"X": 100, "Y": 0, "Z": 500}, + steps=50, rate=0.02 +) +api.motion.move_joint_trajectory( + {"A1": 0, "A2": 0, "A3": 0, "A4": 0, "A5": 0, "A6": 0}, + {"A1": 30, "A2": -15, "A3": 45, "A4": 0, "A5": 30, "A6": 0}, + steps=100, rate=0.4 +) + +# Cancel a running trajectory from another thread +api.motion.cancel_trajectory() +``` + +#### Trajectory Queue + +```python +api.motion.queue_cartesian_trajectory(p0, p1, steps=50) +api.motion.queue_cartesian_trajectory(p1, p2, steps=50) +api.motion.queue_joint_trajectory(j0, j1, steps=100, rate=0.4) + +print(api.motion.get_queue()) # Metadata for queued items +api.motion.execute_queued_trajectories() # Run all in sequence +api.motion.clear_queue() # Discard without executing +``` + +#### Geometric Primitives + +```python +# Circular arc +arc = api.motion.generate_arc( + center={"X": 100, "Y": 0, "Z": 500}, + radius=50.0, + start_angle=0, end_angle=90, + steps=50, plane="XY" +) + +# Full circle +circle = api.motion.generate_circle( + center={"X": 100, "Y": 0, "Z": 500}, + radius=50.0, steps=100, plane="XY" +) + +# Spiral +spiral = api.motion.generate_spiral( + center={"X": 100, "Y": 0, "Z": 500}, + start_radius=10.0, end_radius=50.0, + pitch=5.0, revolutions=5, + steps=200, plane="XY", axis="Z" +) + +api.motion.execute_trajectory(arc, space="cartesian") +``` + +#### Velocity Profiles + +```python +traj = api.motion.generate_trajectory(p0, p1, steps=100) + +# Trapezoidal (bang-bang acceleration) +profiled = api.motion.generate_velocity_profile( + traj, max_velocity=200.0, max_acceleration=500.0, + profile="trapezoidal" +) + +# S-curve (jerk-limited, smoother) +profiled = api.motion.generate_velocity_profile( + traj, max_velocity=200.0, max_acceleration=500.0, + profile="s-curve" +) + +# Each element is (waypoint_dict, velocity_float) +for waypoint, velocity in profiled: + print(f"Velocity: {velocity:.2f} mm/s") +``` + +#### Path Blending + +```python +traj1 = api.motion.generate_trajectory(p0, p1, 50) +traj2 = api.motion.generate_trajectory(p1, p2, 50) + +blended = api.motion.blend_trajectories( + traj1, traj2, + blend_radius=10.0, # mm from junction + blend_steps=20 +) +api.motion.execute_trajectory(blended) +``` + +#### Coordinate Transforms + +```python +world_pose = api.motion.transform_coordinates( + pose={"X": 100, "Y": 0, "Z": 500}, + from_frame="BASE", to_frame="WORLD", + frame_offset={"X": 500, "Y": 200, "Z": 0} +) +``` + +### `api.io` -- Digital I/O + +```python +# Set output by channel number +api.io.set_output(1, True) # Digout.o1 = ON +api.io.set_output(3, False) # Digout.o3 = OFF + +# Generic toggle (any group) +api.io.toggle("DiO", "1", True) + +# Read input +if api.io.get_input(1): # Digin.i1 + print("Sensor triggered") + +# Timed pulse (blocking) +api.io.pulse(2, duration=0.1) # 100ms pulse on Digout.o2 +``` + +### `api.krl` -- KRL Coordination + +```python +# Wait for KRL to set a digital input (synchronization) +if api.krl.wait_for_signal(3, timeout=10.0): + print("KRL ready") + +# Signal KRL that Python is done +api.krl.signal_complete(2) # Sets Digout.o2 = HIGH + +# Pass data to KRL via Tech.C variables (slots 11-199) +api.krl.write_param("C12", 120.0) # KRL reads $TECH.C[12] +api.krl.write_param(13, -50.0) + +# Read data from KRL via Tech.T variables +force = api.krl.read_param("T11") # KRL writes $TECH.T[11] +actual_x = api.krl.read_param(12) + +# Parse KRL .src/.dat files to CSV +api.krl.parse_to_csv("robot_prog.src", "robot_prog.dat", "output.csv") + +# Inject RSI commands into existing KRL program +api.krl.inject_rsi("robot_prog.src", "robot_prog_rsi.src") +``` + +### `api.safety` -- Safety Management + +```python +# Emergency stop (software-level, NOT safety-rated) +api.safety.stop() +api.safety.is_stopped() # True + +# Reset E-stop +api.safety.reset() + +# Configure correction limits +api.safety.set_limit("RKorr.X", -50.0, 50.0) +api.safety.set_limit("AKorr.A1", -10.0, 10.0) + +# View all limits +limits = api.safety.get_limits() +for var, (lo, hi) in limits.items(): + print(f"{var}: [{lo}, {hi}]") + +# Full status +status = api.safety.status() +# {"emergency_stop": False, "safety_override": False, "limits": {...}} + +# Override limits (use with extreme caution) +api.safety.override(True) +# ... calibration work ... +api.safety.override(False) +``` + +### `api.monitoring` -- Live Data + +```python +# Comprehensive snapshot +data = api.monitoring.get_live_data() +# {"position": {X,Y,Z,A,B,C}, "velocity": {...}, "force": {...}, "ipoc": 123456} + +# Individual reads +pos = api.monitoring.get_position() # {X, Y, Z, A, B, C} +force = api.monitoring.get_force() # {A1, A2, A3, A4, A5, A6} motor currents +ipoc = api.monitoring.get_ipoc() # Interrupt point counter + +# NumPy/Pandas formats +arr = api.monitoring.get_live_data_as_numpy() # shape (4, 6) +df = api.monitoring.get_live_data_as_dataframe() # single-row DataFrame + +# Console watch (blocking, Ctrl+C to stop) +api.monitoring.watch_network(duration=10, rate=0.2) +``` + +### `api.diagnostics` -- Network Health + +```python +stats = api.diagnostics.get_stats() +timing = api.diagnostics.get_timing() # cycle time, jitter +quality = api.diagnostics.get_network_quality() # packet loss, IPOC gaps + +if not api.diagnostics.is_healthy(): + for w in api.diagnostics.get_warnings(): + print(f"Warning: {w}") + +if api.diagnostics.check_watchdog(): + api.reconnect() + +print(api.diagnostics.format_stats()) +# Network Diagnostics: +# Cycle Time: 4.01ms (+/-0.12ms jitter) +# Packet Loss: 0.05% +# ... +``` + +### `api.logging` -- CSV Logging + +```python +# Start logging (auto-generates filename in logs/) +path = api.logging.start() +print(path) # logs/17-04-2026_14-32-45.csv + +# Or specify filename +api.logging.start("my_experiment.csv") + +api.logging.is_active() # True +api.logging.stop() +``` + +Logs include British-format timestamps, all send/receive variables per cycle. Logging runs in a separate process to avoid interfering with the 4 ms control loop. + +### `api.viz` -- Visualization + +```python +# Static plots from CSV logs +api.viz.plot_static("logs/test.csv", "3d") +api.viz.plot_static("logs/test.csv", "position") # Position vs time +api.viz.plot_static("logs/test.csv", "velocity") +api.viz.plot_static("logs/test.csv", "joints") +api.viz.plot_static("logs/test.csv", "force") +api.viz.plot_static("logs/test.csv", "2d_xy") # 2D projections + +# Deviation from planned path +api.viz.plot_static("logs/actual.csv", "deviation", overlay_path="logs/planned.csv") + +# Comprehensive multi-plot visualization +api.viz.visualize_csv_log("logs/test.csv") +api.viz.visualize_csv_log("logs/test.csv", export=True) # Save to disk + +# Compare two runs +api.viz.compare_runs("run1.csv", "run2.csv") + +# Live plotting (runs in background thread) +api.viz.start_live_plot("3d", interval=100) # 100ms update +api.viz.change_live_plot_mode("position") +api.viz.stop_live_plot() +``` + +### `api.tools` -- Utilities + +```python +# Low-level variable access +api.tools.update_variable("RKorr.X", 10.0) +api.tools.show_variables() # Print all available variables +api.tools.show_config() # Network settings + variable structure +api.tools.reset_variables() # Zero out corrections + +# Reports and comparison +api.tools.generate_report("logs/test.csv", "pdf") +diffs = api.tools.compare_runs("run1.csv", "run2.csv") +``` + +--- + +## RSI Mode and Rate Limiting + +### Absolute vs Relative Mode + +The `rsi_mode` parameter must match what your KRL program uses with `RSI_MOVECORR()`: + +| Mode | Behavior | Use Case | +|------|----------|----------| +| `"relative"` | Corrections are **added** to the programmed path each cycle. Sending `X=1.0` every cycle moves 1mm/cycle continuously. | Continuous adjustments, sensor feedback | +| `"absolute"` | Corrections specify **total offset** from programmed path. Sending `X=10.0` holds 10mm offset regardless of how many cycles. | Target position offsets | + +### Cycle Time + +KUKA RSI supports two cycle rates, configured on the robot controller side. RSIPI's network loop is reactive (it responds to whatever the robot sends), but the `cycle_time` parameter ensures diagnostics, health checks, and jitter warnings use the correct baseline: + +```python +# 4ms cycle / 250Hz (default) +api = RSIAPI("RSI_EthernetConfig.xml", cycle_time=0.004) + +# 12ms cycle / 83Hz +api = RSIAPI("RSI_EthernetConfig.xml", cycle_time=0.012) +``` + +| Cycle Time | Frequency | Use Case | +|------------|-----------|----------| +| `0.004` (4ms) | 250 Hz | High-frequency corrections, sensor feedback loops | +| `0.012` (12ms) | 83 Hz | Standard motion corrections, less demanding applications | + +### Rate Limiting + +Rate limiting caps the per-cycle change to prevent sudden jumps: + +```python +api = RSIAPI( + "RSI_EthernetConfig.xml", + rsi_mode="relative", + max_cartesian_rate=0.5, # Max 0.5 mm per cycle + max_joint_rate=0.1, # Max 0.1 deg per cycle + cycle_time=0.004 # 4ms cycle +) +``` + +Set rates to `0.0` (default) to disable rate limiting. Clamping is applied in the network process right before the response is sent to the robot. + +--- + +## Testing + +### Echo Server + +RSIPI includes an echo server that simulates a KUKA controller for offline development: + +```bash +python -m RSIPI.rsi_echo_server +``` + +The echo server binds to the same UDP port as a real robot, sends XML state packets at 250 Hz, and accepts correction responses. Use it to test your control logic without hardware. + +### Running with pytest + +```bash +pip install -e ".[dev]" +pytest +``` + +Test files go in the `tests/` directory. The project uses `src` layout with `pythonpath = ["src"]` configured in `pyproject.toml`. + +--- + +## Architecture + +``` +KUKA Robot Controller + | + UDP/XML (4ms cycle) + | + NetworkProcess <- multiprocessing.Process, owns the socket + | + Manager().dict() <- shared send_variables / receive_variables + | + RSIClient <- orchestrator: config, safety, network + | + RSIAPI <- runs RSIClient in daemon thread + / | \ \ +motion io krl safety monitoring logging viz diagnostics tools +``` + +- **NetworkProcess** runs in a separate OS process. It receives XML from the robot, parses it into `send_variables` (what the robot tells us), and builds the response XML from `receive_variables` (what we tell the robot). IPOC synchronization (`IPOC + 4`) is handled automatically. +- **RSIClient** creates the `multiprocessing.Manager` dicts for cross-process variable sharing, initializes the `ConfigParser` and `SafetyManager`, and manages the network process lifecycle. +- **RSIAPI** wraps RSIClient in a daemon thread and exposes the namespaced sub-APIs (`motion`, `io`, `krl`, etc.). + +The 4 ms cycle is driven by the robot controller, not by RSIPI. If a response is not sent within the cycle window, the robot uses the last held values (for `HOLDON="1"` variables) or drops to zero. + +--- + +## Examples + +The `examples/` directory contains runnable scripts: + +| Script | Description | +|--------|-------------| +| `example_01_start_stop.py` | Basic lifecycle: connect, wait, disconnect | +| `example_02_send_cartesian.py` | Send Cartesian corrections (RKorr) | +| `example_03_send_joint.py` | Send joint corrections (AKorr) | +| `example_04_external_axes.py` | Control external axes (E1, E2, ...) | +| `example_05_digital_io.py` | Digital I/O: set outputs, read inputs, pulse | +| `example_06_logging_csv.py` | Start/stop CSV logging | +| `example_07_graphing_live.py` | Live 3D plot during operation | +| `example_08_safety_limits.py` | Configure and test safety limits | +| `example_09_trajectory_cartesian.py` | Generate and execute Cartesian trajectory | +| `example_10_shutdown_safe.py` | Graceful shutdown pattern | + +Advanced examples: + +| Directory | Scripts | +|-----------|---------| +| `examples/advanced_motion/` | Velocity profiles, arcs/circles/spirals, path blending, coordinate transforms | +| `examples/coordination/` | Python-KRL handshake, parameter passing via Tech variables, state machine coordination | + +--- + +## CLI + +Start the interactive command-line interface: + +```bash +python -m RSIPI.rsi_cli --config RSI_EthernetConfig.xml +``` + +The CLI provides the same capabilities as the Python API through text commands: `start`, `stop`, `set `, `move_cartesian`, `log start`, `safety-stop`, etc. + +--- + +## License + +MIT diff --git a/RSI_EthernetConfig_Full.xml b/RSI_EthernetConfig_Full.xml new file mode 100644 index 0000000..49061a4 --- /dev/null +++ b/RSI_EthernetConfig_Full.xml @@ -0,0 +1,139 @@ + + + 10.10.10.10 + 64000 + ImFree + FALSE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/TestServer.exe b/TestServer.exe new file mode 100644 index 0000000000000000000000000000000000000000..e65e6283012e405ab709882a16a5f0514d02a115 GIT binary patch literal 323584 zcmeEv3t&{m_5SQ8Bx^uj9)_s2+4ySENQnU@ziJDqk3T3a1g%1AZH$SgJXSZMYEvk+ zh!w&^h-eU%qM)KdL4y_zq9`gxu(bvijUs9kYhx4}k=Ffx=ghr#@4fp-2v|rxm%Fnw zXXeg)=Q}fJ=FYuKMvj}Ol9W=(_=LksJ%(>OhH{uVn1<}bGS?oa9y#Qttj9d(y_9wS z6=mgtD?`D{Lq!t<#YK}Q1uFuVmIOkRCk4tT1HlX-52z@)rUL0_?l$xyUG2b- zhf97nAXFYIMx3#c?hvZ=mbOT+t%eQ=l}retpvG!&K;>=6P^AiNNr?&r!hkR!3WX}&JV;|-AZ658rB>%U?<1*vyd!B~ zlDf&r^oCH&mF2jrO`CEEZqdD=Ln+;p(!(g7-3w7~b}HKOhOYCdE3?x$y3~yF1;O4Z zH5Iiarw98W8d{BPEksp@&|5hK@zCGsO5Db$1=A_$iy%9LVn-r4@wZ7iy}gwupkPjS z{T)0Ca&J=h(Ue_K4!kMZ-$6Y4yCi@fy$2?%U*j7&J(ETSGa>FbHtnQSl18Nj14v|! z$~y+}zGHQ2cwQf*GK|#s^HNfdL}?%L(^Aq0Zo+r>lX@kMN;S*>aDWf-?y2bmcS4$y zIyRU^4WHr-HJTip2n;FeYhdsKgV%ewCmZHSnUW1Pyp_bGW{IE$k3pb?-b$i8^Z}~r zH#-Lk0D$**--E1jID)E^AzGT!*AR_}h%yY(xe?KkhUn~w=qN*UPDFIHAwpL&s}CLv zJSP|^|LBF;j$?rdk#t~@?hi&mwl5XbQr0neJkl@-aq$jnMc?e{Er%)7rjR{f1~=Yp zXqQ%wI@8j;<^3t^hsZLb=s9K-c1!kZ`jO>)qlhm2PUs}cgVIT!zEUO9VY1Xm| zZX@%kaui7mLQz8;+D;b(u#wIjmf5=;s-y*h(U2V(1j}amHY!gZOly(p4}y0js88Ef zq_n9b#Um=FJ5}Th;W|v6D4i1Oo)kkT%0h#n-e@pAs1>A)eoo~`6gr-2(aKReJ%~PS zTpGBa_;Bb-MUFOfrMRVA0Ez4~3F29NJ*7?EIFC)MO+x9uL3k&$Lve2cYCYG;L z;4%y`+$_WdE+zD1i*0c03BuhPS=r}N4I`~|@F$3-hpwXXKXpp`+NI>W?2J*`WQ_A{ zjOjeh7{wio=}c{9iU)cTW4SKIC?`}NVT{sl##lDQV2l$cW0U}6{VZxUV}{tx*fA(= zSB=u9YMf_NjnZakptwUdeW6y3;<_`qRim8HA0w(!+N~PP@(tBEVX8(6sD^pK(HRV} zTQ!4uDD75_Wv3geal%xM5>O2b zGKXr0*sU7U4%H}as>XRX)hKORjp7c~^o3eAifgOwR?Zte7#BSASX=YcA(xz&hXH$*RrfL*-sHQK>XVoaKRlD9b z)+i^mHKH1&-Kw$dM}}&gFjb=jRKuj`P|Xm#Rf8jSs77g1HO{lCrt@@%q_{&hovBr$ zxbBc{)hH+QQA9OLyH#V^4-M5gVX8(6sD`zyLp4L}R*ja9QK-g!V*KqqJK!mi@p`jT5G7lz?hjLpxM6#BSB-R=}33X4l)@PJaorr3bcB{s+Qw`NPVX8(6sD^pWp_(CftM+56 zJ3Bavhd(87DQ=Ha%11*IEM)Nzh-2PF=$jGBHdp7b4Rx)~DKmQv)uorWlr~qe6#q`( z4v%x{oh5 zawbvewn*EQ4(tTL%%1kj4+{93k%n=5)oG|au(vf4o zL!-2*9L3`*#~0d^qjXAWTP)=$%TkUKrg9YTmpRm~9En0-M3mFTwQ>}Xp&W@b&oz|e zgsB|uI+zMv=Dv%jOnWD$bYK^{Mc^yfx{a$z>SlQ*N@M6AY41HOJI?^Y7xbhHbm!$v zb12jPM_f>pX$M7V4JsYT>_l-jM?^qT+6{_jBMne|!LbogoN0m@ln4}M+CfoTgF3|p zimUlv1Qey+pjh@30~B8{I0A|@O;Cdqfuc-1C`xNkKXHNLYVsqXDD4KtvY#5D_=0mH zpg7Y6b!s9|lxYV=X$|VvE>K*}xe-v5c7tNsC<7E#x_905gXHz<~kHbC(O<0GIr(*)I*+C7uppWYy$bV?{lrBTkYFGX2_R4-ieXnP+g zacEWq52Z76Gjr_&k7b1h9!g-YOSk5_3)w4v2Vy%CPLCmEU=d!E(kY?aVj-g}jqE#i zWF!tf8bL;BH!_xuF_2LL$owv3w11gk7N+d%iwLNukkY2_@Vgxv7q)3d>6FlOv9zKr z%Og`F(1xxMc*o`VC@1uOL`%Nx_{<~h{Il$5hL)7bFt(g%Kh{nPYdp zI#Ol=9Vr{DtynthCbW)R3+w2Z&`BJ+KB6O~-8!;ttf3=ca9c!2&NOwTJ^YwZ^{SP! z?R}2YrrMXemuFnqHeR@HKb%ltyK>I8x|)a%l+GOD90x4>g`opq@N7f}&NTfZCD28& z-DWJvwUavDg`LtVp)FL2yK7NaARkB_JNG0FeH=ka>CD5NYiyQXY@nnBfMmLWfJ%$B zs5LER+9wlAXJpu2BVWEJ0)x^4JoZ$XhuQrGi8Fs`AmMAer&|4*(m>`TWVY>ULwtxO z9?R|<{mOb|iVQH6hyjKkprwR9i`6(~W$wxRkG(6AEOeC5G6R>% z$jq>tfn}E(%~B#gIF7AAaX>&PH~9EDi%*6)C3GrP>h2trm01|r=IRV24qX&MLFvF^ z>d?p8f$-Ir89*pu0-@)F*}p*J#w1SZl+dJD2q-J_vMB#U;?SH30xp)GnQm{MWhF-Q zl(07s9*(sJ&2Z*{ia?BgA7hA(wTlCmcm#_JP+AXrj6rm9N}F>F#Z4z0V5C6&3vsywtbqAnHS8}`5tz`9*&%E>G>l*Uj6reARt6b}}0l=?yN zQcG%IPEo1sVq_WMC>{8UEZf5lki?-!B6ujBd3fgGb|fsj+=e7Z*P}8JyS#6BfRh)D@0=fZm)Iqmt4guXvIs|mf>k!b5xkGSBB^X#vL55p^!Q?E_&n-B{EjZdOp#51#vq!oG*aUQzpvAc3k{`MSxKDDHIKnOH z;}+0}b5u&6-XWl!JBNVgMu&hpycWcB8|m4XQ#VctJ#Bf(^Y9Eh`wBfI%JkQ-=_~Xa zie#foMBk&*AO_=Qy;3SYonMQ{9#0Ysyl&=HkUhbWSVJ+Jog&IuqcB_hL+%h0sW`vD zksh2x@$_u^I5Kixl>b9sQt&DWf*}N<1&07@Ibu2I zdw)lnI?S$s$aq6!GNNg{6pO}Y|AsQ`C9igt?B2_hO`@^c*C2$&@%~Ygf(aCq?U@|B z7SaIyZY%G^?B61en(a+;&0IltC33wf*}tP|dJpWT&PE+6s3Us{U52-jxNl5FBD)82 zAmHA^fcqe3gq)2{@1J#3=mA`}e(5AqFM&;1 zFLm)GU6HdMR@lNvIwq`#ipQ*nd9vyuIuh1H#f^H7#=CvTdH8pFJp=UPyV;{L3Sdw( z+I{MDurL*zCYPflruFdp%4gtv%9)UOLr;Py&a670Mh_fQ8o$6e^#;V!`HDlml~lG@ zQt(fZrFoC)KH}I`gucNIN{DiNrrpU zph*`nPa8LEP{S{R_UImJLk~|+3~SGC zr&95g(>2E6O{m!$IyudFz4OFN^?QQHS7Hn#PesQ}9y^7ik&JW9jEkmFG)Bf)SH}N~ zTtU&u6;{UBDV#y?r;Z`8I2dNmx#YVJr*1PJK+6{(m~s~C@K#cIRIr-hbG&cxW-A_C z$k898NbnX!(=|nwZg6;7w~MAwGy=%V80*Ry5xIh*kt?i>u~VEG=SHreXyghjW9$@X zhQ;_rn(&4loP+}&At_yoLK6u%t3YL0{3Jqej+#{{?JeP&~KxKM#8eLVPM-o z30+9p=*P)bzc5D$XLCnC`B-n|P~?S*P>RKUr@Lw_d4Zbwe`9SF4BB=^qqP5|GB0Bbq8Nl8mVx6Pt%3#XM9yc0o6a0$W_ zta&pHWv9ZiCQpS|OdfCz$J6i*@>xl!3F}(*U3@T4s*?)6kvC{V_X9-vpHUSr2u?cH zp7*#j?m`@UfxP5pBy^3yD^ez;IQOsQ{t_pD!kdJpdsD2^c(RbA*DCs>D%!} zyi!MFygvbyJ*eL;LyKwI%MtcYPVe1q2;3o7K|Q-I>Yf_BhtS-MARFW1q#nMU$*I8= zNcPK{fW3bpNDbakMIJzKf>FGd1S=8rPB|&NXG-uvL=QbA_z;O#A;`$jBW2O;(1wojS%_9^|{%u)9C`TCb4?Nd77Lzi$uAaP#<0$;K%0)c3a3xP82 z5GeiKfENweQE(NuN6M6rfr7*t1`599h#tn+(Zeu%4H28mM3RNmgO6JAxCp6W_G5J6 zsaE04K;}R@J}MLX0X3$rMCm|56iXyFUj*k%Mmq6j1pmTKP?xh~rL>8a;wILi?mc?G zaBT!DrLhrvg&ix&`UW4j8nSO4={)!c^fZcR1naGMU?}tnRKZ2qH*%=dQ3mA{4_z2h ziH3_^4Nha3Ywqk(+EgWKY;a+lDwNI)1dej`46ftn5#}kanRje^lGuDLoiDMxs`jaw z=F>^EG7=vr-z!1`#r>!OfTB z4e?-0JeHXwXUNmA@qQJharF>OFEF0wJ=$!49i;+GfJg_Ja*SVV@URX1jymB_sS{wU zA&s*npi$y_{5^iOU^4RuNYGiDk+*soX0BH8AV`H>(f>GiX-dlaUezflUUcTsPc@A`ZfMcslP7)mfxPFx z+j8N^BL-b`-TO_yynJmz(TF}zjQ``6xBhMbI@6Ib7aiRZ|2IA>5kC#Kd=T*~5y$9L z+Yvt%-<$Bg6W??3c?q9ipxji*#^Cb@#7AI}+vhk{p{`J6s$30F!&Nc1bta*xQW@h^ z3B(f+=5p3VRirLQitapYnWs21Z>?}-<~cG~y^BZbm#WEVv4Y!iR=jDH$C>S{x@{t| zivcJItR-q9K7`nWb=)kE$|!-4DdWmRj?Dke@u+^6LxD1AcLlyDp`iWh6Cd!X@0uCq zXrv4UoDCd~pQ8q%i=5-A=XZ~J)ColAWqkc4U@3-O2=wLn*qc25a<9rMv+$pXi^@^l zsq`2~LI6^PazC8tRq3Pv8EK-Uu2BoTs@Ft_Cpp9~+=(lKOt`!C758}6saK-SVze-s z+bF>G*RZh*&|a|`56B_NCNoeh? z-1GzQ53)SS@<t6;4;b=*ArBbxfPrBGLmn{X0Ye@z;M(ou0biL8g17A zQwR_>eZ{&)~#FBTW`IkcI?=pcJAD%T3cJymtTIVKHU>m zcQ09jaXby|(iosG)W<2Rmy%(Xh6{|Nx;_m@Yaa^J()_Kv`?4U_Z>D#j13|r+PTjqm zO0@3u>jHgRA*kNfx3x93kKdn}lS%2m_2(djC?_X}(&um*5+mPA`_uUPb9VJbDdhY8 zRR8Y2Y1N!e^>g;_>a==ppidv24M44E&EJ-)l>k)62N4}RTN$5D`2BrSk#DL?-)Vez zF%7F|8>=FO`grCr{70ksm`8X|SWaO;7!U@8fiA}YCU^1Q!hkR! z35u1-`~R za^b(5?Ed@3^0qCN+J!ex%tbL_P8bjdgaKjTz%t<~OYf5MlPJZIQGI(?7*Uy)`bCKKo}4Poc^0OPv&GaE-z@}6I&>H|D++vu=;Pl^ZJLvtvjgx&XuPke8-i)( zDOVT}2801H5Z`}C|6w>GOc)RbgaKjT;9($^|NiGoPd_;%E&30`L5Qq_2fH8<2801& z;M>eVl>a8@{iC#ae;984IqBlUzs>zcbQcDMfrE#EDF00>?}UFC4h0S#_<}?j5C(*S zZ!H5(|K0IF4EL=iOcWOegaKjTAYs7izdQbi;SLh`0z()O284lcEd%i1U$j{#{tuT9 z{$V)l5{U={!hkR!3>**!;J=GLjihPAcrs@3g!kPFv;7*0Rfwf->N`e8j#K>YUswV5<73%h?e1 zg#lqe80b0-i2v?7wi2YmfG{8o*ctdA{=;z1atH&$fG{8obU_A`>VlvJvM?YF2m``E zv-of61j2wYAPjU(29#>~xFyExx^R8{fQqT;s(45F$%lxG*3L2m`{v!N5S2|0d@>M8*5VaP_DA#DyOW zb`uQ3fG{8oIQ@6q+fB6cPWXr6Pyoe*IblE;5C(*S1IvKZe|P*3!yQ=E(z-Ap3{13wkbHacyAPfit2bKY+|EA58IT?-13!32H?MKN5}s#ob4is3IoD`Fdz&Z00x}?yW@Wt z?f^iQCWQfEKo}4PYz#R4ciRqne=zYMhNJffcQo0mkf<;q3A$g;L%-h{ zL@r$aVK}|JWBp+`{ItjT{mucvDoqLl!hkR!3`7`+^52FFAE=`LFdSCiksn`1s*sp4 zAPfit!oUGwAeR4z3&##gj6V#AZ4>LiV>tj=rAc8x7!U@8fd~UG{~gJYm@ps=2m`{v zLCt{p?}OSr0!|nZ284mQ3>@r#7;a`irMv|r>5Gn2@yW&~*Z6o;24ebn<}jt`V;;53 zm+kF&@1K|4I{&6QGygRE#vA8V&0a8fp8cE$`3=|4oi(#&{!M#Zvo5dp>aqoMW>?Mp z)54ppmn~gZyP~#kC7reAsgs6q#j+*0-?3os%&Hr2SiJD&y=tYSnC8uzx!|UGsJd=- zUEONNv7)wi1tE(%A!IoaqwvbwB}?v@J!|I8bLVytS^Jn!7ol14K>ZqOfZBt((cYT6 z`gQdiH*DPeBF@d5w`@kZ@wttV)UT}v8p4W_pp(?7TYlFvaJ=-kMeQPM2NQY*>N3z! z1Iqvf4L82h*o^<~t+~17W1PVhjCpGkiK(?mtwp(h@)e^&s ziwO#$=RUIP5mcnB05UYw%v+#sEiLVwpCi8opaE|4a~omqWy~%rL2Xqx{wV>h*qH9P zb>VHdEMOB*UC3b5IX;qI*43_sG1e*lf8m-E})gZZ!c~K^H?g zbdI*q+t?EEiLpS$i7>%bN7L3u0J31s`O}POw%nNJ&Z#1kL7U|asM!P*r?I%^i!N_+ z*7SB0%tu`UeXI5XI4~D12d3G7x{*3H_eWxAKLRo#YQoUHz!spOV_;lkR}&}Z26${t3#w-7ZUn=@>P8=mY6kMK zKG4S~H_AUnUE)-RAY$+$gtG+{nB|e$2jqm9b_-}^8 zY}(pn0D~Xu1T0YXc^gbYEU*PJ(lNDbSOzAOB-`;;a$*gYv6q zR5icdY#;+x^64?uh$keqVZm3(g0Kwyjx`1e6S^2W8jXS#nah?7@Z4)=H~RTnlc?@mKh7v;A)RB9)Qi@ zf~bP|*eR%an4-Ct0Tx_Y0u>FOEKD1w7p|=ww{AKiTU(hsv(SHfmOsN^aC*Vwg^Mw! z3}gvA8QG+z1@oZBv}n<-cP+k6!FLl$D{3*Iw4Zb{Y}zpfrZts?IDu>fBU@eGADwIv zAuAj{qIPL5U4lb*f3qG({d)AlfAXSq*&U0~rPQ36vvdc?WCE&*CmgY^kAZ0|V{#%J zv0{8yS%L33KXw1H#r13I(eVL{YL50~Jo@xh18Oig!&53O7z|9*!JFfC@EHAxZAU!W z&vlhBC1@+Sqc~%1uFCNBNB6(B5;F;q#jG4y$$6R!YupYoCbk_sJqU_O-Ar`s7N?SK&Qv;aJlH zD8NapJbl}^d>MR`!u_?ulQHgM54v`Ef;&Lhez9uAit?-?_UwVI@kPZO*VdzF)1WgJ zHtxy|OjzP+U$Ar;ZYSXjZbZ)_|AZAm{K+mhChphdoPY+-Y4QkPPKLjaf86Lov;n%w zIq_)2bdFp^T+vz z_xJVj!GRlV72N*Ox&Vt9OfTA%M=2Ds-OTE&)3Gin z9E?!JJ!t6raym zII^&AMJ*~dIx>~%C>mm2dnRk~w5HCn_5~DpbYsRz(9@}wHr%IUDFPpeiIdQDxa(+c ztWc{BN3Cx@vG~SH4oD21@P%Czj6>XNp#*xBu|SC5s|++m=U#9V0i@OAB?`I`Mwop| z!?hyxQuagm666|qfFSI6+c8TTfSQ|izlJGb6Ko|$?{K;?0hZof z!7$WpVF8aes1J1(7NKFquv~$!p=x>gk5OffH9J)?Fd-WYt4CH5TU2z#GEhy^v#uJ* zIt)%j(bfm$mYy(KJV0nMHYQ}~jSgIlj-@*zZ)wtk9c^gZ5RX1A=(-p4RTkWH6K43S2HlQo!DfkPFO+bWY^@%uefKfn!GPE+|-UF|Q zByPCv`%HEm_INv(bWwVgFn6Yjj9X%GdgkV3D{-k$!a=#zk!iwbnz28DhX`1&+1rj6 z?+_*n88}1VG+Ls@_`!<-!iZC@g>OYIW_P&64x-Tw?FA-%0lESnPGAndWkEG&Vysai zCmcY={Ra2f^Z;W{6?_T^wszn2b9X@WT;cO8i4odVg$SS$F z5!HQ|^5FR*P4x6lM-MN8(POSHuPB)~;LK3b)n$_|&k6jjBvf7&oOH%10|wH+oWPGK zPpFt2Dmi0P$>fSq(S)49sL7X3C@Vg<uImu@hRTQOqsqI6O5FAdyUOX(_fCr`YF^|1v!YU8T-hRu%Tjt+% z^V~UeXU;^=yQ!+`<~eiV-~W8uBACJ|xwnzM?f|zfym|JGcyv2+;k-GE7cJJ_OAik1 zK9((ATs^;f{@i(0Gp64(>&8xWhK^!_-k@RbjW^u6gmwnu{HGGjnNg zY*F?6S%11A=HwWUzYbx#sp`i0H_ZhN)EJ&M0s(mq>m)$N$@l4cEE9A{&Nr34}!f&Fk#XfCk`rGB9U?9Zgs`U_QWPfawA25t^6uWB~UI zaCCV+IGz>t*vO7X#}i?KvF==a3!|awoSgfbI>)5RD`~x+c7&MT?UN?YY(Pd;t*M(o zYi44?;$lJ%#f-FZ13$9nl`cQM)r$*S)YIgx=W=s_VI_^#hq?C0-ix)k0NIG+eh{NI znxRBIRsHEtb#;$`aGw5ZvAooKxU|XA@fD)A`rsQ>*c2q`H4k8+htZRw4lZ~ANDmyk zekJZW8bJi3VM1cvrNL4OPF@j_GEj^OqdNxJars@h$7IWGJNTZuRgVCZR>-*5h_lC< z07rBd3`(0v<{dpOfR2Iw8r`=XnBbH_CQQbFYB7kgLkIn$8|a(_ggIa_TktY2rys$D zjD_>7L8Jp`eB#{q^iFk#lbbFp#D3fp1LPMtcsI^J8u=Eq*0;M%ks+!N3icIvb5vCkqD$nZIlGU#{t6x)lXG=z(%|2go zLQKRRdIhY6E(Hfkn6RKB-mqRXUr%TcQzc;v08@Ks0GJxv!Ng{<9!9_)Va2Af{39=b z*(d%(8{NlEp7uW`a#XZxrH7@Ml@*+ofhqpO(r*qE42R0eBk+2foWQJ^YVNEmjS22c zcb)&H?FbXzQ_v4f*{5S+0!-*ni3VL%AK#z0qsq;P%H%omuyn?B=Bd4h%HOQ*ES|8X z157u7CmMI$$I+j7yX-*Nu8Ec;upPE!`C;kxj&TPIJ00dXYdh{wJnm=@4C4+rOA1^O zPkcYAZv=qK1uKs2;4>4tsE+$5{W1scAnsU>5Ie1NN|Ks{A1z!M@=_xOv?fSL({f)ll;^Lyh*mJOUPS$fk8g$cXzQf1r zQt0fsXN|X3sb5#uKL?odeaY26UzIO;dZH5reAQ3@<(a4E=yw9NO>ldyCt%!#>g^Z8 zgc#D=e6g1+SkvtZqqmJO8;|$OvI6} zZ?4V9tAiLX<+4unq_lnwCd^Lw%iscm9*m?=wdMn=5Av%#{ZwyQX2-65&*{Ag_ ztaiPHMn~M3a2tb#y6#xG%wUVFi`g?Q0@{f%Jp>4^lbAbWunG3MT!4^tV}b~)bmQ)d z*#f@K|ENK44QQ3DA2sk!Bmh}AzAjiXFlkuC7H_lA&@g`d(G03|57l7;Zd}D+FWyDs zr%tFgKA_k{oy8zR?)Bq#3A9(k`)96 zYGBg9K#p!Wfn@J6>58x$3#HLm)t{zgL4k&C=S)1GK`%t7*;^8W^PieHw} z7ais=OZ|w`$NtOG{r0fb;^v!+Y*<&n`holH^*^?HRsCa++9g&Tg|QfG`Q#Jp5|sRE z_ihNSGWH0Hp)hja+rAxgBz57YZQJa{>~XYimC@HCv=@kuTNqr}qDxv5;6Y&=ue`j) z62^;c-t>Ylj1bu3k)toy6rm)pf|~jgi!N6ama-O%kFb5u47*C|B+8?}>#x3Q<)UWj zqUp7S2U|qmf-n~HFburttqbO3cYE%iZm6Dj6W)h0&V>}YY4+?nH_n(>HEXY4$FbXc z?>zg$dGqGZGTs=%@_`%%ou%3)< zlc15s^B|TCz=vi)Ud@69b}|yg+s6cM2n`-$>va_`2lOfc?O}C=8#gRn@G2TsFNpK% zg`dA`xL7jq8wdD}Bc{u{`Gx8PfRNP=CRhq}nZ(fQ0C6;o#Xm3c%*8Qn5SVek?Zg@o zK(PX*1q?585J3{Jx`OgfVnPqq^l+t&W^FyKhV@H><{JledxJyYY;n|B6Z4~G!~qUE z*eiKt)x)!A#<+p8fX3ztw#RFJ7CopDYfIXb)vp^^Alkd{BYX@F$Y4{0ur+m0{N)Ms zFbjk$5(5*)`91mDOoZ{>73^5e`*bcqNV+k>WO&t+ zt^^?cAe87~t8g={I`5gLExLH-WB3LM+1JR?-C=+W_f_1v{23z!I>cjTu)w(JNCK_d{9snTP)vy4W zR={IIocNheM!)|4K_mP_3Zu?(kYx1nbQdGQ!3lcZ70IvIvEz; zqGj(&{dK|oI6oQ3$ftV}zBi{%9mPD%0ZfbMS7i+u;U8U@QB<9AaaH6TTa{6=*f+d9 zBPSn~xSt0*Vd-u_4?LL#wqWFcZZK>|M!k<-z=73h*rSRVn6URsowRD^^zyRenCJM5 z$6;FwyCBhftS+`_%awjI&bx_t0sH0!)_W`lChpn<2-5{^j>Q6mN;M|@LL1hh^E&4_ zr4sKKOuueA`l4pSg~O4A1?eZ_`ghPY|I9JRAYocCcQzc79{kuEpey5I3RlmZ4(s@b zjrWb7?i*QYKmDVp`!BBc_sP#6k`E7=*nDV(sM5RB%!Spf2-BRZS=wqCoC!!m73fdJ zqelQu#-iqow3`+1_=HJ&Bu#y5-C8A_Vz@qQ(BJ>iknS`)4%y z=QL-m2&=N%jDh)!XHKu4Q&l*2T>h{EbatMUh%9?O_Bam(JQdRaIYRwS{2w+bV~XY- zBUF*#de{QJt$Kb{anZOD!wYhU2=#KDl_?rt+%sS%V6ISY1%%&*+aI z`Vm_RFtPct@Phd;2H}q|`tXDpzZi&&xCEG>AJihvXdkSp>6OhJvAwMphTj>U`d7R02kY;E?Nxu;*zPQkL)_DK?CjX*ll~V}jXg+`%IGk?NN+Or08vPc;@?}`oD%^Nb z2ZyWVi8H~)RxL(EgQE)!{xB+06?&3y*f`&y!rapf&=N2wupO!bCUA)kj>6=aX3r)} znkS4&Y79plh1!*aGmH{gu)Jg(PIPt{g(*r@CtxKzO6)p4i>yKagwCe>>A=wpg-ttezu}q=? zhx-=b(!XMY`8weaG$1&1Sj{(by1JlRW%MVjX;;~vhPd2t1bl}*=LY(puXz15f1Y-m zw4iIRo70#l2FMfgIB&yiC-gR8PN5o9>_0BIYDy&mLVkOIz!5MR6ARre(Mrz1WKP30 z6k$HiXY3kL3$aZLQ6NLVMyJWm&Qd+HRKMJUvkK}~(EkdAmLiv_@dPTyDX+E_Jn{c&`jmz@&4oTec2;2vU4%D!AVwKS6RDs@#g31 zo3`S4AD*i5iT?o(KQ`IAx$(Jrcq!_s`u5c%Omk_HM8Af%_VtJ-oODZ0Klvf5bg$CH zS|pLNV5xO=SZDgnmSrq#qGj&IeDgGHT>sNYy>9cTbaly!Oxn~t3 zxBsAg5E+?gbWbvWGL9DKn6D&9ctVV*t9`&&BhWk(S&LC!ZEb*y|H?xRDfZ84%2*zz zb7?r^jyBwC`e!o4tLlAawZ4h9SaYh9W$I$A?5ZgoS*4OQaC2;RHIM{D!(O^4VGqY5 z$$<$8^fIi~={X{PGOm9F%{>4n1H?ky9r%J3`L3#06XBVV zTt@QI)79{Dg(dR9LM%ix1{UNGDL}P=V!&#qx1azBz8g23)?E}ZxG`ZpMY}xab_`rT zRbigw(-zX59W=(YUwFYdth)=&8j(AsAb;2htd`KdU^U2#i;D5aHr5F6oM;2H7O(Va zv~oA1UuXj&Y=W%ljVPl(X@Cgn&fz}#Kh#JG{M-e14UCCGXi08|d;vxr$j6BlaN~;` zjgy~gL)e1<{*imMc3|{#5FE5)`ddv;z(J&UOMLpQufyr=xLJG$^<6=T-EE$ZSjMHGGnTL_telkvGLN9ThBm|ML z6y`Eef_|xcG|)Uz_Xs+5bRV*VON0sKa53#B+(&jKH=%t!y>iO`vFeu17*N#N^{)hR z)2Dwjj-7G*Y%Zb+?W$wg!YxPd6xz|~csrPMA@~Sd+3T9~ z#ssin=IX>x*X`9Fj;*X#FkoSsQV8H{`) zjaN7m%tn|^uyd)*Ot&YZFD~D>(Dkz~%Z~)?i>c(?A z`aiPiu_tB{oB*L&iK>IHp|_kHQJiSi2P21?@c-5Uim{Ra?x;F6`Dhi9h+T}TqQ3To zQLC*E%#COR2xvWKA#mYf9jVN=Mi>BCVJxL+TGbrjSLC=Mj%nUD+Wf~XrY`~$fWpgr5Z*CRCu1HynXa4;|+i?oBm3W7lx5C((+VL%uV2801&Ko}4P zgaKhd7!U@80bxKG5C((+VL%uV2801&Ko}4PgaKhd7!U@80bxKG5C((+VL%uV2801& zKo}4PgaKhd7!U@80bxKG5C((+VL%uV2801&Ko}4Pgn@4r1NYy3*FAq;vV6&%atZ^& zfG}`C7=R0cBidh%_ui#{zUAiX`Sa$AzvC5284nA&cOcl zX>#v*Ko;B5s4yT5SPbmnd(Zu~-jjNS0bxKG5C((+VL%uV2801&Ko}4PgaKhd7!U@8 z0bxKG5C((+VL%uV2801&Ko}4PgaKhd7!U@80bxKG5C((+VL%uV2801&Ko}4PgaKhd z7!U@80bxKG5C((+VL%uV2801&Ko}4P{6i+@h&4 z@ITLh_Gvo&pI25C6b6KW|6v9!Z`OU7@+hS!P2Y6%@FEyJ=GyX#l8FP(3>95nHtF)5 zz|TrTN1XPh!%ApOe;{CM(&ipimpGbWWxt_T%P$O(*^eCdR;;&V%`JwG_UWYQUz z4$LbZTzXn*Zti7+2NvZOb)))^a8^5@psZqI(Up{wd}dKaQL2~PJ|qdj;S~I)n^N9_ zk@Q8!srY2$bA<8ns0_sPvEeYK=wlvsu#9#==7Vc^2y%>pG1f_B#%{GoE~HKBD^jelxF z=`&Yc`t0RJ8xbN73FOE{8I(kMT!m{DD1lZ0hPsiB)+S!Qq2!Y1etFTRapyNIzw5nw z?tTw(NFYZp%Ah35<0@RMPz{KIZh(i*g+iD9;`xilymZTg9aiTUwMw1bOFfAt#Ts~`IGheO`@(f_@Eb@}!OR_y%SUq5U7`=?L@ zB~cz%;aUaTLLV>;l5h>6jz0J0x`$gn`|Ll1zTb4p$!{b4!KvHM8rt~REua1UZ>``0 zB~cz%;aY|60Sf~Xbi?(~`Mh&pY1s6|3(x;6=X>vFANw9cQ~?xQHh+!+`e^)nGp@q5 z3QVGtz|Jrg@&Nwav*`NgpZgas&g$2U5GbI?mX|(<3b4p+H@}RM&pg$LYh$;+?5?ti!p7D?3q_O<`->-f{d zc%R0_2xeLn!{-7)0x%6A?4Ju&tDZ{fcmtlkx$*XN@x@{nhR z=YCxA5NvcWzBhWF_SEBhgXam)=d!3Q4w;0k))qB=4gBy)+)AkQSA7a=@vn_OQ zxgTvobFb#|Q}2f;)#h!98Se7@(?dBeo{x?1k39dNw6_`I$Da2+^xf>)!QbzCwtL=1 zdWYw2PZPekd*1N8j_)SVR!^ht`!&yBJuf5O==qyxGk?FbU%%b$Mba8qlY!5~ZGMxM zq>t7vqO;aL9!FZ{9RTsQW4JTJ%&WrzW?1`~3;ogzEaHKQ{aCEkMVw*1`{J#SmbRE4Oa?U!& zywX7{TI_VBeDl6rn@8?u&laA0qQ}6!m?vol{m}CsK4Jsg$Ore#SLbcO+?(_A3alcR zdcwB;Z?0ZG#C$B=o8zI)^SP%DJ<;f^&hNdqg<*qjyjG)iCf3k48{BJ*ey{<)*v zyoHs_8}UZZUdXo_EB#&IdRHen-ap(DFLbXG(--WmynWx1q}%GO4c_)_^SrgUs5(&o zeJ|Z_?yV*E%>sJuwqJO6YZ%`S`^djJ!u}`5fU$0S(Y|im=W$>%6zALUMS7pZ*pumm zFKWf^3ElgDLHjLKXS|PR7w^b?W|WVEsyz>ieKyA~P$$>py2o?KJS3b0yXvhN8kK%bD(?apCk0W>4g7>X#z5GG1+z+{# zGntNQ2|X6}w$HH@%zLA#bvbN6J2P}wDmw$FHMEC{aHe-$jeT9_(=6H1Z(U=f=M~<$ zUXHze>`U6l!adj<&^u(ZLld=sgbrGS%o}gjkvU3wX?VvE6UR9aRsH7h`?A52$T> zmcSHGTirT6mC{d}-RmBc=?C!4G*Zi#v>WFQ8+&s9F}|bq{fu`BbQ`-opXleOo{#wM zJ^Bf16F)(1f`4NP8+Q!rh2!!QSVxNb!xNcdCzkwy%`}rPk0I8v$nJ;-)Q6; zS7T;#)Mlm4d9(?4?fSW`-Xk=hlbYX==cL#j;IU=cF%}oJ7BbdDCPyDY*Z1)xx)x7B zV)>bs#xwDi#$5X#%3%eXL|(~!c2)2B3%@x~?|sxE)lasbz!NTdui!6uYEJo&;kn}) zPCv$Jl0U-fhdrzKdo_R8c^>5NhrJ|y7%4(XIF{qP{dWa;FEv3qy*^om5-U;PD%7vP zb^1}=2?8bkgldM3RM@wo9;1YxH1T4?ha_5C5YRpI`K(@Vk~eO1{04%~ks9(m39ztEVgiFVq4 z>WRmj-+%w(k3MR6{<#P5jl9|Hs-<(u*ln-8XZfaQpZ%!0`L$PHeWdn*hwi&~%Zr=- zv2*7uTQ;x0e?{k7ioVL-_No>4z1;936h(-*>w@PuZ20hl58l|ib6tm~*t3G&d$Hu3hLb1<3`%J&^3+vd6C5Tho zU=p=!)9Rv+KJ?JG*ItA0+4WCq$rF#R+5UDD)LQqK$E@m}d2-zc@4XlP`fKa0V?@v` zph>$YDv59oCQMoe|=$Za&O~~wl}P+hoW!3{`zC9 zS6PiA@_5}NFr+2fyy=C#Z6;=Uqiu995Cxk({J{NI8;GobeC_)?-bJ5+Id*>dq16ZL zR@T1y*O$N8wJT<&9TXaEYbMcaEbG4T%roflpcl<(G%?2>s6r3@`al1%x7QhaqgOZn z?XRsZEx7n^n_r6OLjVgy9HY@1cg~k}u-rLS( z;>+y0ARnC(V+4M}E^jv;8Q&-h(!eC#g3E;^uBCHVp-xBJx(#?~j4Qa8#7tUqUFX<} z-L^)6(e-*`Bf4}nllJk3=~b8SL>NPFdo37g=!uw4Fmlk0HZ6ueMw-1?=i=?Ott$n+ z7<(9jaC5G?u5+#Gt2)&7zBG{F+HY;!2ltH!?w;s3iu6T?dE?O+@gwo+=#2+DES7%y zw8t%>6+tdetD~R|EB?@Z_rdEt_w>`xKeGXYKXM{zNzC}hr`E$sqFQ~qy&kMlumZw5 zZ`bFaw|jop+6o_LEiml$pmIzs7=KveSmz7RY`_|0^NX7>%~?W3FbS@C=)qOyLf>AG zrJxl-bK73q`qfuo{qvK5pb1MJ9Wm4yd<5wSAAI&%t0iyw=Rb9M$aS5%p8x#kKX1PA z#TGjg-<@d7MQJi+S=OCL*OsL##?W^p5TP7 zU;XP}Q2=wyKR*5#z`ks2v)2PBzhnD$RArHnIP#B=cfSALdmzD85AeMA?z`{raQt30 zK~-`e$w zp4*z<#OX>J!nfag^KV-=KUMz(R`izL;qg#EJUpEnI($C;)a#9};##X7L@?yx0TLg3 zzWPd^Rxu66UJv5?a>9=t+!MU)Ebgs&BMXw4S7JSYIZ<{N589nYm({Svh}&5#GhA{j zN%q!zgq_}5jCYTQ8%XV=u^7cRmbF9UXdCNXbf~@BS+q6|w>LGleDcY^zW4$Qmbc-I zZF`E?HL}kSSav}@cyUMDI)PO;DC)@0BKE`VmF{gETawt)!X18OjA0oK?cZ!~XAwYg zTfVpL#Vk*4@9oYaE{R!J2Zgu`!A*mqcPDlh?REuQ*x|&QA6vY3xy#;IqQl;LFHT!I z!g;567IEJKJ>dIXTOn3I$K4dRPGa|Spx7@vi&`5i#10;$M7xo^nWvckl%Rx4F4G#b{6T24s&NQ4e_4%*moA6!T)?lAM=<# zV|s!UrAoqyPUR~LCpeYzO$;YEmGWN|PH^ItjH|*4PUKGxCpeYLx;C8PRI2}!aDo&0 z*M$?DO6AT7CpeYLpAk-QB7bH$!KqZioN$6usS)$T2~OlM3@131ntt8%1n0qi$Nig$ z-0|s)4zuI$#Q*!$8|g$HKfEU_r!XK42m``Emt#PwE(civ3j@M{Fdz(o0rB5*`ymVn z1HwSpWI+6P*R-1~K7;{bKo}7JE&f{=5C(*SuE~J-@2+V#S$PNp!hkR!{#*RFFdz&F z16`8=@!wt3ZnE+a2801&K>WA(Z(%?f5C*y?1LD8Crrl)aAq)ru!T|g?{*z`og#lqe z7!U@!8Uyg(atH&$fG{8obU_Bhe|JHn31neF7!U@)fcS6e2Eu?aAPjU(2E>1NO}hzd zVL%uV2E>0$9}otF0b!tPG9doDYuZgv3j@M{Fd+V0`hYMX3WA(Z(%?f5C*y?1LD8Crrl)aAq)ru!hra1@!!ILFdz(cO$Nk&cTKy= z%0n0s2803e-{QZ80bxKG=$Z_`e}B;?r!XK42m``Emtz3_TMl7B7!U@8fiB2^`0p-g zG=VG(2m`_Z7!dz0-9Q)+284mG$$x)mFdz(cO$Nk&cTKwq zYGFVa5C+75OCJyhgaKioYce4IyKCA_PzwXXfG{BbTl#=7APfitU6XiY1P9O{j1HwSpWI(Bwk6Yvv2801&Kp5z942b`h z=}Q<8284mG$$T;tw40z72801&0Dl-xDRo>Ar8aMB zS?59gCVVE0xwgEbWa5CrlJel>P;p7Q6)P+$x~wFW6PQ?D91Kk;yEG^8vyxDGS#Z)B zrwkZK|8fF9o;;yqa;W5tNhOmjLPZmD0;48hI-#ui+>&e04~{RHbjGEpomMosc<`yY z`FVp%1`f$THoCFNlPbz4mYA)NDXH)Rl=&$&%AXl3y1H!AUYkKF6sF zb%iQZvDTPx_ zo2I`IBC6J}U$54`xn6y`X}L;HP6h&>>d~Wz>e;iWN>5K${rdG&#~gEv>fgV=y5NEf z)X0$|RY5_4%FoYN(|#9Lqeg|*0@bL(|7cJf8(UOkbF-RvUo9HS!>1IV8hjdbG7qvm z$nqe|BUxA%DTTZg@>0l4AuolTE~|mO2J#vtY9OzH9AByd@&?EoAa8)60rD`?VWh(i z+;SemJhY#Oit=z#o(d~&9zh;3|*4fu$6Um!hFkR9Xrw zOan4Y5h(?hQea_PfTI*RN`a#kI7)$osRE8tU?>HKQeYtJN`a#k7)pVm0d0j5GKFYI z|3jMTC3b)cZP%a_Q;oK3fT;$J)}VnJV5&jeHOQ<%B{gWf25qxBxNXSMehu2MLHlH) z8nj;nZfby`1{i9<1DOr1u=bGP8zGqy95kT424HIdjs~>HrX?KUpaD1<2nQ5nbK{~0 zU}*rB27qWlgWLsxr2!WXYU|dm>aDllQag6+ zP&;?-RIRP8>dP;`RG;n%tGkyh!8o1pdfPT#n$`!M#R|E=lg2-ozZCu;KJ$u#+-dTEZ}sf8zqQ-RQ1Fp|FL zI29jrVxY;vqcTX$$A-g{qK|phE?;)go*UNHuYTZuJG{qMud09SQM<&7qcE(~^2sOG zB`EpT?%fbtW$Y0YLt*5;w|zV0Nb15(+qT(@+2d&4DxZ?{g zs2RFwdM)9>7U@olD;grQw!$Qd$r5DHgvb-HtgTAHg{-a69l(REtuQtcudS+iDb?DC z7F20z{?^@njkT4T-hGZbU&(+WZLuD+4A6&E-MAw;ycLhPJ+N<%_x zD|_0HwH2jz^`-3Ht$sh%zq@Z*H78U3oV~j`t)3ge+KRIQs1++YzkO{*aE!&&H@~(b z|Eq6u#Si1AN$zoRUnTdr@V9c0+u3{EK~IxEjrif8YeA}Zv;Nkfv*2kno%(YY*!(aA z7wj4o@xxqT5JJNbTWLth58Kn^hbg^_$|E0sn7#+4UBF55!xXt7?P+cRewdPkQ2em$ zHOgM2>@{|sy~dQ6pG&mYxMYLV5C3ux3rxS;`b!pUFw?2OWP!~OBfVtTo`@gj0(%e| ze%MMwLVnnuCO=H+T~r?V@Wb@ICv6E=Kz>-KH*f>+!<+=dgTW7<%A0f^JR-C|^Em-4 zEaD1_kmeiKk&#|tJo8Qv*&nMuQ;mSUZaXgWl z+V<&Ze0{3r2<;IxjLYfznEAWfx&Djxu1Eg9wEsoRzTpo8e``YhQ(FxFW9lDHZH2L5 zo;du34ZFn6PyGxH{FCe(v;JMFpTcr<#LoYsWdZk#MES_0i8E&Xz}K&$0`p}2{4s@v zV>sWMIBk*7TiPlL3o9tUpE>W^zt01@e*L)peq&-C$iC(lZXYySQ?)r|AKlv0c1rw*|R=Hd1r|{lN`%i*x8JQt_tAfj$bxj3}Td+H~~5=Qs2s z>)?T>opKsUy5T)brOwBjA;(U|iUi-U^+9}0MF_8+mQ#sak5}qVTzKpSV^qZ{`VGTl z&$(dunaG=t^m{4DE0_?xlnUd(+bEt3zvJ~CLNDHab$lMZnn4qG_<9$uPEQ$OHvxR0*Al*4(x-$VX4-)e-P zap*sU+B%lQfrn7ZpKw?VNs5|!2(>J1LxkIE?B)^Dt`n zvBQo@O;N3oq^V;LC*;54@G69dsXrY4o5K%Na}lPgMJ!)+I9ck+!z&Q(Jly9yO!f4W z1+)BgZLXhk&*1P%mMlb=qMr0q{Y@;{;U~N7MtGPyre|``!_>JPUWG7SJ=62Co~XI! z7RXb3QSPxEjz*ZKs(O)p5ldd?u(cPp{{7zldZ(x{y-D{=IJ|+wCpm0E2!wsEMqk#w znL>WQ1n)p+qVH2Y3ElogfDZ4v4lJ#86> zwH(%QSg*>w>FRH462k53Duf?ElBN8f3U5z!w5JSVw&xmzr*V9krxNkAIUEH^uDZlC z72&TD_EeKSX{x81j?klOJlA9uAYd zR9klrdvka+hsSbwo_7XvFZ9ktSb`qyQ4?8S!Qm7RZ{YAI4i|Iyh<6UkKgscDIedx3 zzjL^a!*@CSh{I1gY~#?IM0k!)x(#7}jt@?{6Y+DB?m{?*CBNjbjKeue_dvdwp8pup-0&@otr|fYs?Edu94r)agFMMls1%E zoJ1kDI4VGWX+}mk=}|~N;nXv#Tk_*b^~j>Uo7ADn>+x1#fKz`^y^^0nYN3%Dki5y8 zr0zCS`N@Cvc2f@>L)ZO5{lDZ#fq(JYIyFAI5vhUa=+qRXe5%4o%}ss__Ich&-GWqi zb=?2cCI5_64>iF^-H+6v>JcMVhty%}-$v?5w0F2Ve}um7Z$NgqDmPMZ|Nr)`2Dpvm zI={OEK!QIY0ZO(Ni54WqbrhSFKvF-HCE4JQWJIJylay^aoze&3NL~cspu<5Tl(r_O zX?C%bPLei1&P0vvjx%+dCUG*I#Bmb#d%Jh| zBNe;TnYNPxB=+{b-F^G^y|-`o_HGxbFHCQ9q3;9gp}Sq^7l78&!!Fd}|C)L!ecgpN z0@^@7bfMS#pHw%}u_LmEy-4e&MHe~*Xfu^vXco{GdXEbw0d1qdcA-3=E9fZ~`dvU* z((^9#HbC3ymoD@kKvz-wgsktQsJ)MRT| zUFbzXd+0-xlBO1TQXQs`y3l$+*U&$?(6+z>>H#v2%AEa3J3x24&^|!d(o+JubIL;CUsWFKIp^n;Lf}R9FkO9&;kVN5sPB4Ol8~^- z>uE(m(ju>?=oIHH(Ow5RNqWYGo6B?}_k72vvINBAD9Lmqnh7g|#`GaVgJ>$zesvJb>A*LYStXUqu^1`$4>Zcr`_`m`;|_Fz!kt_%mm2;UgIREsGmgT2}YBBXp!3<_5c2;+a4*7d1P^E@=(*qu>S}Mh z9`VcCW;9*d-1f3s5@pLuqV2b|I}l&cwhK;%5#}^qgf|FGLWJj(KWKYEjVpH`Jg?l{ z=7671w4Ft%FSeDn`$6Fkf%!?>yR`Eda^H)1u>Gv|gmPv3-N>`2{bL9d?e}YOQ2P{8 zkGDUJFxh@VeHLXIlW%_vVX1uu9`4TezXzSW+Z~kpOT^>KlkH#8UQ%9Ye;V;0wSOD< zUx=I?9pBM{>WT)RqFmkaJ*4hKI7~Np{1_A#I~>IAjE z4V~Z<)K-AkJ3D=GWvO!=!Vh zP{J1%sd42Sp<{?Y9|AujXPo{G@iP4~^hR(M?9!3CvFlTy&sr#{S9P5Pey|Hxq3%b> z_I3)G$*$WFjt73vw?Q$x-sZbQ{iCjPzH?~l`+X+#_-DYJ2gam(y6*MeqdwU6N#FhI z7rGuo_?50lfq6K{m~VA`9(kVc`l9ba_2*q|pKaY=2j;5o2h=YMEt~?iANfwvME6Sw zPjo+|o}xD)RTp7U5VzM$UKP5v$Pf$o6cgjKiugY@z403`WT zcaL8O<;(qwwxYe>e_q|(Htc^^gxv2QP{Z0__&R@B)5C}T8?-yalm6}6Ps207?2XJL zWT|qm$sqMiq~K3zpNy3J9rRe_?S5T*KJrfg9qLNt-To64?wQd}Y1j1J=HvML{pSSz z^U7F{qm{IiJrDcOD{n^p9IX0r#P1UL5BEHzmbAY@c#fXzc?z6A-NW{vtouiQT^2U3h+h!?+}ifjMiFkK0OD6sFTxqj9{cGU(h*`Zf$$`K9O0YkafB)Q2*LvWZQxe=7tB;g zwG-L_->h%mSMt5ncfq&8{~P|8|3m)o_}}99Ewk5E`A*#ZY{s<;u{WL-DfPJA8aR39 zQk(CcYCvo)(Zrp*tU#eZ56c+SCOU*Qgii&0BCaF;Ohm?i8}Y@0-t>NV7)4nKA6shTU&08Fa|9v#}Z^VIS?RvE+>EnM_ho zoupCSc8r3l=SD5tHc}3D@rptN=3r)LJ1X&T-tPE?sdSu4pO2pZDe%EJZ;qQ5KpIVqfnSI3r-@v zL{paSP#()Ic`LWzv33{>Dtg8|i?ZO_-Ey4B&0AC~80pa>??9&6MH}0dQyB{=!HsO> zjAP_!(M+d}oRi8^##*pMP;e|1vh}oS@y4NPD``1`)6}AF7bNo{3jT|EY&e6@BVM9i4N`X>EyGA8|5_u~{2DE6|w1Di3 zdM;gw#E8K{#!MU4*x{;>g^UwfeZgR@lIg_;$jqTq%CN*@1M)g3FT3Prx4i6;m%Z{b zOqpcH-FZ$~SU~+AFxZTlbC3XI$Xf+d7>QMMpy(+*hc@RqjM0>tVnZ3RmSA9B6pCJi z$(b;wnkaPcVF$h1xHpls@R6dK!Nz!!HsL`IGJyC1&0;&dO}weyj(Mez8czR=l{#rS z1v;M2k7i7$go>4z+azhkFn`ya>FgQ9bPQYKsXxUE88ZuLr$LiY42>^g-+jtqg}L2t z{4_|%Z7f&{r!Y5R>1jG_K;`JM@m%2ym`mg!Chk(`imnyVBnz-wn5i=XfgZEYcuDYDN>!3Y#(B3*|xDJ{t=I2--ZYOlp1mrBjWuoKF z)4ZKUg78yb-*Pi@zp6-B$h;0CWM1)Q(7Jh=ov`drUgNm;70#GWYLP@Ia7jyBP2(5^ zDE~ChW}rm8P|Brn?gG3ziFjqO7%>*ioSSVtmv&=u8lh3TnNH9xgkpx{)+{e>e;&dh zB>Dll{UHY1bJ$tiS>eBr?(7!tn;)mZtekQE?<-`83v1^4+;xpCEoBb&^WW=$t!#2xr?_+XZBFR}em(uETQ#OX_l(RZv)XJA^KF?rv(kuoeBdgmdS0}kcUNymM zr93;MKaqovu<|?~FmZqfp;O;z7R+RAdk+`Qbd`YDde^P4PB?_|Y*nIHt#?ug9hJv^ zz#8PyArpFTp$NxcrRNoKIc0YP%62WQKnVHr>|ja63f1z{vLKg3Qqs(rPK~@>mp5yT zV$_b?wq7C`vYcnFijvs|$Cr#$5uSd*YkMX2sE+XxImFc)Xci1-&WpngQn~}N<{aC~ zV4C2}iQyn2U9k>9)68sKdg&_f@q8YBOtgs54)|)*A*MTFKb$gYLkxPGjJ;3B)nal1LYzS<4kjb(9yAGgpWlzaB0I)X-Xn4%t@L z#Z(3YPnnZ?4w}SRF;mRv;iIc#2D$zLt(hqnn2ia&1ZM*QWq88C;<5EyA+Os!%CWdc zFb9>XhoI~krvx4tUxf}qxEQf=ZsK8E&o7#(!f}jLGN(ufzZlw*F2=_2#WgmvjMllr z6VnipTgDPugC{~RrW&8HTs<*U!VdVX5|OPEnbnhtT-sO?cEDq7#$3o{y%w@T2#jd| zNQNifOia`(6Q;T2&sr14ylhe}JX*917A!$t1r1h=AmQRF)Fc-bP|UKbqqo$jO?o;# zR~@`7(D6J8mbetG#enT&CgU+GF|r9Z5Bi)^qf9ApK>$(D#j?f)4wh^9}+?@uFkRV2rC;>S_!c z2qP3`%I;vqvf&?Ux>y5_59$%sRw9%7l9`3M^VC=fz|7UbRsE+*P_B@zm{p3o0s1HU zUomUA5@G$^EZTX(b2BuF5!kTBEgL5b^RV)BQTnG-p+nKn)pO&j-(l^fe> zBaa?z&`1fk>5gHT!&71k8nto5O7UC?5F{XPKaOJzj_n5K^lna>Hc|#|biqIT7d+HB z_XC6}OPCo~9iJIZBv?W&JBV>@+=bBZF|Rl5oSqq9N*Q8uA{sJo+ZJpC4$O7@@}8oa zsTvbAO!qcI%)8zAlx?B|K<2FJIv`P~nz)>*6Srz`fJXQQh*=Ax1$;E~UU6X}GO1%Q zn{s6hiRw1i860=g;`UrI-5tX{hqb`g!ZWo+7`;H?zDC^G@L0&VSeQ9UTdEGF5%i?*`Lch3CGgFkPX1g^;hfFL$ z9J4*Q=hn3rSE~ddCIGCnMiGbv46nSPvlgX>_PIH$?ejQAJ#3FMW?Wb|%b$~rclfXl;B#*|6(mb#h%0%&c z)eK5AHO>LogR4!aMYta?mQ-m-nw&3%v-r5g0L4V@F&wW( zHU{olihSjk>=Y?GTshL_TA4(Jlyt}l%XiSj0#VmZkO>wxghUOZ%`CSBP0=x|33$5L z;p*)mR;}@BTh}q@U>>EI2FfhowI8ZD0unaae3Oc2^y?B7naAGzLbGp)oNieHTn?M+GCzqS{>QoQ0WTg+-ji zNLvab?ulz2cs7^nIgPY_tsK-d1u5{~E*Mq}AzmTea0^6bLTE-Kjn3K>>Pk`(>Tyt4 z4kfA26uj`MkP|4+X3oZ0e-aY{-Elyw+zZ3HjWJmg3F@Z50P)ZjK`^&&cG|Qe*m_JaVQWl_E;P z)uYssknjj3lEU+)1>o^S0n$v@NR2LCyIifF>g7x|daBnayM?MgYV$!dof+_v1ZNpM zAtMbhdDLK_UpyVJ#Xt_)Ep-^^3{S6X^FXWGNF4_9)$lgU#(Le>aaE@e=&&wtLrd%O zqOH9PXajflMO;~qb(F)6skFgB9#hz`U7xqJHf62;)xgOb{R{lGA9ec|tZ0os+Mtg$ zXh0L|Ck2iD7@fSogQWB{wgr+3O2^7FyZDK%)K#BGT~)n{UbbA0!bKj(H;#&@VUkIF z9bxQII*iVnB)m!lW}&P#=h0Ir_BFo_Z4>Z{YC!i)tdtP({130B8 zi_~1bLRut_E@kz~QJ_w_?NvNo%H0SPEl{b?Q^nJje9~~1Rpvb&itIKS>cs|uX%URu0;{8e7L_&+2pvvy_&=sdOALu1+jUfOqb&Q2F{bPw#C%B;=gs z9Lx9qZ!0#65iN=lE=g|!+w2SX2_u@NIZT0OVOHZXxT7MjwBy-~+dS%&qm&Fui{eG~ zgBTyF<)D~?(JmfaRnsB=M#3ALSu zjh#Icr*a)$-{#RscBX!$a5tch5-uTM27eab!mp%lkZC-!@T8Tylw0b=je_zSv@3@) zEDx3pk6YDmis#El3}H@v^k- zmFZKb)XhFI-L4&7*yueC<{&|KOgtjj=0P_Zj)P+!6{Me&Ejx;w4kX34a1xxZc0&cC zD_BEW{PCE~a$Bt-Oez~XCb$aj7SmfE{h+U!Q53U{ZS~XKDb#O4^2|3&kZ%vD(rLjx zTDV4zmTBaKRkYX4+t%NNQIE$m+=+r`3+)1T_!O?b>wEqGd|~X~G$a?IZXj-C)wdK6MpLy#wdpB6Q|CAM5(q$)lHFdoCOC8A(W1E^ltJ6BISfoN;%Og>ziziD!u;hNO{snin2+x z@C2F`>h&Y+^@oB{t+m(RiVUo?(57`QExrC;KVb2%N6JRTrDe*NWRbBGFQGzMVN@0h zL4|_v5?GG-rBOT@(pnTzE?oZomKHw>batXVXT}TEy^-?mC=)5aU4(aZbSui`v_4Wk zhl{@rf_k4I2^p#|3^WsY?{157mV1zTZ>tx9^m<4nCq#))01@eXO-l<*t9(1?AT!ri zDIgp1sJsXx3zwge&=(=2mQaA$#+ZL~JvJ=i9hYcdRKBSfot{a+qS>tJ5{1G?jvt9f z#hYh_9X*^)Vk5|F6k1=iJXf<4jU7w0t@8Rn^fUKFR~3&A?2KW=+o;eb7fai&&}--C zle>+fLA`JH(4f&bq^Dzjx{*%y4G!)e-l^|S?Hb&Z#P&#qT4LB8i7zI4IBv(lE0L|< z@eY^sUaeX6lAG|D_NbMsJa)h%aJ)pQ{PANxZlw791;?W<&~W)+O+KPItd-*_{Un}% z$#)M8qd$$eY522p1!#mDiZ{Wg1ey|PN}wr$rUaT2XiA_dfu;nS5_nY-@L^vOVV#VY zL%2@i+KuZXu9xAuEdCLxeB=(QcLUGm?pu|Po0{8*zOV+KZ+D)#2>z#Q;JM0fE>!oC zi0DP|J1>I2?jrap#y{^9zZU0Jv-n+~I62gVyMOG%^XTg5)@Ob6L2kVJ*^Bh=*3kc; zhV?}}b)3O!`i){GonJaQhV}G0T#w=jx7Z9$pZ5LWm;3;>@q4XHm+zcv$O`P3z}<)y zP#bjQ(_vmS1B6=fQq_D36^_v0ygzjygvm zG~woHz|GTuo2LPbxrp~0GCWIJ#JdeRB{C}KEc^>H-crvG9{Rie*DhrE9rwl|vAXhlBwP@$| z6;g}X=U?c{n%E9hu;!h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rsi_config/RSIPI_Full.rsi.diagram b/rsi_config/RSIPI_Full.rsi.diagram new file mode 100644 index 0000000..ed040ad --- /dev/null +++ b/rsi_config/RSIPI_Full.rsi.diagram @@ -0,0 +1,256 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rsi_config/RSIPI_Full.rsi.xml b/rsi_config/RSIPI_Full.rsi.xml new file mode 100644 index 0000000..ba38586 --- /dev/null +++ b/rsi_config/RSIPI_Full.rsi.xml @@ -0,0 +1,203 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rsi_config/RSIPI_Test.src b/rsi_config/RSIPI_Test.src new file mode 100644 index 0000000..7e44837 --- /dev/null +++ b/rsi_config/RSIPI_Test.src @@ -0,0 +1,207 @@ +&H NOBOUNDSCHECK +DEF RSIPI_Test() + ; ========================================================================= + ; RSIPI Comprehensive Test Program + ; ========================================================================= + ; Tests all RSIPI Python library functionality: + ; 1. RSI initialisation and connection + ; 2. Cartesian correction (RSI_MOVECORR) + ; 3. Tech variable exchange (Python <-> KRL) + ; 4. $SEN_PREA variable reading + ; 5. Digital I/O coordination + ; 6. Handshake patterns (wait for Python, signal back) + ; + ; Matching Python script: rsi_config/rsipi_test.py + ; + ; Protocol (Tech.C11 = state from KRL, Tech.T11 = command from Python): + ; State 0: Idle + ; State 1: RSI connected, waiting for Python + ; State 2: Running corrections (RSI_MOVECORR active) + ; State 3: Corrections complete, reading SEN_PREA + ; State 4: I/O test phase + ; State 5: Complete + ; State 99: Error + ; + ; Python commands via Tech.T11: + ; 0: No command + ; 1: Python ready, start corrections + ; 2: Stop corrections + ; 3: SEN_PREA values written, read them + ; 4: Start I/O test + ; 5: Shutdown + ; ========================================================================= + + DECL INT ret, CONTID + DECL INT python_cmd + DECL REAL sen_val1, sen_val2, sen_val3 + DECL E6POS start_pos + + INI + + ; -- Store start position ------------------------------------------------ + start_pos = $POS_ACT + + ; -- Move to a safe starting position ------------------------------------ + ; (Adjust these coordinates for your robot/cell) + PTP start_pos + + ; ======================================================================== + ; PHASE 1: Initialise RSI + ; ======================================================================== + + ; Load RSI signal flow configuration + ret = RSI_CREATE("RSIPI_Full.rsi", CONTID, TRUE) + IF (ret <> RSIOK) THEN + MsgNotify("RSI_CREATE failed", "RSIPI_Test") + HALT + ENDIF + + ; Activate RSI in RELATIVE mode at 4ms cycle + ; Change to #ABSOLUTE / #IPO as needed + ret = RSI_ON(#RELATIVE, #IPO_FAST) + IF (ret <> RSIOK) THEN + MsgNotify("RSI_ON failed", "RSIPI_Test") + HALT + ENDIF + + ; Signal state 1: RSI connected, waiting for Python + $TECH.C[11] = 1 + MsgNotify("RSI active - waiting for Python...", "RSIPI_Test") + + ; ======================================================================== + ; PHASE 2: Wait for Python to connect and signal ready + ; ======================================================================== + + ; Poll Tech.T11 for Python's "ready" command (value = 1) + python_cmd = 0 + WHILE (python_cmd <> 1) + python_cmd = $TECH.T[11] + WAIT SEC 0.012 ; Check every 12ms + ENDWHILE + + MsgNotify("Python connected - starting corrections", "RSIPI_Test") + + ; Send current position to Python via Tech.C12-C17 + $TECH.C[12] = $POS_ACT.X + $TECH.C[13] = $POS_ACT.Y + $TECH.C[14] = $POS_ACT.Z + $TECH.C[15] = $POS_ACT.A + $TECH.C[16] = $POS_ACT.B + $TECH.C[17] = $POS_ACT.C + + ; ======================================================================== + ; PHASE 3: Sensor-guided motion (Python sends corrections) + ; ======================================================================== + + ; Signal state 2: corrections active + $TECH.C[11] = 2 + + ; RSI_MOVECORR - robot is now purely controlled by Python corrections + ; The robot will hold position and apply RKorr corrections from Python + ; Python sends corrections via api.motion.update_cartesian() + ; This blocks until Python sends stop command (Tech.T11 = 2) + ; or until RSI is turned off + RSI_MOVECORR() + + MsgNotify("Corrections phase complete", "RSIPI_Test") + + ; ======================================================================== + ; PHASE 4: Read $SEN_PREA values from Python + ; ======================================================================== + + ; Signal state 3: ready to read SEN_PREA + $TECH.C[11] = 3 + + ; Wait for Python to write SEN_PREA values (command = 3) + python_cmd = 0 + WHILE (python_cmd <> 3) + python_cmd = $TECH.T[11] + WAIT SEC 0.012 + ENDWHILE + + ; Read values that Python wrote via MAP2SEN_PREA + sen_val1 = $SEN_PREA[1] + sen_val2 = $SEN_PREA[2] + sen_val3 = $SEN_PREA[3] + + ; Echo them back to Python via Tech.C18-C20 + $TECH.C[18] = sen_val1 + $TECH.C[19] = sen_val2 + $TECH.C[110] = sen_val3 + + MsgNotify("SEN_PREA read complete", "RSIPI_Test") + + ; ======================================================================== + ; PHASE 5: Digital I/O test + ; ======================================================================== + + ; Signal state 4: I/O test phase + $TECH.C[11] = 4 + + ; Wait for Python to start I/O test (command = 4) + python_cmd = 0 + WHILE (python_cmd <> 4) + python_cmd = $TECH.T[11] + WAIT SEC 0.012 + ENDWHILE + + ; Activate gripper (digital output 1) + $OUT[1] = TRUE + MsgNotify("Gripper ON - $OUT[1] = TRUE", "RSIPI_Test") + WAIT SEC 1.0 + + ; Python should see Digout.o1 = TRUE in its send_variables + ; Signal to Python that gripper is on + $TECH.C[11] = 41 ; Sub-state: gripper activated + + ; Wait for Python acknowledgement (command = 41) + python_cmd = 0 + WHILE (python_cmd <> 41) + python_cmd = $TECH.T[11] + WAIT SEC 0.012 + ENDWHILE + + ; Deactivate gripper + $OUT[1] = FALSE + MsgNotify("Gripper OFF - $OUT[1] = FALSE", "RSIPI_Test") + WAIT SEC 0.5 + + ; Signal gripper off + $TECH.C[11] = 42 ; Sub-state: gripper deactivated + + ; Wait for Python acknowledgement + python_cmd = 0 + WHILE (python_cmd <> 42) + python_cmd = $TECH.T[11] + WAIT SEC 0.012 + ENDWHILE + + ; ======================================================================== + ; PHASE 6: Shutdown + ; ======================================================================== + + ; Signal state 5: complete + $TECH.C[11] = 5 + MsgNotify("Test complete - shutting down RSI", "RSIPI_Test") + + ; Wait for Python to acknowledge shutdown (command = 5) + python_cmd = 0 + WHILE (python_cmd <> 5) + python_cmd = $TECH.T[11] + WAIT SEC 0.012 + ENDWHILE + + ; Clean up RSI + ret = RSI_OFF() + IF (ret <> RSIOK) THEN + MsgNotify("RSI_OFF warning", "RSIPI_Test") + ENDIF + + ret = RSI_DELETE(CONTID) + + ; Return to start + PTP start_pos + + MsgNotify("RSIPI_Test complete!", "RSIPI_Test") + +END diff --git a/rsi_config/RSI_EthernetConfig_Full.xml b/rsi_config/RSI_EthernetConfig_Full.xml new file mode 100644 index 0000000..310d10e --- /dev/null +++ b/rsi_config/RSI_EthernetConfig_Full.xml @@ -0,0 +1,159 @@ + + + 10.10.10.10 + 64000 + ImFree + FALSE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rsi_config/rsipi_test.py b/rsi_config/rsipi_test.py new file mode 100644 index 0000000..7d528c1 --- /dev/null +++ b/rsi_config/rsipi_test.py @@ -0,0 +1,196 @@ +""" +RSIPI Comprehensive Test Script +================================ +Matching Python counterpart for RSIPI_Test.src KRL program. +Tests all RSIPI functionality in coordination with the robot. + +Protocol (Tech.C11 = state from KRL, Tech.T11 = command from Python): + KRL States: 0=Idle, 1=Waiting, 2=Corrections, 3=SEN_PREA, 4=I/O, 5=Done + Python Cmds: 1=Ready, 2=Stop, 3=SEN_PREA written, 4=Start I/O, 5=Shutdown + +Usage: + 1. Load RSIPI_Test.src on the robot controller + 2. Run this script: python rsipi_test.py + 3. Start the KRL program on the pendant +""" + +import time +import sys +import os +from multiprocessing import freeze_support + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'src')) +from RSIPI import RSIAPI + +# ── Helpers ────────────────────────────────────────────────────────────── + +def wait_for_state(api, state, timeout=30): + """Wait for KRL to reach a specific state via Tech.C11.""" + start = time.time() + while time.time() - start < timeout: + try: + krl_state = int(api.krl.read_param('C11')) + if krl_state == state: + return True + except Exception: + pass + time.sleep(0.05) + print(f" TIMEOUT waiting for KRL state {state}") + return False + + +def send_command(api, cmd): + """Send a command to KRL via Tech.T11.""" + api.krl.write_param('T11', cmd) + print(f" -> Sent command: {cmd}") + + +# ── Main Test Sequence ─────────────────────────────────────────────────── + +if __name__ == '__main__': + freeze_support() + + api = RSIAPI( + os.path.join(os.path.dirname(__file__), '..', 'RSI_EthernetConfig_Full.xml'), + rsi_mode='relative', + max_cartesian_rate=0.5, + max_joint_rate=0.2, + cycle_time=0.004 + ) + + print("=" * 60) + print("RSIPI Comprehensive Test") + print("=" * 60) + + # ── Start RSI and wait for robot ──────────────────────────────── + print("\n[1] Starting RSI connection...") + api.start() + if not api.wait_for_connection(timeout=30): + print(" FAILED: No connection. Is the KRL program running?") + api.stop() + sys.exit(1) + print(f" Connected! IPOC: {api.monitoring.get_ipoc()}") + + # ── Wait for KRL to reach state 1 (RSI active, waiting) ──────── + print("\n[2] Waiting for KRL program to initialise RSI...") + if not wait_for_state(api, 1): + print(" FAILED: KRL did not reach state 1") + api.stop() + sys.exit(1) + print(" KRL is ready and waiting for us") + + # Read the position KRL sent us + try: + pos_x = api.krl.read_param('C12') + pos_y = api.krl.read_param('C13') + pos_z = api.krl.read_param('C14') + print(f" Robot position from KRL: X={pos_x:.1f} Y={pos_y:.1f} Z={pos_z:.1f}") + except Exception as e: + print(f" Could not read position: {e}") + + # ── Signal ready and start corrections ────────────────────────── + print("\n[3] Signalling ready, starting correction phase...") + send_command(api, 1) + + if not wait_for_state(api, 2): + print(" FAILED: KRL did not enter correction mode") + api.stop() + sys.exit(1) + print(" KRL is in RSI_MOVECORR - sending corrections...") + + # Send a small circle pattern as corrections + circle = api.motion.generate_circle( + center={"X": 0, "Y": 0, "Z": 0}, + radius=3, steps=100) + + # Convert to relative deltas + circle_rel = [] + prev = circle[0] + for pt in circle[1:]: + delta = {k: pt[k] - prev.get(k, 0) for k in pt} + circle_rel.append(delta) + prev = pt + + print(f" Executing circle: {len(circle_rel)} steps, radius=3mm") + api.motion.execute_trajectory(circle_rel, space="cartesian", rate=0.012) + print(" Circle complete!") + + time.sleep(1) + + # Tell KRL to stop corrections + print(" Stopping corrections...") + send_command(api, 2) + time.sleep(1) + + # ── SEN_PREA test ─────────────────────────────────────────────── + print("\n[4] Testing SEN_PREA variable exchange...") + if not wait_for_state(api, 3): + print(" FAILED: KRL did not reach SEN_PREA phase") + api.stop() + sys.exit(1) + + # Write test values to SEN_PREA via the corrections + # (MAP2SEN_PREA in the RSI config maps ETHERNET Out1-3 to SEN_PREA[1-3]) + # These go through RKorr.X/Y/Z → ETHERNET Out1-3 → MAP2SEN_PREA + test_vals = [42.0, 123.456, -99.9] + print(f" Writing SEN_PREA test values: {test_vals}") + api.motion.update_cartesian(X=test_vals[0], Y=test_vals[1], Z=test_vals[2]) + time.sleep(0.5) + + # Signal KRL to read them + send_command(api, 3) + time.sleep(1) + + # Read back what KRL echoed via Tech.C18-C110 + try: + echo1 = api.krl.read_param('C18') + echo2 = api.krl.read_param('C19') + echo3 = api.krl.read_param('C110') + print(f" KRL echoed back: [{echo1}, {echo2}, {echo3}]") + print(f" Match: {abs(echo1 - test_vals[0]) < 0.1 and abs(echo2 - test_vals[1]) < 0.1}") + except Exception as e: + print(f" Could not read echo values: {e}") + + # Zero corrections + api.motion.update_cartesian(X=0, Y=0, Z=0) + + # ── Digital I/O test ──────────────────────────────────────────── + print("\n[5] Testing Digital I/O coordination...") + if not wait_for_state(api, 4, timeout=10): + print(" Skipping I/O test (KRL not in I/O phase)") + else: + send_command(api, 4) + + # Wait for KRL to activate gripper (state 41) + if wait_for_state(api, 41, timeout=10): + # Read digital output state from robot + live = api.monitoring.get_live_data() + print(f" KRL activated gripper ($OUT[1])") + print(f" Live position: {live['position']}") + + # Acknowledge + send_command(api, 41) + + # Wait for KRL to deactivate (state 42) + if wait_for_state(api, 42, timeout=10): + print(f" KRL deactivated gripper ($OUT[1])") + send_command(api, 42) + else: + print(" TIMEOUT waiting for gripper off") + else: + print(" TIMEOUT waiting for gripper on") + + # ── Shutdown ──────────────────────────────────────────────────── + print("\n[6] Shutting down...") + if wait_for_state(api, 5, timeout=10): + send_command(api, 5) + print(" KRL acknowledged shutdown") + else: + print(" KRL did not reach shutdown state, stopping anyway") + + time.sleep(1) + api.stop() + + print("\n" + "=" * 60) + print("RSIPI Test Complete!") + print("=" * 60) diff --git a/src/RSIPI.egg-info/PKG-INFO b/src/RSIPI.egg-info/PKG-INFO index a6d5435..d703a65 100644 --- a/src/RSIPI.egg-info/PKG-INFO +++ b/src/RSIPI.egg-info/PKG-INFO @@ -251,6 +251,8 @@ Requires-Dist: numpy>=1.22 Requires-Dist: matplotlib>=3.5 Requires-Dist: lxml>=4.9 Requires-Dist: scipy>=1.8 +Provides-Extra: dev +Requires-Dist: pytest>=7.0; extra == "dev" Dynamic: author Dynamic: license-file Dynamic: requires-python diff --git a/src/RSIPI.egg-info/SOURCES.txt b/src/RSIPI.egg-info/SOURCES.txt index 1ddb924..7d41a75 100644 --- a/src/RSIPI.egg-info/SOURCES.txt +++ b/src/RSIPI.egg-info/SOURCES.txt @@ -4,12 +4,20 @@ README.md pyproject.toml setup.py src/RSIPI/__init__.py +src/RSIPI/auto_reconnect.py src/RSIPI/config_parser.py +src/RSIPI/diagnostics_api.py src/RSIPI/echo_server_gui.py +src/RSIPI/exceptions.py src/RSIPI/inject_rsi_to_krl.py +src/RSIPI/io_api.py +src/RSIPI/krl_api.py src/RSIPI/krl_to_csv_parser.py src/RSIPI/kuka_visualiser.py src/RSIPI/live_plotter.py +src/RSIPI/logging_api.py +src/RSIPI/monitoring_api.py +src/RSIPI/motion_api.py src/RSIPI/network_handler.py src/RSIPI/rsi_api.py src/RSIPI/rsi_cli.py @@ -18,13 +26,19 @@ src/RSIPI/rsi_config.py src/RSIPI/rsi_echo_server.py src/RSIPI/rsi_graphing.py src/RSIPI/rsi_limit_parser.py +src/RSIPI/safety_api.py src/RSIPI/safety_manager.py src/RSIPI/static_plotter.py +src/RSIPI/timing_metrics.py +src/RSIPI/tools_api.py src/RSIPI/trajectory_planner.py +src/RSIPI/viz_api.py src/RSIPI/xml_handler.py src/RSIPI.egg-info/PKG-INFO src/RSIPI.egg-info/SOURCES.txt src/RSIPI.egg-info/dependency_links.txt src/RSIPI.egg-info/requires.txt src/RSIPI.egg-info/top_level.txt -tests/test_rsipi.py \ No newline at end of file +tests/test_safety_manager.py +tests/test_trajectory_planner.py +tests/test_xml_handler.py \ No newline at end of file diff --git a/src/RSIPI.egg-info/requires.txt b/src/RSIPI.egg-info/requires.txt index 1279b93..9f30ce0 100644 --- a/src/RSIPI.egg-info/requires.txt +++ b/src/RSIPI.egg-info/requires.txt @@ -3,3 +3,6 @@ numpy>=1.22 matplotlib>=3.5 lxml>=4.9 scipy>=1.8 + +[dev] +pytest>=7.0 diff --git a/src/RSIPI/io_api.py b/src/RSIPI/io_api.py index ddf7b98..0467b55 100644 --- a/src/RSIPI/io_api.py +++ b/src/RSIPI/io_api.py @@ -24,6 +24,8 @@ class IOAPI: client: RSIClient instance for variable access """ self.client = client + from .tools_api import ToolsAPI + self._tools = ToolsAPI(client) def toggle(self, group: str, name: str, state: Union[bool, int]) -> str: """ @@ -54,21 +56,14 @@ class IOAPI: var_name = f"{group}.{name}" state_value = int(bool(state)) # Ensure binary 0 or 1 - # Import here to avoid circular dependency - from .tools_api import ToolsAPI - - tools = ToolsAPI(self.client) - result = tools.update_variable(var_name, state_value) - logging.debug(f"I/O {var_name} set to {state_value}") + result = self._tools.update_variable(var_name, state_value) + logging.debug("I/O %s set to %d", var_name, state_value) return result def set_output(self, channel: int, value: bool, group: str = 'Digout') -> str: """ Set digital output by channel number. - High-level wrapper for setting digital outputs. More convenient than - toggle() when working with standard digital output channels. - Args: channel: Output channel number (1-based, e.g., 1 for o1) value: Desired state (True = ON, False = OFF) @@ -83,16 +78,11 @@ class IOAPI: Example: >>> api.io.set_output(1, True) # Turn ON output 1 - 'Updated Digout.o1 to 1' >>> api.io.set_output(3, False) # Turn OFF output 3 - 'Updated Digout.o3 to 0' - >>> api.io.set_output(5, True, group='DiO') # Custom group - 'Updated DiO.5 to 1' Note: - The default group 'Digout' corresponds to standard KUKA digital - outputs configured in RSI. Channel numbering starts at 1 to match - KUKA controller conventions. + Digout must be configured in the RSI config RECEIVE section + for this to work. Check your RSI_EthernetConfig.xml. """ channel_name = f"o{channel}" return self.toggle(group, channel_name, value) @@ -192,13 +182,13 @@ class IOAPI: # Turn ON self.set_output(channel, True, group=group) - logging.debug(f"Pulse started on {var_name}") + logging.debug("Pulse started on %s", var_name) # Wait for duration time.sleep(duration) # Turn OFF self.set_output(channel, False, group=group) - logging.info(f"Pulse completed on {var_name} (duration: {duration}s)") + logging.info("Pulse completed on %s (duration: %ss)", var_name, duration) return f"Pulse completed on {var_name} (duration: {duration}s)" diff --git a/src/RSIPI/monitoring_api.py b/src/RSIPI/monitoring_api.py index a0ad3fd..0e88684 100644 --- a/src/RSIPI/monitoring_api.py +++ b/src/RSIPI/monitoring_api.py @@ -178,7 +178,7 @@ class MonitoringAPI: try: while True: live_data = self.get_live_data() - ipoc = live_data.get("IPOC", "N/A") + ipoc = live_data.get("ipoc", "N/A") rpos = live_data.get("position", {}) timestamp = datetime.datetime.now().strftime('%H:%M:%S') print(f"[{timestamp}] IPOC: {ipoc} | RIst: {rpos}") diff --git a/src/RSIPI/motion_api.py b/src/RSIPI/motion_api.py index 3a1acec..ebc243f 100644 --- a/src/RSIPI/motion_api.py +++ b/src/RSIPI/motion_api.py @@ -1,7 +1,7 @@ """Motion control API namespace for RSIPI.""" import logging -import asyncio +import threading import math import numpy as np from typing import Dict, List, Any, Optional, Tuple, TYPE_CHECKING @@ -29,6 +29,9 @@ class MotionAPI: self.client = client self.trajectory_queue: List[Dict[str, Any]] = [] + from .tools_api import ToolsAPI + self._tools = ToolsAPI(client) + def update_cartesian(self, **kwargs: float) -> None: """ Update Cartesian correction values (RKorr). @@ -62,13 +65,9 @@ class MotionAPI: logging.warning("RKorr not configured in receive_variables. Skipping Cartesian update.") return - # Import here to avoid circular dependency - from .tools_api import ToolsAPI - tools = ToolsAPI(self.client) - for axis, value in kwargs.items(): - tools.update_variable(f"RKorr.{axis}", float(value)) - logging.debug(f"RKorr.{axis} set to {value}") + self._tools.update_variable(f"RKorr.{axis}", float(value)) + logging.debug("RKorr.%s set to %s", axis, value) def update_joints(self, **kwargs: float) -> None: """ @@ -99,12 +98,29 @@ class MotionAPI: logging.warning("AKorr not configured in receive_variables. Skipping Joint update.") return - from .tools_api import ToolsAPI - tools = ToolsAPI(self.client) - for axis, value in kwargs.items(): - tools.update_variable(f"AKorr.{axis}", float(value)) - logging.debug(f"AKorr.{axis} set to {value}") + self._tools.update_variable(f"AKorr.{axis}", float(value)) + logging.debug("AKorr.%s set to %s", axis, value) + + def get_current_pose(self) -> Dict[str, float]: + """ + Get current TCP position from robot. + + Returns: + Dict with X, Y, Z (mm) and A, B, C (degrees) + """ + rist = self.client.send_variables.get("RIst", {}) + return dict(rist) if isinstance(rist, dict) else {"X": 0, "Y": 0, "Z": 0, "A": 0, "B": 0, "C": 0} + + def get_current_joints(self) -> Dict[str, float]: + """ + Get current joint positions from robot. + + Returns: + Dict with A1-A6 in degrees + """ + aspos = self.client.send_variables.get("ASPos", {}) + return dict(aspos) if isinstance(aspos, dict) else {"A1": 0, "A2": 0, "A3": 0, "A4": 0, "A5": 0, "A6": 0} def correct_position(self, correction_type: str, axis: str, value: float) -> str: """ @@ -130,9 +146,7 @@ class MotionAPI: >>> api.motion.correct_position('AKorr', 'A1', 5.0) 'Updated AKorr.A1 to 5.0' """ - from .tools_api import ToolsAPI - tools = ToolsAPI(self.client) - return tools.update_variable(f"{correction_type}.{axis}", value) + return self._tools.update_variable(f"{correction_type}.{axis}", value) def move_external_axis(self, axis: str, value: float) -> str: """ @@ -156,9 +170,7 @@ class MotionAPI: >>> api.motion.move_external_axis('E1', 500.0) 'Updated ELPos.E1 to 500.0' """ - from .tools_api import ToolsAPI - tools = ToolsAPI(self.client) - return tools.update_variable(f"ELPos.{axis}", value) + return self._tools.update_variable(f"ELPos.{axis}", value) def adjust_speed(self, tech_param: str, value: float) -> str: """ @@ -183,9 +195,7 @@ class MotionAPI: Tech variable meanings depend on your KRL program implementation. Coordinate with your KRL developer on parameter assignments. """ - from .tools_api import ToolsAPI - tools = ToolsAPI(self.client) - return tools.update_variable(tech_param, value) + return self._tools.update_variable(tech_param, value) @staticmethod def generate_trajectory( @@ -245,10 +255,10 @@ class MotionAPI: rate: float = 0.012 ) -> None: """ - Execute a trajectory asynchronously. + Execute a trajectory, blocking until complete. Sends waypoints sequentially to the robot at the specified rate. - Uses asyncio for non-blocking execution. + Can be cancelled via cancel_trajectory(). Args: trajectory: List of waypoint dictionaries @@ -257,96 +267,98 @@ class MotionAPI: Raises: RSITrajectoryError: If space is invalid - - Example: - >>> # Generate and execute Cartesian trajectory - >>> traj = api.motion.generate_trajectory( - ... {"X":0, "Y":0, "Z":500}, - ... {"X":100, "Y":0, "Z":500}, - ... steps=50 - ... ) - >>> api.motion.execute_trajectory(traj, space="cartesian", rate=0.02) - - Note: - This method uses asyncio. If no event loop is running, one will - be created automatically. The trajectory executes in the background. """ + import time from .exceptions import RSITrajectoryError - async def runner(): + self._trajectory_cancel = threading.Event() + + def runner(): for idx, point in enumerate(trajectory): + if self._trajectory_cancel.is_set(): + logging.info("Trajectory cancelled at step %d/%d", idx, len(trajectory)) + return if space == "cartesian": self.update_cartesian(**point) elif space == "joint": self.update_joints(**point) else: raise RSITrajectoryError("space must be 'cartesian' or 'joint'") - logging.debug(f"Trajectory step {idx + 1}/{len(trajectory)}") - await asyncio.sleep(rate) + logging.debug("Trajectory step %d/%d", idx + 1, len(trajectory)) + time.sleep(rate) - try: - loop = asyncio.get_running_loop() - asyncio.create_task(runner()) - except RuntimeError: - # No event loop running, create one - asyncio.run(runner()) + self._trajectory_thread = threading.Thread(target=runner, daemon=True) + self._trajectory_thread.start() + self._trajectory_thread.join() # Block until complete + + # Zero corrections after trajectory to stop motion in relative mode + if space == "cartesian": + self.update_cartesian(**{k: 0.0 for k in trajectory[0]}) + elif space == "joint": + self.update_joints(**{k: 0.0 for k in trajectory[0]}) + + def cancel_trajectory(self) -> None: + """Cancel a running trajectory execution.""" + if hasattr(self, '_trajectory_cancel'): + self._trajectory_cancel.set() def move_cartesian_trajectory( self, - start_pose: Dict[str, float], end_pose: Dict[str, float], + start_pose: Optional[Dict[str, float]] = None, steps: int = 50, rate: float = 0.012 ) -> None: """ Generate and execute Cartesian trajectory in one call. - Convenience method that combines generate_trajectory() and - execute_trajectory() for Cartesian motion. - Args: - start_pose: Starting Cartesian pose - end_pose: Ending Cartesian pose + end_pose: Target Cartesian pose + start_pose: Starting pose (default: current robot position) steps: Number of waypoints (default: 50) rate: Time between waypoints in seconds (default: 0.012) Example: + >>> # Move to target from current position + >>> api.motion.move_cartesian_trajectory({"X":100, "Y":0, "Z":500}) + >>> # Explicit start >>> api.motion.move_cartesian_trajectory( - ... {"X":0, "Y":0, "Z":500}, ... {"X":100, "Y":0, "Z":500}, - ... steps=50, - ... rate=0.02 + ... start_pose={"X":0, "Y":0, "Z":500} ... ) """ + if start_pose is None: + start_pose = self.get_current_pose() trajectory = self.generate_trajectory(start_pose, end_pose, steps=steps, space="cartesian") self.execute_trajectory(trajectory, space="cartesian", rate=rate) def move_joint_trajectory( self, - start_joints: Dict[str, float], end_joints: Dict[str, float], + start_joints: Optional[Dict[str, float]] = None, steps: int = 50, rate: float = 0.4 ) -> None: """ Generate and execute joint-space trajectory in one call. - Convenience method for joint-space motion with sensible defaults - (slower rate typical for joint motion). - Args: - start_joints: Starting joint configuration - end_joints: Ending joint configuration + end_joints: Target joint configuration + start_joints: Starting joints (default: current robot joints) steps: Number of waypoints (default: 50) - rate: Time between waypoints in seconds (default: 0.4 for smooth joint motion) + rate: Time between waypoints in seconds (default: 0.4) Example: + >>> # Move to target from current joints + >>> api.motion.move_joint_trajectory({"A1":30, "A2":-15, "A3":45}) + >>> # Explicit start >>> api.motion.move_joint_trajectory( - ... {"A1":0, "A2":0, "A3":0, "A4":0, "A5":0, "A6":0}, - ... {"A1":30, "A2":-15, "A3":45, "A4":0, "A5":30, "A6":0}, - ... steps=100 + ... {"A1":30, "A2":-15, "A3":45}, + ... start_joints={"A1":0, "A2":0, "A3":0} ... ) """ + if start_joints is None: + start_joints = self.get_current_joints() trajectory = self.generate_trajectory(start_joints, end_joints, steps=steps, space="joint") self.execute_trajectory(trajectory, space="joint", rate=rate) @@ -380,7 +392,7 @@ class MotionAPI: "space": space, "rate": rate, }) - logging.debug(f"Queued trajectory: {len(trajectory)} points, {space} space") + logging.debug("Queued trajectory: %d points, %s space", len(trajectory), space) def queue_cartesian_trajectory( self, @@ -465,9 +477,9 @@ class MotionAPI: >>> api.motion.execute_queued_trajectories() >>> # Both trajectories executed sequentially """ - logging.info(f"Executing {len(self.trajectory_queue)} queued trajectories") + logging.info("Executing %d queued trajectories", len(self.trajectory_queue)) for idx, item in enumerate(self.trajectory_queue): - logging.debug(f"Executing queued trajectory {idx + 1}/{len(self.trajectory_queue)}") + logging.debug("Executing queued trajectory %d/%d", idx + 1, len(self.trajectory_queue)) self.execute_trajectory(item["trajectory"], item["space"], item["rate"]) self.clear_queue() @@ -481,7 +493,7 @@ class MotionAPI: """ count = len(self.trajectory_queue) self.trajectory_queue.clear() - logging.debug(f"Cleared {count} queued trajectories") + logging.debug("Cleared %d queued trajectories", count) def get_queue(self) -> List[Dict[str, Any]]: """ @@ -945,11 +957,11 @@ def _trapezoidal_profile( for i in range(n): if distance_traveled < total_distance / 2: # Acceleration phase - velocities[i] = min(peak_velocity, math.sqrt(2 * max_acceleration * distance_traveled)) + velocities[i] = min(peak_velocity, math.sqrt(max(0, 2 * max_acceleration * distance_traveled))) else: # Deceleration phase remaining = total_distance - distance_traveled - velocities[i] = min(peak_velocity, math.sqrt(2 * max_acceleration * remaining)) + velocities[i] = min(peak_velocity, math.sqrt(max(0, 2 * max_acceleration * remaining))) if i < len(distances): distance_traveled += distances[i] @@ -960,14 +972,14 @@ def _trapezoidal_profile( for i in range(n): if distance_traveled < accel_distance: # Acceleration phase - velocities[i] = math.sqrt(2 * max_acceleration * distance_traveled) + velocities[i] = math.sqrt(max(0, 2 * max_acceleration * distance_traveled)) elif distance_traveled < (total_distance - accel_distance): # Constant velocity phase velocities[i] = max_velocity else: # Deceleration phase remaining = total_distance - distance_traveled - velocities[i] = math.sqrt(2 * max_acceleration * remaining) + velocities[i] = math.sqrt(max(0, 2 * max_acceleration * remaining)) if i < len(distances): distance_traveled += distances[i] @@ -1001,10 +1013,10 @@ def _s_curve_profile( # Smooth acceleration at start, smooth deceleration at end if s < 0.5: # First half: smooth acceleration - v_normalized = 0.5 * (1 - math.cos(math.pi * s)) + v_normalized = 0.5 * (1 - math.cos(2 * math.pi * s)) else: # Second half: smooth deceleration - v_normalized = 0.5 * (1 + math.cos(math.pi * (s - 0.5))) + v_normalized = 0.5 * (1 - math.cos(2 * math.pi * (1 - s))) velocities[i] = v_normalized * max_velocity @@ -1043,7 +1055,7 @@ def _cubic_blend( keys = set(p1.keys()) | set(p2.keys()) for i in range(steps): - t = i / (steps - 1) + t = i / max(steps - 1, 1) # Cubic Hermite spline with zero velocity at endpoints h1 = 2 * t**3 - 3 * t**2 + 1 h2 = -2 * t**3 + 3 * t**2 diff --git a/src/RSIPI/network_handler.py b/src/RSIPI/network_handler.py index c6cb731..e46aec5 100644 --- a/src/RSIPI/network_handler.py +++ b/src/RSIPI/network_handler.py @@ -1,48 +1,33 @@ import multiprocessing import socket import logging +import threading import xml.etree.ElementTree as ET import os import datetime -from queue import Empty +from queue import Empty, Queue as ThreadQueue from typing import Dict, Any, Tuple, Optional -from .xml_handler import XMLGenerator +from .xml_handler import XMLGenerator, FastXMLGenerator from .safety_manager import SafetyManager from .exceptions import RSINetworkError, RSITimeoutError, RSIPacketError, RSILoggingError from .timing_metrics import TimingMetrics -class CSVLogger(multiprocessing.Process): +class CSVLogger(threading.Thread): """ - Separate process for writing CSV logs without blocking the network loop. + Background thread for writing CSV logs without blocking the network loop. - Runs in background and consumes log entries from a queue, writing them - to CSV file with British date format timestamps. + Uses a thread instead of a subprocess to avoid Windows restrictions on + daemon processes spawning child processes. """ - def __init__(self, log_queue: multiprocessing.Queue, stop_event: multiprocessing.Event, filename: str) -> None: - """ - Initialize CSV logger process. - - Args: - log_queue: Queue containing log entry dictionaries - stop_event: Event to signal shutdown - filename: Path to output CSV file - """ - super().__init__() - self.log_queue: multiprocessing.Queue = log_queue - self.stop_event: multiprocessing.Event = stop_event - self.filename: str = filename - self.daemon = True + def __init__(self, log_queue: ThreadQueue, stop_event: threading.Event, filename: str) -> None: + super().__init__(daemon=True) + self.log_queue = log_queue + self.stop_event = stop_event + self.filename = filename def run(self) -> None: - """ - Write log entries from queue to CSV file. - - Creates directory if needed, writes header on first entry, - timestamps each row with British date format (DD/MM/YYYY HH:MM:SS.mmm). - """ - # Ensure logs directory exists log_dir = os.path.dirname(self.filename) if log_dir and not os.path.exists(log_dir): os.makedirs(log_dir, exist_ok=True) @@ -54,16 +39,14 @@ class CSVLogger(multiprocessing.Process): while not self.stop_event.is_set(): try: entry = self.log_queue.get(timeout=0.5) - if entry is None: # Poison pill + if entry is None: break - # Write header on first entry if not header_written: headers = ['Timestamp'] + list(entry.keys()) f.write(','.join(headers) + '\n') header_written = True - # Write data row timestamp = datetime.datetime.now().strftime("%d/%m/%Y %H:%M:%S.%f")[:-3] values = [timestamp] + [str(v) for v in entry.values()] f.write(','.join(values) + '\n') @@ -72,10 +55,10 @@ class CSVLogger(multiprocessing.Process): except Empty: continue except Exception as e: - logging.error(f"CSV logging error: {e}") + logging.error("CSV logging error: %s", e) except Exception as e: - logging.error(f"Failed to open log file {self.filename}: {e}") + logging.error("Failed to open log file %s: %s", self.filename, e) class NetworkProcess(multiprocessing.Process): @@ -97,22 +80,13 @@ class NetworkProcess(multiprocessing.Process): config_parser: Any, # ConfigParser type start_event: multiprocessing.Event, command_queue: multiprocessing.Queue, - metrics_dict: Optional[Any] = None # multiprocessing.Manager().dict() + metrics_dict: Optional[Any] = None, # multiprocessing.Manager().dict() + connected_event: Optional[multiprocessing.Event] = None, + rsi_mode: str = 'relative', + max_cartesian_rate: float = 0.0, + max_joint_rate: float = 0.0, + cycle_time: float = 0.004 ) -> None: - """ - Initialize network process. - - Args: - ip: IP address to bind UDP socket to - port: UDP port number - send_variables: Shared dict for variables to send to robot - receive_variables: Shared dict for variables received from robot - stop_event: Event to signal shutdown - config_parser: ConfigParser instance with network settings - start_event: Event to signal when to start communication - command_queue: Queue for receiving commands from parent process - metrics_dict: Optional shared dict for timing metrics - """ super().__init__() self.send_variables = send_variables self.receive_variables = receive_variables @@ -121,16 +95,24 @@ class NetworkProcess(multiprocessing.Process): self.config_parser = config_parser self.command_queue: multiprocessing.Queue = command_queue self.safety_manager: SafetyManager = SafetyManager(config_parser.safety_limits) + self.connected_event = connected_event + + # RSI correction mode and rate limiting + self.rsi_mode: str = rsi_mode # 'absolute' or 'relative' + self.max_cartesian_rate: float = max_cartesian_rate # mm/cycle, 0 = disabled + self.max_joint_rate: float = max_joint_rate # degrees/cycle, 0 = disabled + self.cycle_time: float = cycle_time # expected cycle time for metrics self.client_address: Tuple[str, int] = (ip, port) self.logging_active: Any = multiprocessing.Value('b', False) # c_bool wrapper + self.estop_active: Any = multiprocessing.Value('b', False) self.controller_ip_and_port: Optional[Tuple[str, int]] = None self.udp_socket: Optional[socket.socket] = None # Logging infrastructure (created when logging starts) - self.log_queue: Optional[multiprocessing.Queue] = None - self.log_stop_event: Optional[multiprocessing.Event] = None + self.log_queue: Optional[ThreadQueue] = None + self.log_stop_event: Optional[threading.Event] = None self.csv_logger: Optional[CSVLogger] = None # Timing metrics (Phase 2) @@ -146,8 +128,8 @@ class NetworkProcess(multiprocessing.Process): """ # Initialize timing metrics in child process if self.metrics_dict is not None: - self.timing_metrics = TimingMetrics() - logging.info("Timing metrics initialized") + self.timing_metrics = TimingMetrics(expected_cycle_time=self.cycle_time) + logging.info("Timing metrics initialized (expected cycle: %.1fms)", self.cycle_time * 1000) # Wait for start signal, but check stop_event periodically to allow clean shutdown while not self.start_event.wait(timeout=0.5): @@ -168,49 +150,74 @@ class NetworkProcess(multiprocessing.Process): Falls back to 0.0.0.0 if specified IP is invalid. """ if not self.is_valid_ip(self.client_address[0]): - logging.warning(f"Invalid IP address '{self.client_address[0]}'. Falling back to '0.0.0.0'.") + logging.warning("Invalid IP address '%s'. Falling back to '0.0.0.0'.", self.client_address[0]) self.client_address = ('0.0.0.0', self.client_address[1]) self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.udp_socket.settimeout(5) self.udp_socket.bind(self.client_address) - logging.info(f"Network process bound on {self.client_address}") + logging.info("Network process bound on %s", self.client_address) def _run_loop(self) -> None: """ Main communication loop. - Receives UDP messages from robot, processes them, sends responses, - and optionally logs data to CSV. Records timing metrics if enabled. - Uses local dict snapshots to avoid per-key IPC overhead on multiprocessing.Manager dicts within the 4ms cycle. """ - update_counter = 0 # For periodic metrics updates - metrics_counter = 0 # For periodic receive_variables sync + update_counter = 0 + metrics_counter = 0 + cmd_counter = 0 + first_packet = True # Variable naming follows KUKA convention (robot's perspective): # send_variables = what the robot SENDS to us (RIst, RSol, IPOC, etc.) # receive_variables = what the robot RECEIVES from us (RKorr, DiO, EStr, etc.) - # - # So: parse incoming XML → send_variables, build response XML ← receive_variables # Local working copies — avoid Manager IPC in the hot path - local_robot_out = dict(self.send_variables) # robot's outgoing data (we read) - local_robot_in = dict(self.receive_variables) # robot's incoming data (we write) + local_robot_out = dict(self.send_variables) + local_robot_in = dict(self.receive_variables) network_settings = self.config_parser.network_settings + # FastXMLGenerator for comparison testing + fast_gen = FastXMLGenerator(local_robot_in, root_tag="Sen", type_attr=network_settings["sentype"]) + xml_mismatch_logged = False + + # Cache zero-correction template for E-stop + zero_robot_in = dict(self.receive_variables) + for key, value in zero_robot_in.items(): + if isinstance(value, dict): + zero_robot_in[key] = {k: 0.0 for k in value} + + # Previous correction state for absolute mode ramping + prev_corrections: Dict[str, Dict[str, float]] = {} + for key in ('RKorr', 'AKorr'): + if key in local_robot_in and isinstance(local_robot_in[key], dict): + prev_corrections[key] = {k: 0.0 for k in local_robot_in[key]} + while not self.stop_event.is_set(): - # Check for commands (non-blocking) - self._process_commands() + # Check for commands periodically (every 50 cycles ~200ms) + cmd_counter += 1 + if cmd_counter >= 50: + self._process_commands() + cmd_counter = 0 try: - self.udp_socket.settimeout(5) data_received, self.controller_ip_and_port = self.udp_socket.recvfrom(1024) message = data_received.decode() - # Parse robot's outgoing data into local dict (no IPC) - self._parse_received_data(message, local_robot_out) + # Parse robot's outgoing data (ElementTree — handles any attribute order) + try: + self._parse_received_data(message, local_robot_out) + except RSIPacketError: + logging.warning("Parse failed, sending last known good response") + + # Signal connection on first valid packet + if first_packet: + first_packet = False + if self.connected_event: + self.connected_event.set() # Snapshot receive_variables to pick up user changes (single IPC call) local_robot_in = dict(self.receive_variables) @@ -219,10 +226,35 @@ class NetworkProcess(multiprocessing.Process): if "IPOC" in local_robot_out: ipoc = local_robot_out["IPOC"] local_robot_in["IPOC"] = ipoc + 4 - self.receive_variables["IPOC"] = ipoc + 4 - # Build and send response XML from our corrections (no IPC) - send_xml = XMLGenerator.generate_send_xml(local_robot_in, network_settings) + # Rate-limit corrections + self._apply_rate_limit(local_robot_in, prev_corrections) + + # E-stop: zero all corrections, keep IPOC sync + if self.estop_active.value: + estop_response = dict(zero_robot_in) + estop_response["IPOC"] = local_robot_in.get("IPOC", 0) + send_xml = XMLGenerator.generate_send_xml(estop_response, network_settings) + else: + send_xml = XMLGenerator.generate_send_xml(local_robot_in, network_settings) + + # Compare FastXMLGenerator output (debug — log first mismatch only) + if not xml_mismatch_logged: + try: + fast_xml = fast_gen.generate(local_robot_in) + if fast_xml != send_xml: + xml_mismatch_logged = True + logging.warning("XML MISMATCH DETECTED") + logging.warning("ET output: %s", send_xml[:200]) + logging.warning("Fast output: %s", fast_xml[:200]) + elif metrics_counter == 0: + # Log match confirmation once (on first sync cycle) + logging.info("XML generators match OK") + xml_mismatch_logged = True + except Exception as e: + logging.warning("FastXMLGenerator error: %s", e) + xml_mismatch_logged = True + self.udp_socket.sendto(send_xml.encode(), self.controller_ip_and_port) # Sync robot's outgoing data → Manager dict periodically (every 10 cycles ~40ms) @@ -235,22 +267,20 @@ class NetworkProcess(multiprocessing.Process): if self.timing_metrics is not None: self.timing_metrics.record_cycle(local_robot_out.get("IPOC", 0)) - # Update shared metrics dict every 100 cycles (~400ms) update_counter += 1 if update_counter >= 100: self._update_metrics_dict() update_counter = 0 if self.logging_active.value and self.log_queue: - self._queue_log_entry() + self._queue_log_entry(local_robot_out, local_robot_in) except socket.timeout: logging.warning("No message received within timeout period") - # Check watchdog on timeout if self.timing_metrics and self.timing_metrics.check_watchdog(): logging.error("Watchdog timeout - communication lost!") except Exception as e: - logging.error(f"Network process error: {e}") + logging.error("Network process error: %s", e) def _process_commands(self) -> None: """Process any pending commands from the parent process.""" @@ -265,14 +295,64 @@ class NetworkProcess(multiprocessing.Process): self._start_logging(cmd.get('filename')) elif action == 'stop_logging': self._stop_logging() + elif action == 'estop': + self.estop_active.value = True + elif action == 'estop_reset': + self.estop_active.value = False except Empty: pass except Exception as e: - logging.error(f"Error processing command: {e}") + logging.error("Error processing command: %s", e) + + def _apply_rate_limit(self, robot_in: dict, prev: Dict[str, Dict[str, float]]) -> None: + """ + Apply per-cycle rate limiting to correction values in-place. + + In relative mode: clamp each value directly (it IS the per-cycle delta). + In absolute mode: clamp the change from previous cycle and ramp toward target. + + Args: + robot_in: Current outgoing corrections dict (modified in-place) + prev: Previous cycle's correction values (updated in-place) + """ + cartesian_keys = {'X', 'Y', 'Z', 'A', 'B', 'C'} + joint_keys = {'A1', 'A2', 'A3', 'A4', 'A5', 'A6'} + + for corr_key, max_rate, axis_set in [ + ('RKorr', self.max_cartesian_rate, cartesian_keys), + ('AKorr', self.max_joint_rate, joint_keys), + ]: + if max_rate <= 0: + continue # Rate limiting disabled for this type + if corr_key not in robot_in or not isinstance(robot_in[corr_key], dict): + continue + + corr = robot_in[corr_key] + prev_corr = prev.get(corr_key, {}) + + if self.rsi_mode == 'relative': + # Each value is a per-cycle delta — clamp directly + for axis in corr: + if axis in axis_set: + val = corr[axis] + corr[axis] = max(-max_rate, min(max_rate, val)) + else: + # Absolute mode — clamp the change from previous + for axis in corr: + if axis in axis_set: + target = corr[axis] + previous = prev_corr.get(axis, 0.0) + delta = target - previous + clamped_delta = max(-max_rate, min(max_rate, delta)) + corr[axis] = previous + clamped_delta + prev_corr[axis] = corr[axis] + + robot_in[corr_key] = corr + prev[corr_key] = prev_corr def _update_metrics_dict(self) -> None: - """Update shared metrics dictionary with current timing statistics (Phase 2).""" + """Update shared metrics dictionary with current timing statistics.""" if self.metrics_dict is None or self.timing_metrics is None: return @@ -280,7 +360,6 @@ class NetworkProcess(multiprocessing.Process): stats = self.timing_metrics.get_current_stats() health = self.timing_metrics.get_health_status() - # Update shared dict (Manager dict supports item assignment) for key, value in stats.items(): self.metrics_dict[key] = value @@ -289,56 +368,48 @@ class NetworkProcess(multiprocessing.Process): self.metrics_dict['watchdog_timeout'] = health['watchdog_timeout'] except Exception as e: - logging.debug(f"Failed to update metrics dict: {e}") + logging.debug("Failed to update metrics dict: %s", e) - def _queue_log_entry(self) -> None: - """Queue current state for CSV logging (non-blocking).""" + def _queue_log_entry(self, local_robot_out: dict, local_robot_in: dict) -> None: + """Queue current state for CSV logging using local dicts (no IPC).""" try: entry = {} - # Flatten send variables - for key, value in dict(self.send_variables).items(): + for key, value in local_robot_out.items(): if isinstance(value, dict): for subkey, subval in value.items(): entry[f"Send.{key}.{subkey}"] = subval else: entry[f"Send.{key}"] = value - # Flatten receive variables - for key, value in dict(self.receive_variables).items(): + for key, value in local_robot_in.items(): if isinstance(value, dict): for subkey, subval in value.items(): entry[f"Receive.{key}.{subkey}"] = subval else: entry[f"Receive.{key}"] = value - # Non-blocking put - drop entry if queue is full try: self.log_queue.put_nowait(entry) except: pass # Queue full, skip this entry rather than block except Exception as e: - logging.debug(f"Failed to queue log entry: {e}") + logging.debug("Failed to queue log entry: %s", e) def _start_logging(self, filename: str) -> None: - """ - Start CSV logging to the specified file. - - Args: - filename: Path to CSV output file - """ + """Start CSV logging to the specified file.""" if self.logging_active.value: logging.warning("Logging already active") return - self.log_queue = multiprocessing.Queue(maxsize=1000) - self.log_stop_event = multiprocessing.Event() + self.log_queue = ThreadQueue(maxsize=1000) + self.log_stop_event = threading.Event() self.csv_logger = CSVLogger(self.log_queue, self.log_stop_event, filename) self.csv_logger.start() self.logging_active.value = True - logging.info(f"CSV logging started: {filename}") + logging.info("CSV logging started: %s", filename) def _stop_logging(self) -> None: """Stop CSV logging and cleanup resources.""" @@ -358,8 +429,6 @@ class NetworkProcess(multiprocessing.Process): if self.csv_logger and self.csv_logger.is_alive(): self.csv_logger.join(timeout=2) - if self.csv_logger.is_alive(): - self.csv_logger.terminate() self.csv_logger = None self.log_queue = None @@ -368,7 +437,6 @@ class NetworkProcess(multiprocessing.Process): def _cleanup(self) -> None: """Clean up resources on shutdown.""" - # Stop logging first self._stop_logging() if self.udp_socket: @@ -376,20 +444,11 @@ class NetworkProcess(multiprocessing.Process): self.udp_socket.close() logging.info("Network socket closed") except Exception as e: - logging.error(f"Error closing socket: {e}") + logging.error("Error closing socket: %s", e) self.udp_socket = None @staticmethod def is_valid_ip(ip: str) -> bool: - """ - Check if an IP address is valid and bindable. - - Args: - ip: IP address string to validate - - Returns: - True if IP is valid and can be bound to - """ try: socket.inet_aton(ip) with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: @@ -400,43 +459,31 @@ class NetworkProcess(multiprocessing.Process): @staticmethod def _parse_received_data(xml_string: str, target: dict) -> None: - """ - Parse received XML message into a local dict (no IPC). - - Args: - xml_string: XML message string from robot controller - target: Plain dict to update with parsed values - - Raises: - RSIPacketError: If XML parsing fails - """ + """Parse received XML message into a local dict (no IPC).""" try: root = ET.fromstring(xml_string) for element in root: if element.tag in target: if len(element.attrib) > 0: - target[element.tag] = {k: float(v) for k, v in element.attrib.items()} + existing = target.get(element.tag) + if isinstance(existing, dict): + for k, v in element.attrib.items(): + existing[k] = float(v) + else: + target[element.tag] = {k: float(v) for k, v in element.attrib.items()} else: target[element.tag] = element.text if element.tag == "IPOC": target["IPOC"] = int(element.text) except ET.ParseError as e: - logging.error(f"XML parse error in received message: {e}") + logging.error("XML parse error in received message: %s", e) raise RSIPacketError(f"Failed to parse received XML: {e}") from e except Exception as e: - logging.error(f"Error processing received message: {e}") + logging.error("Error processing received message: %s", e) raise RSIPacketError(f"Unexpected error parsing packet: {e}") from e def process_received_data(self, xml_string: str) -> None: - """ - Parse received XML message and update send_variables (Manager dict). - - Legacy method kept for compatibility (e.g. echo server). The hot loop - uses _parse_received_data() with local dicts instead. - - Args: - xml_string: XML message string from robot controller - """ + """Legacy method kept for compatibility (e.g. echo server).""" self._parse_received_data(xml_string, self.send_variables) if "IPOC" in self.send_variables: self.receive_variables["IPOC"] = self.send_variables["IPOC"] + 4 diff --git a/src/RSIPI/rsi_api.py b/src/RSIPI/rsi_api.py index a6abb42..67ed9a5 100644 --- a/src/RSIPI/rsi_api.py +++ b/src/RSIPI/rsi_api.py @@ -1,242 +1,148 @@ -""" -RSIPI - Robot Sensor Interface Python Integration - -Main API orchestrator providing namespaced access to all RSI functionality. -""" - -import logging -from threading import Thread -from typing import Optional, TYPE_CHECKING - -from .motion_api import MotionAPI -from .io_api import IOAPI -from .krl_api import KRLAPI -from .safety_api import SafetyAPI -from .monitoring_api import MonitoringAPI -from .logging_api import LoggingAPI -from .diagnostics_api import DiagnosticsAPI -from .viz_api import VizAPI -from .tools_api import ToolsAPI - -if TYPE_CHECKING: - from .rsi_client import RSIClient, ClientState - - -class RSIAPI: - """ - High-level API orchestrator for KUKA RSI robot control. - - Provides namespaced access to all RSIPI functionality through specialized - sub-APIs. This is the main entry point for most users. - - Namespaces: - - motion: Motion control (Cartesian, joints, trajectories) - - io: Digital I/O control - - krl: KRL program manipulation utilities - - safety: Safety management and limits - - monitoring: Live data access and monitoring - - logging: CSV data logging - - diagnostics: Network and performance diagnostics (Phase 2) - - viz: Static and live visualization - - tools: Utilities, debugging, and inspection - - Core Methods (direct access): - - start(): Start RSI communication - - stop(): Stop RSI communication - - reconnect(): Restart network connection - - state: Current client state (property) - - Example: - >>> api = RSIAPI('RSI_EthernetConfig.xml') - >>> api.start() - >>> api.motion.update_cartesian(X=10, Y=5, Z=0) - >>> api.logging.start('test.csv') - >>> # ... robot operation ... - >>> api.logging.stop() - >>> api.stop() - >>> api.viz.plot_static('test.csv', '3d') - """ - - def __init__(self, config_file: str = "RSI_EthernetConfig.xml") -> None: - """ - Initialize RSIAPI with configuration file. - - Creates RSIClient instance (lazy initialization) and sets up all - namespace APIs for organized access to functionality. - - Args: - config_file: Path to RSI_EthernetConfig.xml configuration file - - Example: - >>> api = RSIAPI('config/RSI_EthernetConfig.xml') - >>> print(api.state) - ClientState.INITIALIZED - """ - self.config_file: str = config_file - self.client: Optional['RSIClient'] = None - self._thread: Optional[Thread] = None - - # Initialize client - self._ensure_client() - - # Initialize namespace APIs - self.motion = MotionAPI(self.client) - self.io = IOAPI(self.client) - self.krl = KRLAPI(self.client) - self.safety = SafetyAPI(self.client) - self.monitoring = MonitoringAPI(self.client) - self.logging = LoggingAPI(self.client) - self.diagnostics = DiagnosticsAPI(self.client) - self.viz = VizAPI(self.client) - self.tools = ToolsAPI(self.client) - - logging.info("RSIAPI initialized with namespaced structure") - - def _ensure_client(self) -> None: - """ - Ensure RSIClient is initialized (lazy initialization). - - Imports and creates RSIClient only when needed, avoiding circular - dependencies and improving startup time. - """ - if self.client is None: - from .rsi_client import RSIClient - self.client = RSIClient(self.config_file) - logging.debug("RSIClient initialized") - - @property - def state(self) -> 'ClientState': - """ - Get current client state. - - Returns: - ClientState enum value (INITIALIZED, STARTING, RUNNING, STOPPING, STOPPED, ERROR) - - Example: - >>> api = RSIAPI() - >>> print(api.state) - ClientState.INITIALIZED - >>> api.start() - >>> print(api.state) - ClientState.RUNNING - """ - return self.client.state - - def start(self) -> str: - """ - Start RSI communication in background thread. - - Creates a daemon thread that runs the RSI client's main communication - loop. The thread handles UDP message exchange with the robot controller. - - Returns: - Status message - - Raises: - RSIClientNotReady: If client is not in appropriate state to start - - Example: - >>> api = RSIAPI() - >>> api.start() - 'RSI started in background' - >>> api.state - ClientState.RUNNING - - Note: - The background thread runs as a daemon, so it will automatically - terminate when the main program exits. - """ - self._thread = Thread(target=self.client.start, daemon=True) - self._thread.start() - logging.info("RSI communication started in background thread") - return "RSI started in background" - - def stop(self) -> str: - """ - Stop RSI communication. - - Gracefully shuts down the network process, closes sockets, and - stops any active logging. Waits for background threads to complete. - - Returns: - Status message - - Example: - >>> api.stop() - 'RSI stopped' - - Note: - This method blocks until the network process has fully shut down, - which typically takes 1-3 seconds. - """ - self.client.stop() - logging.info("RSI communication stopped") - return "RSI stopped" - - def reconnect(self) -> str: - """ - Restart network connection without stopping RSI client. - - Terminates the current network process, creates a fresh one with new - communication resources, and restarts the connection. Useful for - recovering from network errors. - - Returns: - Status message - - Example: - >>> # Network issue detected - >>> api.reconnect() - 'Network connection restarted' - - Note: - This creates fresh multiprocessing Events and Queues. Any queued - but unprocessed data in the old network process will be lost. - """ - self.client.reconnect() - logging.info("Network connection restarted") - return "Network connection restarted" - - def is_running(self) -> bool: - """ - Check if RSI client is in RUNNING state. - - Returns: - True if actively communicating with robot - - Example: - >>> api = RSIAPI() - >>> api.is_running() - False - >>> api.start() - >>> api.is_running() - True - """ - return self.client.is_running() - - def is_stopped(self) -> bool: - """ - Check if RSI client is fully stopped. - - Returns: - True if in STOPPED state - - Example: - >>> api.stop() - >>> api.is_stopped() - True - """ - return self.client.is_stopped() - - # Deprecated methods for backward compatibility (Phase 5.1 - to be removed) - # These are kept temporarily to ease migration. Use namespaced methods instead. - - def start_rsi(self) -> str: - """DEPRECATED: Use api.start() instead.""" - logging.warning("start_rsi() is deprecated. Use api.start() instead.") - return self.start() - - def stop_rsi(self) -> str: - """DEPRECATED: Use api.stop() instead.""" - logging.warning("stop_rsi() is deprecated. Use api.stop() instead.") - return self.stop() +""" +RSIPI - Robot Sensor Interface Python Integration + +Main API orchestrator providing namespaced access to all RSI functionality. +""" + +import logging +from threading import Thread +from typing import Optional, TYPE_CHECKING + +from .motion_api import MotionAPI +from .io_api import IOAPI +from .krl_api import KRLAPI +from .safety_api import SafetyAPI +from .monitoring_api import MonitoringAPI +from .logging_api import LoggingAPI +from .diagnostics_api import DiagnosticsAPI +from .viz_api import VizAPI +from .tools_api import ToolsAPI + +if TYPE_CHECKING: + from .rsi_client import RSIClient, ClientState + + +class RSIAPI: + """ + High-level API orchestrator for KUKA RSI robot control. + + Supports context manager usage for safe cleanup: + >>> with RSIAPI('RSI_EthernetConfig.xml') as api: + ... api.start() + ... api.motion.update_cartesian(X=10) + """ + + def __init__( + self, + config_file: str = "RSI_EthernetConfig.xml", + rsi_mode: str = 'relative', + max_cartesian_rate: float = 0.0, + max_joint_rate: float = 0.0, + cycle_time: float = 0.004 + ) -> None: + """ + Args: + config_file: Path to RSI_EthernetConfig.xml + rsi_mode: 'absolute' or 'relative' — must match KRL RSI_MOVECORR() mode + max_cartesian_rate: Max mm/cycle for RKorr corrections (0 = no limit) + max_joint_rate: Max degrees/cycle for AKorr corrections (0 = no limit) + cycle_time: Expected RSI cycle time in seconds (0.004 = 4ms/250Hz, 0.012 = 12ms/83Hz) + """ + self.config_file: str = config_file + self.rsi_mode: str = rsi_mode + self.max_cartesian_rate: float = max_cartesian_rate + self.max_joint_rate: float = max_joint_rate + self.cycle_time: float = cycle_time + self.client: Optional['RSIClient'] = None + self._thread: Optional[Thread] = None + + self._ensure_client() + + self.motion = MotionAPI(self.client) + self.io = IOAPI(self.client) + self.krl = KRLAPI(self.client) + self.safety = SafetyAPI(self.client) + self.monitoring = MonitoringAPI(self.client) + self.logging = LoggingAPI(self.client) + self.diagnostics = DiagnosticsAPI(self.client) + self.viz = VizAPI(self.client) + self.tools = ToolsAPI(self.client) + + logging.info("RSIAPI initialized with namespaced structure") + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + try: + self.stop() + except Exception: + pass + return False + + def _ensure_client(self) -> None: + if self.client is None: + from .rsi_client import RSIClient + self.client = RSIClient( + self.config_file, + rsi_mode=self.rsi_mode, + max_cartesian_rate=self.max_cartesian_rate, + max_joint_rate=self.max_joint_rate, + cycle_time=self.cycle_time + ) + + @property + def state(self) -> 'ClientState': + return self.client.state + + def start(self) -> str: + """Start RSI communication in background thread.""" + self._thread = Thread(target=self.client.start, daemon=True) + self._thread.start() + logging.info("RSI communication started in background thread") + return "RSI started in background" + + def stop(self) -> str: + """Stop RSI communication gracefully.""" + self.client.stop() + if self._thread and self._thread.is_alive(): + self._thread.join(timeout=3) + self._thread = None + logging.info("RSI communication stopped") + return "RSI stopped" + + def wait_for_connection(self, timeout: float = 10.0) -> bool: + """ + Block until the robot's first packet is received. + + Args: + timeout: Maximum time to wait in seconds + + Returns: + True if connected, False if timeout + """ + return self.client.wait_for_connection(timeout) + + def reconnect(self) -> str: + """Restart network connection with fresh resources.""" + self.client.reconnect() + # Start client in new thread + self._thread = Thread(target=self.client.start, daemon=True) + self._thread.start() + logging.info("Network connection restarted") + return "Network connection restarted" + + def is_running(self) -> bool: + return self.client.is_running() + + def is_stopped(self) -> bool: + return self.client.is_stopped() + + # Deprecated methods + def start_rsi(self) -> str: + logging.warning("start_rsi() is deprecated. Use api.start() instead.") + return self.start() + + def stop_rsi(self) -> str: + logging.warning("stop_rsi() is deprecated. Use api.stop() instead.") + return self.stop() diff --git a/src/RSIPI/rsi_client.py b/src/RSIPI/rsi_client.py index d9f6623..78eda5c 100644 --- a/src/RSIPI/rsi_client.py +++ b/src/RSIPI/rsi_client.py @@ -1,299 +1,336 @@ -import logging -import multiprocessing -import time -from enum import Enum, auto -from threading import Lock, Thread -from typing import Optional -from .config_parser import ConfigParser -from .network_handler import NetworkProcess -from .safety_manager import SafetyManager -from .exceptions import RSIStateError, RSIInvalidTransition, RSIClientNotReady -from .auto_reconnect import AutoReconnectManager, ReconnectStrategy - - -class ClientState(Enum): - """Connection states for RSIClient.""" - INITIALIZED = auto() # After __init__, network process spawned but not started - STARTING = auto() # Start signal sent, waiting for network to be ready - RUNNING = auto() # Actively communicating with robot - STOPPING = auto() # Shutdown in progress - STOPPED = auto() # Fully stopped, cannot be restarted (use reconnect) - ERROR = auto() # Error state - - -class RSIClient: - """Main RSI API class that integrates network, config handling, and message processing.""" - - # Valid state transitions - _VALID_TRANSITIONS = { - ClientState.INITIALIZED: {ClientState.STARTING, ClientState.STOPPING}, - ClientState.STARTING: {ClientState.RUNNING, ClientState.STOPPING, ClientState.ERROR}, - ClientState.RUNNING: {ClientState.STOPPING, ClientState.ERROR}, - ClientState.STOPPING: {ClientState.STOPPED, ClientState.ERROR}, - ClientState.STOPPED: {ClientState.INITIALIZED}, # Via reconnect - ClientState.ERROR: {ClientState.STOPPING, ClientState.INITIALIZED}, # Via reconnect - } - - def __init__( - self, - config_file: str, - rsi_limits_file: Optional[str] = None, - enable_auto_reconnect: bool = False, - auto_reconnect_retries: int = 5, - auto_reconnect_delay: float = 5.0 - ) -> None: - """ - Initialize RSI client with configuration and safety limits. - - Args: - config_file: Path to RSI_EthernetConfig.xml - rsi_limits_file: Optional path to .rsi.xml safety limits file - enable_auto_reconnect: Enable automatic reconnection on communication loss - auto_reconnect_retries: Maximum reconnection attempts (0 = unlimited) - auto_reconnect_delay: Base delay between retries in seconds - """ - logging.info(f"Loading RSI configuration from {config_file}...") - - self._state: ClientState = ClientState.INITIALIZED - self._state_lock: Lock = Lock() - - self.config_parser: ConfigParser = ConfigParser(config_file, rsi_limits_file) - network_settings = self.config_parser.get_network_settings() - - self.manager: multiprocessing.Manager = multiprocessing.Manager() - self.send_variables = self.manager.dict(self.config_parser.send_variables) - self.receive_variables = self.manager.dict(self.config_parser.receive_variables) - self.stop_event: multiprocessing.Event = multiprocessing.Event() - self.start_event: multiprocessing.Event = multiprocessing.Event() - self.command_queue: multiprocessing.Queue = multiprocessing.Queue() - - self.safety_manager: SafetyManager = SafetyManager(self.config_parser.safety_limits) - - # Shared logging state (readable from parent process) - self._logging_active = multiprocessing.Value('b', False) - - # Shared metrics dictionary (Phase 2) - self.metrics_dict = self.manager.dict() - - # Create NetworkProcess but don't start communication yet - self.network_process: NetworkProcess = NetworkProcess( - network_settings["ip"], - network_settings["port"], - self.send_variables, - self.receive_variables, - self.stop_event, - self.config_parser, - self.start_event, - self.command_queue, - self.metrics_dict - ) - # Share the logging_active flag - self.network_process.logging_active = self._logging_active - self.network_process.start() - - self.logger: Optional[any] = None # Reserved for future use - self.running: bool = False - self.thread: Optional[Thread] = None - - # Auto-reconnect manager (Phase 2) - self.auto_reconnect_manager: Optional[AutoReconnectManager] = None - if enable_auto_reconnect: - self.auto_reconnect_manager = AutoReconnectManager( - client=self, - enabled=True, - max_retries=auto_reconnect_retries, - retry_delay=auto_reconnect_delay, - strategy=ReconnectStrategy.LINEAR_BACKOFF - ) - logging.info("Auto-reconnect enabled") - - @property - def state(self) -> ClientState: - """Get current client state (thread-safe).""" - with self._state_lock: - return self._state - - def _transition_to(self, new_state: ClientState) -> bool: - """ - Attempt to transition to a new state. - - Args: - new_state: Target state to transition to - - Returns: - True if transition was valid and completed, False otherwise - """ - with self._state_lock: - if new_state in self._VALID_TRANSITIONS.get(self._state, set()): - old_state = self._state - self._state = new_state - logging.debug(f"State transition: {old_state.name} -> {new_state.name}") - return True - else: - logging.warning( - f"Invalid state transition attempted: {self._state.name} -> {new_state.name}" - ) - return False - - def start(self) -> None: - """ - Send start signal to NetworkProcess and run control loop. - - Transitions through STARTING → RUNNING states and maintains - control loop until stopped. - - Raises: - RSIClientNotReady: If client is not in appropriate state to start - """ - if not self._transition_to(ClientState.STARTING): - error_msg = f"Cannot start from state {self.state.name}" - logging.error(error_msg) - raise RSIClientNotReady(error_msg) - - logging.info("RSIClient sending start signal to NetworkProcess...") - self.start_event.set() - - if not self._transition_to(ClientState.RUNNING): - error_msg = "Failed to transition to RUNNING state" - logging.error(error_msg) - raise RSIStateError(error_msg) - - self.running = True - logging.info("RSI Client Started") - - # Start auto-reconnect monitor (Phase 2) - if self.auto_reconnect_manager: - self.auto_reconnect_manager.start() - - try: - while self.running and not self.stop_event.is_set(): - time.sleep(2) - except KeyboardInterrupt: - self.stop() - except Exception as e: - logging.error(f"RSI Client encountered an error: {e}") - self._transition_to(ClientState.ERROR) - raise - - def stop(self) -> None: - """Stop the network process and the client thread safely.""" - if self.state in (ClientState.STOPPED, ClientState.STOPPING): - logging.debug("Already stopped or stopping") - return - - if not self._transition_to(ClientState.STOPPING): - logging.warning("Could not transition to STOPPING state") - # Continue anyway to ensure cleanup - - logging.info("Stopping RSI Client...") - - self.running = False - self.stop_event.set() - - if self.network_process and self.network_process.is_alive(): - self.network_process.join(timeout=3) - if self.network_process.is_alive(): - logging.warning("Forcing network process termination...") - self.network_process.terminate() - self.network_process.join() - - if self.thread and self.thread.is_alive(): - self.thread.join(timeout=2) - self.thread = None - - # Stop auto-reconnect monitor (Phase 2) - if self.auto_reconnect_manager: - self.auto_reconnect_manager.stop() - - self._transition_to(ClientState.STOPPED) - logging.info("RSI Client Stopped") - - def reconnect(self) -> None: - """ - Reconnect the network process safely. - - Stops existing connection, resets state, and creates fresh - network process with new communication resources. - """ - logging.info("Reconnecting RSI Client network...") - - # Stop if currently running - if self.state in (ClientState.RUNNING, ClientState.STARTING): - self.stop() - - if self.network_process and self.network_process.is_alive(): - self.stop_event.set() - self.network_process.terminate() - self.network_process.join() - - # Reset to initialized state - with self._state_lock: - self._state = ClientState.INITIALIZED - - # Fresh new events and queue - self.stop_event = multiprocessing.Event() - self.start_event = multiprocessing.Event() - self.command_queue = multiprocessing.Queue() - - # Reset metrics dictionary (Phase 2) - self.metrics_dict.clear() - - # Create new network process - network_settings = self.config_parser.get_network_settings() - self.network_process = NetworkProcess( - network_settings["ip"], - network_settings["port"], - self.send_variables, - self.receive_variables, - self.stop_event, - self.config_parser, - self.start_event, - self.command_queue, - self.metrics_dict - ) - self.network_process.logging_active = self._logging_active - self.network_process.start() - - # Fresh control thread - self.thread = Thread(target=self.start, daemon=True) - self.thread.start() - - def is_running(self) -> bool: - """ - Check if client is in running state. - - Returns: - True if currently running - """ - return self.state == ClientState.RUNNING - - def is_stopped(self) -> bool: - """ - Check if client is fully stopped. - - Returns: - True if in STOPPED state - """ - return self.state == ClientState.STOPPED - - def start_logging(self, filename: str) -> None: - """ - Start CSV logging to the specified file. - - Args: - filename: Path to output CSV file - """ - self.command_queue.put({'action': 'start_logging', 'filename': filename}) - - def stop_logging(self) -> None: - """Stop CSV logging.""" - self.command_queue.put({'action': 'stop_logging'}) - - def is_logging_active(self) -> bool: - """ - Check if CSV logging is currently active. - - Returns: - True if logging is active - """ - return self._logging_active.value +import logging +import multiprocessing +import time +from enum import Enum, auto +from threading import Lock, Thread +from typing import Optional +from .config_parser import ConfigParser +from .network_handler import NetworkProcess +from .safety_manager import SafetyManager +from .exceptions import RSIStateError, RSIInvalidTransition, RSIClientNotReady +from .auto_reconnect import AutoReconnectManager, ReconnectStrategy + + +class ClientState(Enum): + """Connection states for RSIClient.""" + INITIALIZED = auto() # After __init__, network process spawned but not started + STARTING = auto() # Start signal sent, waiting for network to be ready + RUNNING = auto() # Actively communicating with robot + STOPPING = auto() # Shutdown in progress + STOPPED = auto() # Fully stopped, cannot be restarted (use reconnect) + ERROR = auto() # Error state + + +class RSIClient: + """Main RSI API class that integrates network, config handling, and message processing.""" + + _VALID_TRANSITIONS = { + ClientState.INITIALIZED: {ClientState.STARTING, ClientState.STOPPING}, + ClientState.STARTING: {ClientState.RUNNING, ClientState.STOPPING, ClientState.ERROR}, + ClientState.RUNNING: {ClientState.STOPPING, ClientState.ERROR}, + ClientState.STOPPING: {ClientState.STOPPED, ClientState.ERROR}, + ClientState.STOPPED: {ClientState.INITIALIZED}, + ClientState.ERROR: {ClientState.STOPPING, ClientState.INITIALIZED}, + } + + def __init__( + self, + config_file: str, + rsi_limits_file: Optional[str] = None, + enable_auto_reconnect: bool = False, + auto_reconnect_retries: int = 5, + auto_reconnect_delay: float = 5.0, + rsi_mode: str = 'relative', + max_cartesian_rate: float = 0.0, + max_joint_rate: float = 0.0, + cycle_time: float = 0.004 + ) -> None: + """ + Args: + config_file: Path to RSI_EthernetConfig.xml + rsi_limits_file: Optional path to .rsi.xml safety limits file + enable_auto_reconnect: Enable automatic reconnection on communication loss + auto_reconnect_retries: Maximum reconnection attempts (0 = unlimited) + auto_reconnect_delay: Base delay between retries in seconds + rsi_mode: 'absolute' or 'relative' — must match KRL RSI_MOVECORR() mode + max_cartesian_rate: Max mm/cycle for RKorr corrections (0 = disabled) + max_joint_rate: Max degrees/cycle for AKorr corrections (0 = disabled) + cycle_time: Expected RSI cycle time in seconds (0.004 or 0.012) + """ + logging.info("Loading RSI configuration from %s...", config_file) + self.rsi_mode = rsi_mode + self.max_cartesian_rate = max_cartesian_rate + self.max_joint_rate = max_joint_rate + self.cycle_time = cycle_time + + self._state: ClientState = ClientState.INITIALIZED + self._state_lock: Lock = Lock() + + self.config_parser: ConfigParser = ConfigParser(config_file, rsi_limits_file) + network_settings = self.config_parser.get_network_settings() + + # Validate config on startup + self._validate_config() + + self.manager: multiprocessing.Manager = multiprocessing.Manager() + self.send_variables = self.manager.dict(self.config_parser.send_variables) + self.receive_variables = self.manager.dict(self.config_parser.receive_variables) + self.stop_event: multiprocessing.Event = multiprocessing.Event() + self.start_event: multiprocessing.Event = multiprocessing.Event() + self.connected_event: multiprocessing.Event = multiprocessing.Event() + self.command_queue: multiprocessing.Queue = multiprocessing.Queue() + + self.safety_manager: SafetyManager = SafetyManager(self.config_parser.safety_limits) + + self._logging_active = multiprocessing.Value('b', False) + self._receive_dirty = multiprocessing.Value('b', True) # Dirty flag for IPC optimization + + self.metrics_dict = self.manager.dict() + + self._create_network_process(network_settings) + + self.logger: Optional[any] = None + self.running: bool = False + self.thread: Optional[Thread] = None + + self.auto_reconnect_manager: Optional[AutoReconnectManager] = None + if enable_auto_reconnect: + self.auto_reconnect_manager = AutoReconnectManager( + client=self, + enabled=True, + max_retries=auto_reconnect_retries, + retry_delay=auto_reconnect_delay, + strategy=ReconnectStrategy.LINEAR_BACKOFF + ) + logging.info("Auto-reconnect enabled") + + def _validate_config(self) -> None: + """Validate config and warn about common misconfigurations.""" + send = self.config_parser.send_variables + recv = self.config_parser.receive_variables + + # Check correction variables are in receive (what we send to robot) + if "RKorr" not in recv and "AKorr" not in recv: + logging.warning( + "Config validation: Neither RKorr nor AKorr found in RECEIVE section. " + "You won't be able to send motion corrections to the robot. " + "Check your RSI_EthernetConfig.xml elements." + ) + + # Check position feedback is in send (what robot sends to us) + if "RIst" not in send: + logging.warning( + "Config validation: RIst not found in SEND section. " + "You won't receive Cartesian position feedback from the robot." + ) + + if "IPOC" not in send: + logging.warning( + "Config validation: IPOC not found in SEND section. " + "IPOC synchronisation may not work correctly." + ) + + # Validate RSI mode + if self.rsi_mode not in ('absolute', 'relative'): + logging.warning( + "Config validation: rsi_mode='%s' is not valid. " + "Use 'absolute' or 'relative'. Defaulting to 'relative'.", + self.rsi_mode + ) + self.rsi_mode = 'relative' + + # Log summary + send_keys = [k for k in send if k != "IPOC"] + recv_keys = [k for k in recv if k not in ("IPOC", "FREE")] + logging.info( + "Config validated: SEND=[%s] RECEIVE=[%s] mode=%s", + ", ".join(send_keys), ", ".join(recv_keys), self.rsi_mode + ) + + def _create_network_process(self, network_settings: dict) -> None: + """Create and start the NetworkProcess with current settings.""" + self.network_process: NetworkProcess = NetworkProcess( + network_settings["ip"], + network_settings["port"], + self.send_variables, + self.receive_variables, + self.stop_event, + self.config_parser, + self.start_event, + self.command_queue, + self.metrics_dict, + self.connected_event, + rsi_mode=self.rsi_mode, + max_cartesian_rate=self.max_cartesian_rate, + max_joint_rate=self.max_joint_rate, + cycle_time=self.cycle_time + ) + self.network_process.logging_active = self._logging_active + self.network_process.receive_dirty = self._receive_dirty + self.network_process.start() + + @property + def state(self) -> ClientState: + """Get current client state (thread-safe).""" + with self._state_lock: + return self._state + + def _transition_to(self, new_state: ClientState) -> bool: + with self._state_lock: + if new_state in self._VALID_TRANSITIONS.get(self._state, set()): + old_state = self._state + self._state = new_state + logging.debug("State transition: %s -> %s", old_state.name, new_state.name) + return True + else: + logging.warning( + "Invalid state transition attempted: %s -> %s", self._state.name, new_state.name + ) + return False + + def start(self) -> None: + """ + Send start signal to NetworkProcess and run control loop. + + Raises: + RSIClientNotReady: If client is not in appropriate state to start + """ + if not self._transition_to(ClientState.STARTING): + error_msg = f"Cannot start from state {self.state.name}" + logging.error(error_msg) + raise RSIClientNotReady(error_msg) + + logging.info("RSIClient sending start signal to NetworkProcess...") + self.start_event.set() + + if not self._transition_to(ClientState.RUNNING): + error_msg = "Failed to transition to RUNNING state" + logging.error(error_msg) + raise RSIStateError(error_msg) + + self.running = True + logging.info("RSI Client Started") + + if self.auto_reconnect_manager: + self.auto_reconnect_manager.start() + + try: + while self.running and not self.stop_event.is_set(): + time.sleep(2) + except KeyboardInterrupt: + self.stop() + except Exception as e: + logging.error("RSI Client encountered an error: %s", e) + self._transition_to(ClientState.ERROR) + raise + + def stop(self) -> None: + """Stop the network process and the client thread safely.""" + if self.state in (ClientState.STOPPED, ClientState.STOPPING): + logging.debug("Already stopped or stopping") + return + + if not self._transition_to(ClientState.STOPPING): + logging.warning("Could not transition to STOPPING state") + + logging.info("Stopping RSI Client...") + + self.running = False + self.stop_event.set() + + if self.network_process and self.network_process.is_alive(): + self.network_process.join(timeout=3) + if self.network_process.is_alive(): + logging.warning("Forcing network process termination...") + self.network_process.terminate() + self.network_process.join() + + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=2) + self.thread = None + + if self.auto_reconnect_manager: + self.auto_reconnect_manager.stop() + + # Shutdown Manager to avoid resource leaks + try: + self.manager.shutdown() + except Exception: + pass + + self._transition_to(ClientState.STOPPED) + logging.info("RSI Client Stopped") + + def reconnect(self) -> None: + """ + Reconnect the network process safely. + + Stops existing connection, resets state, and creates fresh + network process with new communication resources. + """ + logging.info("Reconnecting RSI Client network...") + + if self.state in (ClientState.RUNNING, ClientState.STARTING): + self.stop() + + if self.network_process and self.network_process.is_alive(): + self.stop_event.set() + self.network_process.terminate() + self.network_process.join() + + # Fresh Manager (old one was shut down in stop()) + self.manager = multiprocessing.Manager() + self.send_variables = self.manager.dict(self.config_parser.send_variables) + self.receive_variables = self.manager.dict(self.config_parser.receive_variables) + self.metrics_dict = self.manager.dict() + + with self._state_lock: + self._state = ClientState.INITIALIZED + + self.stop_event = multiprocessing.Event() + self.start_event = multiprocessing.Event() + self.connected_event = multiprocessing.Event() + self.command_queue = multiprocessing.Queue() + self._receive_dirty = multiprocessing.Value('b', True) + + network_settings = self.config_parser.get_network_settings() + self._create_network_process(network_settings) + + def wait_for_connection(self, timeout: float = 10.0) -> bool: + """ + Block until the first valid packet is received from the robot. + + Args: + timeout: Maximum time to wait in seconds + + Returns: + True if connected, False if timeout + """ + return self.connected_event.wait(timeout=timeout) + + def emergency_stop(self) -> None: + """Send E-stop command to network process to zero all corrections.""" + self.safety_manager.emergency_stop() + self.command_queue.put({'action': 'estop'}) + logging.critical("Emergency stop activated") + + def emergency_reset(self) -> None: + """Reset E-stop and resume normal corrections.""" + self.safety_manager.reset_stop() + self.command_queue.put({'action': 'estop_reset'}) + logging.info("Emergency stop reset") + + def is_running(self) -> bool: + return self.state == ClientState.RUNNING + + def is_stopped(self) -> bool: + return self.state == ClientState.STOPPED + + def start_logging(self, filename: str) -> None: + self.command_queue.put({'action': 'start_logging', 'filename': filename}) + + def stop_logging(self) -> None: + self.command_queue.put({'action': 'stop_logging'}) + + def is_logging_active(self) -> bool: + return self._logging_active.value diff --git a/src/RSIPI/rsi_echo_server.py b/src/RSIPI/rsi_echo_server.py index 6340d55..7048172 100644 --- a/src/RSIPI/rsi_echo_server.py +++ b/src/RSIPI/rsi_echo_server.py @@ -1,11 +1,12 @@ +import copy import socket import time import xml.etree.ElementTree as ET import logging import threading -from .rsi_config import RSIConfig +from .config_parser import ConfigParser -# ✅ Toggle logging for debugging purposes +# Toggle logging for debugging purposes LOGGING_ENABLED = True if LOGGING_ENABLED: @@ -16,6 +17,13 @@ if LOGGING_ENABLED: datefmt="%Y-%m-%d %H:%M:%S" ) +# Maps correction tags from client XML to robot state tags in XML +CORRECTION_TO_STATE = { + "RKorr": "RIst", + "AKorr": "AIPos", + "EKorr": "ELPos", +} + class EchoServer: """ @@ -35,7 +43,7 @@ class EchoServer: delay_ms (int): Delay between messages in milliseconds. mode (str): Correction mode ("relative" or "absolute"). """ - self.config = RSIConfig(config_file) + self.config = ConfigParser(config_file) network_settings = self.config.get_network_settings() self.server_address = ("0.0.0.0", 50000) # Local bind @@ -48,14 +56,12 @@ class EchoServer: self.delay_ms = delay_ms / 1000 # Convert to seconds self.mode = mode.lower() - # Internal state to simulate robot values - self.state = { - "RIst": {k: 0.0 for k in ["X", "Y", "Z", "A", "B", "C"]}, - "AIPos": {f"A{i}": 0.0 for i in range(1, 7)}, - "ELPos": {f"E{i}": 0.0 for i in range(1, 7)}, - "DiO": 0, - "DiL": 0 - } + # Build internal state from config send_variables (what the robot sends out). + # Deep copy so mutations to self.state don't affect the parser's data. + self.state = copy.deepcopy(self.config.send_variables) + + # Ensure IPOC is managed separately (we increment it ourselves) + self.state.pop("IPOC", None) self.running = True self.thread = threading.Thread(target=self.send_message, daemon=True) @@ -66,7 +72,8 @@ class EchoServer: def receive_and_process(self): """ Handles one incoming UDP message and updates the internal state accordingly. - Supports RKorr, AKorr, DiO, DiL, and IPOC updates. + Supports correction tags (RKorr->RIst, AKorr->AIPos, EKorr->ELPos), + scalar state updates (DiO, DiL, etc.), and IPOC synchronisation. """ try: self.udp_socket.settimeout(self.delay_ms) @@ -77,32 +84,37 @@ class EchoServer: for elem in root: tag = elem.tag - if tag in ["RKorr", "AKorr"]: - for axis, value in elem.attrib.items(): - value = float(value) - if tag == "RKorr" and axis in self.state["RIst"]: - # Apply Cartesian correction - if self.mode == "relative": - self.state["RIst"][axis] += value - else: - self.state["RIst"][axis] = value - elif tag == "AKorr" and axis in self.state["AIPos"]: - # Apply joint correction - if self.mode == "relative": - self.state["AIPos"][axis] += value - else: - self.state["AIPos"][axis] = value - elif tag in ["DiO", "DiL"]: - if tag in self.state: - self.state[tag] = int(elem.text.strip()) + + if tag in CORRECTION_TO_STATE: + # Apply correction (RKorr/AKorr/EKorr) to corresponding state variable + state_key = CORRECTION_TO_STATE[tag] + if state_key in self.state and isinstance(self.state[state_key], dict): + for axis, value in elem.attrib.items(): + if axis in self.state[state_key]: + value = float(value) + if self.mode == "relative": + self.state[state_key][axis] += value + else: + self.state[state_key][axis] = value + elif tag == "IPOC": self.ipoc_value = int(elem.text.strip()) + elif tag in self.state: + # Update scalar state values (DiO, DiL, etc.) + if isinstance(self.state[tag], dict): + # Structured variable sent as attributes + for attr, value in elem.attrib.items(): + if attr in self.state[tag]: + self.state[tag][attr] = float(value) + elif isinstance(self.state[tag], (int, float)): + self.state[tag] = int(elem.text.strip()) if isinstance(self.state[tag], int) else float(elem.text.strip()) + logging.debug(f"Processed input: {ET.tostring(root).decode()}") except socket.timeout: pass # No data within delay window except ConnectionResetError: - print("⚠️ Connection was reset by client. Waiting before retry...") + print("Connection was reset by client. Waiting before retry...") time.sleep(0.5) except Exception as e: print(f"[ERROR] Failed to process input: {e}") @@ -111,16 +123,22 @@ class EchoServer: """ Creates a reply XML message based on current state. Format matches KUKA RSI's expected response structure. + Iterates over all state variables from the config's send_variables. """ root = ET.Element("Rob", Type="KUKA") - for key in ["RIst", "AIPos", "ELPos"]: - element = ET.SubElement(root, key) - for sub_key, value in self.state[key].items(): - element.set(sub_key, f"{value:.2f}") - - for key in ["DiO", "DiL"]: - ET.SubElement(root, key).text = str(self.state[key]) + for key, value in self.state.items(): + if isinstance(value, dict): + # Structured variable (RIst, AIPos, etc.) -> XML attributes + element = ET.SubElement(root, key) + for sub_key, sub_value in value.items(): + element.set(sub_key, f"{float(sub_value):.2f}") + elif isinstance(value, bool): + ET.SubElement(root, key).text = "1" if value else "0" + elif isinstance(value, (int, float)): + ET.SubElement(root, key).text = str(value) + elif isinstance(value, str): + ET.SubElement(root, key).text = value ET.SubElement(root, "IPOC").text = str(self.ipoc_value) return ET.tostring(root, encoding="utf-8").decode() diff --git a/src/RSIPI/safety_api.py b/src/RSIPI/safety_api.py index 57d248c..a6f68df 100644 --- a/src/RSIPI/safety_api.py +++ b/src/RSIPI/safety_api.py @@ -35,7 +35,7 @@ class SafetyAPI: This is NOT a hardware E-stop and should not be relied upon for safety-critical applications. Always use proper hardware E-stops. """ - self.client.safety_manager.emergency_stop() + self.client.emergency_stop() logging.critical("Emergency stop activated via SafetyAPI") def reset(self) -> None: @@ -45,7 +45,7 @@ class SafetyAPI: Clears the E-stop flag, allowing motion commands to proceed again. Use with caution after ensuring the robot workspace is safe. """ - self.client.safety_manager.reset_stop() + self.client.emergency_reset() logging.info("Emergency stop reset via SafetyAPI") def status(self) -> Dict[str, Any]: diff --git a/src/RSIPI/tools_api.py b/src/RSIPI/tools_api.py index 6dda153..1a2cd5e 100644 --- a/src/RSIPI/tools_api.py +++ b/src/RSIPI/tools_api.py @@ -73,14 +73,18 @@ class ToolsAPI: safe_value = self.client.safety_manager.validate(full_path, float(value)) current[child] = safe_value target[parent] = current - logging.debug(f"Updated {name} to {safe_value}") + if hasattr(self.client, '_receive_dirty'): + self.client._receive_dirty.value = True + logging.debug("Updated %s to %s", name, safe_value) return f"Updated {name} to {safe_value}" else: raise RSIVariableError(f"Parent variable '{parent}' not found in receive_variables") else: safe_value = self.client.safety_manager.validate(name, float(value)) target[name] = safe_value - logging.debug(f"Updated {name} to {safe_value}") + if hasattr(self.client, '_receive_dirty'): + self.client._receive_dirty.value = True + logging.debug("Updated %s to %s", name, safe_value) return f"Updated {name} to {safe_value}" def show_variables(self) -> None: @@ -242,7 +246,7 @@ class ToolsAPI: else: raise ValueError(f"Unsupported format: {format_type}. Use 'csv', 'json', or 'pdf'.") - logging.info(f"Report generated: {output_path}") + logging.info("Report generated: %s", output_path) return f"Report saved as {output_path}" @staticmethod @@ -290,5 +294,5 @@ class ToolsAPI: "max_diff": float(delta.max()), } - logging.info(f"Compared {len(shared_cols)} position columns between runs") + logging.info("Compared %d position columns between runs", len(shared_cols)) return diffs diff --git a/src/RSIPI/xml_handler.py b/src/RSIPI/xml_handler.py index 8f114bb..be45e24 100644 --- a/src/RSIPI/xml_handler.py +++ b/src/RSIPI/xml_handler.py @@ -1,58 +1,198 @@ -import xml.etree.ElementTree as ET - -class XMLGenerator: - """ - Converts structured dictionaries of RSI send/receive variables into - valid XML strings for UDP transmission to/from the robot controller. - """ - - @staticmethod - def generate_send_xml(send_variables, network_settings): - """ - Build an outgoing XML message based on the current send variables. - - Args: - send_variables (dict): Structured dictionary of values to send. - network_settings (dict): Contains 'sentype' used for the root element. - - Returns: - str: XML-formatted string ready for UDP transmission. - """ - root = ET.Element("Sen", Type=network_settings["sentype"]) - - # Convert structured keys (e.g. RKorr, Tech) and flat elements (e.g. IPOC) - for key, value in send_variables.items(): - if key == "FREE": - continue # Skip unused placeholder fields - - if isinstance(value, dict): - element = ET.SubElement(root, key) - for sub_key, sub_value in value.items(): - element.set(sub_key, f"{float(sub_value):.2f}") - else: - ET.SubElement(root, key).text = str(value) - - return ET.tostring(root, encoding="utf-8").decode() - - @staticmethod - def generate_receive_xml(receive_variables): - """ - Build an incoming XML message for emulation/testing purposes. - - Args: - receive_variables (dict): Structured dictionary of values to simulate reception. - - Returns: - str: XML-formatted string mimicking a KUKA robot's reply. - """ - root = ET.Element("Rob", Type="KUKA") - - for key, value in receive_variables.items(): - if isinstance(value, dict) or hasattr(value, "items"): - element = ET.SubElement(root, key) - for sub_key, sub_value in value.items(): - element.set(sub_key, f"{float(sub_value):.2f}") - else: - ET.SubElement(root, key).text = str(value) - - return ET.tostring(root, encoding="utf-8").decode() +import re +import xml.etree.ElementTree as ET +from typing import Dict, Any, List, Tuple, Optional + + +class XMLGenerator: + """ + Converts structured dictionaries of RSI send/receive variables into + valid XML strings for UDP transmission to/from the robot controller. + """ + + @staticmethod + def generate_send_xml(send_variables, network_settings): + """ + Build an outgoing XML message based on the current send variables. + + Args: + send_variables (dict): Structured dictionary of values to send. + network_settings (dict): Contains 'sentype' used for the root element. + + Returns: + str: XML-formatted string ready for UDP transmission. + """ + root = ET.Element("Sen", Type=network_settings["sentype"]) + + for key, value in send_variables.items(): + if key == "FREE": + continue + + if isinstance(value, dict): + element = ET.SubElement(root, key) + for sub_key, sub_value in value.items(): + element.set(sub_key, f"{float(sub_value):.2f}") + else: + ET.SubElement(root, key).text = str(value) + + return ET.tostring(root, encoding="utf-8").decode() + + @staticmethod + def generate_receive_xml(receive_variables): + """ + Build an incoming XML message for emulation/testing purposes. + + Args: + receive_variables (dict): Structured dictionary of values to simulate reception. + + Returns: + str: XML-formatted string mimicking a KUKA robot's reply. + """ + root = ET.Element("Rob", Type="KUKA") + + for key, value in receive_variables.items(): + if isinstance(value, dict) or hasattr(value, "items"): + element = ET.SubElement(root, key) + for sub_key, sub_value in value.items(): + element.set(sub_key, f"{float(sub_value):.2f}") + else: + ET.SubElement(root, key).text = str(value) + + return ET.tostring(root, encoding="utf-8").decode() + + +class FastXMLGenerator: + """ + Pre-compiled string template XML generator for the 4ms hot path. + + Compiles format strings at init time from the variable structure, + then generates XML via string formatting (no DOM construction). + ~5-10x faster than ElementTree for fixed-schema RSI messages. + """ + + def __init__(self, variables: dict, root_tag: str = "Sen", type_attr: str = "ImFree") -> None: + """ + Compile a format template from the variable structure. + + Args: + variables: Dict of variable names → values/dicts (defines the XML schema) + root_tag: Root XML element name ('Sen' for outgoing, 'Rob' for incoming) + type_attr: Value for the Type attribute on root element + """ + self._keys: List[Tuple[str, Optional[List[str]]]] = [] + parts = [f'<{root_tag} Type="{type_attr}">'] + + for key, value in variables.items(): + if key == "FREE": + continue + + if isinstance(value, dict): + subkeys = list(value.keys()) + self._keys.append((key, subkeys)) + # Use __ separator to avoid Python format_map treating . as attribute access + attr_template = " ".join(f'{sk}="{{{key}__{sk}:.2f}}"' for sk in subkeys) + parts.append(f"<{key} {attr_template} />") + else: + self._keys.append((key, None)) + parts.append(f"<{key}>{{{key}}}") + + # Ensure IPOC is always in template (required by robot) + if "IPOC" not in variables: + self._keys.append(("IPOC", None)) + parts.append("{IPOC}") + + parts.append(f"") + self._template = "".join(parts) + + def generate(self, variables: dict) -> str: + """ + Generate XML string from current variable values using pre-compiled template. + + Args: + variables: Current variable values dict + + Returns: + XML string ready for UDP transmission + """ + fmt_args = {} + for key, subkeys in self._keys: + if subkeys is not None: + val = variables.get(key, {}) + if isinstance(val, dict): + for sk in subkeys: + fmt_args[f"{key}__{sk}"] = float(val.get(sk, 0.0)) + else: + for sk in subkeys: + fmt_args[f"{key}__{sk}"] = 0.0 + else: + fmt_args[key] = variables.get(key, "") + return self._template.format_map(fmt_args) + + +class FastXMLParser: + """ + Pre-compiled regex XML parser for the 4ms hot path. + + Compiles regex patterns at init time from the known variable structure, + then parses incoming XML via targeted regex extraction (no DOM construction). + ~3-5x faster than ElementTree for fixed-schema RSI messages. + """ + + def __init__(self, variables: dict) -> None: + """ + Compile regex patterns from the variable structure. + + Args: + variables: Dict of expected variable names → values/dicts + """ + self._dict_patterns: List[Tuple[str, re.Pattern, List[str]]] = [] + self._scalar_patterns: List[Tuple[str, re.Pattern]] = [] + + for key, value in variables.items(): + if isinstance(value, dict): + subkeys = list(value.keys()) + attr_pattern = r"\s+".join( + rf'{sk}="([^"]*)"' for sk in subkeys + ) + # Also match any order with a looser pattern as fallback + pattern = re.compile(rf"<{re.escape(key)}\s+{attr_pattern}") + self._dict_patterns.append((key, pattern, subkeys)) + else: + pattern = re.compile(rf"<{re.escape(key)}>([^<]*)") + self._scalar_patterns.append((key, pattern)) + + # Always parse IPOC specifically + self._ipoc_pattern = re.compile(r"(\d+)") + + def parse(self, xml_string: str, target: dict) -> None: + """ + Parse XML string into target dict using pre-compiled patterns. + + Args: + xml_string: Raw XML string from robot + target: Dict to update with parsed values (modified in-place) + """ + # Parse dict-type elements (attributes) + for key, pattern, subkeys in self._dict_patterns: + if key not in target: + continue + match = pattern.search(xml_string) + if match: + existing = target.get(key) + if isinstance(existing, dict): + for i, sk in enumerate(subkeys): + existing[sk] = float(match.group(i + 1)) + else: + target[key] = {sk: float(match.group(i + 1)) for i, sk in enumerate(subkeys)} + + # Parse scalar elements (text content) + for key, pattern in self._scalar_patterns: + if key not in target: + continue + match = pattern.search(xml_string) + if match: + target[key] = match.group(1) + + # IPOC always parsed as int + ipoc_match = self._ipoc_pattern.search(xml_string) + if ipoc_match: + target["IPOC"] = int(ipoc_match.group(1)) diff --git a/src/__pycache__/__init__.cpython-313.pyc b/src/__pycache__/__init__.cpython-313.pyc index 25719b3b8461b30d225b5a4c1ce15626be79e633..3c7f1c37dde9eea5eab5d726d7f91126b0f5cf24 100644 GIT binary patch delta 49 zcmeyzxSWyaGcPX}0}#BJl9f4;$B5BwqP>zxQGQlxa!GMaP_So!XG~FXrfxyz#4|Yn2GcPX}0}y;|xR^eX$B1$6M0=(B#DdhK;usCjJcWRw{Pd#K;$lq&&%Bb< z^rFO)%)E33?@(_?g`oVT{E}jY^30M9g@DSEjQl(WM+Lvsa XML +from a client, and responds with XML containing the current robot state. + +Usage: + python test_server.py + python test_server.py --config path/to/RSI_EthernetConfig.xml +""" + +import socket +import threading +import tkinter as tk +from tkinter import messagebox +import xml.etree.ElementTree as ET +import argparse +import copy +import sys +import os +import time + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "src")) +from RSIPI.config_parser import ConfigParser +from RSIPI.xml_handler import XMLGenerator + +# --- Theme colours --- +BG_DARK = "#1e1e2e" +BG_MID = "#282840" +BG_CARD = "#313150" +BG_INPUT = "#3b3b5c" +FG = "#cdd6f4" +FG_DIM = "#7f849c" +FG_BRIGHT = "#ffffff" +ACCENT = "#f5a623" +ACCENT_HOVER = "#f7bc5e" +GREEN = "#a6e3a1" +RED = "#f38ba8" +BLUE = "#89b4fa" +BORDER = "#45475a" +AXIS_COLOURS = { + "X": "#f38ba8", "Y": "#a6e3a1", "Z": "#89b4fa", + "A": "#fab387", "B": "#cba6f7", "C": "#94e2d5", +} + + +class RSITestServer: + """UDP server simulating a KUKA robot controller for RSI testing.""" + + def __init__(self, config_file: str): + self.config = ConfigParser(config_file) + self.network_settings = self.config.get_network_settings() + self.port = self.network_settings["port"] + + self.robot_state = copy.deepcopy(self.config.send_variables) + self.receive_template = copy.deepcopy(self.config.receive_variables) + + self.ipoc = 0 + self.correction_step = 0.01 + self.running = False + self.receive_only = False + self.udp_socket = None + self.listen_thread = None + self.client_address = None + self.last_received_xml = "" + self.last_sent_xml = "" + self.packet_count = 0 + self.lock = threading.Lock() + + def start(self): + if self.running: + return + try: + self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.udp_socket.bind(("0.0.0.0", self.port)) + self.udp_socket.settimeout(0.5) + self.running = True + self.packet_count = 0 + self.listen_thread = threading.Thread(target=self._listen_loop, daemon=True) + self.listen_thread.start() + return True + except OSError as e: + self.running = False + return str(e) + + def stop(self): + self.running = False + if self.listen_thread: + self.listen_thread.join(timeout=2) + if self.udp_socket: + self.udp_socket.close() + self.udp_socket = None + self.client_address = None + + def _listen_loop(self): + while self.running: + try: + data, addr = self.udp_socket.recvfrom(4096) + self.client_address = addr + xml_string = data.decode("utf-8", errors="replace") + + with self.lock: + self.last_received_xml = xml_string + self._process_received(xml_string) + self.packet_count += 1 + + if not self.receive_only: + response = self._generate_response() + self.last_sent_xml = response + self.udp_socket.sendto(response.encode("utf-8"), addr) + + except socket.timeout: + continue + except OSError: + break + except Exception as e: + if self.running: + print(f"[TestServer] Error: {e}") + + def _process_received(self, xml_string: str): + try: + root = ET.fromstring(xml_string) + ipoc_elem = root.find("IPOC") + if ipoc_elem is not None and ipoc_elem.text: + self.ipoc = int(ipoc_elem.text.strip()) + except ET.ParseError: + self.ipoc = self._extract_ipoc_string(xml_string) + + def _extract_ipoc_string(self, xml_string: str) -> int: + end_tag = "" + start_tag = "" + end_idx = xml_string.find(end_tag) + if end_idx == -1: + return self.ipoc + start_idx = xml_string.find(start_tag) + if start_idx == -1: + return self.ipoc + try: + return int(xml_string[start_idx + len(start_tag):end_idx].strip()) + except ValueError: + return self.ipoc + + def _generate_response(self) -> str: + state = copy.deepcopy(self.robot_state) + state["IPOC"] = self.ipoc + return XMLGenerator.generate_receive_xml(state) + + def adjust_position(self, axis: str, delta: float): + with self.lock: + if "RIst" in self.robot_state and axis in self.robot_state["RIst"]: + self.robot_state["RIst"][axis] += delta + + def reset_positions(self): + with self.lock: + if "RIst" in self.robot_state: + for axis in self.robot_state["RIst"]: + self.robot_state["RIst"][axis] = 0.0 + + +class TestServerGUI: + """Dark-themed tkinter GUI for the RSI TestServer.""" + + def __init__(self, config_file: str): + self.server = RSITestServer(config_file) + self._prev_packet_count = 0 + self._prev_time = time.time() + self._packets_per_sec = 0.0 + self.recv_text: tk.Text = None # type: ignore[assignment] + self.sent_text: tk.Text = None # type: ignore[assignment] + + self.root = tk.Tk() + self.root.title("RSI TestServer") + self.root.configure(bg=BG_DARK) + self.root.geometry("1100x750") + self.root.resizable(True, True) + self.root.protocol("WM_DELETE_WINDOW", self._on_close) + + self._build_ui() + self._update_display() + + # ── helpers ────────────────────────────────────────────────────── + + def _card(self, parent, **kw): + f = tk.Frame(parent, bg=BG_CARD, highlightbackground=BORDER, + highlightthickness=1, **kw) + return f + + def _label(self, parent, text="", font=("Segoe UI", 10), fg=FG, **kw): + return tk.Label(parent, text=text, font=font, fg=fg, bg=parent["bg"], **kw) + + def _btn(self, parent, text, command, width=4, accent=False, **kw): + bg = ACCENT if accent else BG_INPUT + fg_c = BG_DARK if accent else FG + hover = ACCENT_HOVER if accent else "#4e4e7a" + b = tk.Button(parent, text=text, command=command, width=width, + font=("Segoe UI", 10, "bold"), fg=fg_c, bg=bg, + activeforeground=fg_c, activebackground=hover, + relief="flat", cursor="hand2", bd=0, + highlightthickness=0, **kw) + b.bind("", lambda e, b=b, h=hover: b.config(bg=h)) + b.bind("", lambda e, b=b, n=bg: b.config(bg=n)) + return b + + # ── layout ─────────────────────────────────────────────────────── + + def _build_ui(self): + # ── Header ── + header = tk.Frame(self.root, bg=BG_MID, height=56) + header.grid(row=0, column=0, sticky="ew") + header.grid_columnconfigure(1, weight=1) + + self._label(header, "RSI TestServer", + font=("Segoe UI", 16, "bold"), fg=ACCENT).grid( + row=0, column=0, padx=16, pady=10, sticky="w") + + self._label(header, f"Port {self.server.port}", + font=("Segoe UI Semibold", 11), fg=FG_DIM).grid( + row=0, column=1, sticky="w") + + self.status_dot = tk.Canvas(header, width=14, height=14, + bg=BG_MID, highlightthickness=0) + self.status_dot.create_oval(2, 2, 12, 12, fill=RED, outline="", tags="dot") + self.status_dot.grid(row=0, column=2, padx=(0, 4)) + + self.status_label = self._label(header, "Offline", + font=("Segoe UI Semibold", 11), fg=RED) + self.status_label.grid(row=0, column=3, padx=(0, 16)) + + # ── Body ── + body = tk.Frame(self.root, bg=BG_DARK) + body.grid(row=1, column=0, sticky="nsew", padx=12, pady=8) + body.grid_columnconfigure(1, weight=1) + + # ── Left column: controls + jog ── + left = tk.Frame(body, bg=BG_DARK) + left.grid(row=0, column=0, sticky="n", padx=(0, 8)) + + # Controls card + ctrl = self._card(left) + ctrl.grid(row=0, column=0, sticky="ew", pady=(0, 8)) + + btn_row = tk.Frame(ctrl, bg=BG_CARD) + btn_row.grid(row=0, column=0, padx=10, pady=10, sticky="ew") + + self.start_btn = self._btn(btn_row, "Start", self._start, + width=8, accent=True) + self.start_btn.grid(row=0, column=0, padx=(0, 4)) + + self.stop_btn = self._btn(btn_row, "Stop", self._stop, width=8) + self.stop_btn.config(state="disabled") + self.stop_btn.grid(row=0, column=1, padx=(0, 12)) + + self.mode_var = tk.StringVar(value="Send + Recv") + mode_btn = tk.Menubutton(btn_row, textvariable=self.mode_var, + font=("Segoe UI", 9), fg=FG, bg=BG_INPUT, + activeforeground=FG, activebackground="#4e4e7a", + relief="flat", highlightthickness=0, + indicatoron=True, cursor="hand2", width=12) + mode_menu = tk.Menu(mode_btn, tearoff=0, bg=BG_INPUT, fg=FG, + activebackground=ACCENT, activeforeground=BG_DARK) + mode_menu.add_command(label="Send + Recv", + command=lambda: self._set_mode("Send + Recv")) + mode_menu.add_command(label="Recv only", + command=lambda: self._set_mode("Recv only")) + mode_btn["menu"] = mode_menu + mode_btn.grid(row=0, column=2, padx=(0, 8)) + + self._label(btn_row, "Step:", font=("Segoe UI", 9), fg=FG_DIM).grid( + row=0, column=3) + self.step_var = tk.StringVar(value="0.01") + step_e = tk.Entry(btn_row, textvariable=self.step_var, width=7, + font=("Consolas", 10), fg=ACCENT, bg=BG_INPUT, + insertbackground=ACCENT, relief="flat", + highlightthickness=1, highlightcolor=ACCENT, + highlightbackground=BORDER) + step_e.grid(row=0, column=4, padx=4) + step_e.bind("", self._step_changed) + step_e.bind("", self._step_changed) + + # Jog card + jog = self._card(left) + jog.grid(row=1, column=0, sticky="ew") + + jog_title = tk.Frame(jog, bg=BG_CARD) + jog_title.grid(row=0, column=0, columnspan=4, sticky="ew", padx=10, + pady=(8, 4)) + self._label(jog_title, "Jog Control", + font=("Segoe UI Semibold", 11), fg=FG_BRIGHT).pack( + side="left") + + axes = ["X", "Y", "Z", "A", "B", "C"] + self.pos_labels = {} + + for i, axis in enumerate(axes): + row_frame = tk.Frame(jog, bg=BG_CARD) + row_frame.grid(row=i + 1, column=0, sticky="ew", padx=10, pady=2) + row_frame.grid_columnconfigure(1, weight=1) + + colour = AXIS_COLOURS[axis] + + # Colour pip + pip = tk.Canvas(row_frame, width=8, height=8, + bg=BG_CARD, highlightthickness=0) + pip.create_oval(1, 1, 7, 7, fill=colour, outline="") + pip.grid(row=0, column=0, padx=(0, 6)) + + self._label(row_frame, f"{axis}", + font=("Segoe UI Semibold", 11), fg=colour).grid( + row=0, column=1, sticky="w") + + self.pos_labels[axis] = self._label( + row_frame, "0.0000", + font=("Consolas", 12), fg=FG_BRIGHT) + self.pos_labels[axis].grid(row=0, column=2, padx=8) + + self._btn(row_frame, "+", + lambda a=axis: self._adjust(a, 1), width=3).grid( + row=0, column=3, padx=1) + self._btn(row_frame, "-", + lambda a=axis: self._adjust(a, -1), width=3).grid( + row=0, column=4, padx=1) + + reset_row = tk.Frame(jog, bg=BG_CARD) + reset_row.grid(row=len(axes) + 1, column=0, sticky="ew", + padx=10, pady=(6, 10)) + self._btn(reset_row, "Reset All", self._reset, + width=36, accent=False).pack(fill="x") + + # ── Right column: state + xml ── + right = tk.Frame(body, bg=BG_DARK) + right.grid(row=0, column=1, sticky="nsew") + right.grid_rowconfigure(1, weight=1) + + # Info bar + info = self._card(right) + info.grid(row=0, column=0, sticky="ew", pady=(0, 8)) + + info_inner = tk.Frame(info, bg=BG_CARD) + info_inner.grid(padx=10, pady=8, sticky="ew") + info.grid_columnconfigure(0, weight=1) + info_inner.grid_columnconfigure(1, weight=1) + info_inner.grid_columnconfigure(3, weight=1) + + self._label(info_inner, "IPOC", font=("Segoe UI", 9), fg=FG_DIM).grid( + row=0, column=0, sticky="w") + self.ipoc_val = self._label(info_inner, "0", + font=("Consolas", 12, "bold"), fg=BLUE) + self.ipoc_val.grid(row=1, column=0, sticky="w") + + self._label(info_inner, "Client", font=("Segoe UI", 9), fg=FG_DIM).grid( + row=0, column=1, sticky="w", padx=(20, 0)) + self.client_val = self._label(info_inner, "--", + font=("Consolas", 11), fg=FG) + self.client_val.grid(row=1, column=1, sticky="w", padx=(20, 0)) + + self._label(info_inner, "Packets/s", font=("Segoe UI", 9), fg=FG_DIM).grid( + row=0, column=2, sticky="w", padx=(20, 0)) + self.pps_val = self._label(info_inner, "0", + font=("Consolas", 12, "bold"), fg=GREEN) + self.pps_val.grid(row=1, column=2, sticky="w", padx=(20, 0)) + + # Robot state card — grid layout + state_card = self._card(right) + state_card.grid(row=1, column=0, sticky="nsew", pady=(0, 8)) + + state_hdr = tk.Frame(state_card, bg=BG_CARD) + state_hdr.grid(row=0, column=0, sticky="ew", padx=10, pady=(8, 4)) + self._label(state_hdr, "Robot State", + font=("Segoe UI Semibold", 11), fg=FG_BRIGHT).pack( + side="left") + + # Scrollable grid area + state_outer = tk.Frame(state_card, bg=BG_MID) + state_outer.grid(row=1, column=0, sticky="nsew", padx=8, pady=(0, 8)) + state_card.grid_rowconfigure(1, weight=1) + + state_canvas = tk.Canvas(state_outer, bg=BG_MID, highlightthickness=0, + height=280) + state_canvas.pack(side="left", fill="both", expand=True) + state_scroll = tk.Scrollbar(state_outer, orient="vertical", + command=state_canvas.yview, + bg=BG_MID, troughcolor=BG_MID) + state_scroll.pack(side="right", fill="y") + state_canvas.configure(yscrollcommand=state_scroll.set) + + self._state_grid = tk.Frame(state_canvas, bg=BG_MID) + state_canvas.create_window((0, 0), window=self._state_grid, anchor="nw") + self._state_canvas = state_canvas + + # Build grid from robot_state + self._state_cells = {} # (group, subkey) -> Label + self._build_state_grid() + + # XML card + xml_card = self._card(right) + xml_card.grid(row=2, column=0, sticky="nsew") + + xml_hdr = tk.Frame(xml_card, bg=BG_CARD) + xml_hdr.grid(row=0, column=0, columnspan=2, sticky="ew", padx=10, + pady=(8, 4)) + self._label(xml_hdr, "XML Traffic", + font=("Segoe UI Semibold", 11), fg=FG_BRIGHT).pack( + side="left") + + xml_card.grid_columnconfigure(1, weight=1) + for i, (lbl, attr) in enumerate([("RX", "recv_text"), ("TX", "sent_text")]): + tag_bg = GREEN if lbl == "RX" else BLUE + tag = tk.Label(xml_card, text=f" {lbl} ", font=("Consolas", 9, "bold"), + fg=BG_DARK, bg=tag_bg) + tag.grid(row=i + 1, column=0, sticky="nw", padx=(10, 4), pady=2) + t = tk.Text(xml_card, width=60, height=6, font=("Consolas", 9), + fg=FG_DIM, bg=BG_MID, relief="flat", bd=0, + padx=6, pady=4, wrap="word", state="disabled") + t.grid(row=i + 1, column=1, sticky="nsew", padx=(0, 8), pady=2) + setattr(self, attr, t) + + # bottom pad + tk.Frame(xml_card, bg=BG_CARD, height=8).grid( + row=3, column=0, columnspan=2) + + # ── state grid builder ──────────────────────────────────────────── + + def _build_state_grid(self): + """Build the header row and value cells from robot_state structure.""" + grid = self._state_grid + row = 0 + + for key, value in self.server.robot_state.items(): + if isinstance(value, dict): + # Group header + self._label(grid, key, font=("Segoe UI Semibold", 9), + fg=ACCENT).grid(row=row, column=0, sticky="w", + padx=(8, 4), pady=(6, 0)) + row += 1 + + # Sub-key headers + value cells in columns + subkeys = list(value.keys()) + # Chunk into rows of 6 for wide dicts like Tech + chunk_size = 6 + for chunk_start in range(0, len(subkeys), chunk_size): + chunk = subkeys[chunk_start:chunk_start + chunk_size] + for col, sk in enumerate(chunk): + # Header + self._label(grid, sk, font=("Consolas", 8), + fg=FG_DIM).grid( + row=row, column=col + 1, padx=4, sticky="e") + row += 1 + for col, sk in enumerate(chunk): + # Value cell + cell = self._label(grid, "0.0000", + font=("Consolas", 10), fg=FG_BRIGHT) + cell.grid(row=row, column=col + 1, padx=4, sticky="e") + self._state_cells[(key, sk)] = cell + row += 1 + else: + # Scalar value + self._label(grid, key, font=("Segoe UI Semibold", 9), + fg=ACCENT).grid(row=row, column=0, sticky="w", + padx=(8, 4), pady=(6, 0)) + cell = self._label(grid, str(value), + font=("Consolas", 10), fg=FG_BRIGHT) + cell.grid(row=row, column=1, padx=4, sticky="e") + self._state_cells[(key, None)] = cell + row += 1 + + # Update scroll region after grid is built + grid.update_idletasks() + self._state_canvas.configure(scrollregion=self._state_canvas.bbox("all")) + + def _update_state_grid(self): + """Update all cell values from current robot_state.""" + for key, value in self.server.robot_state.items(): + if isinstance(value, dict): + for sk, sv in value.items(): + cell = self._state_cells.get((key, sk)) + if cell: + try: + cell.config(text=f"{float(sv):>8.4f}") + except (ValueError, TypeError): + cell.config(text=str(sv)) + else: + cell = self._state_cells.get((key, None)) + if cell: + try: + cell.config(text=f"{float(value):>8.4f}") + except (ValueError, TypeError): + cell.config(text=str(value)) + + # ── actions ────────────────────────────────────────────────────── + + def _start(self): + result = self.server.start() + if result is True: + self.status_label.config(text="Online", fg=GREEN) + self.status_dot.itemconfig("dot", fill=GREEN) + self.start_btn.config(state="disabled", bg=BG_INPUT, cursor="arrow") + self.stop_btn.config(state="normal", cursor="hand2") + self._prev_packet_count = 0 + self._prev_time = time.time() + else: + messagebox.showerror("Error", + f"Port {self.server.port} is already in use.\n{result}") + + def _stop(self): + self.server.stop() + self.status_label.config(text="Offline", fg=RED) + self.status_dot.itemconfig("dot", fill=RED) + self.client_val.config(text="--") + self.pps_val.config(text="0") + self.start_btn.config(state="normal", bg=ACCENT, cursor="hand2") + self.stop_btn.config(state="disabled", cursor="arrow") + + def _set_mode(self, mode): + self.mode_var.set(mode) + self.server.receive_only = (mode == "Recv only") + + def _step_changed(self, event=None): + try: + self.server.correction_step = float(self.step_var.get()) + except ValueError: + self.step_var.set(str(self.server.correction_step)) + + def _adjust(self, axis: str, direction: int): + self.server.adjust_position(axis, direction * self.server.correction_step) + + def _reset(self): + self.server.reset_positions() + + # ── display loop ───────────────────────────────────────────────── + + def _update_display(self): + with self.server.lock: + # Position values + rist = self.server.robot_state.get("RIst", {}) + for axis, label in self.pos_labels.items(): + val = rist.get(axis, 0.0) + label.config(text=f"{val:+.4f}") + + # IPOC + self.ipoc_val.config(text=str(self.server.ipoc)) + + # Client + if self.server.client_address: + self.client_val.config( + text=f"{self.server.client_address[0]}:{self.server.client_address[1]}") + + # Packets/sec + now = time.time() + dt = now - self._prev_time + if dt >= 1.0: + delta = self.server.packet_count - self._prev_packet_count + self._packets_per_sec = delta / dt + self._prev_packet_count = self.server.packet_count + self._prev_time = now + self.pps_val.config(text=f"{self._packets_per_sec:.0f}") + + # Robot state grid + self._update_state_grid() + + # XML traffic (pretty-printed) + self._set_xml(self.recv_text, self.server.last_received_xml) + self._set_xml(self.sent_text, self.server.last_sent_xml) + + self.root.after(100, self._update_display) + + @staticmethod + def _set_text(widget, text): + widget.config(state="normal") + widget.delete("1.0", "end") + widget.insert("1.0", text) + widget.config(state="disabled") + + @staticmethod + def _set_xml(widget, xml_string): + """Pretty-print XML into a text widget.""" + formatted = xml_string + if xml_string.strip(): + try: + root = ET.fromstring(xml_string) + ET.indent(root) + formatted = ET.tostring(root, encoding="unicode") + except ET.ParseError: + pass + widget.config(state="normal") + widget.delete("1.0", "end") + widget.insert("1.0", formatted) + widget.config(state="disabled") + + def _on_close(self): + self.server.stop() + self.root.destroy() + + def run(self): + self.root.mainloop() + + +def main(): + parser = argparse.ArgumentParser(description="RSI TestServer - KUKA Robot Simulator") + parser.add_argument( + "--config", type=str, default="RSI_EthernetConfig.xml", + help="Path to RSI_EthernetConfig.xml" + ) + args = parser.parse_args() + + if not os.path.exists(args.config): + print(f"Error: Config file not found: {args.config}") + sys.exit(1) + + app = TestServerGUI(args.config) + app.run() + + +if __name__ == "__main__": + main() diff --git a/tests/__pycache__/test_safety_manager.cpython-313-pytest-8.4.1.pyc b/tests/__pycache__/test_safety_manager.cpython-313-pytest-8.4.1.pyc index c9b837858f8f7ca21f35ca96b280c51dffc0d2b5..dbf9a8f2065e1ce2ef87501e8b69ea8673b58886 100644 GIT binary patch delta 2486 zcmai0ZA?>F818Ayr9f}_s6Y!M1!>_*L5hMEE3K;{Lg|Ex3W$!6E9>N=o?9~UV}3*v zx6C>Bn#?UGZqc~-!_4g_%a-{AGJkBzmTVDmX_jo!CB_(+CHrCG7PIr7LdzI)H|f*U z=bZDN_v1P5?TydLi7$xwhbX3N*yBERFHFygR?>8?{K3%>2??#)Ejcu&#Z1jMjYU(c z38fZjLWVjmuGVPi63(SDEVRVLI+5Tzu_+Z-D3rj6%c9nrdI@pD+nLXV_<3+X%VVc0xW~fanr?w5 zMaNj1%|<2oE2~K;Mq54@c3GugMf2I-H+rWFD{pvji~b*trn9ZDw_Zs5VAZs-{E1Fu z+DPt-{u$8^r?X?E76x)k{0q}lO_NbKqYqmc4DIF5lF1-#MJ|YECf%6Ic|j;gio46((0}uW=^B=o%K-@`=(h4*$6oZxd?d-f)rCp zOBg5y1}rW7VY#`S+d@{tZ@F)>v;#j*OnrfY%EiH^E-Q9$ zMc8o#%8ynsQ1HISMG!|Og{iW&B&hyb2E@p-iv0V2x~#Cw4uUw zC(}WbHBMn`r1Zs-LD%7S4}uTDkFXJ80|N!yaWV%i1J^6v!b>>w5FS;2Tg;SjM|382 zFn2Ml?uyPC(W%~XzN*`qxGS^ew)uke5VJuKBk*gvXiRKYGQ}(Z&ro=A=@FP}Ea^9i^==|P9XF`enU}Q<_EaULk&ITl=^YQ zO({oBbInDKuVr(w4*O})9%j8orl-bvm8shLfwkDGVt1nix$QgC_m;QfFR_6O?7 zyYNw9YS!Y3wo5wnDRnlJaJ(~*nc3CeL@vVRc6P8j(y;<9U+C;0AHt)~Nlx9;LZpi_ z`hHhO-c|JZ7Q(!~A!1fZKlF8%FIy~+=Wsp3DY)8Qx0LV1pOS@QDPo5H@$hgoJgU$_ zc+$N}u;Tgv)bs?m@&6`Uw^YmQze*$;9%E~;2gI8_e@|c=;zi;oEa+4*gJ)!HaC}&9 aq%&+CVIO52KI>`K{jBi<*;T1WXZ{ASxgLT5 delta 2435 zcmai#YfMvT7{~jzcx+2gxz`rC9H6um3a+3~Do`$h$fX-FNyK6;(rFh3_MSEqoy#s{ zw>TMl%(84`Gnq^DgNw<@5>56&Gm~YvPbz4$EM_03OH6i6wk%n)_j!92aXU$WJ?D8Z z&-p)>_q=qOT)afY-$hZcVwd{8r^7i*qJ=as+3${itk3V5eCp3rnHsp)*;Y5h!{@i=Aqq*s7Ew_S+s2`!WlVKLQ4e# zmryJ07B2{Pb{n(6V@3;97;83~*rQAdbQPGDL&iyxRm%);YO@LMy_C*UiYw1+);N(h%qmF|wL@KApHPOocQ8GYwCp(-xGuKeG8h*JUmtu| zcc$rv!TzU4W$aiL+gHSPxSSUwPB@)k0he+OuC($#wg=tGaPTR96<+btU1%G|&C~w= z%I_9zXyX2C5Bb}A!%&mq?{E49n+!qqh;qB&B1y_zEHbqiaCb}v%|YZM3J`@1K8n$$ z2iY`s?dhbCb_T>!s>3ML zO$<8;qvnAE80lXb# zh#5wsGm-F-j6!j!9OUrHNj$Rkt50^;ms_0%3-O6jF zqe4C}|16qO_;A0<^7t0EkFSqnh!es6#rq$@Wz!rY&0>EPr^obRnZ;ywZZ0&P40ly? zOix2{D8dJLy*mtP8Ptl4-MBcrb8bEml;seOG}BhNU@f=t%BjK`)d*ggnCf`3#K}tL zH)|^yR-Dy9d_2CG0-r-mCvnj}L;$fL5k!O$QN$EtP0FUxhUf;-ZjPsHnx|}s-Anw+ zf_*{Lyqk^RWj}UVuOx3e6DJHa(nbV^nKmJs5iQVBCp}P&*0EH==@6UZ(61L8+KQ_^ zhz>*-q8s65ko64dkg$M!U*{H3vV0FpoZpl!dv1!hbU`G0*uE;-Rz#b!vHn@j{!~{b z@-B7y=yCM$9D?`Px{BDS!x%4t2bJ(t(-XKl(Ro#aj_kO)jN=r?G5!A|5l`;4L_8UX zTtfopQT_+Jw&D*#Wh`8rd}DHr*yI6ehuf~JS$5R>LE~^1(Zl`bV*Y73rtKPJCviiU zi+vhQE!CCycC6J>y>ZMKuS%)9W^R6~L#FjG+44+9yoF+#xp^1OM+JDW(HX=nVhjA- zvMDR6-OL(ALUUUC0zO)y*WF&pYh1?X2!kv#WT@qBe%9Scjw-8eiHv0wx$eHzRQ{eDW+{ZGCLhT9O|Gq**wC2 PGKCHPK^XJb@sH!btN{GV diff --git a/tests/test_config_parser.py b/tests/test_config_parser.py new file mode 100644 index 0000000..5e9d9c6 --- /dev/null +++ b/tests/test_config_parser.py @@ -0,0 +1,261 @@ +"""Tests for ConfigParser.""" +import pytest +import sys +import os + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'src')) + +from RSIPI.config_parser import ConfigParser + +# Path to the real config file in the project root +CONFIG_FILE = os.path.join(os.path.dirname(__file__), '..', 'RSI_EthernetConfig.xml') + + +@pytest.fixture +def parser(): + """Create a ConfigParser from the real config file.""" + return ConfigParser(CONFIG_FILE) + + +class TestParseWithoutError: + """Verify that the config file parses successfully.""" + + def test_parser_initializes(self, parser): + """ConfigParser should initialize without raising.""" + assert parser is not None + + def test_send_variables_is_dict(self, parser): + """send_variables should be a dict after parsing.""" + assert isinstance(parser.send_variables, dict) + + def test_receive_variables_is_dict(self, parser): + """receive_variables should be a dict after parsing.""" + assert isinstance(parser.receive_variables, dict) + + +class TestNetworkSettings: + """Verify network_settings extraction from .""" + + def test_has_ip(self, parser): + """network_settings must contain 'ip'.""" + assert "ip" in parser.network_settings + assert parser.network_settings["ip"] == "10.10.10.10" + + def test_has_port(self, parser): + """network_settings must contain 'port' as int.""" + assert "port" in parser.network_settings + assert parser.network_settings["port"] == 64000 + assert isinstance(parser.network_settings["port"], int) + + def test_has_sentype(self, parser): + """network_settings must contain 'sentype'.""" + assert "sentype" in parser.network_settings + assert parser.network_settings["sentype"] == "ImFree" + + def test_has_onlysend(self, parser): + """network_settings must contain 'onlysend' as bool.""" + assert "onlysend" in parser.network_settings + assert parser.network_settings["onlysend"] is False + + def test_get_network_settings_matches(self, parser): + """get_network_settings() should return the same dict.""" + assert parser.get_network_settings() == parser.network_settings + + +class TestSendVariables: + """Verify send_variables contains expected keys from the config file.""" + + def test_contains_rist(self, parser): + """RIst (robot actual position) should be in send_variables.""" + assert "RIst" in parser.send_variables + + def test_rist_has_cartesian_keys(self, parser): + """RIst should expand to X, Y, Z, A, B, C sub-keys.""" + rist = parser.send_variables["RIst"] + assert isinstance(rist, dict) + for axis in ["X", "Y", "Z", "A", "B", "C"]: + assert axis in rist + + def test_contains_rsol(self, parser): + """RSol (robot commanded position) should be in send_variables.""" + assert "RSol" in parser.send_variables + + def test_contains_ipoc(self, parser): + """IPOC timestamp should always be in send_variables.""" + assert "IPOC" in parser.send_variables + + def test_contains_delay(self, parser): + """Delay should be in send_variables.""" + assert "Delay" in parser.send_variables + + def test_contains_digout(self, parser): + """Digout group should be parsed from dotted tags.""" + assert "Digout" in parser.send_variables + + def test_digout_has_channels(self, parser): + """Digout should contain o1, o2, o3 sub-keys from the config.""" + digout = parser.send_variables["Digout"] + assert isinstance(digout, dict) + for ch in ["o1", "o2", "o3"]: + assert ch in digout + + def test_contains_source1(self, parser): + """Source1 should be in send_variables as a DOUBLE default.""" + assert "Source1" in parser.send_variables + assert parser.send_variables["Source1"] == 0.0 + + def test_contains_dil(self, parser): + """DiL should be in send_variables as a LONG default.""" + assert "DiL" in parser.send_variables + assert parser.send_variables["DiL"] == 0 + + +class TestReceiveVariables: + """Verify receive_variables contains expected keys.""" + + def test_contains_rkorr(self, parser): + """RKorr (correction values) should be in receive_variables.""" + assert "RKorr" in parser.receive_variables + + def test_rkorr_has_cartesian_keys(self, parser): + """RKorr should have X, Y, Z, A, B, C from individual tags.""" + rkorr = parser.receive_variables["RKorr"] + assert isinstance(rkorr, dict) + for axis in ["X", "Y", "Z", "A", "B", "C"]: + assert axis in rkorr + + def test_contains_estr(self, parser): + """EStr should be in receive_variables.""" + assert "EStr" in parser.receive_variables + + def test_contains_free(self, parser): + """FREE should be in receive_variables.""" + assert "FREE" in parser.receive_variables + + def test_contains_dio(self, parser): + """DiO should be in receive_variables.""" + assert "DiO" in parser.receive_variables + + +class TestDefPrefixStripping: + """Verify that DEF_ prefix is stripped from tags.""" + + def test_rist_not_def_rist(self, parser): + """Key should be 'RIst', not 'DEF_RIst'.""" + assert "DEF_RIst" not in parser.send_variables + assert "RIst" in parser.send_variables + + def test_rsol_not_def_rsol(self, parser): + """Key should be 'RSol', not 'DEF_RSol'.""" + assert "DEF_RSol" not in parser.send_variables + assert "RSol" in parser.send_variables + + def test_estr_not_def_estr(self, parser): + """Key should be 'EStr', not 'DEF_EStr'.""" + assert "DEF_EStr" not in parser.receive_variables + assert "EStr" in parser.receive_variables + + def test_tech_not_def_tech(self, parser): + """Tech keys should not retain DEF_ prefix.""" + for key in parser.send_variables: + assert not key.startswith("DEF_") + for key in parser.receive_variables: + assert not key.startswith("DEF_") + + +class TestIPOCAlwaysPresent: + """IPOC must always be in send_variables, even if not in config.""" + + def test_ipoc_present(self, parser): + """IPOC should be in send_variables.""" + assert "IPOC" in parser.send_variables + + def test_ipoc_forced_when_missing(self, tmp_path): + """IPOC should be injected even if the config has no DEF_IPOC.""" + # Create a minimal config with no IPOC element + minimal_xml = tmp_path / "minimal.xml" + minimal_xml.write_text(""" + + 127.0.0.1 + 12345 + Test + FALSE + + + + + + + + """) + p = ConfigParser(str(minimal_xml)) + assert "IPOC" in p.send_variables + + +class TestTechKeyMerging: + """Verify that Tech.CX and Tech.TX keys merge into a single 'Tech' dict.""" + + def test_tech_merged_in_send(self, parser): + """Tech.C1 in send config should merge into a single 'Tech' key.""" + assert "Tech" in parser.send_variables + # Should not have the dotted form + for key in parser.send_variables: + assert not key.startswith("Tech.") + + def test_tech_merged_in_receive(self, parser): + """Tech.T2 in receive config should merge into a single 'Tech' key.""" + assert "Tech" in parser.receive_variables + for key in parser.receive_variables: + assert not key.startswith("Tech.") + + def test_tech_contains_sub_keys(self, parser): + """Merged Tech dict should contain the sub-keys from the internal structure.""" + tech_send = parser.send_variables["Tech"] + assert isinstance(tech_send, dict) + # Tech.C1 should have expanded to C11, C12, ... C110 + assert "C11" in tech_send + + def test_tech_receive_contains_sub_keys(self, parser): + """Tech in receive should have T2x keys from Tech.T2.""" + tech_recv = parser.receive_variables["Tech"] + assert isinstance(tech_recv, dict) + assert "T21" in tech_recv + + +class TestGetDefaultValue: + """Test the static get_default_value helper.""" + + def test_bool_default(self): + assert ConfigParser.get_default_value("BOOL") is False + + def test_string_default(self): + assert ConfigParser.get_default_value("STRING") == "" + + def test_long_default(self): + assert ConfigParser.get_default_value("LONG") == 0 + + def test_double_default(self): + assert ConfigParser.get_default_value("DOUBLE") == 0.0 + + def test_unknown_default(self): + assert ConfigParser.get_default_value("UNKNOWN") is None + + +class TestRenameTechKeys: + """Test the static rename_tech_keys helper.""" + + def test_merges_tech_keys(self): + d = {"Tech.C1": {"C11": 0}, "Tech.T2": {"T21": 0}, "Other": 5} + ConfigParser.rename_tech_keys(d) + assert "Tech" in d + assert "Tech.C1" not in d + assert "Tech.T2" not in d + assert "C11" in d["Tech"] + assert "T21" in d["Tech"] + assert d["Other"] == 5 + + def test_no_tech_keys_leaves_dict_unchanged(self): + d = {"RIst": {"X": 0}, "IPOC": 0} + ConfigParser.rename_tech_keys(d) + assert "Tech" not in d + assert "RIst" in d diff --git a/tests/test_io_api.py b/tests/test_io_api.py new file mode 100644 index 0000000..98183e4 --- /dev/null +++ b/tests/test_io_api.py @@ -0,0 +1,184 @@ +"""Tests for IOAPI.""" +import pytest +import sys +import os +from unittest.mock import MagicMock + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'src')) + +from RSIPI.io_api import IOAPI +from RSIPI.exceptions import RSIVariableError + + +def _make_mock_client(send_vars=None, receive_vars=None): + """ + Build a mock RSIClient with plain dict send/receive variables + and a safety_manager that passes everything through. + """ + client = MagicMock() + client.send_variables = send_vars if send_vars is not None else {} + client.receive_variables = receive_vars if receive_vars is not None else {} + + # Safety manager: validate returns the value unchanged + client.safety_manager.validate.side_effect = lambda path, val: val + + return client + + +@pytest.fixture +def io_with_digout(): + """IOAPI with Digout in receive_variables and Digin in send_variables.""" + send = { + "Digin": {"i1": 1, "i2": 0, "i3": 1}, + "IPOC": 12345, + } + recv = { + "Digout": {"o1": 0, "o2": 0, "o3": 0}, + "DiO": 0, + } + client = _make_mock_client(send_vars=send, receive_vars=recv) + return IOAPI(client) + + +# --------------------------------------------------------------------------- +# toggle +# --------------------------------------------------------------------------- +class TestToggle: + """Tests for IOAPI.toggle().""" + + def test_toggle_sets_value_via_update_variable(self, io_with_digout): + """toggle should call _tools.update_variable with group.name and int state.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "Updated Digout.o1 to 1" + + result = io.toggle("Digout", "o1", True) + + io._tools.update_variable.assert_called_once_with("Digout.o1", 1) + assert result == "Updated Digout.o1 to 1" + + def test_toggle_false_sends_zero(self, io_with_digout): + """toggle with False should send 0.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "Updated Digout.o2 to 0" + + io.toggle("Digout", "o2", False) + io._tools.update_variable.assert_called_once_with("Digout.o2", 0) + + def test_toggle_int_one(self, io_with_digout): + """toggle with int 1 should send 1.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "Updated Digout.o3 to 1" + + io.toggle("Digout", "o3", 1) + io._tools.update_variable.assert_called_once_with("Digout.o3", 1) + + def test_toggle_truthy_value_normalizes_to_one(self, io_with_digout): + """Any truthy value should normalize to 1.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "ok" + + io.toggle("Digout", "o1", 42) + io._tools.update_variable.assert_called_once_with("Digout.o1", 1) + + +# --------------------------------------------------------------------------- +# set_output +# --------------------------------------------------------------------------- +class TestSetOutput: + """Tests for IOAPI.set_output().""" + + def test_formats_channel_name(self, io_with_digout): + """set_output(3, True) should call toggle with 'o3'.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "Updated Digout.o3 to 1" + + io.set_output(3, True) + io._tools.update_variable.assert_called_once_with("Digout.o3", 1) + + def test_custom_group(self, io_with_digout): + """set_output with custom group should use that group.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "Updated DiO.o5 to 1" + + io.set_output(5, True, group="DiO") + io._tools.update_variable.assert_called_once_with("DiO.o5", 1) + + def test_set_output_off(self, io_with_digout): + """set_output with False should send 0.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "Updated Digout.o1 to 0" + + io.set_output(1, False) + io._tools.update_variable.assert_called_once_with("Digout.o1", 0) + + +# --------------------------------------------------------------------------- +# get_input +# --------------------------------------------------------------------------- +class TestGetInput: + """Tests for IOAPI.get_input().""" + + def test_reads_true_from_send_variables(self, io_with_digout): + """get_input should read from send_variables (what robot sends us).""" + result = io_with_digout.get_input(1, group="Digin") + assert result is True # i1 = 1 + + def test_reads_false_from_send_variables(self, io_with_digout): + """get_input for channel with value 0 should return False.""" + result = io_with_digout.get_input(2, group="Digin") + assert result is False # i2 = 0 + + def test_missing_group_raises(self, io_with_digout): + """get_input with non-existent group should raise RSIVariableError.""" + with pytest.raises(RSIVariableError, match="not found in send_variables"): + io_with_digout.get_input(1, group="NonExistent") + + def test_missing_channel_raises(self, io_with_digout): + """get_input with non-existent channel should raise RSIVariableError.""" + with pytest.raises(RSIVariableError, match="not found in group"): + io_with_digout.get_input(99, group="Digin") + + def test_default_group_is_digin(self): + """Default group should be 'Digin'.""" + send = {"Digin": {"i1": 1}} + client = _make_mock_client(send_vars=send) + io = IOAPI(client) + assert io.get_input(1) is True + + +# --------------------------------------------------------------------------- +# pulse (basic structure test, not timing) +# --------------------------------------------------------------------------- +class TestPulse: + """Tests for IOAPI.pulse().""" + + def test_pulse_calls_on_then_off(self, io_with_digout): + """pulse should turn output on then off.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "ok" + + result = io.pulse(1, duration=0.0) # zero duration for fast test + + calls = io._tools.update_variable.call_args_list + # First call: ON (value=1), second call: OFF (value=0) + assert calls[0].args == ("Digout.o1", 1) + assert calls[1].args == ("Digout.o1", 0) + assert "Pulse completed" in result + + def test_pulse_returns_message_with_duration(self, io_with_digout): + """Return string should include the duration.""" + io = io_with_digout + io._tools = MagicMock() + io._tools.update_variable.return_value = "ok" + + result = io.pulse(2, duration=0.0) + assert "0.0s" in result + assert "Digout.o2" in result diff --git a/tests/test_motion_api.py b/tests/test_motion_api.py new file mode 100644 index 0000000..73c62b9 --- /dev/null +++ b/tests/test_motion_api.py @@ -0,0 +1,439 @@ +"""Tests for MotionAPI static/pure methods.""" +import pytest +import math +import sys +import os + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'src')) + +from RSIPI.motion_api import ( + MotionAPI, + _calculate_distance, + _trapezoidal_profile, + _s_curve_profile, + _cubic_blend, + _find_blend_point, +) + + +# --------------------------------------------------------------------------- +# generate_arc +# --------------------------------------------------------------------------- +class TestGenerateArc: + """Tests for MotionAPI.generate_arc().""" + + def test_basic_arc_length(self): + """Arc with 50 steps should return 50 waypoints.""" + arc = MotionAPI.generate_arc( + center={"X": 0, "Y": 0, "Z": 0}, + radius=10.0, + start_angle=0, + end_angle=90, + steps=50, + ) + assert len(arc) == 50 + + def test_first_point_on_circle(self): + """First point should be at start_angle on the circle.""" + arc = MotionAPI.generate_arc( + center={"X": 100, "Y": 0, "Z": 0}, + radius=50.0, + start_angle=0, + end_angle=90, + steps=10, + ) + assert arc[0]["X"] == pytest.approx(150.0) + assert arc[0]["Y"] == pytest.approx(0.0) + + def test_last_point_at_end_angle(self): + """Last point should be at end_angle on the circle.""" + arc = MotionAPI.generate_arc( + center={"X": 0, "Y": 0, "Z": 0}, + radius=10.0, + start_angle=0, + end_angle=90, + steps=10, + ) + assert arc[-1]["X"] == pytest.approx(10.0 * math.cos(math.radians(90))) + assert arc[-1]["Y"] == pytest.approx(10.0 * math.sin(math.radians(90))) + + def test_xz_plane(self): + """Arc in XZ plane should vary X and Z, keep Y constant.""" + arc = MotionAPI.generate_arc( + center={"X": 0, "Y": 5, "Z": 0}, + radius=10.0, + start_angle=0, + end_angle=90, + steps=5, + plane="XZ", + ) + for pt in arc: + assert pt["Y"] == 5 + + def test_yz_plane(self): + """Arc in YZ plane should vary Y and Z, keep X constant.""" + arc = MotionAPI.generate_arc( + center={"X": 7, "Y": 0, "Z": 0}, + radius=10.0, + start_angle=0, + end_angle=180, + steps=5, + plane="YZ", + ) + for pt in arc: + assert pt["X"] == 7 + + def test_preserves_orientation(self): + """Orientation keys A, B, C should carry through from center.""" + arc = MotionAPI.generate_arc( + center={"X": 0, "Y": 0, "Z": 0, "A": 10, "B": 20, "C": 30}, + radius=5.0, + start_angle=0, + end_angle=45, + steps=3, + ) + for pt in arc: + assert pt["A"] == 10 + assert pt["B"] == 20 + assert pt["C"] == 30 + + def test_invalid_plane_raises(self): + with pytest.raises(ValueError, match="Unknown plane"): + MotionAPI.generate_arc( + center={"X": 0, "Y": 0, "Z": 0}, + radius=5.0, start_angle=0, end_angle=90, steps=5, plane="AB" + ) + + def test_zero_radius_raises(self): + with pytest.raises(ValueError, match="positive"): + MotionAPI.generate_arc( + center={"X": 0, "Y": 0, "Z": 0}, + radius=0.0, start_angle=0, end_angle=90, steps=5 + ) + + def test_one_step_raises(self): + with pytest.raises(ValueError, match="at least 2"): + MotionAPI.generate_arc( + center={"X": 0, "Y": 0, "Z": 0}, + radius=5.0, start_angle=0, end_angle=90, steps=1 + ) + + +# --------------------------------------------------------------------------- +# generate_circle +# --------------------------------------------------------------------------- +class TestGenerateCircle: + """Tests for MotionAPI.generate_circle().""" + + def test_returns_360_degree_arc(self): + """Circle should span full 360 degrees.""" + circle = MotionAPI.generate_circle( + center={"X": 0, "Y": 0, "Z": 0}, + radius=10.0, + steps=100, + ) + assert len(circle) == 100 + # First and last points should be nearly identical (full revolution) + assert circle[0]["X"] == pytest.approx(circle[-1]["X"], abs=0.5) + assert circle[0]["Y"] == pytest.approx(circle[-1]["Y"], abs=0.5) + + def test_circle_points_on_radius(self): + """All points should lie on the specified radius from center.""" + cx, cy = 50.0, 30.0 + radius = 20.0 + circle = MotionAPI.generate_circle( + center={"X": cx, "Y": cy, "Z": 0}, + radius=radius, + steps=36, + ) + for pt in circle: + dist = math.sqrt((pt["X"] - cx) ** 2 + (pt["Y"] - cy) ** 2) + assert dist == pytest.approx(radius, abs=1e-10) + + +# --------------------------------------------------------------------------- +# generate_spiral +# --------------------------------------------------------------------------- +class TestGenerateSpiral: + """Tests for MotionAPI.generate_spiral().""" + + def test_expanding_spiral_radius_increases(self): + """Radius should increase from start_radius to end_radius.""" + spiral = MotionAPI.generate_spiral( + center={"X": 0, "Y": 0, "Z": 0}, + start_radius=5.0, + end_radius=50.0, + pitch=0.0, + revolutions=2, + steps=50, + ) + first_r = math.sqrt(spiral[0]["X"] ** 2 + spiral[0]["Y"] ** 2) + last_r = math.sqrt(spiral[-1]["X"] ** 2 + spiral[-1]["Y"] ** 2) + assert last_r > first_r + + def test_contracting_spiral_radius_decreases(self): + """Radius should decrease when end_radius < start_radius.""" + spiral = MotionAPI.generate_spiral( + center={"X": 0, "Y": 0, "Z": 0}, + start_radius=50.0, + end_radius=5.0, + pitch=0.0, + revolutions=2, + steps=50, + ) + first_r = math.sqrt(spiral[0]["X"] ** 2 + spiral[0]["Y"] ** 2) + last_r = math.sqrt(spiral[-1]["X"] ** 2 + spiral[-1]["Y"] ** 2) + assert last_r < first_r + + def test_pitch_adds_height(self): + """Positive pitch should increase Z across the spiral.""" + spiral = MotionAPI.generate_spiral( + center={"X": 0, "Y": 0, "Z": 100}, + start_radius=10.0, + end_radius=10.0, + pitch=20.0, + revolutions=1, + steps=20, + ) + assert spiral[-1]["Z"] > spiral[0]["Z"] + + def test_step_count(self): + """Should return exactly the requested number of steps.""" + spiral = MotionAPI.generate_spiral( + center={"X": 0, "Y": 0, "Z": 0}, + start_radius=10.0, + end_radius=20.0, + pitch=5.0, + steps=33, + ) + assert len(spiral) == 33 + + def test_too_few_steps_raises(self): + with pytest.raises(ValueError, match="at least 2"): + MotionAPI.generate_spiral( + center={"X": 0, "Y": 0, "Z": 0}, + start_radius=5, end_radius=10, pitch=1, steps=1 + ) + + +# --------------------------------------------------------------------------- +# blend_trajectories +# --------------------------------------------------------------------------- +class TestBlendTrajectories: + """Tests for MotionAPI.blend_trajectories().""" + + @staticmethod + def _line(start_x, end_x, n=20): + """Helper: straight line along X.""" + return [{"X": start_x + i * (end_x - start_x) / (n - 1), "Y": 0, "Z": 0} for i in range(n)] + + def test_blended_length(self): + """Blended trajectory should have points from both segments plus blend zone.""" + t1 = self._line(0, 100, 20) + t2 = self._line(100, 200, 20) + blended = MotionAPI.blend_trajectories(t1, t2, blend_radius=10.0, blend_steps=10) + # Should have some points; exact count depends on blend_radius vs traj length + assert len(blended) > 0 + + def test_blended_starts_at_traj1_start(self): + """First point of blended should match first point of traj1.""" + t1 = self._line(0, 100, 20) + t2 = self._line(100, 200, 20) + blended = MotionAPI.blend_trajectories(t1, t2, blend_radius=5.0, blend_steps=10) + assert blended[0]["X"] == pytest.approx(0.0) + + def test_blended_ends_at_traj2_end(self): + """Last point of blended should match last point of traj2.""" + t1 = self._line(0, 100, 20) + t2 = self._line(100, 200, 20) + blended = MotionAPI.blend_trajectories(t1, t2, blend_radius=5.0, blend_steps=10) + assert blended[-1]["X"] == pytest.approx(200.0) + + def test_empty_traj_raises(self): + with pytest.raises(ValueError, match="non-empty"): + MotionAPI.blend_trajectories([], [{"X": 0}], blend_radius=1.0) + + def test_zero_blend_radius_raises(self): + with pytest.raises(ValueError, match="positive"): + MotionAPI.blend_trajectories( + [{"X": 0}], [{"X": 1}], blend_radius=0.0 + ) + + +# --------------------------------------------------------------------------- +# transform_coordinates +# --------------------------------------------------------------------------- +class TestTransformCoordinates: + """Tests for MotionAPI.transform_coordinates().""" + + def test_no_offset_returns_copy(self): + """No frame_offset should return a copy of the input pose.""" + pose = {"X": 10, "Y": 20, "Z": 30} + result = MotionAPI.transform_coordinates(pose) + assert result == pose + # Should be a copy, not the same object + assert result is not pose + + def test_translational_offset(self): + """Frame offset should add to position values.""" + pose = {"X": 100, "Y": 0, "Z": 500} + offset = {"X": 500, "Y": 200, "Z": 0} + result = MotionAPI.transform_coordinates(pose, frame_offset=offset) + assert result["X"] == 600 + assert result["Y"] == 200 + assert result["Z"] == 500 + + def test_rotational_offset(self): + """Frame offset should add to orientation values.""" + pose = {"X": 0, "Y": 0, "Z": 0, "A": 10, "B": 20, "C": 30} + offset = {"A": 5, "B": -10, "C": 15} + result = MotionAPI.transform_coordinates(pose, frame_offset=offset) + assert result["A"] == 15 + assert result["B"] == 10 + assert result["C"] == 45 + + def test_partial_offset(self): + """Offset with fewer keys should only affect matching axes.""" + pose = {"X": 100, "Y": 200, "Z": 300} + offset = {"X": 10} + result = MotionAPI.transform_coordinates(pose, frame_offset=offset) + assert result["X"] == 110 + assert result["Y"] == 200 # unchanged + assert result["Z"] == 300 # unchanged + + +# --------------------------------------------------------------------------- +# generate_velocity_profile +# --------------------------------------------------------------------------- +class TestGenerateVelocityProfile: + """Tests for MotionAPI.generate_velocity_profile().""" + + @staticmethod + def _simple_traj(n=50): + """Evenly spaced points along X.""" + return [{"X": i * 1.0, "Y": 0, "Z": 0} for i in range(n)] + + def test_trapezoidal_returns_correct_length(self): + traj = self._simple_traj(50) + result = MotionAPI.generate_velocity_profile(traj, profile='trapezoidal') + assert len(result) == 50 + + def test_scurve_returns_correct_length(self): + traj = self._simple_traj(50) + result = MotionAPI.generate_velocity_profile(traj, profile='s-curve') + assert len(result) == 50 + + def test_trapezoidal_starts_and_ends_near_zero(self): + """Velocity should be near zero at start and end.""" + traj = self._simple_traj(100) + result = MotionAPI.generate_velocity_profile( + traj, max_velocity=10.0, max_acceleration=5.0, profile='trapezoidal' + ) + _, v_first = result[0] + _, v_last = result[-1] + assert v_first == pytest.approx(0.0, abs=0.5) + assert v_last == pytest.approx(0.0, abs=0.5) + + def test_scurve_starts_and_ends_near_zero(self): + traj = self._simple_traj(100) + result = MotionAPI.generate_velocity_profile( + traj, max_velocity=10.0, max_acceleration=5.0, profile='s-curve' + ) + _, v_first = result[0] + _, v_last = result[-1] + assert v_first == pytest.approx(0.0, abs=0.5) + assert v_last == pytest.approx(0.0, abs=0.5) + + def test_empty_trajectory(self): + assert MotionAPI.generate_velocity_profile([]) == [] + + def test_single_point(self): + result = MotionAPI.generate_velocity_profile([{"X": 0}]) + assert len(result) == 1 + assert result[0][1] == 0.0 + + def test_unknown_profile_raises(self): + traj = self._simple_traj(10) + with pytest.raises(ValueError, match="Unknown profile"): + MotionAPI.generate_velocity_profile(traj, profile='banana') + + def test_trapezoidal_velocity_does_not_exceed_max(self): + """No velocity should exceed max_velocity.""" + traj = self._simple_traj(100) + max_v = 5.0 + result = MotionAPI.generate_velocity_profile( + traj, max_velocity=max_v, max_acceleration=2.0, profile='trapezoidal' + ) + for _, v in result: + assert v <= max_v + 1e-9 + + +# --------------------------------------------------------------------------- +# _cubic_blend edge cases +# --------------------------------------------------------------------------- +class TestCubicBlend: + """Tests for _cubic_blend helper.""" + + def test_steps_one_no_crash(self): + """steps=1 should not cause division by zero.""" + result = _cubic_blend({"X": 0, "Y": 0}, {"X": 10, "Y": 10}, steps=1) + assert len(result) == 1 + + def test_steps_two(self): + """steps=2 should interpolate from p1 to p2.""" + result = _cubic_blend({"X": 0}, {"X": 100}, steps=2) + assert len(result) == 2 + assert result[0]["X"] == pytest.approx(0.0) + assert result[-1]["X"] == pytest.approx(100.0) + + def test_midpoint_is_blended(self): + """Midpoint (t=0.5) of cubic Hermite with zero velocities should be average.""" + result = _cubic_blend({"X": 0}, {"X": 100}, steps=3) + # At t=0.5: h1=0.5, h2=0.5 -> blended = 50 + assert result[1]["X"] == pytest.approx(50.0) + + +# --------------------------------------------------------------------------- +# _trapezoidal_profile edge cases +# --------------------------------------------------------------------------- +class TestTrapezoidalProfileEdgeCases: + """Edge-case tests for the _trapezoidal_profile helper.""" + + def test_single_distance(self): + """Profile with one distance segment should not crash.""" + velocities = _trapezoidal_profile([10.0], 10.0, 5.0, 2.0) + assert len(velocities) == 2 # n = len(distances) + 1 + + def test_zero_total_distance(self): + """Zero total distance should produce zero velocities without error.""" + velocities = _trapezoidal_profile([0.0], 0.0, 5.0, 2.0) + assert all(v == 0.0 for v in velocities) + + def test_triangular_profile_triggered(self): + """Short distance forces triangular profile (no constant phase).""" + # accel_distance = v^2 / (2*a) = 100/4 = 25 + # total = 10 < 2*25 => triangular + velocities = _trapezoidal_profile([5.0, 5.0], 10.0, 10.0, 2.0) + assert len(velocities) == 3 + + +# --------------------------------------------------------------------------- +# _calculate_distance +# --------------------------------------------------------------------------- +class TestCalculateDistance: + """Tests for _calculate_distance helper.""" + + def test_zero_distance(self): + assert _calculate_distance({"X": 0, "Y": 0}, {"X": 0, "Y": 0}) == 0.0 + + def test_unit_distance(self): + assert _calculate_distance({"X": 0}, {"X": 1}) == pytest.approx(1.0) + + def test_3d_distance(self): + d = _calculate_distance({"X": 0, "Y": 0, "Z": 0}, {"X": 3, "Y": 4, "Z": 0}) + assert d == pytest.approx(5.0) + + def test_ignores_non_motion_keys(self): + """Keys not in the motion set should be ignored.""" + d = _calculate_distance({"X": 0, "FOO": 0}, {"X": 3, "FOO": 1000}) + assert d == pytest.approx(3.0) diff --git a/tests/test_safety_manager.py b/tests/test_safety_manager.py index e6cc04e..73319e1 100644 --- a/tests/test_safety_manager.py +++ b/tests/test_safety_manager.py @@ -6,6 +6,7 @@ import os sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'src')) from RSIPI.safety_manager import SafetyManager +from RSIPI.exceptions import RSILimitExceeded, RSIEmergencyStop class TestValidate: @@ -32,15 +33,15 @@ class TestValidate: limits = {"RKorr.X": (-5.0, 5.0)} sm = SafetyManager(limits) - with pytest.raises(ValueError, match="out of bounds"): + with pytest.raises(RSILimitExceeded, match="out of bounds"): sm.validate("RKorr.X", 5.1) def test_validate_below_min(self): - """Test that values below min raise ValueError.""" + """Test that values below min raise RSILimitExceeded.""" limits = {"RKorr.X": (-5.0, 5.0)} sm = SafetyManager(limits) - with pytest.raises(ValueError, match="out of bounds"): + with pytest.raises(RSILimitExceeded, match="out of bounds"): sm.validate("RKorr.X", -5.1) def test_validate_unlisted_path(self): @@ -71,7 +72,7 @@ class TestEmergencyStop: sm = SafetyManager() sm.emergency_stop() - with pytest.raises(RuntimeError, match="E-STOP"): + with pytest.raises(RSIEmergencyStop, match="E-STOP"): sm.validate("RKorr.X", 0.0) def test_estop_reset(self): @@ -99,7 +100,7 @@ class TestSetLimit: sm.set_limit("RKorr.Y", -10.0, 10.0) assert sm.validate("RKorr.Y", 5.0) == 5.0 - with pytest.raises(ValueError): + with pytest.raises(RSILimitExceeded): sm.validate("RKorr.Y", 15.0) def test_override_existing_limit(self): @@ -108,7 +109,7 @@ class TestSetLimit: sm = SafetyManager(limits) # Original limit blocks this - with pytest.raises(ValueError): + with pytest.raises(RSILimitExceeded): sm.validate("RKorr.X", 8.0) # Override limit