Working calibration

dora_ulite6/README.md

@@ -11,6 +11,7 @@ Dora node for controlling a UFactory Lite6 robot via REST API, web UI, and publi
   inputs:
     tick: dora/timer/millis/10
   outputs: [status, joint_pose, tcp_pose]
+  inputs: [command]
   env:
     ROBOT_IP: "192.168.1.192"
     DEFAULT_SPEED: "30"
@@ -90,3 +91,12 @@ curl -X POST http://localhost:8080/api/move_to_pose \
 - `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.

dora_ulite6/dora_ulite6/main.py

@@ -732,16 +732,55 @@ def _send_tcp_pose(node: Node, tcp_pose: Dict[str, float]) -> None:
     node.send_output("tcp_pose", pa.array(vec), metadata=metadata)
 
 
-def _send_error_status(node: Node, action: str, message: str) -> None:
+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)
     node.send_output("status", pa.array([json.dumps(payload)]))
 
 
+def _send_command_status(
+    node: Node,
+    command_id: str,
+    action: str,
+    ok: bool,
+    message: 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)
+    node.send_output("status", pa.array([json.dumps(payload)]))
+
+
+def _status_snapshot(helper: ULite6Helper) -> Dict[str, Any]:
+    status = helper.get_status()
+    return {
+        "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 main() -> None:
     node = Node()
 
@@ -773,7 +812,107 @@ def main() -> None:
                 _send_joint_pose(node, joints)
                 _send_tcp_pose(node, tcp_pose)
             except Exception as exc:
-                _send_error_status(node, "publish_state", str(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",
+                        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}",
+                    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",
+                    status=_status_snapshot(helper),
+                )
+                continue
+
+            try:
+                if action == "home":
+                    code = helper.go_home()
+                    _send_command_status(
+                        node,
+                        command_id=command_id,
+                        action=action,
+                        ok=code == 0,
+                        message="Home command executed",
+                        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))
+                    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",
+                        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}",
+                        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),
+                    status=_status_snapshot(helper),
+                )
 
 
 if __name__ == "__main__":