refactor: revamp core API, network handler, XML processing, and examples
This commit is contained in:
parent
fbd8dfacdc
commit
4a9fc50ff3
8
.gitignore
vendored
Normal file
8
.gitignore
vendored
Normal file
@ -0,0 +1,8 @@
|
||||
# Python
|
||||
__pycache__/
|
||||
*.pyc
|
||||
|
||||
# Claude Code
|
||||
.claude/
|
||||
CLAUDE.md
|
||||
.claudeignore
|
||||
692
README.md
692
README.md
@ -1,178 +1,582 @@
|
||||
# RSIPI: Robot Sensor Interface - Python Integration
|
||||
# RSIPI: Robot Sensor Interface for Python
|
||||
|
||||
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.
|
||||
[](https://www.python.org/downloads/)
|
||||
[](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 is a powerful tool that directly interfaces with industrial robotic systems. Improper use can lead to dangerous movements, property damage, or personal injury.
|
||||
## Safety Notice
|
||||
|
||||
⚠️ 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.
|
||||
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.
|
||||
|
||||
---
|
||||
|
||||
## 📄 Description
|
||||
## Installation
|
||||
|
||||
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.
|
||||
Requires Python 3.10+.
|
||||
|
||||
### 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
|
||||
```
|
||||
# Development install (editable)
|
||||
pip install -e .
|
||||
|
||||
### Selected Commands
|
||||
| Command | Description |
|
||||
|---------|-------------|
|
||||
| `start` / `stop` | Start or stop RSI client. |
|
||||
| `set <var> <value>` | Update send variable. |
|
||||
| `get <var>` | 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 <type> <csv>` | Static plotting (position, velocity, deviation, etc.). |
|
||||
| `safety-stop`, `safety-reset`, `safety-status` | Emergency stop and limit management. |
|
||||
| `krlparse <src> <dat> <out>` | Parse KRL to CSV. |
|
||||
| `inject_rsi <src> [out] [config]` | Inject RSI code into KRL file. |
|
||||
# Or install dependencies directly
|
||||
pip install pandas>=2.0 numpy>=1.22 matplotlib>=3.5 lxml>=4.9 scipy>=1.8
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📃 Example Usage
|
||||
## Quick Start
|
||||
|
||||
### Update TCP position live
|
||||
```python
|
||||
api.start_rsi()
|
||||
api.update_variable('RKorr.X', 100.0)
|
||||
api.update_variable('RKorr.Y', 50.0)
|
||||
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
|
||||
```
|
||||
|
||||
### Plan and execute a Cartesian move
|
||||
Without the context manager:
|
||||
|
||||
```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)
|
||||
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()
|
||||
```
|
||||
|
||||
### CLI session sample
|
||||
---
|
||||
|
||||
## Configuration
|
||||
|
||||
RSIPI reads `RSI_EthernetConfig.xml` to determine network settings and which variables are exchanged with the robot.
|
||||
|
||||
```xml
|
||||
<ROOT>
|
||||
<CONFIG>
|
||||
<IP_NUMBER>10.10.10.10</IP_NUMBER> <!-- External PC IP -->
|
||||
<PORT>64000</PORT> <!-- UDP port -->
|
||||
<SENTYPE>ImFree</SENTYPE> <!-- XML root element name -->
|
||||
<ONLYSEND>FALSE</ONLYSEND> <!-- FALSE = bidirectional -->
|
||||
</CONFIG>
|
||||
|
||||
<!-- SEND: What the robot sends TO us (read-only from Python) -->
|
||||
<SEND>
|
||||
<ELEMENTS>
|
||||
<ELEMENT TAG="DEF_RIst" TYPE="DOUBLE" INDX="INTERNAL" /> <!-- TCP position -->
|
||||
<ELEMENT TAG="DEF_RSol" TYPE="DOUBLE" INDX="INTERNAL" /> <!-- Commanded position -->
|
||||
<ELEMENT TAG="DEF_Delay" TYPE="LONG" INDX="INTERNAL" /> <!-- Packet delay count -->
|
||||
<ELEMENT TAG="Digout.o1" TYPE="BOOL" INDX="2" /> <!-- Digital output state -->
|
||||
</ELEMENTS>
|
||||
</SEND>
|
||||
|
||||
<!-- RECEIVE: What the robot receives FROM us (writable from Python) -->
|
||||
<RECEIVE>
|
||||
<ELEMENTS>
|
||||
<ELEMENT TAG="RKorr.X" TYPE="DOUBLE" INDX="1" HOLDON="1" /> <!-- Cartesian correction -->
|
||||
<ELEMENT TAG="RKorr.Y" TYPE="DOUBLE" INDX="2" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.Z" TYPE="DOUBLE" INDX="3" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.A" TYPE="DOUBLE" INDX="4" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.B" TYPE="DOUBLE" INDX="5" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.C" TYPE="DOUBLE" INDX="6" HOLDON="1" />
|
||||
<ELEMENT TAG="DiO" TYPE="LONG" INDX="8" HOLDON="1" /> <!-- Digital I/O -->
|
||||
</ELEMENTS>
|
||||
</RECEIVE>
|
||||
</ROOT>
|
||||
```
|
||||
|
||||
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
|
||||
> 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
|
||||
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.
|
||||
|
||||
## 📅 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.
|
||||
### Running with pytest
|
||||
|
||||
---
|
||||
|
||||
## 🚀 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]}
|
||||
}
|
||||
pip install -e ".[dev]"
|
||||
pytest
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## ⚖️ License
|
||||
RSIPI is licensed under the MIT License.
|
||||
Test files go in the `tests/` directory. The project uses `src` layout with `pythonpath = ["src"]` configured in `pyproject.toml`.
|
||||
|
||||
---
|
||||
|
||||
## 🚧 Disclaimer
|
||||
RSIPI is intended for research and experimental purposes only. Always ensure safe operation with appropriate safety measures in place.
|
||||
## 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 <var> <value>`, `move_cartesian`, `log start`, `safety-stop`, etc.
|
||||
|
||||
---
|
||||
|
||||
## License
|
||||
|
||||
MIT
|
||||
|
||||
139
RSI_EthernetConfig_Full.xml
Normal file
139
RSI_EthernetConfig_Full.xml
Normal file
@ -0,0 +1,139 @@
|
||||
<ROOT>
|
||||
<CONFIG>
|
||||
<IP_NUMBER>10.10.10.10</IP_NUMBER>
|
||||
<PORT>64000</PORT>
|
||||
<SENTYPE>ImFree</SENTYPE>
|
||||
<ONLYSEND>FALSE</ONLYSEND>
|
||||
</CONFIG>
|
||||
|
||||
<!-- =================================================================
|
||||
RSI Channel Budget: 64 max across SEND + RECEIVE
|
||||
INTERNAL tags don't count toward the 64-channel limit
|
||||
|
||||
SEND channels used: 12 (DiL, Digout x3, Source1-4, Digin x4)
|
||||
RECEIVE channels used: 20 (RKorr x6, AKorr x6, DiO x4, FREE x4)
|
||||
Total: 32 / 64
|
||||
|
||||
All DEF_ tags are INTERNAL (free)
|
||||
================================================================= -->
|
||||
|
||||
<!-- ===================== SEND: Robot → PC ========================= -->
|
||||
<SEND>
|
||||
<ELEMENTS>
|
||||
<!-- Cartesian actual position (X,Y,Z,A,B,C in mm/deg) -->
|
||||
<ELEMENT TAG="DEF_RIst" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Cartesian setpoint position -->
|
||||
<ELEMENT TAG="DEF_RSol" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Robot axis actual positions (A1-A6 in deg) -->
|
||||
<ELEMENT TAG="DEF_AIPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Robot axis setpoint positions (A1-A6 in deg) -->
|
||||
<ELEMENT TAG="DEF_ASPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- External axis actual positions (E1-E6) -->
|
||||
<ELEMENT TAG="DEF_EIPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- External axis setpoint positions (E1-E6) -->
|
||||
<ELEMENT TAG="DEF_ESPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Robot motor currents (A1-A6, % of max) -->
|
||||
<ELEMENT TAG="DEF_MACur" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- External motor currents (E1-E6, % of max) -->
|
||||
<ELEMENT TAG="DEF_MECur" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Late packet counter -->
|
||||
<ELEMENT TAG="DEF_Delay" TYPE="LONG" INDX="INTERNAL" />
|
||||
|
||||
<!-- Tech channels C1-C6 (main run parameters, robot → PC) -->
|
||||
<ELEMENT TAG="DEF_Tech.C1" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C2" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C3" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C4" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C5" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C6" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Tech channels T1-T6 (advance parameters, robot → PC) -->
|
||||
<ELEMENT TAG="DEF_Tech.T1" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T2" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T3" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T4" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T5" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T6" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Digital input latch (channel 1) -->
|
||||
<ELEMENT TAG="DiL" TYPE="LONG" INDX="1" />
|
||||
|
||||
<!-- Digital output readback (channels 2-4) -->
|
||||
<ELEMENT TAG="Digout.o1" TYPE="BOOL" INDX="2" />
|
||||
<ELEMENT TAG="Digout.o2" TYPE="BOOL" INDX="3" />
|
||||
<ELEMENT TAG="Digout.o3" TYPE="BOOL" INDX="4" />
|
||||
|
||||
<!-- Analog/general sources (channels 5-8) -->
|
||||
<ELEMENT TAG="Source1" TYPE="DOUBLE" INDX="5" />
|
||||
<ELEMENT TAG="Source2" TYPE="DOUBLE" INDX="6" />
|
||||
<ELEMENT TAG="Source3" TYPE="DOUBLE" INDX="7" />
|
||||
<ELEMENT TAG="Source4" TYPE="DOUBLE" INDX="8" />
|
||||
|
||||
<!-- Digital input readback (channels 9-12) -->
|
||||
<ELEMENT TAG="Digin.i1" TYPE="BOOL" INDX="9" />
|
||||
<ELEMENT TAG="Digin.i2" TYPE="BOOL" INDX="10" />
|
||||
<ELEMENT TAG="Digin.i3" TYPE="BOOL" INDX="11" />
|
||||
<ELEMENT TAG="Digin.i4" TYPE="BOOL" INDX="12" />
|
||||
</ELEMENTS>
|
||||
</SEND>
|
||||
|
||||
<!-- =================== RECEIVE: PC → Robot ======================== -->
|
||||
<RECEIVE>
|
||||
<ELEMENTS>
|
||||
<!-- Status/error string to robot -->
|
||||
<ELEMENT TAG="DEF_EStr" TYPE="STRING" INDX="INTERNAL" />
|
||||
|
||||
<!-- Tech channels T1-T6 (advance parameters, PC → robot) -->
|
||||
<ELEMENT TAG="DEF_Tech.T1" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T2" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T3" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T4" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T5" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T6" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
|
||||
<!-- Tech channels C1-C6 (main run parameters, PC → robot) -->
|
||||
<ELEMENT TAG="DEF_Tech.C1" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C2" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C3" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C4" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C5" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C6" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
|
||||
<!-- Cartesian corrections (channels 1-6, HOLDON keeps last value) -->
|
||||
<ELEMENT TAG="RKorr.X" TYPE="DOUBLE" INDX="1" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.Y" TYPE="DOUBLE" INDX="2" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.Z" TYPE="DOUBLE" INDX="3" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.A" TYPE="DOUBLE" INDX="4" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.B" TYPE="DOUBLE" INDX="5" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.C" TYPE="DOUBLE" INDX="6" HOLDON="1" />
|
||||
|
||||
<!-- Joint corrections (channels 7-12) -->
|
||||
<ELEMENT TAG="AKorr.A1" TYPE="DOUBLE" INDX="7" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A2" TYPE="DOUBLE" INDX="8" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A3" TYPE="DOUBLE" INDX="9" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A4" TYPE="DOUBLE" INDX="10" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A5" TYPE="DOUBLE" INDX="11" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A6" TYPE="DOUBLE" INDX="12" HOLDON="1" />
|
||||
|
||||
<!-- Digital outputs (channels 13-16) -->
|
||||
<ELEMENT TAG="DiO1" TYPE="LONG" INDX="13" HOLDON="1" />
|
||||
<ELEMENT TAG="DiO2" TYPE="LONG" INDX="14" HOLDON="1" />
|
||||
<ELEMENT TAG="DiO3" TYPE="LONG" INDX="15" HOLDON="1" />
|
||||
<ELEMENT TAG="DiO4" TYPE="LONG" INDX="16" HOLDON="1" />
|
||||
|
||||
<!-- Spare channels (17-20) for future use -->
|
||||
<ELEMENT TAG="FREE1" TYPE="LONG" INDX="17" HOLDON="1" />
|
||||
<ELEMENT TAG="FREE2" TYPE="LONG" INDX="18" HOLDON="1" />
|
||||
<ELEMENT TAG="FREE3" TYPE="LONG" INDX="19" HOLDON="1" />
|
||||
<ELEMENT TAG="FREE4" TYPE="LONG" INDX="20" HOLDON="1" />
|
||||
</ELEMENTS>
|
||||
</RECEIVE>
|
||||
</ROOT>
|
||||
BIN
TestServer.exe
Normal file
BIN
TestServer.exe
Normal file
Binary file not shown.
@ -174,4 +174,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -221,4 +221,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -278,4 +278,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -302,4 +302,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -394,4 +394,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -110,4 +110,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -130,4 +130,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -205,4 +205,6 @@ def main():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
main()
|
||||
|
||||
@ -1,11 +1,15 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
rsi.start_rsi()
|
||||
api = RSIAPI()
|
||||
|
||||
print("RSI connection started. Press Enter to stop.")
|
||||
input()
|
||||
api.start()
|
||||
|
||||
rsi.stop_rsi()
|
||||
print("RSI connection stopped.")
|
||||
print("RSI connection started. Press Enter to stop.")
|
||||
input()
|
||||
|
||||
api.stop()
|
||||
print("RSI connection stopped.")
|
||||
|
||||
@ -1,9 +1,13 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
rsi.start_rsi()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
# Move TCP 50mm along X-axis
|
||||
rsi.update_cartesian(x=50, y=0, z=0)
|
||||
api = RSIAPI()
|
||||
api.start()
|
||||
|
||||
rsi.stop_rsi()
|
||||
# Move TCP 50mm along X-axis
|
||||
api.motion.update_cartesian(X=50, Y=0, Z=0)
|
||||
|
||||
api.stop()
|
||||
|
||||
@ -1,9 +1,13 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
rsi.start_rsi()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
# Move Joint A1 by 10 degrees
|
||||
rsi.update_joints(a1=10)
|
||||
api = RSIAPI()
|
||||
api.start()
|
||||
|
||||
rsi.stop_rsi()
|
||||
# Move Joint A1 by 10 degrees
|
||||
api.motion.update_joints(A1=10)
|
||||
|
||||
api.stop()
|
||||
|
||||
@ -1,9 +1,13 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
rsi.start_rsi()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
# Move external axis E1 by 100mm
|
||||
rsi.update_external(e1=100)
|
||||
api = RSIAPI()
|
||||
api.start()
|
||||
|
||||
rsi.stop_rsi()
|
||||
# Move external axis E1 by 100mm
|
||||
api.motion.move_external_axis('E1', 100)
|
||||
|
||||
api.stop()
|
||||
|
||||
@ -1,9 +1,13 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
rsi.start_rsi()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
# Set digital output (e.g., to open gripper)
|
||||
rsi.update_digital_io(125) # Example binary pattern
|
||||
api = RSIAPI()
|
||||
api.start()
|
||||
|
||||
rsi.stop_rsi()
|
||||
# Set digital output (e.g., to open gripper)
|
||||
api.io.set_output(1, True)
|
||||
|
||||
api.stop()
|
||||
|
||||
@ -1,11 +1,15 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
rsi.enable_csv_logging()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
rsi.start_rsi()
|
||||
api = RSIAPI()
|
||||
api.logging.start()
|
||||
|
||||
print("Logging robot data to CSV. Press Enter to stop.")
|
||||
input()
|
||||
api.start()
|
||||
|
||||
rsi.stop_rsi()
|
||||
print("Logging robot data to CSV. Press Enter to stop.")
|
||||
input()
|
||||
|
||||
api.stop()
|
||||
|
||||
@ -1,11 +1,15 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
rsi.enable_graphing()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
rsi.start_rsi()
|
||||
api = RSIAPI()
|
||||
api.viz.start_live_plot()
|
||||
|
||||
print("Live graphing started. Press Enter to stop.")
|
||||
input()
|
||||
api.start()
|
||||
|
||||
rsi.stop_rsi()
|
||||
print("Live graphing started. Press Enter to stop.")
|
||||
input()
|
||||
|
||||
api.stop()
|
||||
|
||||
@ -1,14 +1,18 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
# Set X axis soft limits
|
||||
rsi.set_safety_limit(axis="X", min_value=-500, max_value=500)
|
||||
api = RSIAPI()
|
||||
|
||||
rsi.start_rsi()
|
||||
# Set X axis soft limits
|
||||
api.safety.set_limit(axis="X", min_value=-500, max_value=500)
|
||||
|
||||
try:
|
||||
api.start()
|
||||
|
||||
try:
|
||||
while True:
|
||||
pass
|
||||
except KeyboardInterrupt:
|
||||
rsi.stop_rsi()
|
||||
except KeyboardInterrupt:
|
||||
api.stop()
|
||||
|
||||
@ -1,20 +1,24 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
import time
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
rsi.start_rsi()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
# 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}
|
||||
]
|
||||
api = RSIAPI()
|
||||
api.start()
|
||||
|
||||
for point in points:
|
||||
rsi.update_cartesian(**point)
|
||||
# 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)
|
||||
|
||||
rsi.stop_rsi()
|
||||
api.stop()
|
||||
|
||||
@ -1,14 +1,18 @@
|
||||
from RSIPI import rsi_api
|
||||
from RSIPI import RSIAPI
|
||||
|
||||
rsi = rsi_api.RSIAPI()
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import freeze_support
|
||||
freeze_support()
|
||||
|
||||
try:
|
||||
rsi.start_rsi()
|
||||
api = RSIAPI()
|
||||
|
||||
try:
|
||||
api.start()
|
||||
print("Press Ctrl+C to stop RSI safely.")
|
||||
while True:
|
||||
pass
|
||||
|
||||
except KeyboardInterrupt:
|
||||
except KeyboardInterrupt:
|
||||
print("\nEmergency stop triggered.")
|
||||
rsi.safety_stop()
|
||||
rsi.stop_rsi()
|
||||
api.safety.stop()
|
||||
api.stop()
|
||||
|
||||
112
main.py
112
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)
|
||||
|
||||
517
rsi_config/RSIPI_Full.rsi
Normal file
517
rsi_config/RSIPI_Full.rsi
Normal file
@ -0,0 +1,517 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<rSIModel dslVersion="1.0.0.0" name="" xmlns="http://schemas.microsoft.com/dsltools/RSIVisual">
|
||||
<rSIObjects>
|
||||
<!-- =================== Signal Sources (Robot State) =================== -->
|
||||
<rSIElement name="POSACT1" objType="POSACT" objTypeID="46" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="X" />
|
||||
<rSIOutPort name="Y" />
|
||||
<rSIOutPort name="Z" />
|
||||
<rSIOutPort name="A" />
|
||||
<rSIOutPort name="B" />
|
||||
<rSIOutPort name="C" />
|
||||
<rSIOutPort name="S" signalType="Int" />
|
||||
<rSIOutPort name="T" signalType="Int" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Type" value="Measured" paramType="KUKA.RSIVisual.RSI_PosActType" minVal="-2147483648" maxVal="2147483647" isEnum="true" isRuntime="false" index="1" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="AXISACT1" objType="AXISACT" objTypeID="44" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="A1" />
|
||||
<rSIOutPort name="A2" />
|
||||
<rSIOutPort name="A3" />
|
||||
<rSIOutPort name="A4" />
|
||||
<rSIOutPort name="A5" />
|
||||
<rSIOutPort name="A6" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Type" value="Measured" paramType="KUKA.RSIVisual.RSI_AxisActType" minVal="-2147483648" maxVal="2147483647" isEnum="true" isRuntime="false" index="1" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="AXISACTEXT1" objType="AXISACTEXT" objTypeID="69" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="E1" />
|
||||
<rSIOutPort name="E2" />
|
||||
<rSIOutPort name="E3" />
|
||||
<rSIOutPort name="E4" />
|
||||
<rSIOutPort name="E5" />
|
||||
<rSIOutPort name="E6" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Type" value="Measured" paramType="KUKA.RSIVisual.RSI_AxisActType" minVal="-2147483648" maxVal="2147483647" isEnum="true" isRuntime="false" index="1" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="MOTORCURRENT1" objType="MOTORCURRENT" objTypeID="57" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="A1" />
|
||||
<rSIOutPort name="A2" />
|
||||
<rSIOutPort name="A3" />
|
||||
<rSIOutPort name="A4" />
|
||||
<rSIOutPort name="A5" />
|
||||
<rSIOutPort name="A6" />
|
||||
</rSIOutPorts>
|
||||
</rSIElement>
|
||||
<rSIElement name="DIGIN1" objType="DIGIN" objTypeID="29" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" signalType="Int" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="1" paramType="System.Int32" minVal="1" maxVal="4096" isEnum="false" index="1" />
|
||||
<rSIParameter name="DataSize" value="Byte" paramType="KUKA.RSIVisual.RSI_DataSize" minVal="-2147483648" maxVal="2147483647" isEnum="true" index="2" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="DIGOUT1" objType="DIGOUT" objTypeID="43" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" signalType="Int" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="1" paramType="System.Int32" minVal="1" maxVal="4096" isEnum="false" index="1" />
|
||||
<rSIParameter name="DataSize" value="Bit" paramType="KUKA.RSIVisual.RSI_DataSize" minVal="-2147483648" maxVal="2147483647" isEnum="true" index="2" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="DIGOUT2" objType="DIGOUT" objTypeID="43" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" signalType="Int" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="2" paramType="System.Int32" minVal="1" maxVal="4096" isEnum="false" index="1" />
|
||||
<rSIParameter name="DataSize" value="Bit" paramType="KUKA.RSIVisual.RSI_DataSize" minVal="-2147483648" maxVal="2147483647" isEnum="true" index="2" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="DIGOUT3" objType="DIGOUT" objTypeID="43" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" signalType="Int" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="3" paramType="System.Int32" minVal="1" maxVal="4096" isEnum="false" index="1" />
|
||||
<rSIParameter name="DataSize" value="Bit" paramType="KUKA.RSIVisual.RSI_DataSize" minVal="-2147483648" maxVal="2147483647" isEnum="true" index="2" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="SOURCE1" objType="SOURCE" objTypeID="45" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Type" value="Sin" paramType="KUKA.RSIVisual.RSI_SourceType" minVal="-2147483648" maxVal="2147483647" isEnum="true" index="1" />
|
||||
<rSIParameter name="Offset" value="0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="2" />
|
||||
<rSIParameter name="Amplitude" value="0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="3" />
|
||||
<rSIParameter name="Period" value="0" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="4" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="OV_PRO1" objType="OV_PRO" objTypeID="73" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" />
|
||||
</rSIOutPorts>
|
||||
</rSIElement>
|
||||
<rSIElement name="STATUS1" objType="STATUS" objTypeID="72" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" signalType="Int" />
|
||||
</rSIOutPorts>
|
||||
</rSIElement>
|
||||
|
||||
<!-- =================== Action Objects (PC to Robot) =================== -->
|
||||
<rSIElement name="POSCORR1" objType="POSCORR" objTypeID="27" maxInputs="0" maxOutputs="0">
|
||||
<rSIInPorts>
|
||||
<rSIInPort name="CorrX" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrY" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out2" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrZ" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out3" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrA" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out4" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrB" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out5" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrC" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out6" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
</rSIInPorts>
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Stat" signalType="Int" />
|
||||
<rSIOutPort name="X" />
|
||||
<rSIOutPort name="Y" />
|
||||
<rSIOutPort name="Z" />
|
||||
<rSIOutPort name="A" />
|
||||
<rSIOutPort name="B" />
|
||||
<rSIOutPort name="C" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="LowerLimX" value="-500" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="1" />
|
||||
<rSIParameter name="LowerLimY" value="-500" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="2" />
|
||||
<rSIParameter name="LowerLimZ" value="-500" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="3" />
|
||||
<rSIParameter name="UpperLimX" value="500" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="4" />
|
||||
<rSIParameter name="UpperLimY" value="500" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="5" />
|
||||
<rSIParameter name="UpperLimZ" value="500" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="6" />
|
||||
<rSIParameter name="MaxRotAngle" value="500" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="7" />
|
||||
<rSIParameter name="RefCorrSys" value="Base" paramType="KUKA.RSIVisual.RSI_TrafoCosys" minVal="-2147483648" maxVal="2147483647" isEnum="true" isRuntime="false" index="1" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="AXISCORR1" objType="AXISCORR" objTypeID="24" maxInputs="0" maxOutputs="0">
|
||||
<rSIInPorts>
|
||||
<rSIInPort name="CorrA1" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out7" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrA2" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out8" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrA3" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out9" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrA4" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out10" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrA5" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out11" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="CorrA6" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out12" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
</rSIInPorts>
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Stat" signalType="Int" />
|
||||
<rSIOutPort name="A1" />
|
||||
<rSIOutPort name="A2" />
|
||||
<rSIOutPort name="A3" />
|
||||
<rSIOutPort name="A4" />
|
||||
<rSIOutPort name="A5" />
|
||||
<rSIOutPort name="A6" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="LowerLimA1" value="-180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="1" />
|
||||
<rSIParameter name="LowerLimA2" value="-180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="2" />
|
||||
<rSIParameter name="LowerLimA3" value="-180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="3" />
|
||||
<rSIParameter name="LowerLimA4" value="-180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="4" />
|
||||
<rSIParameter name="LowerLimA5" value="-180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="5" />
|
||||
<rSIParameter name="LowerLimA6" value="-180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="6" />
|
||||
<rSIParameter name="UpperLimA1" value="180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="7" />
|
||||
<rSIParameter name="UpperLimA2" value="180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="8" />
|
||||
<rSIParameter name="UpperLimA3" value="180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="9" />
|
||||
<rSIParameter name="UpperLimA4" value="180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="10" />
|
||||
<rSIParameter name="UpperLimA5" value="180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="11" />
|
||||
<rSIParameter name="UpperLimA6" value="180" paramType="System.Double" minVal="-2147483648" maxVal="2147483647" isEnum="false" index="12" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="MAP2DIGOUT1" objType="MAP2DIGOUT" objTypeID="14" maxInputs="0" maxOutputs="0">
|
||||
<rSIInPorts>
|
||||
<rSIInPort name="In1" signalType="Int">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out13" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
</rSIInPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="20" paramType="System.Int32" minVal="1" maxVal="4096" isEnum="false" index="1" />
|
||||
<rSIParameter name="DataSize" value="Word" paramType="KUKA.RSIVisual.RSI_DataSizeX" minVal="-2147483648" maxVal="2147483647" isEnum="true" index="2" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="MAP2SEN_PREA1" objType="MAP2SEN_PREA" objTypeID="17" maxInputs="0" maxOutputs="0">
|
||||
<rSIInPorts>
|
||||
<rSIInPort name="In1">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
</rSIInPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="1" paramType="System.Int32" minVal="1" maxVal="20" isEnum="false" index="1" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="MAP2SEN_PREA2" objType="MAP2SEN_PREA" objTypeID="17" maxInputs="0" maxOutputs="0">
|
||||
<rSIInPorts>
|
||||
<rSIInPort name="In1">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out2" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
</rSIInPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="2" paramType="System.Int32" minVal="1" maxVal="20" isEnum="false" index="1" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="MAP2SEN_PREA3" objType="MAP2SEN_PREA" objTypeID="17" maxInputs="0" maxOutputs="0">
|
||||
<rSIInPorts>
|
||||
<rSIInPort name="In1">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out3" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
</rSIInPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="Index" value="3" paramType="System.Int32" minVal="1" maxVal="20" isEnum="false" index="1" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
|
||||
<!-- =================== Monitoring =================== -->
|
||||
<rSIElement name="POSCORRMON1" objType="POSCORRMON" objTypeID="81" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="X" />
|
||||
<rSIOutPort name="Y" />
|
||||
<rSIOutPort name="Z" />
|
||||
<rSIOutPort name="A" />
|
||||
<rSIOutPort name="B" />
|
||||
<rSIOutPort name="C" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="MaxTrans" value="500" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="1" />
|
||||
<rSIParameter name="MaxRotAngle" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="2" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
<rSIElement name="AXISCORRMON1" objType="AXISCORRMON" objTypeID="82" maxInputs="0" maxOutputs="0">
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="A1" />
|
||||
<rSIOutPort name="A2" />
|
||||
<rSIOutPort name="A3" />
|
||||
<rSIOutPort name="A4" />
|
||||
<rSIOutPort name="A5" />
|
||||
<rSIOutPort name="A6" />
|
||||
<rSIOutPort name="E1" />
|
||||
<rSIOutPort name="E2" />
|
||||
<rSIOutPort name="E3" />
|
||||
<rSIOutPort name="E4" />
|
||||
<rSIOutPort name="E5" />
|
||||
<rSIOutPort name="E6" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="MaxA1" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="1" />
|
||||
<rSIParameter name="MaxA2" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="2" />
|
||||
<rSIParameter name="MaxA3" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="3" />
|
||||
<rSIParameter name="MaxA4" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="4" />
|
||||
<rSIParameter name="MaxA5" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="5" />
|
||||
<rSIParameter name="MaxA6" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="6" />
|
||||
<rSIParameter name="MaxE1" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="7" />
|
||||
<rSIParameter name="MaxE2" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="8" />
|
||||
<rSIParameter name="MaxE3" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="9" />
|
||||
<rSIParameter name="MaxE4" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="10" />
|
||||
<rSIParameter name="MaxE5" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="11" />
|
||||
<rSIParameter name="MaxE6" value="180" paramType="System.Double" minVal="0" maxVal="2147483647" isEnum="false" index="12" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
|
||||
<!-- =================== ETHERNET Communication =================== -->
|
||||
<rSIElement name="ETHERNET1" objType="ETHERNET" objTypeID="64" maxInputs="64" maxOutputs="64">
|
||||
<rSIInPorts>
|
||||
<rSIInPort name="In1" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//DIGIN1/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In2" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//DIGOUT1/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In3" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//DIGOUT2/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In4" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//DIGOUT3/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In5" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//SOURCE1/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In6" mandatory="false" />
|
||||
<rSIInPort name="In7" mandatory="false" />
|
||||
<rSIInPort name="In8" mandatory="false" />
|
||||
<rSIInPort name="In9" mandatory="false" />
|
||||
<rSIInPort name="In10" mandatory="false" />
|
||||
<rSIInPort name="In11" mandatory="false" />
|
||||
<rSIInPort name="In12" mandatory="false" />
|
||||
<rSIInPort name="In13" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//POSACT1/X" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In14" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//POSACT1/Y" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In15" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//POSACT1/Z" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In16" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//POSACT1/A" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In17" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//POSACT1/B" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In18" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//POSACT1/C" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In19" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACT1/A1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In20" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACT1/A2" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In21" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACT1/A3" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In22" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACT1/A4" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In23" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACT1/A5" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In24" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACT1/A6" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In25" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACTEXT1/E1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In26" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACTEXT1/E2" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In27" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACTEXT1/E3" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In28" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACTEXT1/E4" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In29" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACTEXT1/E5" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In30" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//AXISACTEXT1/E6" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In31" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//MOTORCURRENT1/A1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In32" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//MOTORCURRENT1/A2" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In33" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//MOTORCURRENT1/A3" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In34" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//MOTORCURRENT1/A4" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In35" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//MOTORCURRENT1/A5" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In36" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//MOTORCURRENT1/A6" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In37" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//OV_PRO1/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
<rSIInPort name="In38" mandatory="false">
|
||||
<source>
|
||||
<rSIOutPortMoniker name="//STATUS1/Out1" />
|
||||
</source>
|
||||
</rSIInPort>
|
||||
</rSIInPorts>
|
||||
<rSIOutPorts>
|
||||
<rSIOutPort name="Out1" />
|
||||
<rSIOutPort name="Out2" />
|
||||
<rSIOutPort name="Out3" />
|
||||
<rSIOutPort name="Out4" />
|
||||
<rSIOutPort name="Out5" />
|
||||
<rSIOutPort name="Out6" />
|
||||
<rSIOutPort name="Out7" />
|
||||
<rSIOutPort name="Out8" />
|
||||
<rSIOutPort name="Out9" />
|
||||
<rSIOutPort name="Out10" />
|
||||
<rSIOutPort name="Out11" />
|
||||
<rSIOutPort name="Out12" />
|
||||
<rSIOutPort name="Out13" />
|
||||
<rSIOutPort name="Out14" />
|
||||
<rSIOutPort name="Out15" />
|
||||
<rSIOutPort name="Out16" />
|
||||
<rSIOutPort name="Out17" />
|
||||
<rSIOutPort name="Out18" />
|
||||
<rSIOutPort name="Out19" />
|
||||
<rSIOutPort name="Out20" />
|
||||
</rSIOutPorts>
|
||||
<rSIParameters>
|
||||
<rSIParameter name="ConfigFile" value="RSI_EthernetConfig_Full.xml" paramType="System.FileName" minVal="-2147483648" maxVal="2147483647" isEnum="false" isRuntime="false" index="1" />
|
||||
<rSIParameter name="Timeout" value="100" paramType="System.Int32" minVal="0" maxVal="2147483647" isEnum="false" index="1" />
|
||||
<rSIParameter name="Flag" value="1" paramType="System.Int32" minVal="-1" maxVal="999" isEnum="false" index="4" />
|
||||
<rSIParameter name="Precision" value="1" paramType="System.Int32" minVal="1" maxVal="32" isEnum="false" index="8" />
|
||||
</rSIParameters>
|
||||
</rSIElement>
|
||||
</rSIObjects>
|
||||
</rSIModel>
|
||||
256
rsi_config/RSIPI_Full.rsi.diagram
Normal file
256
rsi_config/RSIPI_Full.rsi.diagram
Normal file
@ -0,0 +1,256 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<rSIObjectDiagram dslVersion="1.0.0.0" absoluteBounds="0, 0, 18, 14" name="RSIPI_Full">
|
||||
<rSIModelMoniker name="/" />
|
||||
<nestedChildShapes>
|
||||
<!-- Row 1: Signal source objects -->
|
||||
<rSIElementShape Id="a1000001-0000-0000-0000-000000000001" absoluteBounds="0.5, 0.5, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//POSACT1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000001-0001-0000-0000-000000000001" absoluteBounds="2, 0.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//POSACT1/X" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000001-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="0.515, 1.01, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000002-0000-0000-0000-000000000001" absoluteBounds="2.5, 0.5, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//AXISACT1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000002-0001-0000-0000-000000000001" absoluteBounds="4, 0.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//AXISACT1/A1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000002-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="2.515, 1.01, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000003-0000-0000-0000-000000000001" absoluteBounds="4.5, 0.5, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//AXISACTEXT1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000003-0001-0000-0000-000000000001" absoluteBounds="6, 0.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//AXISACTEXT1/E1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000003-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="4.515, 1.01, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000004-0000-0000-0000-000000000001" absoluteBounds="6.5, 0.5, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//MOTORCURRENT1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000004-0001-0000-0000-000000000001" absoluteBounds="8, 0.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//MOTORCURRENT1/A1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000004-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="6.515, 1.01, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<!-- Row 2: Digital I/O and Source -->
|
||||
<rSIElementShape Id="a1000005-0000-0000-0000-000000000001" absoluteBounds="0.5, 2, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//DIGIN1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000005-0001-0000-0000-000000000001" absoluteBounds="2, 2.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//DIGIN1/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000005-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="0.515, 2.51, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000006-0000-0000-0000-000000000001" absoluteBounds="2.5, 2, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//DIGOUT1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000006-0001-0000-0000-000000000001" absoluteBounds="4, 2.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//DIGOUT1/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000006-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="2.515, 2.51, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000007-0000-0000-0000-000000000001" absoluteBounds="4.5, 2, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//DIGOUT2" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000007-0001-0000-0000-000000000001" absoluteBounds="6, 2.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//DIGOUT2/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000007-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="4.515, 2.51, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000008-0000-0000-0000-000000000001" absoluteBounds="6.5, 2, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//DIGOUT3" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000008-0001-0000-0000-000000000001" absoluteBounds="8, 2.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//DIGOUT3/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000008-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="6.515, 2.51, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000009-0000-0000-0000-000000000001" absoluteBounds="8.5, 2, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//SOURCE1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000009-0001-0000-0000-000000000001" absoluteBounds="10, 2.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//SOURCE1/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000009-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="8.515, 2.51, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a100000a-0000-0000-0000-000000000001" absoluteBounds="10.5, 0.5, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//OV_PRO1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a100000a-0001-0000-0000-000000000001" absoluteBounds="12, 0.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//OV_PRO1/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a100000a-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="10.515, 1.01, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a100000b-0000-0000-0000-000000000001" absoluteBounds="12.5, 0.5, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//STATUS1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a100000b-0001-0000-0000-000000000001" absoluteBounds="14, 0.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//STATUS1/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a100000b-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="12.515, 1.01, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<!-- Row 3: ETHERNET (center, large) -->
|
||||
<rSIElementShape Id="a100000c-0000-0000-0000-000000000001" absoluteBounds="5, 4, 3, 8" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//ETHERNET1" />
|
||||
<relativeChildShapes>
|
||||
<rSIInPortShape Id="a100000c-0001-0000-0000-000000000001" absoluteBounds="4.6, 4.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIInPortMoniker name="//ETHERNET1/In1" />
|
||||
<relativeChildShapes />
|
||||
</rSIInPortShape>
|
||||
<rSIOutPortShape Id="a100000c-0002-0000-0000-000000000001" absoluteBounds="8, 4.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//ETHERNET1/Out1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a100000c-0003-0000-0000-000000000001" absoluteBounds="5.015, 4.51, 2.9700000000000002, 1.0185953776041665" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<!-- Row 4: Action objects -->
|
||||
<rSIElementShape Id="a100000d-0000-0000-0000-000000000001" absoluteBounds="10, 4, 2, 2.875" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//POSCORR1" />
|
||||
<relativeChildShapes>
|
||||
<rSIInPortShape Id="a100000d-0001-0000-0000-000000000001" absoluteBounds="9.6, 4.25, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIInPortMoniker name="//POSCORR1/CorrX" />
|
||||
<relativeChildShapes />
|
||||
</rSIInPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a100000d-0002-0000-0000-000000000001" absoluteBounds="10.015, 4.51, 1.9700000000000002, 1.7878011067708333" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a100000e-0000-0000-0000-000000000001" absoluteBounds="10, 7.5, 2, 2.875" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//AXISCORR1" />
|
||||
<relativeChildShapes>
|
||||
<rSIInPortShape Id="a100000e-0001-0000-0000-000000000001" absoluteBounds="9.6, 7.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIInPortMoniker name="//AXISCORR1/CorrA1" />
|
||||
<relativeChildShapes />
|
||||
</rSIInPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a100000e-0002-0000-0000-000000000001" absoluteBounds="10.015, 8.01, 1.9700000000000002, 2.5570068359375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a100000f-0000-0000-0000-000000000001" absoluteBounds="10, 11, 2, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//MAP2DIGOUT1" />
|
||||
<relativeChildShapes>
|
||||
<rSIInPortShape Id="a100000f-0001-0000-0000-000000000001" absoluteBounds="9.6, 11.25, 0.40000000596046448, 0.075000002980232239" fillColor="BlanchedAlmond">
|
||||
<rSIInPortMoniker name="//MAP2DIGOUT1/In1" />
|
||||
<relativeChildShapes />
|
||||
</rSIInPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a100000f-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="10.015, 11.51, 1.9700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<!-- Row 5: MAP2SEN_PREA objects -->
|
||||
<rSIElementShape Id="a1000010-0000-0000-0000-000000000001" absoluteBounds="13, 4, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//MAP2SEN_PREA1" />
|
||||
<relativeChildShapes>
|
||||
<rSIInPortShape Id="a1000010-0001-0000-0000-000000000001" absoluteBounds="12.6, 4.25, 0.40000000596046448, 0.075000002980232239" fillColor="BlanchedAlmond">
|
||||
<rSIInPortMoniker name="//MAP2SEN_PREA1/In1" />
|
||||
<relativeChildShapes />
|
||||
</rSIInPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000010-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="13.015, 4.51, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000011-0000-0000-0000-000000000001" absoluteBounds="13, 5.25, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//MAP2SEN_PREA2" />
|
||||
<relativeChildShapes>
|
||||
<rSIInPortShape Id="a1000011-0001-0000-0000-000000000001" absoluteBounds="12.6, 5.5, 0.40000000596046448, 0.075000002980232239" fillColor="BlanchedAlmond">
|
||||
<rSIInPortMoniker name="//MAP2SEN_PREA2/In1" />
|
||||
<relativeChildShapes />
|
||||
</rSIInPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000011-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="13.015, 5.76, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000012-0000-0000-0000-000000000001" absoluteBounds="13, 6.5, 1.5, 0.8593896484375001" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//MAP2SEN_PREA3" />
|
||||
<relativeChildShapes>
|
||||
<rSIInPortShape Id="a1000012-0001-0000-0000-000000000001" absoluteBounds="12.6, 6.75, 0.40000000596046448, 0.075000002980232239" fillColor="BlanchedAlmond">
|
||||
<rSIInPortMoniker name="//MAP2SEN_PREA3/In1" />
|
||||
<relativeChildShapes />
|
||||
</rSIInPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000012-0002-0000-0000-000000000001" isExpanded="false" absoluteBounds="13.015, 7.01, 1.4700000000000002, 0.2493896484375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<!-- Row 6: Monitoring objects -->
|
||||
<rSIElementShape Id="a1000013-0000-0000-0000-000000000001" absoluteBounds="14.5, 0.5, 1.5, 2.475" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//POSCORRMON1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000013-0001-0000-0000-000000000001" absoluteBounds="16, 0.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//POSCORRMON1/X" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000013-0002-0000-0000-000000000001" absoluteBounds="14.515, 1.01, 1.4700000000000002, 0.63399251302083326" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
<rSIElementShape Id="a1000014-0000-0000-0000-000000000001" absoluteBounds="14.5, 3.5, 1.5, 4.875" fillColor="BlanchedAlmond">
|
||||
<rSIElementMoniker name="//AXISCORRMON1" />
|
||||
<relativeChildShapes>
|
||||
<rSIOutPortShape Id="a1000014-0001-0000-0000-000000000001" absoluteBounds="16, 3.75, 0.40000000596046448, 0.075000002980232239" fillColor="White">
|
||||
<rSIOutPortMoniker name="//AXISCORRMON1/A1" />
|
||||
<relativeChildShapes />
|
||||
</rSIOutPortShape>
|
||||
</relativeChildShapes>
|
||||
<nestedChildShapes>
|
||||
<elementListCompartment Id="a1000014-0002-0000-0000-000000000001" absoluteBounds="14.515, 4.01, 1.4700000000000002, 2.5570068359375" name="RSIParameters" titleTextColor="Black" itemTextColor="Black" />
|
||||
</nestedChildShapes>
|
||||
</rSIElementShape>
|
||||
</nestedChildShapes>
|
||||
</rSIObjectDiagram>
|
||||
203
rsi_config/RSIPI_Full.rsi.xml
Normal file
203
rsi_config/RSIPI_Full.rsi.xml
Normal file
@ -0,0 +1,203 @@
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
|
||||
<RSIObjects xsi:noNamespaceSchemaLocation="/Roboter/Config/System/Common/Schemes/RSIContext.xsd" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
|
||||
<!-- =================== Signal Sources (Robot State) =================== -->
|
||||
<RSIObject ObjType="POSACT" ObjTypeID="46" ObjID="POSACT1">
|
||||
<Parameters>
|
||||
<Parameter Name="Type" ParamID="1" ParamValue="1" IsRuntime="false" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="AXISACT" ObjTypeID="44" ObjID="AXISACT1">
|
||||
<Parameters>
|
||||
<Parameter Name="Type" ParamID="1" ParamValue="1" IsRuntime="false" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="AXISACTEXT" ObjTypeID="69" ObjID="AXISACTEXT1">
|
||||
<Parameters>
|
||||
<Parameter Name="Type" ParamID="1" ParamValue="1" IsRuntime="false" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="MOTORCURRENT" ObjTypeID="57" ObjID="MOTORCURRENT1">
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="DIGIN" ObjTypeID="29" ObjID="DIGIN1">
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="1" />
|
||||
<Parameter Name="DataSize" ParamID="2" ParamValue="2" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="DIGOUT" ObjTypeID="43" ObjID="DIGOUT1">
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="1" />
|
||||
<Parameter Name="DataSize" ParamID="2" ParamValue="0" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="DIGOUT" ObjTypeID="43" ObjID="DIGOUT2">
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="2" />
|
||||
<Parameter Name="DataSize" ParamID="2" ParamValue="0" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="DIGOUT" ObjTypeID="43" ObjID="DIGOUT3">
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="3" />
|
||||
<Parameter Name="DataSize" ParamID="2" ParamValue="0" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="SOURCE" ObjTypeID="45" ObjID="SOURCE1">
|
||||
<Parameters>
|
||||
<Parameter Name="Type" ParamID="1" ParamValue="1" />
|
||||
<Parameter Name="Offset" ParamID="2" ParamValue="0" />
|
||||
<Parameter Name="Amplitude" ParamID="3" ParamValue="0" />
|
||||
<Parameter Name="Period" ParamID="4" ParamValue="0" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="OV_PRO" ObjTypeID="73" ObjID="OV_PRO1">
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="STATUS" ObjTypeID="72" ObjID="STATUS1">
|
||||
</RSIObject>
|
||||
|
||||
<!-- =================== Action Objects (PC → Robot) =================== -->
|
||||
<RSIObject ObjType="POSCORR" ObjTypeID="27" ObjID="POSCORR1">
|
||||
<Inputs>
|
||||
<Input InIdx="1" OutObjID="ETHERNET1" OutIdx="1" />
|
||||
<Input InIdx="2" OutObjID="ETHERNET1" OutIdx="2" />
|
||||
<Input InIdx="3" OutObjID="ETHERNET1" OutIdx="3" />
|
||||
<Input InIdx="4" OutObjID="ETHERNET1" OutIdx="4" />
|
||||
<Input InIdx="5" OutObjID="ETHERNET1" OutIdx="5" />
|
||||
<Input InIdx="6" OutObjID="ETHERNET1" OutIdx="6" />
|
||||
</Inputs>
|
||||
<Parameters>
|
||||
<Parameter Name="LowerLimX" ParamID="1" ParamValue="-500" />
|
||||
<Parameter Name="LowerLimY" ParamID="2" ParamValue="-500" />
|
||||
<Parameter Name="LowerLimZ" ParamID="3" ParamValue="-500" />
|
||||
<Parameter Name="UpperLimX" ParamID="4" ParamValue="500" />
|
||||
<Parameter Name="UpperLimY" ParamID="5" ParamValue="500" />
|
||||
<Parameter Name="UpperLimZ" ParamID="6" ParamValue="500" />
|
||||
<Parameter Name="MaxRotAngle" ParamID="7" ParamValue="500" />
|
||||
<Parameter Name="RefCorrSys" ParamID="1" ParamValue="1" IsRuntime="false" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="AXISCORR" ObjTypeID="24" ObjID="AXISCORR1">
|
||||
<Inputs>
|
||||
<Input InIdx="1" OutObjID="ETHERNET1" OutIdx="7" />
|
||||
<Input InIdx="2" OutObjID="ETHERNET1" OutIdx="8" />
|
||||
<Input InIdx="3" OutObjID="ETHERNET1" OutIdx="9" />
|
||||
<Input InIdx="4" OutObjID="ETHERNET1" OutIdx="10" />
|
||||
<Input InIdx="5" OutObjID="ETHERNET1" OutIdx="11" />
|
||||
<Input InIdx="6" OutObjID="ETHERNET1" OutIdx="12" />
|
||||
</Inputs>
|
||||
<Parameters>
|
||||
<Parameter Name="LowerLimA1" ParamID="1" ParamValue="-180" />
|
||||
<Parameter Name="LowerLimA2" ParamID="2" ParamValue="-180" />
|
||||
<Parameter Name="LowerLimA3" ParamID="3" ParamValue="-180" />
|
||||
<Parameter Name="LowerLimA4" ParamID="4" ParamValue="-180" />
|
||||
<Parameter Name="LowerLimA5" ParamID="5" ParamValue="-180" />
|
||||
<Parameter Name="LowerLimA6" ParamID="6" ParamValue="-180" />
|
||||
<Parameter Name="UpperLimA1" ParamID="7" ParamValue="180" />
|
||||
<Parameter Name="UpperLimA2" ParamID="8" ParamValue="180" />
|
||||
<Parameter Name="UpperLimA3" ParamID="9" ParamValue="180" />
|
||||
<Parameter Name="UpperLimA4" ParamID="10" ParamValue="180" />
|
||||
<Parameter Name="UpperLimA5" ParamID="11" ParamValue="180" />
|
||||
<Parameter Name="UpperLimA6" ParamID="12" ParamValue="180" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="MAP2DIGOUT" ObjTypeID="14" ObjID="MAP2DIGOUT1">
|
||||
<Inputs>
|
||||
<Input InIdx="1" OutObjID="ETHERNET1" OutIdx="13" />
|
||||
</Inputs>
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="20" />
|
||||
<Parameter Name="DataSize" ParamID="2" ParamValue="2" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="MAP2SEN_PREA" ObjTypeID="17" ObjID="MAP2SEN_PREA1">
|
||||
<Inputs>
|
||||
<Input InIdx="1" OutObjID="ETHERNET1" OutIdx="1" />
|
||||
</Inputs>
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="1" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="MAP2SEN_PREA" ObjTypeID="17" ObjID="MAP2SEN_PREA2">
|
||||
<Inputs>
|
||||
<Input InIdx="1" OutObjID="ETHERNET1" OutIdx="2" />
|
||||
</Inputs>
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="2" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="MAP2SEN_PREA" ObjTypeID="17" ObjID="MAP2SEN_PREA3">
|
||||
<Inputs>
|
||||
<Input InIdx="1" OutObjID="ETHERNET1" OutIdx="3" />
|
||||
</Inputs>
|
||||
<Parameters>
|
||||
<Parameter Name="Index" ParamID="1" ParamValue="3" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
|
||||
<!-- =================== Monitoring =================== -->
|
||||
<RSIObject ObjType="POSCORRMON" ObjTypeID="81" ObjID="POSCORRMON1">
|
||||
<Parameters>
|
||||
<Parameter Name="MaxTrans" ParamID="1" ParamValue="500" />
|
||||
<Parameter Name="MaxRotAngle" ParamID="2" ParamValue="180" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
<RSIObject ObjType="AXISCORRMON" ObjTypeID="82" ObjID="AXISCORRMON1">
|
||||
<Parameters>
|
||||
<Parameter Name="MaxA1" ParamID="1" ParamValue="180" />
|
||||
<Parameter Name="MaxA2" ParamID="2" ParamValue="180" />
|
||||
<Parameter Name="MaxA3" ParamID="3" ParamValue="180" />
|
||||
<Parameter Name="MaxA4" ParamID="4" ParamValue="180" />
|
||||
<Parameter Name="MaxA5" ParamID="5" ParamValue="180" />
|
||||
<Parameter Name="MaxA6" ParamID="6" ParamValue="180" />
|
||||
<Parameter Name="MaxE1" ParamID="7" ParamValue="180" />
|
||||
<Parameter Name="MaxE2" ParamID="8" ParamValue="180" />
|
||||
<Parameter Name="MaxE3" ParamID="9" ParamValue="180" />
|
||||
<Parameter Name="MaxE4" ParamID="10" ParamValue="180" />
|
||||
<Parameter Name="MaxE5" ParamID="11" ParamValue="180" />
|
||||
<Parameter Name="MaxE6" ParamID="12" ParamValue="180" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
|
||||
<!-- =================== ETHERNET Communication =================== -->
|
||||
<RSIObject ObjType="ETHERNET" ObjTypeID="64" ObjID="ETHERNET1">
|
||||
<Inputs>
|
||||
<Input InIdx="1" OutObjID="DIGIN1" OutIdx="1" />
|
||||
<Input InIdx="2" OutObjID="DIGOUT1" OutIdx="1" />
|
||||
<Input InIdx="3" OutObjID="DIGOUT2" OutIdx="1" />
|
||||
<Input InIdx="4" OutObjID="DIGOUT3" OutIdx="1" />
|
||||
<Input InIdx="5" OutObjID="SOURCE1" OutIdx="1" />
|
||||
<Input InIdx="13" OutObjID="POSACT1" OutIdx="1" />
|
||||
<Input InIdx="14" OutObjID="POSACT1" OutIdx="2" />
|
||||
<Input InIdx="15" OutObjID="POSACT1" OutIdx="3" />
|
||||
<Input InIdx="16" OutObjID="POSACT1" OutIdx="4" />
|
||||
<Input InIdx="17" OutObjID="POSACT1" OutIdx="5" />
|
||||
<Input InIdx="18" OutObjID="POSACT1" OutIdx="6" />
|
||||
<Input InIdx="19" OutObjID="AXISACT1" OutIdx="1" />
|
||||
<Input InIdx="20" OutObjID="AXISACT1" OutIdx="2" />
|
||||
<Input InIdx="21" OutObjID="AXISACT1" OutIdx="3" />
|
||||
<Input InIdx="22" OutObjID="AXISACT1" OutIdx="4" />
|
||||
<Input InIdx="23" OutObjID="AXISACT1" OutIdx="5" />
|
||||
<Input InIdx="24" OutObjID="AXISACT1" OutIdx="6" />
|
||||
<Input InIdx="25" OutObjID="AXISACTEXT1" OutIdx="1" />
|
||||
<Input InIdx="26" OutObjID="AXISACTEXT1" OutIdx="2" />
|
||||
<Input InIdx="27" OutObjID="AXISACTEXT1" OutIdx="3" />
|
||||
<Input InIdx="28" OutObjID="AXISACTEXT1" OutIdx="4" />
|
||||
<Input InIdx="29" OutObjID="AXISACTEXT1" OutIdx="5" />
|
||||
<Input InIdx="30" OutObjID="AXISACTEXT1" OutIdx="6" />
|
||||
<Input InIdx="31" OutObjID="MOTORCURRENT1" OutIdx="1" />
|
||||
<Input InIdx="32" OutObjID="MOTORCURRENT1" OutIdx="2" />
|
||||
<Input InIdx="33" OutObjID="MOTORCURRENT1" OutIdx="3" />
|
||||
<Input InIdx="34" OutObjID="MOTORCURRENT1" OutIdx="4" />
|
||||
<Input InIdx="35" OutObjID="MOTORCURRENT1" OutIdx="5" />
|
||||
<Input InIdx="36" OutObjID="MOTORCURRENT1" OutIdx="6" />
|
||||
<Input InIdx="37" OutObjID="OV_PRO1" OutIdx="1" />
|
||||
<Input InIdx="38" OutObjID="STATUS1" OutIdx="1" />
|
||||
</Inputs>
|
||||
<Parameters>
|
||||
<Parameter Name="ConfigFile" ParamID="1" ParamValue="RSI_EthernetConfig_Full.xml" IsRuntime="false" />
|
||||
<Parameter Name="Timeout" ParamID="1" ParamValue="100" />
|
||||
<Parameter Name="Flag" ParamID="4" ParamValue="1" />
|
||||
<Parameter Name="Precision" ParamID="8" ParamValue="1" />
|
||||
</Parameters>
|
||||
</RSIObject>
|
||||
</RSIObjects>
|
||||
207
rsi_config/RSIPI_Test.src
Normal file
207
rsi_config/RSIPI_Test.src
Normal file
@ -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
|
||||
159
rsi_config/RSI_EthernetConfig_Full.xml
Normal file
159
rsi_config/RSI_EthernetConfig_Full.xml
Normal file
@ -0,0 +1,159 @@
|
||||
<ROOT>
|
||||
<CONFIG>
|
||||
<IP_NUMBER>10.10.10.10</IP_NUMBER>
|
||||
<PORT>64000</PORT>
|
||||
<SENTYPE>ImFree</SENTYPE>
|
||||
<ONLYSEND>FALSE</ONLYSEND>
|
||||
</CONFIG>
|
||||
|
||||
<!-- =================================================================
|
||||
RSI Channel Budget: 64 max across SEND + RECEIVE
|
||||
INTERNAL tags don't count toward the 64-channel limit
|
||||
|
||||
SEND channels used: 38 (DiL, Digout x3, Source1, PosAct x6,
|
||||
AxisAct x6, ExtAct x6, MotCur x6,
|
||||
OvPro, Status)
|
||||
RECEIVE channels used: 13 (RKorr x6, AKorr x6, DiO)
|
||||
Total: 51 / 64
|
||||
|
||||
All DEF_ tags are INTERNAL (free)
|
||||
================================================================= -->
|
||||
|
||||
<!-- ===================== SEND: Robot to PC ========================= -->
|
||||
<SEND>
|
||||
<ELEMENTS>
|
||||
<!-- INTERNAL: Cartesian actual position (expanded to X,Y,Z,A,B,C) -->
|
||||
<ELEMENT TAG="DEF_RIst" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: Cartesian setpoint position -->
|
||||
<ELEMENT TAG="DEF_RSol" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: Robot axis actual positions (A1-A6 in deg) -->
|
||||
<ELEMENT TAG="DEF_AIPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: Robot axis setpoint positions (A1-A6 in deg) -->
|
||||
<ELEMENT TAG="DEF_ASPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: External axis actual positions (E1-E6) -->
|
||||
<ELEMENT TAG="DEF_EIPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: External axis setpoint positions (E1-E6) -->
|
||||
<ELEMENT TAG="DEF_ESPos" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: Robot motor currents (A1-A6, % of max) -->
|
||||
<ELEMENT TAG="DEF_MACur" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: External motor currents (E1-E6, % of max) -->
|
||||
<ELEMENT TAG="DEF_MECur" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: Late packet counter -->
|
||||
<ELEMENT TAG="DEF_Delay" TYPE="LONG" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: Tech channels -->
|
||||
<ELEMENT TAG="DEF_Tech.C1" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C2" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C3" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C4" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C5" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.C6" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T1" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T2" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T3" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T4" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T5" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
<ELEMENT TAG="DEF_Tech.T6" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
|
||||
<!-- Channel 1: Digital input latch (from DIGIN1) -->
|
||||
<ELEMENT TAG="DiL" TYPE="LONG" INDX="1" />
|
||||
|
||||
<!-- Channels 2-4: Digital output readback (from DIGOUT1-3) -->
|
||||
<ELEMENT TAG="Digout.o1" TYPE="BOOL" INDX="2" />
|
||||
<ELEMENT TAG="Digout.o2" TYPE="BOOL" INDX="3" />
|
||||
<ELEMENT TAG="Digout.o3" TYPE="BOOL" INDX="4" />
|
||||
|
||||
<!-- Channel 5: Signal source (from SOURCE1) -->
|
||||
<ELEMENT TAG="Source1" TYPE="DOUBLE" INDX="5" />
|
||||
|
||||
<!-- Channels 13-18: Cartesian actual position (from POSACT1) -->
|
||||
<ELEMENT TAG="PosAct.X" TYPE="DOUBLE" INDX="13" />
|
||||
<ELEMENT TAG="PosAct.Y" TYPE="DOUBLE" INDX="14" />
|
||||
<ELEMENT TAG="PosAct.Z" TYPE="DOUBLE" INDX="15" />
|
||||
<ELEMENT TAG="PosAct.A" TYPE="DOUBLE" INDX="16" />
|
||||
<ELEMENT TAG="PosAct.B" TYPE="DOUBLE" INDX="17" />
|
||||
<ELEMENT TAG="PosAct.C" TYPE="DOUBLE" INDX="18" />
|
||||
|
||||
<!-- Channels 19-24: Joint axis actual positions (from AXISACT1) -->
|
||||
<ELEMENT TAG="AxisAct.A1" TYPE="DOUBLE" INDX="19" />
|
||||
<ELEMENT TAG="AxisAct.A2" TYPE="DOUBLE" INDX="20" />
|
||||
<ELEMENT TAG="AxisAct.A3" TYPE="DOUBLE" INDX="21" />
|
||||
<ELEMENT TAG="AxisAct.A4" TYPE="DOUBLE" INDX="22" />
|
||||
<ELEMENT TAG="AxisAct.A5" TYPE="DOUBLE" INDX="23" />
|
||||
<ELEMENT TAG="AxisAct.A6" TYPE="DOUBLE" INDX="24" />
|
||||
|
||||
<!-- Channels 25-30: External axis actual positions (from AXISACTEXT1) -->
|
||||
<ELEMENT TAG="ExtAct.E1" TYPE="DOUBLE" INDX="25" />
|
||||
<ELEMENT TAG="ExtAct.E2" TYPE="DOUBLE" INDX="26" />
|
||||
<ELEMENT TAG="ExtAct.E3" TYPE="DOUBLE" INDX="27" />
|
||||
<ELEMENT TAG="ExtAct.E4" TYPE="DOUBLE" INDX="28" />
|
||||
<ELEMENT TAG="ExtAct.E5" TYPE="DOUBLE" INDX="29" />
|
||||
<ELEMENT TAG="ExtAct.E6" TYPE="DOUBLE" INDX="30" />
|
||||
|
||||
<!-- Channels 31-36: Motor currents (from MOTORCURRENT1) -->
|
||||
<ELEMENT TAG="MotCur.A1" TYPE="DOUBLE" INDX="31" />
|
||||
<ELEMENT TAG="MotCur.A2" TYPE="DOUBLE" INDX="32" />
|
||||
<ELEMENT TAG="MotCur.A3" TYPE="DOUBLE" INDX="33" />
|
||||
<ELEMENT TAG="MotCur.A4" TYPE="DOUBLE" INDX="34" />
|
||||
<ELEMENT TAG="MotCur.A5" TYPE="DOUBLE" INDX="35" />
|
||||
<ELEMENT TAG="MotCur.A6" TYPE="DOUBLE" INDX="36" />
|
||||
|
||||
<!-- Channel 37: Program override percentage (from OV_PRO1) -->
|
||||
<ELEMENT TAG="OvPro" TYPE="DOUBLE" INDX="37" />
|
||||
|
||||
<!-- Channel 38: Robot status (from STATUS1) -->
|
||||
<ELEMENT TAG="Status" TYPE="LONG" INDX="38" />
|
||||
</ELEMENTS>
|
||||
</SEND>
|
||||
|
||||
<!-- =================== RECEIVE: PC to Robot ======================== -->
|
||||
<RECEIVE>
|
||||
<ELEMENTS>
|
||||
<!-- INTERNAL: Status/error string to robot -->
|
||||
<ELEMENT TAG="DEF_EStr" TYPE="STRING" INDX="INTERNAL" />
|
||||
|
||||
<!-- INTERNAL: Tech channels (advance parameters, PC to robot) -->
|
||||
<ELEMENT TAG="DEF_Tech.T1" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T2" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T3" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T4" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T5" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.T6" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
|
||||
<!-- INTERNAL: Tech channels (main run parameters, PC to robot) -->
|
||||
<ELEMENT TAG="DEF_Tech.C1" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C2" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C3" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C4" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C5" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
<ELEMENT TAG="DEF_Tech.C6" TYPE="DOUBLE" INDX="INTERNAL" HOLDON="0" />
|
||||
|
||||
<!-- Channels 1-6: Cartesian corrections (to POSCORR1, HOLDON keeps last value) -->
|
||||
<ELEMENT TAG="RKorr.X" TYPE="DOUBLE" INDX="1" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.Y" TYPE="DOUBLE" INDX="2" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.Z" TYPE="DOUBLE" INDX="3" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.A" TYPE="DOUBLE" INDX="4" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.B" TYPE="DOUBLE" INDX="5" HOLDON="1" />
|
||||
<ELEMENT TAG="RKorr.C" TYPE="DOUBLE" INDX="6" HOLDON="1" />
|
||||
|
||||
<!-- Channels 7-12: Joint corrections (to AXISCORR1) -->
|
||||
<ELEMENT TAG="AKorr.A1" TYPE="DOUBLE" INDX="7" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A2" TYPE="DOUBLE" INDX="8" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A3" TYPE="DOUBLE" INDX="9" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A4" TYPE="DOUBLE" INDX="10" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A5" TYPE="DOUBLE" INDX="11" HOLDON="1" />
|
||||
<ELEMENT TAG="AKorr.A6" TYPE="DOUBLE" INDX="12" HOLDON="1" />
|
||||
|
||||
<!-- Channel 13: Digital output word (to MAP2DIGOUT1) -->
|
||||
<ELEMENT TAG="DiO" TYPE="LONG" INDX="13" HOLDON="1" />
|
||||
</ELEMENTS>
|
||||
</RECEIVE>
|
||||
</ROOT>
|
||||
196
rsi_config/rsipi_test.py
Normal file
196
rsi_config/rsipi_test.py
Normal file
@ -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)
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
tests/test_safety_manager.py
|
||||
tests/test_trajectory_planner.py
|
||||
tests/test_xml_handler.py
|
||||
@ -3,3 +3,6 @@ numpy>=1.22
|
||||
matplotlib>=3.5
|
||||
lxml>=4.9
|
||||
scipy>=1.8
|
||||
|
||||
[dev]
|
||||
pytest>=7.0
|
||||
|
||||
@ -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)"
|
||||
|
||||
@ -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}")
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
# 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)
|
||||
# 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)
|
||||
# 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:
|
||||
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
|
||||
|
||||
@ -26,60 +26,38 @@ 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')
|
||||
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") -> None:
|
||||
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:
|
||||
"""
|
||||
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
|
||||
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
|
||||
|
||||
# Initialize client
|
||||
self._ensure_client()
|
||||
|
||||
# Initialize namespace APIs
|
||||
self.motion = MotionAPI(self.client)
|
||||
self.io = IOAPI(self.client)
|
||||
self.krl = KRLAPI(self.client)
|
||||
@ -92,151 +70,79 @@ class RSIAPI:
|
||||
|
||||
logging.info("RSIAPI initialized with namespaced structure")
|
||||
|
||||
def _ensure_client(self) -> None:
|
||||
"""
|
||||
Ensure RSIClient is initialized (lazy initialization).
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
Imports and creates RSIClient only when needed, avoiding circular
|
||||
dependencies and improving startup time.
|
||||
"""
|
||||
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)
|
||||
logging.debug("RSIClient initialized")
|
||||
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':
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
"""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 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.
|
||||
"""
|
||||
"""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 reconnect(self) -> str:
|
||||
def wait_for_connection(self, timeout: float = 10.0) -> bool:
|
||||
"""
|
||||
Restart network connection without stopping RSI client.
|
||||
Block until the robot's first packet is received.
|
||||
|
||||
Terminates the current network process, creates a fresh one with new
|
||||
communication resources, and restarts the connection. Useful for
|
||||
recovering from network errors.
|
||||
Args:
|
||||
timeout: Maximum time to wait in seconds
|
||||
|
||||
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.
|
||||
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:
|
||||
"""
|
||||
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.
|
||||
|
||||
# Deprecated methods
|
||||
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()
|
||||
|
||||
@ -24,14 +24,13 @@ class ClientState(Enum):
|
||||
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
|
||||
ClientState.STOPPED: {ClientState.INITIALIZED},
|
||||
ClientState.ERROR: {ClientState.STOPPING, ClientState.INITIALIZED},
|
||||
}
|
||||
|
||||
def __init__(
|
||||
@ -40,19 +39,29 @@ class RSIClient:
|
||||
rsi_limits_file: Optional[str] = None,
|
||||
enable_auto_reconnect: bool = False,
|
||||
auto_reconnect_retries: int = 5,
|
||||
auto_reconnect_delay: float = 5.0
|
||||
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:
|
||||
"""
|
||||
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
|
||||
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(f"Loading RSI configuration from {config_file}...")
|
||||
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()
|
||||
@ -60,42 +69,30 @@ class RSIClient:
|
||||
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)
|
||||
|
||||
# Shared logging state (readable from parent process)
|
||||
self._logging_active = multiprocessing.Value('b', False)
|
||||
self._receive_dirty = multiprocessing.Value('b', True) # Dirty flag for IPC optimization
|
||||
|
||||
# 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._create_network_process(network_settings)
|
||||
|
||||
self.logger: Optional[any] = None # Reserved for future use
|
||||
self.logger: Optional[any] = None
|
||||
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(
|
||||
@ -107,6 +104,71 @@ class RSIClient:
|
||||
)
|
||||
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 <RECEIVE> 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)."""
|
||||
@ -114,24 +176,15 @@ class RSIClient:
|
||||
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}")
|
||||
logging.debug("State transition: %s -> %s", old_state.name, new_state.name)
|
||||
return True
|
||||
else:
|
||||
logging.warning(
|
||||
f"Invalid state transition attempted: {self._state.name} -> {new_state.name}"
|
||||
"Invalid state transition attempted: %s -> %s", self._state.name, new_state.name
|
||||
)
|
||||
return False
|
||||
|
||||
@ -139,9 +192,6 @@ class RSIClient:
|
||||
"""
|
||||
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
|
||||
"""
|
||||
@ -161,7 +211,6 @@ class RSIClient:
|
||||
self.running = True
|
||||
logging.info("RSI Client Started")
|
||||
|
||||
# Start auto-reconnect monitor (Phase 2)
|
||||
if self.auto_reconnect_manager:
|
||||
self.auto_reconnect_manager.start()
|
||||
|
||||
@ -171,7 +220,7 @@ class RSIClient:
|
||||
except KeyboardInterrupt:
|
||||
self.stop()
|
||||
except Exception as e:
|
||||
logging.error(f"RSI Client encountered an error: {e}")
|
||||
logging.error("RSI Client encountered an error: %s", e)
|
||||
self._transition_to(ClientState.ERROR)
|
||||
raise
|
||||
|
||||
@ -183,7 +232,6 @@ class RSIClient:
|
||||
|
||||
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...")
|
||||
|
||||
@ -201,10 +249,15 @@ class RSIClient:
|
||||
self.thread.join(timeout=2)
|
||||
self.thread = None
|
||||
|
||||
# Stop auto-reconnect monitor (Phase 2)
|
||||
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")
|
||||
|
||||
@ -217,7 +270,6 @@ class RSIClient:
|
||||
"""
|
||||
logging.info("Reconnecting RSI Client network...")
|
||||
|
||||
# Stop if currently running
|
||||
if self.state in (ClientState.RUNNING, ClientState.STARTING):
|
||||
self.stop()
|
||||
|
||||
@ -226,74 +278,59 @@ class RSIClient:
|
||||
self.network_process.terminate()
|
||||
self.network_process.join()
|
||||
|
||||
# Reset to initialized state
|
||||
# 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
|
||||
|
||||
# Fresh new events and queue
|
||||
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)
|
||||
|
||||
# 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()
|
||||
self._create_network_process(network_settings)
|
||||
|
||||
# Fresh control thread
|
||||
self.thread = Thread(target=self.start, daemon=True)
|
||||
self.thread.start()
|
||||
|
||||
def is_running(self) -> bool:
|
||||
def wait_for_connection(self, timeout: float = 10.0) -> bool:
|
||||
"""
|
||||
Check if client is in running state.
|
||||
Block until the first valid packet is received from the robot.
|
||||
|
||||
Args:
|
||||
timeout: Maximum time to wait in seconds
|
||||
|
||||
Returns:
|
||||
True if currently running
|
||||
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:
|
||||
"""
|
||||
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
|
||||
|
||||
@ -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 <Sen> XML to robot state tags in <Rob> 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"]:
|
||||
|
||||
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 tag == "RKorr" and axis in self.state["RIst"]:
|
||||
# Apply Cartesian correction
|
||||
if self.mode == "relative":
|
||||
self.state["RIst"][axis] += value
|
||||
self.state[state_key][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())
|
||||
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"]:
|
||||
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, 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 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()
|
||||
|
||||
@ -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]:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -1,4 +1,7 @@
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
from typing import Dict, Any, List, Tuple, Optional
|
||||
|
||||
|
||||
class XMLGenerator:
|
||||
"""
|
||||
@ -20,10 +23,9 @@ class XMLGenerator:
|
||||
"""
|
||||
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
|
||||
continue
|
||||
|
||||
if isinstance(value, dict):
|
||||
element = ET.SubElement(root, key)
|
||||
@ -56,3 +58,141 @@ class XMLGenerator:
|
||||
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}}}</{key}>")
|
||||
|
||||
# Ensure IPOC is always in template (required by robot)
|
||||
if "IPOC" not in variables:
|
||||
self._keys.append(("IPOC", None))
|
||||
parts.append("<IPOC>{IPOC}</IPOC>")
|
||||
|
||||
parts.append(f"</{root_tag}>")
|
||||
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)}>([^<]*)</{re.escape(key)}>")
|
||||
self._scalar_patterns.append((key, pattern))
|
||||
|
||||
# Always parse IPOC specifically
|
||||
self._ipoc_pattern = re.compile(r"<IPOC>(\d+)</IPOC>")
|
||||
|
||||
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))
|
||||
|
||||
Binary file not shown.
615
test_server.py
Normal file
615
test_server.py
Normal file
@ -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 <Sen> XML
|
||||
from a client, and responds with <Rob> 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 = "</IPOC>"
|
||||
start_tag = "<IPOC>"
|
||||
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("<Enter>", lambda e, b=b, h=hover: b.config(bg=h))
|
||||
b.bind("<Leave>", 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("<Return>", self._step_changed)
|
||||
step_e.bind("<FocusOut>", 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()
|
||||
Binary file not shown.
261
tests/test_config_parser.py
Normal file
261
tests/test_config_parser.py
Normal file
@ -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 <CONFIG>."""
|
||||
|
||||
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("""<ROOT>
|
||||
<CONFIG>
|
||||
<IP_NUMBER>127.0.0.1</IP_NUMBER>
|
||||
<PORT>12345</PORT>
|
||||
<SENTYPE>Test</SENTYPE>
|
||||
<ONLYSEND>FALSE</ONLYSEND>
|
||||
</CONFIG>
|
||||
<SEND><ELEMENTS>
|
||||
<ELEMENT TAG="DEF_RIst" TYPE="DOUBLE" INDX="INTERNAL" />
|
||||
</ELEMENTS></SEND>
|
||||
<RECEIVE><ELEMENTS>
|
||||
<ELEMENT TAG="RKorr.X" TYPE="DOUBLE" INDX="1" />
|
||||
</ELEMENTS></RECEIVE>
|
||||
</ROOT>""")
|
||||
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
|
||||
184
tests/test_io_api.py
Normal file
184
tests/test_io_api.py
Normal file
@ -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
|
||||
439
tests/test_motion_api.py
Normal file
439
tests/test_motion_api.py
Normal file
@ -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)
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user