> ## 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 阻抗控制参数，使能电机，把每个关节从当前测量位置**平滑插值**到零位，然后保持。

这是控制 Wuji Hand 2 的基本范式。示例本身做的是归零，但 `publisher.send()` 这条路径同样承载遥操作、轨迹回放、上层控制器输出。

对应的 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)。

关节命令的字段与取值范围见[关节状态与控制](https://docs.wuji.tech/docs/zh/wuji-hand/beta1/sdk-reference/joint-control)，控制模式与安全边界见[控制指南](https://docs.wuji.tech/docs/zh/wuji-hand/beta1/control-guide)，本页只讲怎么跑起来与实测结果。

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

## 插值的作用

直接发 `pos=0` 和插值到 `pos=0`，在 MIT 阻抗下是两种完全不同的动作。

MIT 控制的力矩正比于位置偏差。手部原本弯曲时，第一帧 `pos=0` 的偏差就是「当前角度 − 0」，配上 `kp=3` 会产生一个大力矩，手猛地弹回零位。

插值的做法是先读当前位置，再把目标从当前值线性拉到 0。每帧的偏差都很小，力矩随之很小，整段动作柔和。**柔和来自目标轨迹，不是来自调低增益**，所以 `kp` 与 `kd` 保持正常值。

## 前置条件

### 硬件

| 项目 | 要求 |
| - | - |
| 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
    CFG["effort_limit().set() 与 mit_params().set()<br/>标量或元组广播到全部关节"]
    EN["enable()<br/>调用立即返回"]
    WAIT["订阅 joint_diagnostics<br/>等 ext_state 全部变 2（Enabled）"]
    READ["joint_states 订阅<br/>读当前 20 个关节位置"]
    LERP["线性插值<br/>目标从当前位置拉向 0"]
    PUB["joint_command().publish()<br/>每帧 20 个 JointCommand"]
    FW["固件 MIT 阻抗控制"]

    CFG --> EN
    EN -->|"动作已下发，电机尚未就绪"| WAIT
    WAIT -->|"全部 Enabled"| READ
    READ -->|"起始姿态"| LERP
    LERP -->|"200 Hz"| PUB
    PUB --> FW
```

### 参数设置支持广播

`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) 元组，广播
```

## 运行时分支

| 分支点 | 表现 | 处理 |
| - | - | - |
| **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。

## 调用顺序

> [tab] Python
>
> ```python theme={null}
> import time
> from wuji_sdk import DeviceType, JointCommand, SdkManager
>
> manager = SdkManager.instance()
> hands = [d for d in manager.scan() if d.device_type == DeviceType.WujiHand2]
> hand = manager.connect(sn=hands[0].sn, device_name="wuji_hand_2")
>
> # 1. 配置控制参数。固件默认就是 MIT 模式，不需要单独设 control_mode
> hand.effort_limit().set(1.5)
> hand.mit_params().set((3.0, 0.05))
>
> # 2. 使能
> hand.enable()
>
> # 3. 等电机真正就绪，否则指令会抢在使能完成之前
> diag = hand.joint_diagnostics().subscribe()
> deadline = time.monotonic() + 5.0
> while time.monotonic() < deadline:
>     time.sleep(0.05)
>     frame = diag.recv()
>     if frame and frame.joints and all(e.status_word.ext_state == 2 for e in frame.joints):
>         break
> diag.close()
>
> # 4. 读当前姿态作为插值起点
> sub = hand.joint_states().subscribe()
> frame = sub.recv()
> start = [float(j.position) if j.position is not None else 0.0 for j in frame.joints]
> sub.close()
>
> # 5. 按 200 Hz 插值到零位
> publisher = hand.joint_command().publish()
> steps, dt = int(1.5 * 200), 1.0 / 200
> t0 = time.monotonic()
> for i in range(1, steps + 1):
>     a = i / steps
>     target = [p * (1.0 - a) for p in start]
>     publisher.send([JointCommand(v, 0.0, 0.0) for v in target])
>     wait = (t0 + dt * i) - time.monotonic()
>     if wait > 0:
>         time.sleep(wait)
>
> # 6. 清理
> hand.disable()
> publisher.close()
> manager.disconnect_all()
> ```

> [tab] C
>
> ```c theme={null}
> #include "wuji_sdk.h"
>
> /* 1. 配置控制参数 */
> wuji_hand_2_set_effort_limit_all(dev, 1.5f);
> wuji_hand_2_set_mit_params_all(dev, 3.0f, 0.05f);
>
> /* 2. 使能 */
> wuji_hand_2_enable(dev);
>
> /* 3. 等电机就绪：订阅诊断流，检查 ext_state 全为 2 */
> struct WujiSub *diag = NULL;
> wuji_hand_2_subscribe_joint_diagnostics(dev, on_diag, &ctx, &diag);
> /* 轮询 ctx 里由回调写入的状态，直到全部 Enabled 或超时 */
>
> /* 4. 读当前姿态 */
> /* 从 joint_states 订阅取一帧，离线关节按 0 处理 */
>
> /* 5. 按固定频率插值发布 */
> struct WujiPub *pub = NULL;
> wuji_hand_2_joint_command_publisher(dev, &pub);
> WujiJointCommand cmd[20] = {0};
> for (int i = 1; i <= steps; i++) {
>     double a = (double)i / steps;
>     for (int j = 0; j < 20; j++) cmd[j].position = start[j] * (1.0 - a);
>     wuji_hand_2_publish_joint_command(pub, cmd, 20);
>     usleep(5000);   /* 200 Hz */
> }
>
> /* 6. 清理 */
> wuji_hand_2_disable(dev);
> wuji_pub_close(pub);
> wuji_sub_close(diag);
> wuji_dev_disconnect(dev);
> wuji_dev_release(dev);
> ```

第 3 步不能省。第 6 步的失能要早于关闭发布器。

## 运行与输出

> [tab] Python
>
> ```bash theme={null}
> python examples/python/wuji_hand_2/2.publish.py
> ```

> [tab] C
>
> ```bash theme={null}
> ./build/hand2/1_control_motion
> ```

### 实测数据

在 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 阻抗是柔性控制，静摩擦与重力会让实际位置与目标存在稳态偏差，这是正常表现。

## 调整与适配

### 调整归零柔和度

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

### 接入自己的轨迹

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

### 发布频率的选择

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

### 只控制部分关节

每帧仍然要发完整的 20 维数组。不想动的关节应该填**它们当前的测量位置**，而不是填 0。填 0 等于命令它们回到零位，这是 [MIT 关节扫描](https://docs.wuji.tech/docs/zh/wuji-hand/beta1/sdk-practice/mit-sweep)踩过的坑。

## 相关内容

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


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