cristhian aguilera
8e3d976a17
Migrates the ulite6 Python dora node from dora_littlehand into a
self-contained repository following the dora-zed structure. Adds
command lifecycle state ("started"/"done"/"failed") and type
("command"/"error") to the status output so coordinator nodes can
reliably sequence robot actions without polling.
1021 lines
35 KiB
Python
1021 lines
35 KiB
Python
"""Dora node for controlling a UFactory Lite6 robot and publishing state."""
|
|
|
|
import json
|
|
import os
|
|
import threading
|
|
import time
|
|
from typing import Any, Dict, Optional
|
|
|
|
import numpy as np
|
|
import pyarrow as pa
|
|
import uvicorn
|
|
from dora import Node
|
|
from fastapi import FastAPI, HTTPException
|
|
from fastapi.responses import HTMLResponse
|
|
from pydantic import BaseModel
|
|
from scipy.spatial.transform import Rotation
|
|
from xarm.wrapper import XArmAPI
|
|
|
|
|
|
class ULite6Helper:
|
|
"""Minimal ULite6 helper based on ufactory-control."""
|
|
|
|
def __init__(self, robot_ip: str):
|
|
self.robot_ip = robot_ip
|
|
self.arm = XArmAPI(self.robot_ip)
|
|
self.arm.connect()
|
|
self.arm.motion_enable(enable=True)
|
|
|
|
def go_home(self) -> int:
|
|
self.arm.set_mode(0)
|
|
self.arm.set_state(state=0)
|
|
code = self.arm.move_gohome(wait=True)
|
|
# move_gohome may return tuple (code, result) in some versions
|
|
if isinstance(code, tuple):
|
|
code = code[0]
|
|
return code
|
|
|
|
def move_to_pose(
|
|
self,
|
|
x: float,
|
|
y: float,
|
|
z: float,
|
|
roll: float,
|
|
pitch: float,
|
|
yaw: float,
|
|
speed: float = 30.0,
|
|
units: str = "mm",
|
|
) -> int:
|
|
if units == "m":
|
|
x, y, z = x * 1000.0, y * 1000.0, z * 1000.0
|
|
self.arm.set_mode(0)
|
|
self.arm.set_state(0)
|
|
return self.arm.set_position(
|
|
x, y, z, roll=roll, pitch=pitch, yaw=yaw, speed=speed, wait=True
|
|
)
|
|
|
|
def get_current_position(self) -> Dict[str, float]:
|
|
code, pos = self.arm.get_position()
|
|
if code != 0:
|
|
raise RuntimeError(f"get_position failed with code {code}")
|
|
return {
|
|
"x": pos[0],
|
|
"y": pos[1],
|
|
"z": pos[2],
|
|
"roll": pos[3],
|
|
"pitch": pos[4],
|
|
"yaw": pos[5],
|
|
}
|
|
|
|
def get_pose_matrix(self, tcp_offset_mm: float = 0.0) -> np.ndarray:
|
|
pos = self.get_current_position()
|
|
R = Rotation.from_euler(
|
|
"xyz", [pos["roll"], pos["pitch"], pos["yaw"]], degrees=True
|
|
).as_matrix()
|
|
|
|
position_m = np.array([pos["x"], pos["y"], pos["z"]]) / 1000.0
|
|
if tcp_offset_mm != 0.0:
|
|
tcp_offset_local = np.array([0, 0, tcp_offset_mm]) / 1000.0
|
|
tcp_offset_global = R @ tcp_offset_local
|
|
position_m = position_m + tcp_offset_global
|
|
|
|
T = np.eye(4)
|
|
T[:3, :3] = R
|
|
T[:3, 3] = position_m
|
|
return T
|
|
|
|
def get_joint_angles_deg(self) -> np.ndarray:
|
|
code, angles = self.arm.get_servo_angle(is_radian=False)
|
|
if code != 0:
|
|
raise RuntimeError(f"get_servo_angle failed with code {code}")
|
|
return np.array(angles, dtype=float)
|
|
|
|
def get_status(self) -> Dict[str, Any]:
|
|
"""Get robot status information."""
|
|
return {
|
|
"connected": self.arm.connected,
|
|
"state": self.arm.state,
|
|
"mode": self.arm.mode,
|
|
"error_code": self.arm.error_code,
|
|
"warn_code": self.arm.warn_code,
|
|
"has_error": self.arm.has_error,
|
|
"has_warn": self.arm.has_warn,
|
|
"motor_enable_states": self.arm.motor_enable_states,
|
|
}
|
|
|
|
def reset_state(self) -> int:
|
|
"""Clear errors/warnings and reset robot state."""
|
|
self.arm.clean_error()
|
|
self.arm.clean_warn()
|
|
self.arm.motion_enable(enable=True)
|
|
self.arm.set_mode(0)
|
|
return self.arm.set_state(0)
|
|
|
|
def set_vacuum_gripper(self, on: bool) -> int:
|
|
"""Turn vacuum gripper on or off."""
|
|
code = self.arm.set_vacuum_gripper(on)
|
|
if isinstance(code, tuple):
|
|
code = code[0]
|
|
return code
|
|
|
|
def get_vacuum_gripper(self) -> Dict[str, Any]:
|
|
"""Get vacuum gripper status."""
|
|
code, status = self.arm.get_vacuum_gripper()
|
|
return {"code": code, "on": status == 1}
|
|
|
|
def disconnect(self) -> int:
|
|
return self.arm.disconnect()
|
|
|
|
|
|
# --- Pydantic models for API requests ---
|
|
|
|
|
|
class MoveToRequest(BaseModel):
|
|
x: float
|
|
y: float
|
|
z: float
|
|
speed: Optional[float] = None
|
|
roll: Optional[float] = 180.0
|
|
pitch: Optional[float] = 0.0
|
|
yaw: Optional[float] = 0.0
|
|
|
|
|
|
class MoveToPoseRequest(BaseModel):
|
|
x: float
|
|
y: float
|
|
z: float
|
|
roll: float
|
|
pitch: float
|
|
yaw: float
|
|
speed: Optional[float] = None
|
|
|
|
|
|
# --- FastAPI app factory ---
|
|
|
|
|
|
def create_api(
|
|
helper: ULite6Helper, default_speed: float, default_units: str, vacuum_enabled: bool
|
|
) -> FastAPI:
|
|
"""Create FastAPI application with robot control endpoints."""
|
|
app = FastAPI(
|
|
title="ULite6 Robot Control API",
|
|
description="REST API for controlling UFactory Lite6 robot",
|
|
version="0.1.0",
|
|
)
|
|
|
|
@app.get("/", response_class=HTMLResponse)
|
|
def web_ui():
|
|
"""Serve web control interface."""
|
|
return """<!DOCTYPE html>
|
|
<html lang="en">
|
|
<head>
|
|
<meta charset="UTF-8">
|
|
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
|
<title>ULite6 Control</title>
|
|
<style>
|
|
* { box-sizing: border-box; margin: 0; padding: 0; }
|
|
body {
|
|
font-family: -apple-system, BlinkMacSystemFont, 'Segoe UI', Roboto, sans-serif;
|
|
background: #1a1a2e;
|
|
color: #eee;
|
|
min-height: 100vh;
|
|
padding: 20px;
|
|
}
|
|
h1 {
|
|
text-align: center;
|
|
color: #00d4ff;
|
|
margin-bottom: 20px;
|
|
font-size: 1.8rem;
|
|
}
|
|
.container {
|
|
max-width: 800px;
|
|
margin: 0 auto;
|
|
display: grid;
|
|
gap: 16px;
|
|
}
|
|
.card {
|
|
background: #16213e;
|
|
border-radius: 8px;
|
|
padding: 16px;
|
|
border: 1px solid #0f3460;
|
|
}
|
|
.card h2 {
|
|
color: #00d4ff;
|
|
font-size: 1rem;
|
|
margin-bottom: 12px;
|
|
border-bottom: 1px solid #0f3460;
|
|
padding-bottom: 8px;
|
|
}
|
|
.status-grid {
|
|
display: grid;
|
|
grid-template-columns: repeat(auto-fit, minmax(120px, 1fr));
|
|
gap: 8px;
|
|
}
|
|
.status-item {
|
|
background: #0f3460;
|
|
padding: 8px;
|
|
border-radius: 4px;
|
|
text-align: center;
|
|
}
|
|
.status-item .label {
|
|
font-size: 0.75rem;
|
|
color: #888;
|
|
margin-bottom: 4px;
|
|
}
|
|
.status-item .value {
|
|
font-size: 0.9rem;
|
|
font-weight: 600;
|
|
}
|
|
.status-item .value.ok { color: #4ade80; }
|
|
.status-item .value.error { color: #f87171; }
|
|
.pose-grid {
|
|
display: grid;
|
|
grid-template-columns: repeat(6, 1fr);
|
|
gap: 8px;
|
|
}
|
|
.pose-item {
|
|
background: #0f3460;
|
|
padding: 8px;
|
|
border-radius: 4px;
|
|
text-align: center;
|
|
}
|
|
.pose-item .label {
|
|
font-size: 0.7rem;
|
|
color: #888;
|
|
}
|
|
.pose-item .value {
|
|
font-size: 0.85rem;
|
|
font-family: monospace;
|
|
}
|
|
.joints-grid {
|
|
display: grid;
|
|
grid-template-columns: repeat(6, 1fr);
|
|
gap: 8px;
|
|
margin-top: 12px;
|
|
}
|
|
.btn-group {
|
|
display: flex;
|
|
gap: 10px;
|
|
flex-wrap: wrap;
|
|
}
|
|
button {
|
|
background: #0f3460;
|
|
color: #fff;
|
|
border: 1px solid #00d4ff;
|
|
padding: 10px 20px;
|
|
border-radius: 4px;
|
|
cursor: pointer;
|
|
font-size: 0.9rem;
|
|
transition: all 0.2s;
|
|
}
|
|
button:hover { background: #00d4ff; color: #1a1a2e; }
|
|
button:disabled { opacity: 0.5; cursor: not-allowed; }
|
|
button.danger { border-color: #f87171; }
|
|
button.danger:hover { background: #f87171; }
|
|
.form-grid {
|
|
display: grid;
|
|
grid-template-columns: repeat(auto-fit, minmax(100px, 1fr));
|
|
gap: 10px;
|
|
margin-bottom: 12px;
|
|
}
|
|
.form-group {
|
|
display: flex;
|
|
flex-direction: column;
|
|
}
|
|
.form-group label {
|
|
font-size: 0.75rem;
|
|
color: #888;
|
|
margin-bottom: 4px;
|
|
}
|
|
.form-group input {
|
|
background: #0f3460;
|
|
border: 1px solid #16213e;
|
|
color: #fff;
|
|
padding: 8px;
|
|
border-radius: 4px;
|
|
font-size: 0.9rem;
|
|
}
|
|
.form-group input:focus {
|
|
outline: none;
|
|
border-color: #00d4ff;
|
|
}
|
|
#log {
|
|
background: #0a0a15;
|
|
border-radius: 4px;
|
|
padding: 12px;
|
|
font-family: monospace;
|
|
font-size: 0.8rem;
|
|
max-height: 150px;
|
|
overflow-y: auto;
|
|
}
|
|
.log-entry {
|
|
padding: 4px 0;
|
|
border-bottom: 1px solid #16213e;
|
|
}
|
|
.log-entry:last-child { border-bottom: none; }
|
|
.log-entry.success { color: #4ade80; }
|
|
.log-entry.error { color: #f87171; }
|
|
.log-entry .time { color: #666; margin-right: 8px; }
|
|
</style>
|
|
</head>
|
|
<body>
|
|
<h1>ULite6 Robot Control</h1>
|
|
<div class="container">
|
|
<div class="card">
|
|
<h2>Status</h2>
|
|
<div id="status" class="status-grid">
|
|
<div class="status-item">
|
|
<div class="label">Connected</div>
|
|
<div class="value" id="st-connected">--</div>
|
|
</div>
|
|
<div class="status-item">
|
|
<div class="label">State</div>
|
|
<div class="value" id="st-state">--</div>
|
|
</div>
|
|
<div class="status-item">
|
|
<div class="label">Mode</div>
|
|
<div class="value" id="st-mode">--</div>
|
|
</div>
|
|
<div class="status-item">
|
|
<div class="label">Error</div>
|
|
<div class="value" id="st-error">--</div>
|
|
</div>
|
|
<div class="status-item">
|
|
<div class="label">Warning</div>
|
|
<div class="value" id="st-warn">--</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div class="card">
|
|
<h2>TCP Pose (mm, deg)</h2>
|
|
<div class="pose-grid">
|
|
<div class="pose-item"><div class="label">X</div><div class="value" id="pose-x">--</div></div>
|
|
<div class="pose-item"><div class="label">Y</div><div class="value" id="pose-y">--</div></div>
|
|
<div class="pose-item"><div class="label">Z</div><div class="value" id="pose-z">--</div></div>
|
|
<div class="pose-item"><div class="label">Roll</div><div class="value" id="pose-roll">--</div></div>
|
|
<div class="pose-item"><div class="label">Pitch</div><div class="value" id="pose-pitch">--</div></div>
|
|
<div class="pose-item"><div class="label">Yaw</div><div class="value" id="pose-yaw">--</div></div>
|
|
</div>
|
|
<h2 style="margin-top: 16px;">Joint Angles (deg)</h2>
|
|
<div class="joints-grid">
|
|
<div class="pose-item"><div class="label">J1</div><div class="value" id="j0">--</div></div>
|
|
<div class="pose-item"><div class="label">J2</div><div class="value" id="j1">--</div></div>
|
|
<div class="pose-item"><div class="label">J3</div><div class="value" id="j2">--</div></div>
|
|
<div class="pose-item"><div class="label">J4</div><div class="value" id="j3">--</div></div>
|
|
<div class="pose-item"><div class="label">J5</div><div class="value" id="j4">--</div></div>
|
|
<div class="pose-item"><div class="label">J6</div><div class="value" id="j5">--</div></div>
|
|
</div>
|
|
</div>
|
|
|
|
<div class="card">
|
|
<h2>Controls</h2>
|
|
<div class="btn-group" style="margin-bottom: 16px;">
|
|
<button onclick="goHome()" id="btn-home">Home</button>
|
|
<button onclick="resetState()" id="btn-reset">Reset</button>
|
|
</div>
|
|
<div id="vacuum-controls" style="display: none; margin-bottom: 16px;">
|
|
<h2>Vacuum Gripper</h2>
|
|
<div class="btn-group" style="margin-top: 8px;">
|
|
<button onclick="vacuumOn()" id="btn-vacuum-on">Vacuum ON</button>
|
|
<button onclick="vacuumOff()" id="btn-vacuum-off">Vacuum OFF</button>
|
|
<span id="vacuum-status" style="margin-left: 12px; padding: 8px;">Status: --</span>
|
|
</div>
|
|
</div>
|
|
<h2>Move To Position</h2>
|
|
<form id="moveForm" onsubmit="moveTo(event)">
|
|
<div class="form-grid">
|
|
<div class="form-group">
|
|
<label for="move-x">X (mm)</label>
|
|
<input type="number" id="move-x" step="0.1" required>
|
|
</div>
|
|
<div class="form-group">
|
|
<label for="move-y">Y (mm)</label>
|
|
<input type="number" id="move-y" step="0.1" required>
|
|
</div>
|
|
<div class="form-group">
|
|
<label for="move-z">Z (mm)</label>
|
|
<input type="number" id="move-z" step="0.1" required>
|
|
</div>
|
|
<div class="form-group">
|
|
<label for="move-speed">Speed (mm/s)</label>
|
|
<input type="number" id="move-speed" step="1" value="30" min="1" max="200">
|
|
</div>
|
|
</div>
|
|
<button type="submit" id="btn-move">Move</button>
|
|
</form>
|
|
</div>
|
|
|
|
<div class="card">
|
|
<h2>Response Log</h2>
|
|
<div id="log"></div>
|
|
</div>
|
|
</div>
|
|
|
|
<script>
|
|
const $ = id => document.getElementById(id);
|
|
|
|
function log(msg, isError = false) {
|
|
const logEl = $('log');
|
|
const time = new Date().toLocaleTimeString();
|
|
const entry = document.createElement('div');
|
|
entry.className = 'log-entry ' + (isError ? 'error' : 'success');
|
|
entry.innerHTML = '<span class="time">' + time + '</span>' + msg;
|
|
logEl.insertBefore(entry, logEl.firstChild);
|
|
if (logEl.children.length > 50) logEl.removeChild(logEl.lastChild);
|
|
}
|
|
|
|
async function fetchJson(url, opts = {}) {
|
|
try {
|
|
const res = await fetch(url, opts);
|
|
return await res.json();
|
|
} catch (e) {
|
|
return { error: e.message };
|
|
}
|
|
}
|
|
|
|
async function updateStatus() {
|
|
const data = await fetchJson('/api/status');
|
|
if (data.error) {
|
|
$('st-connected').textContent = 'Error';
|
|
$('st-connected').className = 'value error';
|
|
return;
|
|
}
|
|
$('st-connected').textContent = data.connected ? 'Yes' : 'No';
|
|
$('st-connected').className = 'value ' + (data.connected ? 'ok' : 'error');
|
|
$('st-state').textContent = data.state;
|
|
$('st-mode').textContent = data.mode;
|
|
$('st-error').textContent = data.error_code;
|
|
$('st-error').className = 'value ' + (data.error_code === 0 ? 'ok' : 'error');
|
|
$('st-warn').textContent = data.warn_code;
|
|
$('st-warn').className = 'value ' + (data.warn_code === 0 ? 'ok' : 'error');
|
|
}
|
|
|
|
async function updatePose() {
|
|
const pose = await fetchJson('/api/pose');
|
|
if (!pose.error) {
|
|
$('pose-x').textContent = pose.x.toFixed(1);
|
|
$('pose-y').textContent = pose.y.toFixed(1);
|
|
$('pose-z').textContent = pose.z.toFixed(1);
|
|
$('pose-roll').textContent = pose.roll.toFixed(1);
|
|
$('pose-pitch').textContent = pose.pitch.toFixed(1);
|
|
$('pose-yaw').textContent = pose.yaw.toFixed(1);
|
|
}
|
|
const joints = await fetchJson('/api/joints');
|
|
if (!joints.error && joints.joints) {
|
|
for (let i = 0; i < 6; i++) {
|
|
$('j' + i).textContent = joints.joints[i].toFixed(1);
|
|
}
|
|
}
|
|
}
|
|
|
|
async function goHome() {
|
|
$('btn-home').disabled = true;
|
|
log('Sending Home command...');
|
|
const res = await fetchJson('/api/home', { method: 'POST' });
|
|
$('btn-home').disabled = false;
|
|
if (res.ok) {
|
|
log('Home completed (code: ' + res.code + ')');
|
|
} else {
|
|
log('Home failed: ' + (res.detail || res.code), true);
|
|
}
|
|
}
|
|
|
|
async function resetState() {
|
|
$('btn-reset').disabled = true;
|
|
log('Resetting robot state...');
|
|
const res = await fetchJson('/api/reset', { method: 'POST' });
|
|
$('btn-reset').disabled = false;
|
|
if (res.ok) {
|
|
log('Reset completed (code: ' + res.code + ')');
|
|
} else {
|
|
log('Reset failed: ' + (res.detail || res.code), true);
|
|
}
|
|
}
|
|
|
|
async function moveTo(e) {
|
|
e.preventDefault();
|
|
const x = parseFloat($('move-x').value);
|
|
const y = parseFloat($('move-y').value);
|
|
const z = parseFloat($('move-z').value);
|
|
const speed = parseFloat($('move-speed').value) || 30;
|
|
|
|
$('btn-move').disabled = true;
|
|
log('Moving to (' + x + ', ' + y + ', ' + z + ') at ' + speed + ' mm/s...');
|
|
|
|
const res = await fetchJson('/api/move_to', {
|
|
method: 'POST',
|
|
headers: { 'Content-Type': 'application/json' },
|
|
body: JSON.stringify({ x, y, z, speed })
|
|
});
|
|
|
|
$('btn-move').disabled = false;
|
|
if (res.ok) {
|
|
log('Move completed (code: ' + res.code + ')');
|
|
} else {
|
|
log('Move failed: ' + (res.detail || res.code), true);
|
|
}
|
|
}
|
|
|
|
// Vacuum gripper functions
|
|
let vacuumEnabled = false;
|
|
|
|
async function initConfig() {
|
|
const config = await fetchJson('/api/config');
|
|
if (config.vacuum_enabled) {
|
|
vacuumEnabled = true;
|
|
$('vacuum-controls').style.display = 'block';
|
|
updateVacuumStatus();
|
|
setInterval(updateVacuumStatus, 1000);
|
|
}
|
|
}
|
|
|
|
async function updateVacuumStatus() {
|
|
if (!vacuumEnabled) return;
|
|
const data = await fetchJson('/api/vacuum');
|
|
if (!data.error) {
|
|
const statusEl = $('vacuum-status');
|
|
statusEl.textContent = 'Status: ' + (data.on ? 'ON' : 'OFF');
|
|
statusEl.style.color = data.on ? '#4ade80' : '#888';
|
|
}
|
|
}
|
|
|
|
async function vacuumOn() {
|
|
$('btn-vacuum-on').disabled = true;
|
|
log('Turning vacuum ON...');
|
|
const res = await fetchJson('/api/vacuum/on', { method: 'POST' });
|
|
$('btn-vacuum-on').disabled = false;
|
|
if (res.ok) {
|
|
log('Vacuum ON (code: ' + res.code + ')');
|
|
updateVacuumStatus();
|
|
} else {
|
|
log('Vacuum ON failed: ' + (res.detail || res.code), true);
|
|
}
|
|
}
|
|
|
|
async function vacuumOff() {
|
|
$('btn-vacuum-off').disabled = true;
|
|
log('Turning vacuum OFF...');
|
|
const res = await fetchJson('/api/vacuum/off', { method: 'POST' });
|
|
$('btn-vacuum-off').disabled = false;
|
|
if (res.ok) {
|
|
log('Vacuum OFF (code: ' + res.code + ')');
|
|
updateVacuumStatus();
|
|
} else {
|
|
log('Vacuum OFF failed: ' + (res.detail || res.code), true);
|
|
}
|
|
}
|
|
|
|
// Auto-refresh
|
|
setInterval(updateStatus, 1000);
|
|
setInterval(updatePose, 500);
|
|
updateStatus();
|
|
updatePose();
|
|
initConfig();
|
|
</script>
|
|
</body>
|
|
</html>"""
|
|
|
|
@app.get("/api/status")
|
|
def get_status():
|
|
"""Get robot status (connected, enabled, errors)."""
|
|
try:
|
|
return helper.get_status()
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.get("/api/pose")
|
|
def get_pose():
|
|
"""Get current TCP pose [x, y, z, roll, pitch, yaw]."""
|
|
try:
|
|
return helper.get_current_position()
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.get("/api/joints")
|
|
def get_joints():
|
|
"""Get current joint angles in degrees."""
|
|
try:
|
|
angles = helper.get_joint_angles_deg()
|
|
return {"joints": angles.tolist(), "units": "deg"}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.post("/api/home")
|
|
def go_home():
|
|
"""Send robot to home position."""
|
|
try:
|
|
code = helper.go_home()
|
|
return {"ok": code == 0, "code": code}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.post("/api/move_to")
|
|
def move_to(request: MoveToRequest):
|
|
"""Move to position with optional orientation."""
|
|
try:
|
|
speed = request.speed if request.speed is not None else default_speed
|
|
code = helper.move_to_pose(
|
|
request.x,
|
|
request.y,
|
|
request.z,
|
|
request.roll,
|
|
request.pitch,
|
|
request.yaw,
|
|
speed,
|
|
default_units,
|
|
)
|
|
return {"ok": code == 0, "code": code}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.post("/api/move_to_pose")
|
|
def move_to_pose(request: MoveToPoseRequest):
|
|
"""Move to full pose with position and orientation."""
|
|
try:
|
|
speed = request.speed if request.speed is not None else default_speed
|
|
code = helper.move_to_pose(
|
|
request.x,
|
|
request.y,
|
|
request.z,
|
|
request.roll,
|
|
request.pitch,
|
|
request.yaw,
|
|
speed,
|
|
default_units,
|
|
)
|
|
return {"ok": code == 0, "code": code}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.post("/api/reset")
|
|
def reset_state():
|
|
"""Clear errors/warnings and reset robot state."""
|
|
try:
|
|
code = helper.reset_state()
|
|
return {"ok": code == 0, "code": code}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.post("/api/disconnect")
|
|
def disconnect():
|
|
"""Disconnect from robot."""
|
|
try:
|
|
code = helper.disconnect()
|
|
return {"ok": code == 0, "code": code}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
# Vacuum gripper endpoints (only if enabled)
|
|
if vacuum_enabled:
|
|
|
|
@app.get("/api/vacuum")
|
|
def get_vacuum():
|
|
"""Get vacuum gripper status."""
|
|
try:
|
|
return helper.get_vacuum_gripper()
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.post("/api/vacuum/on")
|
|
def vacuum_on():
|
|
"""Turn vacuum gripper on."""
|
|
try:
|
|
code = helper.set_vacuum_gripper(True)
|
|
return {"ok": code == 0, "code": code}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.post("/api/vacuum/off")
|
|
def vacuum_off():
|
|
"""Turn vacuum gripper off."""
|
|
try:
|
|
code = helper.set_vacuum_gripper(False)
|
|
return {"ok": code == 0, "code": code}
|
|
except Exception as e:
|
|
raise HTTPException(status_code=500, detail=str(e))
|
|
|
|
@app.get("/api/config")
|
|
def get_config():
|
|
"""Get API configuration."""
|
|
return {"vacuum_enabled": vacuum_enabled}
|
|
|
|
return app
|
|
|
|
|
|
def run_uvicorn(app: FastAPI, host: str, port: int) -> None:
|
|
"""Run uvicorn server (for use in background thread)."""
|
|
config = uvicorn.Config(app, host=host, port=port, log_level="warning")
|
|
server = uvicorn.Server(config)
|
|
server.run()
|
|
|
|
|
|
def _send_joint_pose(node: Node, joints_deg: np.ndarray) -> None:
|
|
metadata = {"units": "deg", "timestamp_ns": time.time_ns()}
|
|
node.send_output("joint_pose", pa.array(joints_deg.tolist()), metadata=metadata)
|
|
|
|
|
|
def _send_tcp_pose(node: Node, tcp_pose: Dict[str, float]) -> None:
|
|
metadata = {
|
|
"units_position": "mm",
|
|
"units_rotation": "deg",
|
|
"timestamp_ns": time.time_ns(),
|
|
}
|
|
vec = [
|
|
tcp_pose["x"],
|
|
tcp_pose["y"],
|
|
tcp_pose["z"],
|
|
tcp_pose["roll"],
|
|
tcp_pose["pitch"],
|
|
tcp_pose["yaw"],
|
|
]
|
|
node.send_output("tcp_pose", pa.array(vec), metadata=metadata)
|
|
|
|
|
|
def _send_error_status(
|
|
node: Node, action: str, message: str, status: Optional[Dict[str, Any]] = None
|
|
) -> None:
|
|
payload = {
|
|
"ok": False,
|
|
"action": action,
|
|
"message": message,
|
|
"timestamp_ns": time.time_ns(),
|
|
}
|
|
if status:
|
|
payload.update(status)
|
|
payload["type"] = "error"
|
|
node.send_output("status", pa.array([json.dumps(payload)]))
|
|
|
|
|
|
def _send_command_status(
|
|
node: Node,
|
|
command_id: str,
|
|
action: str,
|
|
ok: bool,
|
|
message: str,
|
|
state: str,
|
|
code: Optional[int] = None,
|
|
status: Optional[Dict[str, Any]] = None,
|
|
) -> None:
|
|
payload = {
|
|
"ok": ok,
|
|
"action": action,
|
|
"message": message,
|
|
"timestamp_ns": time.time_ns(),
|
|
"command_id": command_id,
|
|
}
|
|
if code is not None:
|
|
payload["code"] = code
|
|
if status:
|
|
payload.update(status)
|
|
payload["type"] = "command"
|
|
payload["state"] = state
|
|
node.send_output("status", pa.array([json.dumps(payload)]))
|
|
|
|
|
|
def _status_snapshot(helper: ULite6Helper) -> Dict[str, Any]:
|
|
status = helper.get_status()
|
|
return {
|
|
"robot_state": status.get("state"),
|
|
"mode": status.get("mode"),
|
|
"error_code": status.get("error_code"),
|
|
"warn_code": status.get("warn_code"),
|
|
"has_error": status.get("has_error"),
|
|
"has_warn": status.get("has_warn"),
|
|
}
|
|
|
|
|
|
def _log(msg: str) -> None:
|
|
"""Print a timestamped log message."""
|
|
timestamp = time.strftime("%H:%M:%S")
|
|
print(f"[ulite6 {timestamp}] {msg}", flush=True)
|
|
|
|
|
|
def main() -> None:
|
|
node = Node()
|
|
|
|
robot_ip = os.getenv("ROBOT_IP", "192.168.1.192")
|
|
default_speed = float(os.getenv("DEFAULT_SPEED", "30"))
|
|
default_units = os.getenv("DEFAULT_UNITS", "mm")
|
|
api_host = os.getenv("API_HOST", "0.0.0.0")
|
|
api_port = int(os.getenv("API_PORT", "8080"))
|
|
vacuum_enabled = os.getenv("VACUUM_ENABLED", "false").lower() in ("true", "1", "yes")
|
|
|
|
# Initial position settings
|
|
init_mode = os.getenv("INIT_MODE", "none").lower() # "home", "pose", or "none"
|
|
init_x = float(os.getenv("INIT_X", "300.0"))
|
|
init_y = float(os.getenv("INIT_Y", "0.0"))
|
|
init_z = float(os.getenv("INIT_Z", "250.0"))
|
|
init_roll = float(os.getenv("INIT_ROLL", "180.0"))
|
|
init_pitch = float(os.getenv("INIT_PITCH", "0.0"))
|
|
init_yaw = float(os.getenv("INIT_YAW", "0.0"))
|
|
init_speed = float(os.getenv("INIT_SPEED", "50.0"))
|
|
|
|
_log(f"Connecting to robot at {robot_ip}...")
|
|
helper = ULite6Helper(robot_ip)
|
|
_log("Robot connected")
|
|
|
|
# Move to initial position on startup
|
|
if init_mode == "home":
|
|
_log("Moving to home position...")
|
|
code = helper.go_home()
|
|
if code == 0:
|
|
_log("Home position reached")
|
|
else:
|
|
_log(f"Home failed with code {code}")
|
|
elif init_mode == "pose":
|
|
_log(f"Moving to initial pose: [{init_x}, {init_y}, {init_z}] roll={init_roll} pitch={init_pitch} yaw={init_yaw}")
|
|
code = helper.move_to_pose(
|
|
init_x, init_y, init_z,
|
|
init_roll, init_pitch, init_yaw,
|
|
speed=init_speed,
|
|
units="mm",
|
|
)
|
|
if code == 0:
|
|
_log("Initial pose reached")
|
|
else:
|
|
_log(f"Move to initial pose failed with code {code}")
|
|
else:
|
|
_log("Skipping initial position (INIT_MODE=none)")
|
|
|
|
# Create and start FastAPI server in background thread
|
|
app = create_api(helper, default_speed, default_units, vacuum_enabled)
|
|
api_thread = threading.Thread(
|
|
target=run_uvicorn, args=(app, api_host, api_port), daemon=True
|
|
)
|
|
api_thread.start()
|
|
_log(f"Web interface at http://{api_host}:{api_port}")
|
|
|
|
# Dora event loop - only handles tick for state publishing
|
|
for event in node:
|
|
if event["type"] != "INPUT":
|
|
continue
|
|
|
|
if event["id"] == "tick":
|
|
try:
|
|
joints = helper.get_joint_angles_deg()
|
|
tcp_pose = helper.get_current_position()
|
|
_send_joint_pose(node, joints)
|
|
_send_tcp_pose(node, tcp_pose)
|
|
except Exception as exc:
|
|
_send_error_status(
|
|
node, "publish_state", str(exc), status=_status_snapshot(helper)
|
|
)
|
|
elif event["id"] == "command":
|
|
try:
|
|
value = event["value"]
|
|
raw = value[0].as_py() if len(value) else None
|
|
if not raw:
|
|
_send_command_status(
|
|
node,
|
|
command_id="",
|
|
action="command",
|
|
ok=False,
|
|
message="Empty command payload",
|
|
state="failed",
|
|
status=_status_snapshot(helper),
|
|
)
|
|
continue
|
|
payload = json.loads(raw)
|
|
except Exception as exc:
|
|
_send_command_status(
|
|
node,
|
|
command_id="",
|
|
action="command",
|
|
ok=False,
|
|
message=f"Invalid command payload: {exc}",
|
|
state="failed",
|
|
status=_status_snapshot(helper),
|
|
)
|
|
continue
|
|
|
|
command_id = str(payload.get("id", ""))
|
|
action = str(payload.get("action", ""))
|
|
data = payload.get("payload", {}) if isinstance(payload.get("payload", {}), dict) else {}
|
|
|
|
if not command_id:
|
|
_send_command_status(
|
|
node,
|
|
command_id="",
|
|
action=action or "command",
|
|
ok=False,
|
|
message="Missing command id",
|
|
state="failed",
|
|
status=_status_snapshot(helper),
|
|
)
|
|
continue
|
|
|
|
try:
|
|
if action == "home":
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action,
|
|
ok=True,
|
|
message="Executing home",
|
|
state="started",
|
|
)
|
|
code = helper.go_home()
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action,
|
|
ok=code == 0,
|
|
message="Home command executed",
|
|
state="done" if code == 0 else "failed",
|
|
code=code,
|
|
status=_status_snapshot(helper),
|
|
)
|
|
elif action in ("move_to", "move_to_pose"):
|
|
speed = float(data.get("speed", default_speed))
|
|
x = float(data.get("x", 0.0))
|
|
y = float(data.get("y", 0.0))
|
|
z = float(data.get("z", 0.0))
|
|
roll = float(data.get("roll", 180.0))
|
|
pitch = float(data.get("pitch", 0.0))
|
|
yaw = float(data.get("yaw", 0.0))
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action,
|
|
ok=True,
|
|
message="Executing move",
|
|
state="started",
|
|
)
|
|
code = helper.move_to_pose(
|
|
x,
|
|
y,
|
|
z,
|
|
roll,
|
|
pitch,
|
|
yaw,
|
|
speed,
|
|
default_units,
|
|
)
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action,
|
|
ok=code == 0,
|
|
message="Move command executed",
|
|
state="done" if code == 0 else "failed",
|
|
code=code,
|
|
status=_status_snapshot(helper),
|
|
)
|
|
elif action in ("vacuum_on", "vacuum_off"):
|
|
if not vacuum_enabled:
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action,
|
|
ok=False,
|
|
message="Vacuum gripper not enabled",
|
|
state="failed",
|
|
status=_status_snapshot(helper),
|
|
)
|
|
continue
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action,
|
|
ok=True,
|
|
message="Executing vacuum",
|
|
state="started",
|
|
)
|
|
vacuum_on = action == "vacuum_on"
|
|
code = helper.set_vacuum_gripper(vacuum_on)
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action,
|
|
ok=code == 0,
|
|
message="Vacuum command executed",
|
|
state="done" if code == 0 else "failed",
|
|
code=code,
|
|
status=_status_snapshot(helper),
|
|
)
|
|
else:
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action or "command",
|
|
ok=False,
|
|
message=f"Unknown action: {action}",
|
|
state="failed",
|
|
status=_status_snapshot(helper),
|
|
)
|
|
except Exception as exc:
|
|
_send_command_status(
|
|
node,
|
|
command_id=command_id,
|
|
action=action or "command",
|
|
ok=False,
|
|
message=str(exc),
|
|
state="failed",
|
|
status=_status_snapshot(helper),
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|