First commit

dora_ulite6/README.md

@@ -0,0 +1,92 @@
+# Dora ULite6 Node
+
+Dora node for controlling a UFactory Lite6 robot via REST API, web UI, and publishing joint/TCP state.
+
+## Dataflow
+
+```yaml
+- id: ulite6
+  build: uv pip install -e dora_ulite6
+  path: dora_ulite6/dora_ulite6/main.py
+  inputs:
+    tick: dora/timer/millis/10
+  outputs: [status, joint_pose, tcp_pose]
+  env:
+    ROBOT_IP: "192.168.1.192"
+    DEFAULT_SPEED: "30"
+    DEFAULT_UNITS: "mm"
+    API_HOST: "0.0.0.0"
+    API_PORT: "8080"
+    VACUUM_ENABLED: "false"
+```
+
+## Configuration
+
+| Env Variable | Default | Description |
+|--------------|---------|-------------|
+| `ROBOT_IP` | `192.168.1.192` | Robot IP address |
+| `DEFAULT_SPEED` | `30` | Movement speed (mm/s) |
+| `DEFAULT_UNITS` | `mm` | Position units (mm or m) |
+| `API_HOST` | `0.0.0.0` | API server host |
+| `API_PORT` | `8080` | API server port |
+| `VACUUM_ENABLED` | `false` | Enable vacuum gripper controls |
+
+## Web UI
+
+Access at `http://localhost:8080/`
+
+- Live status, TCP pose, and joint angles
+- Home and Reset buttons
+- Move to position form
+- Vacuum gripper controls (when enabled)
+
+## REST API
+
+Interactive docs at `http://localhost:8080/docs`
+
+| Method | Endpoint | Description |
+|--------|----------|-------------|
+| GET | `/` | Web control interface |
+| GET | `/api/status` | Robot status (connected, errors) |
+| GET | `/api/pose` | Current TCP pose |
+| GET | `/api/joints` | Current joint angles |
+| GET | `/api/config` | API configuration |
+| POST | `/api/home` | Go to home position |
+| POST | `/api/reset` | Clear errors and reset state |
+| POST | `/api/move_to` | Move to position |
+| POST | `/api/move_to_pose` | Move to full pose |
+| POST | `/api/disconnect` | Disconnect from robot |
+
+### Vacuum Gripper (when `VACUUM_ENABLED=true`)
+
+| Method | Endpoint | Description |
+|--------|----------|-------------|
+| GET | `/api/vacuum` | Get vacuum status |
+| POST | `/api/vacuum/on` | Turn vacuum on |
+| POST | `/api/vacuum/off` | Turn vacuum off |
+
+## CLI Examples
+
+```bash
+# Get pose
+curl http://localhost:8080/api/pose
+
+# Go home
+curl -X POST http://localhost:8080/api/home
+
+# Move to position
+curl -X POST http://localhost:8080/api/move_to \
+  -H "Content-Type: application/json" \
+  -d '{"x": 200, "y": 0, "z": 300}'
+
+# Move with orientation
+curl -X POST http://localhost:8080/api/move_to_pose \
+  -H "Content-Type: application/json" \
+  -d '{"x": 200, "y": 0, "z": 300, "roll": 180, "pitch": 0, "yaw": 0}'
+```
+
+## Dora Outputs
+
+- `status` - JSON: `{ok, action, message, timestamp_ns}`
+- `joint_pose` - 6 joint angles in degrees
+- `tcp_pose` - `[x, y, z, roll, pitch, yaw]` in mm/deg

dora_ulite6/dora_ulite6/__init__.py

@@ -0,0 +1 @@
+"""Dora ULite6 node package."""

dora_ulite6/dora_ulite6/main.py

@@ -0,0 +1,780 @@
+"""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) -> None:
+    payload = {
+        "ok": False,
+        "action": action,
+        "message": message,
+        "timestamp_ns": time.time_ns(),
+    }
+    node.send_output("status", pa.array([json.dumps(payload)]))
+
+
+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")
+
+    helper = ULite6Helper(robot_ip)
+
+    # 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()
+
+    # 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))
+
+
+if __name__ == "__main__":
+    main()

dora_ulite6/pyproject.toml

@@ -0,0 +1,28 @@
+[project]
+name = "dora-ulite6"
+version = "0.1.0"
+license = "MIT"
+authors = [
+  { name = "Dora" }
+]
+description = "Dora node for controlling the UFactory Lite6 robot"
+
+requires-python = ">=3.8"
+
+dependencies = [
+  "dora-rs >= 0.3.9",
+  "numpy < 2.0.0",
+  "scipy >= 1.14.0",
+  "xarm-python-sdk >= 1.17.3",
+  "fastapi >= 0.109.0",
+  "uvicorn >= 0.27.0",
+]
+
+[dependency-groups]
+dev = ["pytest >=8.1.1", "ruff >=0.9.1"]
+
+[project.scripts]
+dora-ulite6 = "dora_ulite6.main:main"
+
+[tool.setuptools]
+packages = ["dora_ulite6"]