197 lines
7.1 KiB
Python
197 lines
7.1 KiB
Python
"""
|
|
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)
|