From 8e3d976a175d52b6403ce27911f0cb96fe78ec00 Mon Sep 17 00:00:00 2001 From: cristhian aguilera Date: Wed, 1 Apr 2026 08:51:19 -0300 Subject: [PATCH] add dora_ulite6 standalone node with command lifecycle status 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. --- .gitignore | 13 + AGENTS.md | 50 ++ CLAUDE.md | 3 + README.md | 115 ++++- dataflow.yml | 19 + dora_ulite6/__init__.py | 1 + dora_ulite6/main.py | 1020 +++++++++++++++++++++++++++++++++++++++ pyproject.toml | 28 ++ scripts/install.sh | 21 + 9 files changed, 1269 insertions(+), 1 deletion(-) create mode 100644 .gitignore create mode 100644 AGENTS.md create mode 100644 CLAUDE.md create mode 100644 dataflow.yml create mode 100644 dora_ulite6/__init__.py create mode 100644 dora_ulite6/main.py create mode 100644 pyproject.toml create mode 100755 scripts/install.sh diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..df85934 --- /dev/null +++ b/.gitignore @@ -0,0 +1,13 @@ +__pycache__/ +*.py[cod] +*$py.class +*.egg-info/ +dist/ +build/ +*.egg +.env +.venv +.pytest_cache/ +.coverage +.ruff_cache/ +out/ diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..6e1f426 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,50 @@ +# AGENTS.md + +This file provides guidance to AI coding assistants when working with code in this repository. + +## What this is + +A dora-rs Python node that interfaces with a UFactory Lite6 robot arm. On each tick it reads joint angles and TCP pose from the robot and publishes them as dora outputs. It also accepts JSON commands (home, move_to, move_to_pose, vacuum_on/off) via a `command` input. A FastAPI REST server with a web UI runs in a background thread for manual control. The robot control wrapper lives in `dora_ulite6/main.py` as the `ULite6Helper` class, and the dora event loop is the `main()` function in the same file. + +## Build + +```bash +uv pip install -e . +``` + +Requirements: Python >=3.8, uv, and network access to the UFactory Lite6 robot. + +## Run + +```bash +dora up +dora start dataflow.yml +``` + +Robot behavior is configured through environment variables (see README.md for the full list: ROBOT_IP, DEFAULT_SPEED, DEFAULT_UNITS, API_HOST, API_PORT, VACUUM_ENABLED, INIT_MODE, etc.). + +## Architecture + +- **`dora_ulite6/main.py`** -- Single-file implementation containing: + - `ULite6Helper`: Wraps the xArm Python SDK for motion control (go_home, move_to_pose, get_current_position, get_joint_angles_deg, get_pose_matrix, vacuum gripper, status, reset). + - `MoveToRequest` / `MoveToPoseRequest`: Pydantic models for API validation. + - `create_api()`: FastAPI app factory with REST endpoints and embedded HTML/JS web UI for manual control. + - `main()`: Dora event loop. Starts FastAPI in a daemon thread, then processes `tick` (publishes joint_pose and tcp_pose) and `command` (executes robot actions and publishes status) inputs. +- **`dora_ulite6/__init__.py`** -- Package marker. +- **`pyproject.toml`** -- Package metadata, dependencies, and entry point. +- **`dataflow.yml`** -- Example dora dataflow using the git: source format. + +## Key APIs + +- **dora Python API** (`dora`): `Node()`, `node.send_output()`, event iteration via `for event in node`. +- **xArm SDK** (`xarm.wrapper.XArmAPI`): `connect()`, `motion_enable()`, `set_mode()`, `set_state()`, `set_position()`, `move_gohome()`, `get_position()`, `get_servo_angle()`, `set_vacuum_gripper()`. +- Output data is sent as PyArrow arrays with metadata dicts. + +## Tools + +- **Issue tracker:** Gitea. Use the Gitea MCP tools (`mcp__gitea__*`) to read and manage issues, pull requests, and other repository operations. + +## Rules + +- Do not mention AI tools in commit messages or documentation. +- This controls a real robot. Test changes carefully and preserve safety checks. diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..7777f0e --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,3 @@ +# CLAUDE.md + +See [AGENTS.md](AGENTS.md) for repository guidance. diff --git a/README.md b/README.md index 5295803..ffeee22 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,115 @@ -# dora_ulite6 +# Dora ULite6 Node +Dora node for controlling a UFactory Lite6 robot via REST API, web UI, and publishing joint/TCP state. + +## Installation + +```bash +uv pip install -e . +``` + +## Dataflow + +```yaml +nodes: + - id: ulite6 + git: ssh://git@gitea.liria.cl:5001/liria/dora_ulite6.git + git_checkout: v0.1.0 + build: uv pip install -e . + path: dora_ulite6/main.py + inputs: + tick: dora/timer/millis/10 + outputs: [status, joint_pose, tcp_pose] + inputs: [command] + 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 | +| `INIT_MODE` | `none` | Startup positioning: `home`, `pose`, or `none` | +| `INIT_X/Y/Z` | `300/0/250` | Initial pose coordinates (mm) | +| `INIT_ROLL/PITCH/YAW` | `180/0/0` | Initial pose orientation (deg) | +| `INIT_SPEED` | `50` | Startup movement speed (mm/s) | + +## 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 Inputs + +- `command` - JSON string: `{id, action, payload}` + - `id`: unique command id (required) + - `action`: `home`, `move_to`, or `move_to_pose` + - `payload`: fields for move (`x`, `y`, `z`, `roll`, `pitch`, `yaw`, `speed`) + +`status` will include `command_id` for command responses. diff --git a/dataflow.yml b/dataflow.yml new file mode 100644 index 0000000..9b522ef --- /dev/null +++ b/dataflow.yml @@ -0,0 +1,19 @@ +nodes: + - id: ulite6 + git: ssh://git@gitea.liria.cl:5001/liria/dora_ulite6.git + git_checkout: v0.1.0 + build: uv pip install -e . + path: 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: "9000" + VACUUM_ENABLED: "true" diff --git a/dora_ulite6/__init__.py b/dora_ulite6/__init__.py new file mode 100644 index 0000000..1dad1aa --- /dev/null +++ b/dora_ulite6/__init__.py @@ -0,0 +1 @@ +"""Dora ULite6 node package.""" diff --git a/dora_ulite6/main.py b/dora_ulite6/main.py new file mode 100644 index 0000000..b93b7fa --- /dev/null +++ b/dora_ulite6/main.py @@ -0,0 +1,1020 @@ +"""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() diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..b395530 --- /dev/null +++ b/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"] diff --git a/scripts/install.sh b/scripts/install.sh new file mode 100755 index 0000000..c21bc32 --- /dev/null +++ b/scripts/install.sh @@ -0,0 +1,21 @@ +#!/usr/bin/env bash +# Install dependencies for dora-ulite6. +# Requires: Python >=3.8 +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_DIR="${SCRIPT_DIR}/.." + +# Install uv if not present +if ! command -v uv &>/dev/null; then + echo ">>> Installing uv..." + curl -LsSf https://astral.sh/uv/install.sh | sh +fi + +# Create venv and install +echo ">>> Installing dora-ulite6 in editable mode..." +cd "$REPO_DIR" +uv venv --seed --allow-existing +uv pip install -e ".[dev]" + +echo ">>> Done. Activate with: source .venv/bin/activate"