Metadata-Version: 2.4
Name: hex_dora_node_robot
Version: 0.0.1a4
Summary: Dora Node for using Hex Robot
Author-email: Dong Zhaorui <847235539@qq.com>
License-Expression: Apache-2.0
Requires-Python: >=3.10
Description-Content-Type: text/markdown
License-File: LICENSE
Requires-Dist: dora-rs>=0.3.9
Requires-Dist: hex-driver-robot<0.1.0,>=0.0.0
Dynamic: license-file

# hex_dora_node_robot

Dora nodes for controlling HEXFELLOW robot arms. Supports Archer Y6, Firefly Y6, and Hello Y6 robot models with state reading and command sending over the Dora dataflow.

## Nodes

| Node                             | Description                                                   | Inputs                                       | Outputs                              |
| -------------------------------- | ------------------------------------------------------------- | -------------------------------------------- | ------------------------------------ |
| `hex-dora-robot-archer-y6`       | Archer Y6 robot driver (6-DOF arm with optional grip)         | `tick`, `arm_cmd`, `grip_cmd`                | `arm_motor`, `arm_end`, `grip_motor` |
| `hex-dora-robot-firefly-y6`      | Firefly Y6 robot driver (6-DOF arm with optional grip)        | `tick`, `arm_cmd`, `grip_cmd`                | `arm_motor`, `arm_end`, `grip_motor` |
| `hex-dora-robot-hello-y6`        | Hello Y6 robot driver (6-DOF arm with 7-DOF grip and RGB LED) | `tick`, `grip_led_cmd`                       | `arm_motor`, `grip_ctrl`             |
| `hex-dora-robot-test-archer-y6`  | Archer Y6 test node (sends commands and displays state)       | `tick`, `arm_motor`, `arm_end`, `grip_motor` | `arm_cmd`, `grip_cmd`                |
| `hex-dora-robot-test-firefly-y6` | Firefly Y6 test node (sends commands and displays state)      | `tick`, `arm_motor`, `arm_end`, `grip_motor` | `arm_cmd`, `grip_cmd`                |
| `hex-dora-robot-test-hello-y6`   | Hello Y6 test node (sends LED commands and displays state)    | `tick`, `arm_motor`, `grip_ctrl`             | `grip_led_cmd`                       |

## Installation

```bash
pip install hex_dora_node_robot
```

## YAML Examples

### Archer Y6

```yaml
nodes:
  - id: archer_y6
    build: pip install hex_dora_node_robot
    path: hex-dora-robot-archer-y6
    inputs:
      tick: dora/timer/millis/2
      arm_cmd: test_archer_y6/arm_cmd
      grip_cmd: test_archer_y6/grip_cmd
    outputs:
      - arm_motor
      - arm_end
      - grip_motor
    env:
      NODE_NAME: archer_y6
      HOST: 192.168.1.100
      PORT: 8439
      CTRL_RATE: 500
      STATE_BUFFER_SIZE: 200
      SEN_TS: True
      GRIP_TYPE: gp80
      POSE_END_IN_FLANGE: 0.187,0.0,0.0,1.0,0.0,0.0,0.0

  - id: test_archer_y6
    build: pip install hex_dora_node_robot
    path: hex-dora-robot-test-archer-y6
    inputs:
      tick: dora/timer/millis/2
      arm_motor: archer_y6/arm_motor
      arm_end: archer_y6/arm_end
      grip_motor: archer_y6/grip_motor
    outputs:
      - arm_cmd
      - grip_cmd
    env:
      NODE_NAME: test_archer_y6
      ARM_CTRL_MODE: pos
      GRIP_CTRL_MODE: pos
```

### Firefly Y6

```yaml
nodes:
  - id: firefly_y6
    build: pip install hex_dora_node_robot
    path: hex-dora-robot-firefly-y6
    inputs:
      tick: dora/timer/millis/2
      arm_cmd: test_firefly_y6/arm_cmd
      grip_cmd: test_firefly_y6/grip_cmd
    outputs:
      - arm_motor
      - arm_end
      - grip_motor
    env:
      NODE_NAME: firefly_y6
      HOST: 192.168.1.100
      PORT: 8439
      CTRL_RATE: 500
      STATE_BUFFER_SIZE: 200
      SEN_TS: True
      GRIP_TYPE: gp80
      POSE_END_IN_FLANGE: 0.187,0.0,0.0,1.0,0.0,0.0,0.0

  - id: test_firefly_y6
    build: pip install hex_dora_node_robot
    path: hex-dora-robot-test-firefly-y6
    inputs:
      tick: dora/timer/millis/2
      arm_motor: firefly_y6/arm_motor
      arm_end: firefly_y6/arm_end
      grip_motor: firefly_y6/grip_motor
    outputs:
      - arm_cmd
      - grip_cmd
    env:
      NODE_NAME: test_firefly_y6
      ARM_CTRL_MODE: pos
      GRIP_CTRL_MODE: pos
```

### Hello Y6

```yaml
nodes:
  - id: hello_y6
    build: pip install hex_dora_node_robot
    path: hex-dora-robot-hello-y6
    inputs:
      tick: dora/timer/millis/2
      grip_led_cmd: test_hello_y6/grip_led_cmd
    outputs:
      - arm_motor
      - grip_ctrl
    env:
      NODE_NAME: hello_y6
      HOST: 192.168.1.100
      PORT: 8439
      CTRL_RATE: 500
      STATE_BUFFER_SIZE: 200
      SEN_TS: True
      LED_BUFFER_SIZE: 10

  - id: test_hello_y6
    build: pip install hex_dora_node_robot
    path: hex-dora-robot-test-hello-y6
    inputs:
      tick: dora/timer/millis/2
      arm_motor: hello_y6/arm_motor
      grip_ctrl: hello_y6/grip_ctrl
    outputs:
      - grip_led_cmd
    env:
      NODE_NAME: test_hello_y6
```

## Inputs

### Timer

| Input  | Type                  | Description                         |
| ------ | --------------------- | ----------------------------------- |
| `tick` | `dora/timer/millis/*` | Timer tick to trigger state reading |

### Arm Command (Archer Y6 / Firefly Y6 only)

Unified arm command input with a `type` field to select the control mode.

| Input     | Type Field | Additional Fields                                     | Description                                    |
| --------- | ---------- | ----------------------------------------------------- | ---------------------------------------------- |
| `arm_cmd` | `mit`      | `q_tar(6)`, `dq_tar(6)`, `tau(6)`, `kp(6)`, `kd(6)`   | MIT torque command for arm                     |
| `arm_cmd` | `mit_comp` | `q_tar(6)`, `dq_tar(6)`, `tau(6)`, `kp(6)`, `kd(6)`   | MIT command with gravity/friction compensation |
| `arm_cmd` | `pos`      | `q_tar(6)`, `kp(6)`, `kd(6)`, `err_limit(1)`          | Joint position command for arm                 |
| `arm_cmd` | `pose`     | `pos(3)`, `quat(4)`, `kp(6)`, `kd(6)`, `err_limit(1)` | Cartesian pose command for arm                 |

### Grip Command (Archer Y6 / Firefly Y6 only)

Unified grip command input with a `type` field to select the control mode.

| Input      | Type Field | Additional Fields                                             | Description                            |
| ---------- | ---------- | ------------------------------------------------------------- | -------------------------------------- |
| `grip_cmd` | `mit`      | `q_tar(dof)`, `dq_tar(dof)`, `tau(dof)`, `kp(dof)`, `kd(dof)` | MIT torque command for grip            |
| `grip_cmd` | `mit_comp` | `q_tar(dof)`, `dq_tar(dof)`, `tau(dof)`, `kp(dof)`, `kd(dof)` | MIT command with friction compensation |
| `grip_cmd` | `pos`      | `q_tar(dof)`, `kp(dof)`, `kd(dof)`, `err_limit(1)`            | Joint position command for grip        |

### Grip LED Command (Hello Y6 only)

| Input          | Fields                 | Description                               |
| -------------- | ---------------------- | ----------------------------------------- |
| `grip_led_cmd` | `r(6)`, `g(6)`, `b(6)` | RGB LED strip command (0-255 per channel) |

### Test Nodes (`test-archer-y6` / `test-firefly-y6`)

| Input        | Type                  | Description                              |
| ------------ | --------------------- | ---------------------------------------- |
| `tick`       | `dora/timer/millis/*` | Timer tick to refresh display            |
| `arm_motor`  | Arrow array           | Arm motor state from a robot node        |
| `arm_end`    | Arrow array           | Arm end-effector state from a robot node |
| `grip_motor` | Arrow array           | Grip motor state from a robot node       |

### Test Nodes (`test-hello-y6`)

| Input       | Type                  | Description                          |
| ----------- | --------------------- | ------------------------------------ |
| `tick`      | `dora/timer/millis/*` | Timer tick to refresh display        |
| `arm_motor` | Arrow array           | Arm motor state from a robot node    |
| `grip_ctrl` | Arrow array           | Grip control state from a robot node |

## Outputs

### `arm_motor`

Archer Y6 / Firefly Y6: `{"pos": float64[6], "vel": float64[6], "eff": float64[6]}`

Hello Y6: `{"pos": float64[6], "vel": float64[6]}`

### `arm_end`

Archer Y6 / Firefly Y6 only: `{"pos": float64[3], "quat": float64[4]}`

### `grip_motor`

Archer Y6 / Firefly Y6: `{"pos": float64[dof], "vel": float64[dof], "eff": float64[dof]}`

### `grip_ctrl`

Hello Y6: `{"axis": float64[3], "button": float64[4]}`

## Arrow Dict Encoding

All data is transmitted as `dict[str, arr]` via flat `float64` Arrow arrays. The `metadata` dict describes the layout using field names as keys mapped to their array lengths:

```python
# Example arm_motor metadata for Archer/Firefly Y6
metadata = {
    "fields": "pos,vel,eff",  # field order in the flat array
    "pos": "6",               # pos occupies 6 elements
    "vel": "6",
    "eff": "6",
}
```

Use `hex_dora_node_robot.util.dict_decode` to unpack and `dict_encode` to pack:

```python
from hex_dora_node_robot.util import dict_decode, dict_encode

# Decode state from a robot node
state = dict_decode(event["value"], event["metadata"])
print(state["pos"], state["vel"])

# Encode a unified arm command (pos mode) to send to the robot node
import numpy as np
cmd = {
    "type": "pos",
    "q_tar": np.zeros(6),
    "kp": np.array([400.0, 400.0, 500.0, 200.0, 100.0, 100.0]),
    "kd": np.array([5.0, 5.0, 5.0, 5.0, 2.0, 2.0]),
    "err_limit": np.array([0.03]),
}
storage, metadata = dict_encode(cmd.keys(), cmd, dict(event["metadata"]))
node.send_output("arm_cmd", storage, metadata)
```

## Environment Variables

### Common (all robot nodes)

| Variable            | Type    | Default         | Description                   |
| ------------------- | ------- | --------------- | ----------------------------- |
| `NODE_NAME`         | `str`   | `""`            | Dora node name                |
| `HOST`              | `str`   | `192.168.1.100` | Robot controller IP address   |
| `PORT`              | `int`   | `8439`          | WebSocket port                |
| `CTRL_RATE`         | `float` | `500`          | Control loop rate (Hz)        |
| `STATE_BUFFER_SIZE` | `int`   | `200`           | Size of the state ring buffer |
| `SEN_TS`            | `bool`  | `True`          | Use sensor timestamps         |

### Test Nodes (common)

| Variable    | Type  | Default | Description    |
| ----------- | ----- | ------- | -------------- |
| `NODE_NAME` | `str` | `""`    | Dora node name |

### Test Nodes (`test-archer-y6` / `test-firefly-y6`)

| Variable         | Type  | Default | Description                                         |
| ---------------- | ----- | ------- | --------------------------------------------------- |
| `ARM_CTRL_MODE`  | `str` | `pos`   | Arm control mode (`mit`, `mit_comp`, `pos`, `pose`) |
| `GRIP_CTRL_MODE` | `str` | `pos`   | Grip control mode (`mit`, `mit_comp`, `pos`)        |

### Archer Y6 / Firefly Y6

| Variable             | Type  | Default                         | Description                                             |
| -------------------- | ----- | ------------------------------- | ------------------------------------------------------- |
| `GRIP_TYPE`          | `str` | `gp80`                          | Grip model (`gp80` or `empty` for no grip)              |
| `POSE_END_IN_FLANGE` | `str` | `0.187,0.0,0.0,1.0,0.0,0.0,0.0` | End-effector pose in flange frame `[x,y,z,qw,qx,qy,qz]` |

### Hello Y6

| Variable          | Type  | Default | Description                    |
| ----------------- | ----- | ------- | ------------------------------ |
| `LED_BUFFER_SIZE` | `int` | `10`    | Size of the LED command buffer |

## License

This project is licensed under Apache-2.0.
