dora-ulite6/README.md

# dora-ulite6

Dora node for a UFactory Lite6 arm. Every operation is a **service** — dora's own
request/response pattern — and the arm's joint angles, tool pose and connection state
are published as ordinary topics.

One request, one response, correlated by the `request_id` you put in the message
metadata. That is the node's whole control surface: there is no second protocol for the
slow operations. A motion takes a `wait` flag — hold me until the arm stops, or answer me
now and I will follow it on `motion_state`.

Correlation lives in the message metadata because that is dora's convention, not a ROS2
transliteration: `request_id` is defined in dora's own message crate, and the runtime
treats a port carrying it as one where several Arrow schemas legitimately flow — one per
service.

The arm runs on threads of its own, so a move that takes ten seconds — or a controller
whose cable was pulled — never stalls the dora event loop, and `tcp_pose` keeps arriving
throughout.

The driver is ported from the
[littlehand](https://gitea.liria.cl/Littlehand/monolitic-littlehand-python) robot layer,
which was written against this same controller: typed failures, a reachability check
before a move is commanded, and arrival confirmed rather than assumed.

## The services

| Service | Arguments | Moves the arm | Description |
| ------- | --------- | ------------- | ----------- |
| `move_to` | `x`, `y`, `z`, optional `roll`, `pitch`, `yaw`, `speed`, `wait` | yes | Move the tool tip to a base-frame pose. Orientation defaults to tool-down (`180, 0, 0`). Refused before anything moves if the straight-line path leaves the joint envelope. |
| `move_joints` | `angles` (list, degrees), optional `speed`, `wait` | yes | Move the joints directly. |
| `home` | optional `wait` | yes | Joint-space home. |
| `home_and_clear` | optional `wait` | yes | Clear a latched error, then drive home. The answer to a joint-limit error, and only to be sent once the area is clear. |
| `stop` | — | no | Halt the arm. Served by a thread of its own, so it neither queues behind the motion it aborts nor holds up the dataflow; the response says the stop was commanded, and whether it took shows on `motion_state`. |
| `clear` | — | no | Clear a latched controller error. Never moves the arm. |
| `recover` | — | no | Reconnect if needed, clear, re-enable motion. Never moves the arm. |
| `freedrive` | `enabled` (bool, default `true`) | no | Enter or leave manual teach mode. |
| `grasp` | — | no | Take hold with whatever `TOOL_TYPE` names. |
| `release` | — | no | Let go. |
| `get_state` | — | no | Adds `connected`, `ready`, `tool_engaged`, `freedrive`, `pose`, `joints` to the response. Served on the event loop and read from the controller's report cache, so it answers *during* a motion. |

The four that move the arm report `moving` on `robot_state` while they run, and so does
a jog — an arm that is travelling never looks idle. `stop` interrupts whatever is moving;
the interrupted request comes back `ok: false` saying it was aborted.

## Calling a service

Send a one-row struct on `request`, with your own id in the metadata:

```python
node.send_output(
    "request",
    pa.array([{"service": "move_to", "x": 250.0, "y": 0.0, "z": 200.0, "speed": 40.0}]),
    metadata={"request_id": "move-1"},
)
```

The answer arrives on `response` under the same `request_id`:

```python
{"ok": True, "service": "move_to", "message": "the tool is at (250, 0, 200 mm)"}
```

Exactly one response per request, always — including for a request that was refused
before the arm was touched. Because the answer is matched by the id in the metadata and
not by arrival order, several requests can be in flight at once.

Poses are the **tool tip**, the same frame `tcp_pose` reports, so the arm can be sent
back to a pose that was read off the dataflow. The tool's reach is taken back off before
the flange is commanded.

## Blocking and non-blocking motions

`wait` decides only *when* the one response is sent.

```python
# wait: true (the default) — the response arrives when the arm has stopped.
{"service": "move_to", "x": 250.0, "y": 0.0, "z": 200.0}

# wait: false — the response arrives at once, as an acknowledgement.
{"service": "move_to", "x": 250.0, "y": 0.0, "z": 200.0, "wait": False}
```

Either way it is **one response per request**. With `wait: false` that response is the
acknowledgement, and how the motion actually ended is reported on `motion_state` — never
as a second response, which would break the id a caller correlates by.

### Following a motion: `motion_state`

Published on every change, as `{request_id, service, state, message}`:

| State | Meaning |
| ----- | ------- |
| `accepted` | Taken, not started. |
| `moving` | The arm is being driven. |
| `done` | Finished; the arm arrived. |
| `failed` | The arm refused it, or never got there. |
| `preempted` | A newer motion took its place. |

`accepted` is not decoration. Without it, a caller that sends a motion and then polls a
two-valued `moving`/`idle` state reads "not moving" in the window before the control
thread picks the request up, and concludes the motion it just asked for had already
finished. Keying the state by `request_id` and publishing `accepted` closes that window:
the caller can always tell *"mine has not started"* from *"mine has finished"*.

The `request_id` is in the fields rather than the metadata, because this is a broadcast of
the arm's state rather than an answer to a request — anyone may read it, and
`get_state` sees the same value.

`tcp_pose` also keeps arriving throughout, at `TELEMETRY_INTERVAL_MS`: the thread that
publishes it is not the thread inside the motion.

### A new motion preempts the one in play

The arm is one resource — there is never more than one motion. A new motion cuts the
running one short (through the same stop a caller could send) and takes its place; a
motion still waiting its turn is dropped without ever moving anything. Each one is still
answered exactly once: by its response if a caller is still waiting, and on
`motion_state` as `preempted` either way.

So a sequence of positions is driven by the caller, one at a time — send the next when
you see `done`. Sending them all at once does not queue them; the last one wins.

## Jogging

`jog` is a topic, not a service: a sender keeps sending and nobody waits for an answer.

```python
node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}]))
```

It still drives the arm, so it behaves like a motion where that matters: the first
non-zero jog preempts a motion in play and `robot_state` turns to `moving`. A zero
velocity stops it, and so does simply going quiet — the node stops the arm and returns to
`ready` once `VELOCITY_COMMAND_TTL_S` has passed.

**The velocity is set once and held.** The controller is given a velocity with no
duration, so it keeps it until a different one arrives: re-sending an unchanged velocity
does not reach the arm at all. Sending it repeatedly re-armed a timed segment and made the
controller re-plan the same motion, which on a real arm is felt as stepping rather than
travel — even at four messages a second.

That also means **the controller no longer stops the arm on its own**, so
`VELOCITY_COMMAND_TTL_S` is enforced here instead. It is a real deadman now rather than a
description of one: a sender that dies mid-jog is what it exists for.

## Telemetry

| Output | Type | Description |
| ------ | ---- | ----------- |
| `joint_pose` | `Float32` | The six joint angles in degrees, base to wrist. |
| `tcp_pose` | `Float32` | `[x, y, z, roll, pitch, yaw]` in mm and degrees: the tool tip in the base frame. |
| `robot_state` | `String` | `connecting`, `ready`, `moving`, `recovering` or `unavailable`, on every change. |
| `motion_state` | struct | Where the current motion stands — see above. |

`tick` sets how often the newest reading is *published*; `TELEMETRY_INTERVAL_MS` sets how
often the arm is *read*. A tick with nothing new publishes nothing rather than repeating
a stale pose.

## Usage

Add to your dataflow:

```yaml
nodes:
  - id: ulite6
    git: ssh://git@gitea.liria.cl:5001/liria/dora-ulite6.git
    tag: v0.3.0
    build: pip install .
    path: dora-ulite6
    inputs:
      tick: dora/timer/millis/100
      request: planner/request
    outputs:
      - response
      - motion_state
      - joint_pose
      - tcp_pose
      - robot_state
    env:
      ROBOT_IP: "192.168.1.192"
      TOOL_TYPE: "vacuum"
```

Run the dataflow with `--uv` (`dora run dataflow.yml --uv`) so the node gets its own
managed environment.

### Runnable example

[`dataflow.yml`](dataflow.yml) builds the node from this checkout and wires
[`examples/caller.py`](examples/caller.py), which sends a non-blocking motion, follows it
on `motion_state`, and then preempts it with a second one:

```bash
dora run dataflow.yml --uv
```

Building from the checkout (`path:` + `build:`) rather than from a tag is what lets
`dora validate` pick up the manifest in [`dora-node.yml`](dora-node.yml).

**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), Python 3.11+, `uv`, and
network access to the arm's controller.

## Lifecycle

The node connects to the dataflow **before** it touches the arm, so a startup that never
joins cannot have moved a robot first.

**On a restart** (`restart_policy` in your dataflow), `INIT_MODE` is skipped: the arm is
left wherever it was. Driving it home because the process came back is a motion nobody
asked for.

**On shutdown**, the arm is halted before the node exits. A motion lives in the
controller, not in this process — dropping the connection does not abort it — so exiting
without commanding a stop would leave an arm travelling with nobody left to stop it. If
the control thread is still inside a blocking SDK call afterwards, the node leaves
without it rather than hanging until dora loses patience.

## Configuration

| Env Variable | Default | Description |
| ------------ | ------- | ----------- |
| `ROBOT_IP` | `192.168.1.192` | Controller address |
| `TOOL_TYPE` | `none` | What is mounted: `none`, `vacuum`, `gripper` |
| `TOOL_OFFSET_MM` | `0.0` | Extra reach past the flange, on top of the tool's own |
| `DEFAULT_SPEED` | `30` | Cartesian speed (mm/s) for a `move_to` with no speed |
| `DEFAULT_JOINT_SPEED` | `20` | Joint speed (deg/s) for a `move_joints` with no speed |
| `TELEMETRY_INTERVAL_MS` | `100` | How often the arm is read, and how often telemetry is produced |
| `RECONNECT_INTERVAL_S` | `5.0` | How long to wait before retrying an unreachable arm |
| `INIT_MODE` | `none` | Startup positioning: `none`, `home`, `pose` |
| `INIT_X/Y/Z` | `300/0/250` | Startup pose position (mm, tool tip) |
| `INIT_ROLL/PITCH/YAW` | `180/0/0` | Startup pose orientation (deg) |
| `INIT_SPEED` | `50` | Startup move speed (mm/s) |

The motion limits the driver enforces are tunable too, and the defaults are the ones
littlehand runs with. The arrival tolerances are wide on purpose: whether a motion
happened is decided by the controller's error, and these only catch an arm that never
left where it stood.

| Env Variable | Default | Description |
| ------------ | ------- | ----------- |
| `ARRIVAL_TOLERANCE_MM` | `25.0` | How far off the target still counts as arrived |
| `ARRIVAL_TOLERANCE_DEG` | `15.0` | The same for orientation, and for homing |
| `ARRIVAL_TIMEOUT_S` | `2.0` | How long to keep asking before calling a move interrupted |
| `JOINT_LIMIT_MARGIN_DEG` | `1.0` | How clear of its bound a joint must stay for a pose to be commanded |
| `PATH_SAMPLES` | `4` | How many points along the straight line are checked for reachability |
| `READY_TIMEOUT_S` | `3.0` | How long `recover` waits for a movable state |
| `HOME_TIMEOUT_S` | `20.0` | How long homing may take |
| `TOOL_SETTLE_S` | `2.0` | How long to wait for suction to build or fingers to close |
| `VELOCITY_COMMAND_TTL_S` | `0.5` | How long a `jog` stream may go quiet before this node stops the arm |

## Development

```bash
uv sync --group dev
uv run pytest      # 92 tests, no robot needed
uv run ruff check .
```

## Bridging to ROS2

Nothing here depends on ROS2, and no ROS2 message definition is shipped. It is worth
knowing anyway that dora's `ros2:` service bridge speaks exactly this convention — it
emits `request` and takes a response under the same `request_id`. So exposing these
services to a ROS2 graph later is a matter of adding bridge nodes to a dataflow and
mapping the struct fields, with no change to this code.

## Migrating from v0.2.x

- **The `command` input and the `status` output are gone**, replaced by `request` and
  `response`. A command's `{id, action, payload}` JSON becomes a service request, and the
  id moves from the payload into the message metadata, where dora's own pattern puts it.
- **Payloads are Arrow structs, not JSON strings.** `pa.array([{...}])` instead of
  `json.dumps`.
- **`action` is now `service`**, and `vacuum_on`/`vacuum_off` are `grasp`/`release`.
- **`jog` is its own input** rather than a command.
- **There is no `started` message.** A request produces exactly one response. Follow a
  motion on `motion_state`, which reports `accepted`, `moving` and then how it ended.
- **Telemetry no longer stops while the arm is moving**, thanks to the reporter thread.
- **`stop` answers "commanded" rather than "done."** It is handed to a thread of its own
  instead of being run inline, because the SDK spends up to three seconds confirming a
  stop and a dataflow router cannot go away that long.

## Migrating from v0.1.x

See the v0.2.0 notes in the git history for the removal of the FastAPI REST server and
web UI (still reachable at tag `v0.1.2`), the `DEFAULT_UNITS` removal, and the move to
tool-tip poses.