> ## Documentation Index
> Fetch the complete documentation index at: https://docs.wuji.tech/llms.txt
> Use this file to discover all available pages before exploring further.

# 发布控制指令

## 概述

本篇实践介绍如何配置 MIT 参数并发布关节控制指令，将在线关节从当前位置平滑移动到零位。应用场景包括平滑归零、接入遥操作和发送上层控制器生成的目标轨迹。

对应的 Python 示例为 [2.publish.py](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/python/wuji_hand_2/2.publish.py)，C 示例为 [1\_control\_motion.c](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/c/wuji_hand_2/1_control_motion.c)。 指令字段见 [JointCommand](/docs/wuji-hand/latest/sdk-reference/joint-control#jointcommand)，控制参数见 [MitParam](/docs/wuji-hand/latest/sdk-reference/effort-and-mit#mitparam)。

<Info>
  本示例会使能电机并驱动全部在线关节。开始前清空手部周围的人员、线缆与台面物品，全程有人监护，并保持随时断电的条件。
</Info>

## 前置条件

### 硬件

| 项目 | 要求 |
| - | - |
| Wuji Hand 2 | 1 只，至少 1 个关节在线 |
| 连接方式 | 以太网 |

本示例连接扫到的**第一台** Wuji Hand 2（`hand_devices[0]`）。共用网络里要改成指定 SN。

### 软件

| 项目 | 要求 |
| - | - |
| 操作系统 | Ubuntu 22.04 LTS 及以上版本 |
| Wuji SDK | v2026.8.31 或更高版本 |
| Python | 3.10 及以上版本 |
| C 工具链 | CMake 3.16 及以上版本，以及解压后的 SDK 包 |

### 控制参数

| 参数 | 示例取值 | 含义 |
| - | - | - |
| `KP` | 3.0 | 位置刚度。越大跟随越紧，冲击也越大 |
| `KD` | 0.05 | 阻尼 |
| `EFFORT_LIMIT` | 1.5 | 力矩上限，单位安培 |
| `PUB_HZ` | 200 | 指令发布频率 |
| `HOME_SECONDS` | 1.5 | 归零时长，调大更柔和 |

这三个控制量与出厂默认值一致，实测读回也是 `kp=3, kd=0.05, effort_limit=1.5`。

## 数据流

```mermaid theme={null}
flowchart TD
    CONNECT["扫描并连接 Wuji Hand 2<br/>Python: scan()、connect()<br/>C: wuji_scan()、wuji_connect()"]
    CONFIG["设置 effort_limit 与 mit_params<br/>Python: set() / C: wuji_hand_2_set_all_*()"]
    ENABLE["使能设备<br/>Python: enable() / C: wuji_hand_2_enable()"]
    WAIT["订阅 joint_diagnostics<br/>等待在线关节全部进入 Enabled"]
    PUB["创建 joint_command 发布器<br/>Python: publish() / C: wuji_hand_2_joint_command_publish()"]
    TRAJ["发送 20 关节指令帧<br/>Python: 读取当前姿态后平滑归零<br/>C: 按轨迹计算并发送"]
    CLEAN["关闭发布器、失能并断开设备"]

    CONNECT --> CONFIG --> ENABLE --> WAIT --> PUB --> TRAJ --> CLEAN
```

### 参数设置支持广播

`effort_limit` 与 `mit_params` 的 `set()` 是多态的：

```python theme={null}
hand.effort_limit().set(1.5)          # 标量，广播到全部关节
hand.effort_limit().set([1.5] * 20)   # 20 元素列表，逐关节生效
hand.mit_params().set((3.0, 0.05))    # (kp, kd) 元组，广播
```

## 集成步骤

### 1. 连接并配置控制参数

扫描并连接目标 Wuji Hand 2 后，设置力矩上限和 MIT 参数。Python 使用参数访问器，C 使用对应的 Hand 2 设置函数。固件默认使用 MIT 模式，无需额外设置控制模式。

<Tabs>
  <Tab title="Python">
    ```python theme={null}
    hand.effort_limit().set(1.5)
    hand.mit_params().set((3.0, 0.05))
    ```
  </Tab>

  <Tab title="C">
    ```c theme={null}
    wuji_hand_2_set_all_effort_limit(dev, 1.5f);
    float kp[20], kd[20];
    for (int i = 0; i < 20; i++) { kp[i] = 3.0f; kd[i] = 0.05f; }
    wuji_hand_2_set_all_mit_params(dev, kp, kd);
    ```
  </Tab>
</Tabs>

### 2. 使能并确认电机就绪

使能是一个动作请求。发送运动指令前，订阅 `joint_diagnostics` 并确认所有在线关节已进入 `Enabled`。Python 示例等待最多 5 秒，超时退出，诊断订阅在 `finally` 中关闭。

```python theme={null}
hand.enable()
diag = hand.joint_diagnostics().subscribe()
try:
    deadline = time.monotonic() + 5.0
    while time.monotonic() < deadline:
        time.sleep(0.2)
        frame = diag.recv()
        if frame and frame.joints and all(e.status_word.ext_state == 2 for e in frame.joints):
            break
    else:
        raise TimeoutError("Enable timeout")
finally:
    diag.close()
```

C 调用 `wuji_hand_2_enable(dev, NULL)`，通过 `wuji_hand_2_subscribe_joint_diagnostics()` 注册 `on_diag`。回调更新原子状态，主线程等待就绪或超时，再关闭诊断订阅。超时或订阅失败时失能并退出。

### 3. 创建发布器并发送轨迹

[Python 示例](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/python/wuji_hand_2/2.publish.py) 和 [C 示例](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/c/wuji_hand_2/1_control_motion.c) 演示不同的运动。Python 在 1.5 秒内从当前位置平滑归零，再保持零位 5 秒。C 运行多段正弦轨迹，在每段之间发送零位指令。两者都按 200 Hz 发送 20 关节指令帧。

<Tabs>
  <Tab title="Python">
    ```python theme={null}
    publisher = hand.joint_command().publish()
    smooth_home(hand, publisher, duration=1.5)
    ```
  </Tab>

  <Tab title="C">
    ```c theme={null}
    struct WujiJointCommandPublisher *pub = NULL;
    WujiStatus st = wuji_hand_2_joint_command_publish(dev, &pub);
    /* 创建成功后，在轨迹循环中发送 20 个关节指令。 */
    wuji_joint_command_publisher_send(pub, cmds);
    ```
  </Tab>
</Tabs>

`smooth_home()` 是 [Python 示例](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/python/wuji_hand_2/2.publish.py) 中的辅助函数。它等待有效状态帧，以当前位置为插值起点，每个周期调用 `publisher.send()`。C 的轨迹循环根据时间计算位置和速度，并用在线位图跳过离线关节。保留 [Python 示例](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/python/wuji_hand_2/2.publish.py) 和 [C 示例](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/c/wuji_hand_2/1_control_motion.c) 中的读取超时、发送失败和停止请求处理。

### 4. 停止发送并释放资源

[Python 示例](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/python/wuji_hand_2/2.publish.py) 和 [C 示例](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/c/wuji_hand_2/1_control_motion.c) 在发送循环结束后先关闭发布器，再失能、断开设备。Python 将清理放在 `finally` 中，C 的发送失败路径也跳转到清理代码。

<Tabs>
  <Tab title="Python">
    ```python theme={null}
    if publisher is not None:
        publisher.close()
    hand.disable()
    manager.disconnect_all()
    ```
  </Tab>

  <Tab title="C">
    ```c theme={null}
    wuji_joint_command_publisher_close(pub);
    wuji_hand_2_disable(dev, NULL);
    wuji_dev_disconnect(dev);
    wuji_dev_release(dev);
    wuji_shutdown();
    ```
  </Tab>
</Tabs>

## 运行与输出

<Tabs>
  <Tab title="Python">
    ```bash theme={null}
    python examples/python/wuji_hand_2/2.publish.py
    ```
  </Tab>

  <Tab title="C">
    ```bash theme={null}
    ./build/hand2/1_control_motion
    ```
  </Tab>
</Tabs>

### 实测数据

在 20 关节全在线的右手 Wuji Hand 2 上实跑：

| 指标 | 实测 |
| - | - |
| `enable()` 调用返回 | 0.0020 秒 |
| **全部电机进入 Enabled** | **0.0522 秒** |
| enable 前 `ext_state` | 全部为 1（Ready） |
| enable 后 `ext_state` | 全部为 2（Enabled） |
| 归零帧数与耗时 | 301 帧，1.505 秒 |
| 实际发布频率 | 200.0 Hz |
| 归零起点最大幅度 | 0.0674 rad（3.9 度，手当时已接近零位） |
| 归零后残留 | 前 4 关节 `+0.005 −0.019 −0.021 +0.008` |

`enable()` 返回与电机就绪之间差了 26 倍时间。这段窗口里发出的指令会被丢弃或产生非预期行为，等待步骤是必需的。

归零后各关节没有精确停在 0，残留在 0.02 rad 以内。MIT 阻抗是柔性控制，静摩擦与重力会让实际位置与目标存在稳态偏差，这是正常表现。

## 注意事项

| 分支点 | 表现 | 处理 |
| - | - | - |
| **enable 后立即发指令** | `enable()` 是动作，调用返回不代表电机就绪。实测调用本身 **2 毫秒**返回，全部电机进入 Enabled 要 **52 毫秒** | 订阅 `joint_diagnostics`，等 `status_word.ext_state` 全部变成 2 再发 |
| 无关节在线 | `online_joints_count()` 为 0，示例直接退出 | 先排查关节离线原因 |
| 使能超时 | 5 秒内未全部 Enabled，打印超时 | 查故障码，必要时 `clear_fault()` |
| 离线关节的位置 | `position` 为 `None` | 示例按 0.0 处理，保证插值起点有定义 |
| 网络有多台设备 | 连第一台，扫描顺序变化会换设备 | 改成指定 SN |

`ext_state` 取值：0 为 Init，1 为 Ready，2 为 Enabled，3 为 Stopped。实测 enable 前全部关节是 1，enable 后 52 毫秒内全部变 2。

## 扩展用法

### 调整归零柔和度

改 `HOME_SECONDS`。调大让每帧位置增量更小，动作更柔和。不要用降低 `kp` 的方式求柔和，那会牺牲整体跟随精度。

### 接入自己的轨迹

将第 5 步的插值替换为轨迹源，`publisher.send()` 的调用方式不变。每次 `send()` 发送一个完整的 20 维帧，SDK 序列化后直接发出、不等待响应，因此能支撑高频循环。

### 发布频率的选择

示例用 200 Hz。更高的频率让轨迹更平滑，但要保证生成一帧的耗时稳定小于周期。上面的实测显示 200 Hz 下实际频率精确到 200.0 Hz，说明这个量级留有余量。

### 只控制部分关节

每帧仍然要发完整的 20 维数组。不想动的关节应该填**它们当前的测量位置**，而不是填 0。填 0 等于命令它们回到零位，这是 [MIT 关节扫描](/docs/wuji-hand/latest/sdk-practice/mit-sweep)中需要避免的情况。

## 相关内容

* [MIT 扫描](/docs/wuji-hand/latest/sdk-practice/mit-sweep)：最小的指令链路验证，注意它会把其余关节拉向零位
* [拇指对掌回放](/docs/wuji-hand/latest/sdk-practice/opposition-replay)：完整录制轨迹的高频回放
* [数据订阅：回调](/docs/wuji-hand/latest/sdk-practice/subscribe-callback)：读取关节状态的方式
* [数据类型参考](/docs/wuji-sdk/data-reference/)：`JointCommand` 与诊断结构


This documentation is built and hosted on [Mintlify](https://mintlify.com), a developer documentation platform.