"""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 """ ULite6 Control

ULite6 Robot Control

Status

Connected
--
State
--
Mode
--
Error
--
Warning
--

TCP Pose (mm, deg)

X
--
Y
--
Z
--
Roll
--
Pitch
--
Yaw
--

Joint Angles (deg)

J1
--
J2
--
J3
--
J4
--
J5
--
J6
--

Controls

Move To Position

Response Log

""" @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()