> ## 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.

# 位置控制（Beta）

## 概述

本篇实践介绍如何将 Wuji Hand 2 切换到位置控制模式，按关节调整输出限幅和位置指令低通，再从当前姿态缓起，驱动整手开合。应用场景包括验证位置控制链路和接入上层位置轨迹。

对应的 Python 示例为 [8.position\_mode.py](https://github.com/wuji-technology/wuji-sdk/blob/main/examples/python/wuji_hand_2/8.position_mode.py)。接口定义见[位置控制接口对照](https://docs.wuji.tech/docs/zh/wuji-hand/latest/sdk-reference.md#位置控制接口对照)，模式选型见[位置控制模式](https://docs.wuji.tech/docs/zh/wuji-hand/latest/control-guide.md#位置控制模式)。

> [!WARNING]
>
> 本例程会使能全部 20 个关节，并向所有关节发送相同的开合角度。运行前固定设备，清空周围人员、线缆和障碍物，全程有人监护，并保持随时断电的条件。

## 前置条件

### 硬件

| 项目 | 要求 |
| - | - |
| Wuji Hand 2 | 1 只，20 个关节全部在线 |
| 连接 | 设备已上电，与主机通过以太网连通 |
| 工作区 | 当前姿态到开合轨迹的运动范围内无干涉 |

### 软件

| 项目 | 要求 |
| - | - |
| Python 环境 | 按 [Python SDK 安装](https://docs.wuji.tech/docs/zh/wuji-sdk/latest/quick-start.md#安装-python-sdk)安装 `wuji_sdk` |
| SDK | `v2026.11.9` |
| 固件 | `v3.0.0` 及以上，支持位置控制模式与 N·m 限幅 |

### 运行前准备

确认目标设备序列号。发现多只 Wuji Hand 2 时，例程列出序列号并提示选择。直接按 Enter，或运行环境没有标准输入时，选择列表中的第一只手。

| 参数 | 默认值 | 用途 |
| - | - | - |
| `EFFORT_LIMIT_SCALE` | `0.8` | 各关节位置控制限幅读取值的缩放比例 |
| `COMMAND_CUTOFF_HZ` | `40.0` Hz | 位置指令低通截止频率 |
| `AMPLITUDE_RAD` | `0.35` rad | 开合位置目标的峰值 |
| `SWEEP_PERIOD_S` | `4.0` 秒 | 每次开合的周期 |
| `SWEEP_CYCLES` | `2` | 开合次数 |
| `COMMAND_INTERVAL_S` | `1 / 120` 秒 | 开合指令的发送间隔 |
| `RAMP_DURATION_S` | `1.0` 秒 | 从当前姿态过渡到序列起点的时间 |

## 数据流

```mermaid theme={null}
flowchart TD
    CONNECT["选择设备并确认 20 个关节在线"]
    MODE["失能并切换位置控制模式"]
    READ["读取当前限幅与低通设置"]
    CONFIG["逐关节缩放限幅并读回<br/>设置低通"]
    ENABLE["使能并等待全部关节就绪"]
    MOVE["读取当前姿态并缓起<br/>开合两次"]
    CLEAN["失能并关闭发布器<br/>恢复参数并断开连接"]
    CONNECT --> MODE --> READ --> CONFIG --> ENABLE --> MOVE --> CLEAN
    CONFIG -. 错误或中断 .-> CLEAN
    ENABLE -. 错误或中断 .-> CLEAN
    MOVE -. 错误或中断 .-> CLEAN
```

## 集成步骤

以下代码为例程片段，辅助函数定义见完整源码。

### 1. 连接设备并检查关节

`connect_hand()` 扫描设备并连接所选 Wuji Hand 2。关节不足 20 个时，例程报错退出，不启动运动。

```python theme={null}
hand = connect_hand(manager, "hand_2_position_mode", wuji_sdk)
online = int(hand.online_joints_count().get())
if online != JOINT_COUNT:
    raise RuntimeError(f"expected 20/20 online joints, got {online}/20")
```

### 2. 切换模式并读取原参数

先失能，再切换到位置控制模式。切换成功后才读取限幅，保证保存、修改和恢复的数值都属于该模式，单位为 N·m。

```python theme={null}
hand.disable()
hand.control_mode().set(wuji_sdk.ControlMode.Position)

effort_limit = hand.effort_limit()
cutoff = hand.position_command_low_pass_cutoff_frequency()
original_limits = read_effort_limits(effort_limit)
original_cutoff = read_command_cutoff(cutoff)
```

读取的限幅是当前生效值，不一定是默认值。`read_effort_limits()` 要求全部 20 个值有效，检查失败时不继续配置或使能。

### 3. 逐关节调整限幅并设置低通

每个关节按自身读取值乘以 `EFFORT_LIMIT_SCALE`，避免将一个固定力矩值用于所有关节。

```python theme={null}
scaled_limits = [value * EFFORT_LIMIT_SCALE for value in original_limits]
written_limits = write_effort_limits(effort_limit, scaled_limits)
cutoff.set(COMMAND_CUTOFF_HZ)
```

`write_effort_limits()` 在写入后读取全部关节，任一值与请求不符时报错并列出关节。例程在每次参数写入前登记恢复动作，即使写入报错，也会尝试恢复已保存的参数。接口规则见[输出限幅](https://docs.wuji.tech/docs/zh/wuji-hand/latest/sdk-reference/effort-and-mit.md#effort_limit（set/get）)。

### 4. 使能并从当前姿态缓起

调用 `enable()` 后，`wait_enabled()` 等待最新诊断帧中的全部 20 个关节到达 Enabled。随后读取完整当前位置，按关节索引排列，用 1 秒余弦过渡到开合序列起点。

```python theme={null}
hand.enable()
wait_enabled(hand)
publisher = hand.joint_command().publish()
sweep = generate_sweep()
start = capture_start_positions(hand)
ramp_steps = round(RAMP_DURATION_S / RAMP_INTERVAL_S)
send_sweep(ramp_frames(start, sweep[0], ramp_steps), publisher,
           wuji_sdk, interval=RAMP_INTERVAL_S)
```

使能等待超时或无法获取完整姿态时，进入清理流程，不继续发送开合序列。

### 5. 发送开合序列

所有关节的位置目标在 `0～0.35 rad` 之间按余弦变化，每次开合 4 秒，共 2 次。`send_sweep()` 每帧发送 20 个 `JointCommand`，`velocity` 和 `effort` 均填 `0`。

```python theme={null}
send_sweep(sweep, publisher, wuji_sdk)
```

目标发送间隔为 `1 / 120` 秒，实际频率受主机调度影响。

### 6. 停止并恢复参数

正常结束、异常或 Ctrl+C 都进入清理流程：尝试失能，关闭发布器，恢复原低通和限幅，再断开连接。一项清理失败不会阻止后续清理，限幅恢复后仍逐关节读回检查。

退出后设备仍为位置控制模式。限幅的保存与恢复见[保存与恢复限幅](https://docs.wuji.tech/docs/zh/wuji-hand/latest/control-guide.md#保存与恢复限幅（可选）)。

## 运行与输出

### 运行例程

在 `wuji-sdk` 仓库根目录运行：

```bash theme={null}
python examples/python/wuji_hand_2/8.position_mode.py
```

### 观察输出与结果

终端依次显示所选设备、控制模式、原限幅、缩放比例、写后回读值和低通截止频率。限幅按每根手指 4 个关节分组打印，依次为拇指、食指、中指、无名指和小指。

确认运行完成时，分别检查：

* 正常结束返回退出码 `0`。运行或清理失败返回 `1`，Ctrl+C 返回 `130`
* 设备完成开合后失能，终端没有 `cleanup error`。出现恢复错误时，先检查设备状态与参数，再继续操作
* 需要评估跟踪效果时，另行记录 `joint_states`，对比目标位置与反馈。终端参数输出不是到位验收结果

## 注意事项

| 情况 | 例程行为 |
| - | - |
| 模式切换或原参数读取失败 | 报错，尝试失能并清理，不启动运动 |
| 限幅写入未生效 | 列出回读不符的关节，尝试恢复参数 |
| 使能超时或姿态不完整 | 不发送开合序列，进入清理流程 |
| 运行中 Ctrl+C | 停止发送，尝试失能并恢复参数，不额外执行归零 |
| 清理时关节离线 | 报告恢复错误，不能假定所有参数已恢复 |

## 扩展用法

### 接入其他位置轨迹

替换 `generate_sweep()` 生成的 20 关节位置序列，保留参数快照、写后读回、使能等待、起始过渡和退出清理。所有关节使用相同角度只是本例程的开合方式，不是通用抓握姿态。

## 相关内容

* [位置控制模式](https://docs.wuji.tech/docs/zh/wuji-hand/latest/control-guide.md#位置控制模式)：模式选型
* [位置控制接口对照](https://docs.wuji.tech/docs/zh/wuji-hand/latest/sdk-reference.md#位置控制接口对照)：模式切换、低通与指令发布
* [输出限幅](https://docs.wuji.tech/docs/zh/wuji-hand/latest/sdk-reference/effort-and-mit.md#effort_limit（set/get）)：单位、校验与部分写入
* [回调订阅](https://docs.wuji.tech/docs/zh/wuji-hand/latest/sdk-practice/subscribe-callback.md#集成步骤)：读取关节状态


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