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 0000000..e65e628 Binary files /dev/null and b/TestServer.exe differ diff --git a/examples/advanced_motion/01_velocity_profiles.py b/examples/advanced_motion/01_velocity_profiles.py index 88387d5..b775720 100644 --- a/examples/advanced_motion/01_velocity_profiles.py +++ b/examples/advanced_motion/01_velocity_profiles.py @@ -174,4 +174,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/advanced_motion/02_geometric_primitives.py b/examples/advanced_motion/02_geometric_primitives.py index 63fc424..6fb2821 100644 --- a/examples/advanced_motion/02_geometric_primitives.py +++ b/examples/advanced_motion/02_geometric_primitives.py @@ -221,4 +221,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/advanced_motion/03_path_blending.py b/examples/advanced_motion/03_path_blending.py index 97bf097..181512d 100644 --- a/examples/advanced_motion/03_path_blending.py +++ b/examples/advanced_motion/03_path_blending.py @@ -278,4 +278,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/advanced_motion/04_coordinate_transforms.py b/examples/advanced_motion/04_coordinate_transforms.py index 6a26344..2b49b3b 100644 --- a/examples/advanced_motion/04_coordinate_transforms.py +++ b/examples/advanced_motion/04_coordinate_transforms.py @@ -302,4 +302,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/advanced_motion/05_combined_motion.py b/examples/advanced_motion/05_combined_motion.py index 2614f9d..4374304 100644 --- a/examples/advanced_motion/05_combined_motion.py +++ b/examples/advanced_motion/05_combined_motion.py @@ -394,4 +394,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/coordination/01_basic_handshake.py b/examples/coordination/01_basic_handshake.py index efcf2e9..306ce5f 100644 --- a/examples/coordination/01_basic_handshake.py +++ b/examples/coordination/01_basic_handshake.py @@ -110,4 +110,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/coordination/02_parameter_passing.py b/examples/coordination/02_parameter_passing.py index 1000488..19745a1 100644 --- a/examples/coordination/02_parameter_passing.py +++ b/examples/coordination/02_parameter_passing.py @@ -130,4 +130,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/coordination/03_state_machine.py b/examples/coordination/03_state_machine.py index 360639c..888d0d7 100644 --- a/examples/coordination/03_state_machine.py +++ b/examples/coordination/03_state_machine.py @@ -205,4 +205,6 @@ def main(): if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() main() diff --git a/examples/example_01_start_stop.py b/examples/example_01_start_stop.py index e8e1b44..3dd48c9 100644 --- a/examples/example_01_start_stop.py +++ b/examples/example_01_start_stop.py @@ -1,11 +1,15 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() - -rsi.start_rsi() - -print("RSI connection started. Press Enter to stop.") -input() - -rsi.stop_rsi() -print("RSI connection stopped.") +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + + api.start() + + print("RSI connection started. Press Enter to stop.") + input() + + api.stop() + print("RSI connection stopped.") diff --git a/examples/example_02_send_cartesian.py b/examples/example_02_send_cartesian.py index ae120fd..f56d1c3 100644 --- a/examples/example_02_send_cartesian.py +++ b/examples/example_02_send_cartesian.py @@ -1,9 +1,13 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() -rsi.start_rsi() - -# Move TCP 50mm along X-axis -rsi.update_cartesian(x=50, y=0, z=0) - -rsi.stop_rsi() +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + api.start() + + # Move TCP 50mm along X-axis + api.motion.update_cartesian(X=50, Y=0, Z=0) + + api.stop() diff --git a/examples/example_03_send_joint.py b/examples/example_03_send_joint.py index d9f7712..a1ccd38 100644 --- a/examples/example_03_send_joint.py +++ b/examples/example_03_send_joint.py @@ -1,9 +1,13 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() -rsi.start_rsi() - -# Move Joint A1 by 10 degrees -rsi.update_joints(a1=10) - -rsi.stop_rsi() +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + api.start() + + # Move Joint A1 by 10 degrees + api.motion.update_joints(A1=10) + + api.stop() diff --git a/examples/example_04_external_axes.py b/examples/example_04_external_axes.py index f1e0b19..e3ed65e 100644 --- a/examples/example_04_external_axes.py +++ b/examples/example_04_external_axes.py @@ -1,9 +1,13 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() -rsi.start_rsi() - -# Move external axis E1 by 100mm -rsi.update_external(e1=100) - -rsi.stop_rsi() +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + api.start() + + # Move external axis E1 by 100mm + api.motion.move_external_axis('E1', 100) + + api.stop() diff --git a/examples/example_05_digital_io.py b/examples/example_05_digital_io.py index 4fb323a..89ccbdb 100644 --- a/examples/example_05_digital_io.py +++ b/examples/example_05_digital_io.py @@ -1,9 +1,13 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() -rsi.start_rsi() - -# Set digital output (e.g., to open gripper) -rsi.update_digital_io(125) # Example binary pattern - -rsi.stop_rsi() +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + api.start() + + # Set digital output (e.g., to open gripper) + api.io.set_output(1, True) + + api.stop() diff --git a/examples/example_06_logging_csv.py b/examples/example_06_logging_csv.py index 9dd35b6..bf2147d 100644 --- a/examples/example_06_logging_csv.py +++ b/examples/example_06_logging_csv.py @@ -1,11 +1,15 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() -rsi.enable_csv_logging() - -rsi.start_rsi() - -print("Logging robot data to CSV. Press Enter to stop.") -input() - -rsi.stop_rsi() +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + api.logging.start() + + api.start() + + print("Logging robot data to CSV. Press Enter to stop.") + input() + + api.stop() diff --git a/examples/example_07_graphing_live.py b/examples/example_07_graphing_live.py index 72d2d6f..85e0468 100644 --- a/examples/example_07_graphing_live.py +++ b/examples/example_07_graphing_live.py @@ -1,11 +1,15 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() -rsi.enable_graphing() - -rsi.start_rsi() - -print("Live graphing started. Press Enter to stop.") -input() - -rsi.stop_rsi() +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + api.viz.start_live_plot() + + api.start() + + print("Live graphing started. Press Enter to stop.") + input() + + api.stop() diff --git a/examples/example_08_safety_limits.py b/examples/example_08_safety_limits.py index 8498fd7..e30000c 100644 --- a/examples/example_08_safety_limits.py +++ b/examples/example_08_safety_limits.py @@ -1,14 +1,18 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() - -# Set X axis soft limits -rsi.set_safety_limit(axis="X", min_value=-500, max_value=500) - -rsi.start_rsi() - -try: - while True: - pass -except KeyboardInterrupt: - rsi.stop_rsi() \ No newline at end of file +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + + # Set X axis soft limits + api.safety.set_limit(axis="X", min_value=-500, max_value=500) + + api.start() + + try: + while True: + pass + except KeyboardInterrupt: + api.stop() diff --git a/examples/example_09_trajectory_cartesian.py b/examples/example_09_trajectory_cartesian.py index 41c3a22..3ced454 100644 --- a/examples/example_09_trajectory_cartesian.py +++ b/examples/example_09_trajectory_cartesian.py @@ -1,20 +1,24 @@ -from RSIPI import rsi_api -import time - -rsi = rsi_api.RSIAPI() -rsi.start_rsi() - -# Plan simple trajectory -points = [ - {"x": 0, "y": 0, "z": 0}, - {"x": 50, "y": 0, "z": 0}, - {"x": 50, "y": 50, "z": 0}, - {"x": 0, "y": 50, "z": 0}, - {"x": 0, "y": 0, "z": 0} -] - -for point in points: - rsi.update_cartesian(**point) - time.sleep(0.5) - -rsi.stop_rsi() +from RSIPI import RSIAPI +import time + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + api.start() + + # Plan simple trajectory + points = [ + {"X": 0, "Y": 0, "Z": 0}, + {"X": 50, "Y": 0, "Z": 0}, + {"X": 50, "Y": 50, "Z": 0}, + {"X": 0, "Y": 50, "Z": 0}, + {"X": 0, "Y": 0, "Z": 0} + ] + + for point in points: + api.motion.update_cartesian(**point) + time.sleep(0.5) + + api.stop() diff --git a/examples/example_10_shutdown_safe.py b/examples/example_10_shutdown_safe.py index 3d413b2..a82f5ba 100644 --- a/examples/example_10_shutdown_safe.py +++ b/examples/example_10_shutdown_safe.py @@ -1,14 +1,18 @@ -from RSIPI import rsi_api - -rsi = rsi_api.RSIAPI() - -try: - rsi.start_rsi() - print("Press Ctrl+C to stop RSI safely.") - while True: - pass - -except KeyboardInterrupt: - print("\nEmergency stop triggered.") - rsi.safety_stop() - rsi.stop_rsi() +from RSIPI import RSIAPI + +if __name__ == '__main__': + from multiprocessing import freeze_support + freeze_support() + + api = RSIAPI() + + try: + api.start() + print("Press Ctrl+C to stop RSI safely.") + while True: + pass + + except KeyboardInterrupt: + print("\nEmergency stop triggered.") + api.safety.stop() + api.stop() diff --git a/main.py b/main.py index 647a4eb..0619403 100644 --- a/main.py +++ b/main.py @@ -10,7 +10,7 @@ Usage: """ import argparse -import time +import time # noqa: F401 - used by commented examples from multiprocessing import freeze_support from RSIPI import RSIAPI @@ -20,9 +20,23 @@ if __name__ == '__main__': parser = argparse.ArgumentParser(description="RSIPI Test Runner") parser.add_argument("--config", type=str, default="RSI_EthernetConfig.xml", help="Path to RSI config XML file") + parser.add_argument("--mode", type=str, default="relative", choices=["absolute", "relative"], + help="RSI correction mode (must match KRL program)") + parser.add_argument("--max-cart-rate", type=float, default=0.5, + help="Max Cartesian correction per cycle in mm (0 = no limit)") + parser.add_argument("--max-joint-rate", type=float, default=0.2, + help="Max joint correction per cycle in degrees (0 = no limit)") + parser.add_argument("--cycle-time", type=float, default=0.004, + help="RSI cycle time in seconds (0.004 = 4ms/250Hz, 0.012 = 12ms/83Hz)") args = parser.parse_args() - api = RSIAPI(args.config) + api = RSIAPI( + args.config, + rsi_mode=args.mode, + max_cartesian_rate=args.max_cart_rate, + max_joint_rate=args.max_joint_rate, + cycle_time=args.cycle_time + ) # ========================================================================= # Example 01: Start / Stop @@ -35,25 +49,24 @@ if __name__ == '__main__': # ========================================================================= # Example 02: Send Cartesian correction (move TCP 50mm along X) # ========================================================================= - api.start() - print("Waiting for robot connection...") - while not api.is_running(): - time.sleep(0.1) - # Wait for first IPOC to confirm packets are flowing - while api.monitoring.get_ipoc() == "N/A" or api.monitoring.get_ipoc() == 0: - time.sleep(0.1) - print(f"Connected. IPOC: {api.monitoring.get_ipoc()}") - api.motion.update_cartesian(X=0.01, Y=0, Z=0) - print("Sent Cartesian correction. Press Enter to stop.") - input() - api.stop() + # api.start() + # print("Waiting for robot connection...") + # if not api.wait_for_connection(timeout=10): + # print("Timeout waiting for robot. Exiting.") + # api.stop() + # exit(1) + # print(f"Connected. IPOC: {api.monitoring.get_ipoc()}") + # api.motion.update_cartesian(X=0.01, Y=0, Z=0) + # print("Sent Cartesian correction. Press Enter to stop.") + # input() + # api.stop() # ========================================================================= - # Example 03: Send Joint correction (move A1 by 10 degrees) + # Example 03: Send Joint correction (move A1 by 10 degrees) DOESNT WORK # ========================================================================= # api.start() # time.sleep(1) - # api.motion.update_joints(A1=10) + # api.motion.update_joints(A1=50) # print("Sent joint correction. Press Enter to stop.") # input() # api.stop() @@ -74,8 +87,9 @@ if __name__ == '__main__': # api.start() # time.sleep(1) # api.io.set_output(1, True) - # api.io.set_output(2, False) - # api.io.set_output(3, True) + # time.sleep(5) + # api.io.set_output(1, False) + # print("Set digital outputs. Press Enter to stop.") # input() # api.stop() @@ -207,39 +221,53 @@ if __name__ == '__main__': # Advanced Motion 01: Velocity Profiles (trapezoidal vs S-curve) # ========================================================================= # api.start() - # time.sleep(1) + # time.sleep(4) + # # Relative mode: each point is a per-cycle delta + # # 200 steps × 0.5mm = 100mm total at max rate limit # traj = api.motion.generate_trajectory( # {"X": 0, "Y": 0, "Z": 0}, # {"X": 100, "Y": 0, "Z": 0}, - # steps=50) - # trap_profile = api.motion.generate_velocity_profile( - # traj, max_velocity=50, max_acceleration=100, profile='trapezoidal') - # scurve_profile = api.motion.generate_velocity_profile( - # traj, max_velocity=50, max_acceleration=100, profile='s-curve') - # print(f"Trapezoidal: {len(trap_profile)} points") - # print(f"S-curve: {len(scurve_profile)} points") - # print("Press Enter to stop.") + # steps=200, + # mode="relative") + # print(f"Executing trajectory: {len(traj)} steps, {traj[0]['X']:.2f}mm per step") + # api.motion.execute_trajectory(traj, space="cartesian", rate=0.012) + # print("Trajectory executed. Press Enter to stop.") # input() # api.stop() # ========================================================================= # Advanced Motion 02: Geometric Primitives (arc, circle, spiral) # ========================================================================= - # api.start() - # time.sleep(1) - # arc = api.motion.generate_arc( - # center={"X": 0, "Y": 0, "Z": 0}, - # radius=50, start_angle=0, end_angle=90, steps=20) - # circle = api.motion.generate_circle( - # center={"X": 0, "Y": 0, "Z": 0}, - # radius=50, steps=36) - # spiral = api.motion.generate_spiral( - # center={"X": 0, "Y": 0, "Z": 0}, - # start_radius=10, end_radius=50, pitch=5.0, revolutions=3, steps=60) - # print(f"Arc: {len(arc)} pts, Circle: {len(circle)} pts, Spiral: {len(spiral)} pts") - # print("Press Enter to stop.") - # input() - # api.stop() + api.start() + print("Waiting for robot connection...") + if not api.wait_for_connection(timeout=10): + print("Timeout waiting for robot. Exiting.") + api.stop() + exit(1) + print(f"Connected. IPOC: {api.monitoring.get_ipoc()}") + input("Press Enter to start movement...") + + # Generate absolute circle waypoints, convert to relative deltas + circle_abs = api.motion.generate_circle( + center={"X": 0, "Y": 0, "Z": 0}, + radius=5, steps=200) + + # Convert absolute → relative (delta between consecutive points) + # Start prev at first point so there's no initial jump + circle_rel = [] + prev = circle_abs[0] + for pt in circle_abs[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)} relative steps, radius=5mm") + api.motion.execute_trajectory(circle_rel, space="cartesian", rate=0.012) + # Zero out corrections so robot stops moving in relative mode + api.motion.update_cartesian(X=0, Y=0, Z=0) + print("Circle complete. Press Enter to stop.") + input() + api.stop() # ========================================================================= # Advanced Motion 03: Path Blending (smooth corner transitions) diff --git a/rsi_config/RSIPI_Full.rsi b/rsi_config/RSIPI_Full.rsi new file mode 100644 index 0000000..4da4dec --- /dev/null +++ b/rsi_config/RSIPI_Full.rsi @@ -0,0 +1,517 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 25719b3..3c7f1c3 100644 Binary files a/src/__pycache__/__init__.cpython-313.pyc and b/src/__pycache__/__init__.cpython-313.pyc differ diff --git a/test_server.py b/test_server.py new file mode 100644 index 0000000..4f2a989 --- /dev/null +++ b/test_server.py @@ -0,0 +1,615 @@ +""" +RSI TestServer - Python equivalent of KUKA's RSI TestServer.exe + +Simulates the robot side of an RSI connection. Listens on UDP for 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 c9b8378..dbf9a8f 100644 Binary files a/tests/__pycache__/test_safety_manager.cpython-313-pytest-8.4.1.pyc and b/tests/__pycache__/test_safety_manager.cpython-313-pytest-8.4.1.pyc differ 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