运动控制 · 夹爪控制

1. 模块概述

grasp 组件提供统一的末端执行器控制 C API，用于屏蔽底层硬件差异，并向上层提供夹爪实例创建、抓取/释放控制、精细位置控制、状态反馈、负载检测以及交互式校准等能力。当前该组件已支持 SO-101 夹爪 UART 驱动，并可通过驱动插件机制扩展抓夹、吸盘等其他末端执行器设备。

规格特性

该模块具备以下功能：

• 对外提供纯 C 接口，便于在 C/C++ 工程或上层服务中复用；
• 支持 GRAB、RELEASE、RELAX 三类基础控制命令，以及 grasp_set_position() 精细位置控制；
• 内置 IDLE、MOVING、HOLDING、EMPTY、ERROR 五态状态机，支持按位置与负载自动流转；
• 支持基于归一化开合度 [0.0, 1.0] 的位置映射控制，其中 0.0 表示完全闭合，1.0 表示完全打开；
• 支持 SO-101 单舵机夹爪控制，默认使用 Feetech STS3215 舵机，默认 ID 为 6，默认波特率为 1000000；
• 支持交互式校准流程，可学习实际硬件开合边界，并将校准结果持久化保存到本地配置文件；
• 上层可通过高频周期调用 grasp_tick() 推进状态机与反馈更新，适合与机械臂控制模块协同使用；
• 支持 dummy 驱动单元测试与 SO-101 硬件交互测试。

代码结构

该模块代码位于 components/control/grasp，整体结构如下：

grasp/
├── include/
│   └── grasp.h                          # 公共 API 接口
├── src/
│   ├── grasp.c                          # 核心实现（驱动注册、设备管理）
│   ├── grasp_core.h                     # 内部头文件（驱动接口定义）
│   └── drivers/
│       ├── drv_dummy.c                  # Dummy 驱动（测试/占位）
│       └── drv_uart_so101_gripper.c     # SO-101 夹爪 UART 驱动
├── test/
│   ├── test_grasp.c                     # dummy 驱动单元测试
│   ├── test_hw_so101_gripper.c          # SO-101 硬件交互测试
│   └── HW_TEST_GUIDE.md                 # 硬件测试说明
├── CMakeLists.txt                       # 构建配置
├── package.xml                          # 依赖声明
├── LICENSE                              # Apache-2.0 许可证
├── NOTICE                               # 第三方归属声明
└── README.md                            # 模块说明文档

2. 环境准备

前置条件

硬件与连接

项目	内容
硬件平台	Spacemit K3 开发板
末端执行器	SO-101 夹爪
执行器器件	Feetech STS3215 舵机
通信方式	UART
默认连接	/dev/ttyACM0，波特率 1000000
供电要求	夹爪舵机需稳定供电，推荐使用 7.4V 电池或等效供电方案

夹爪与 SO101 机械臂关节通过一根总线控制，整体通过 USB 串口连接到 K3 开发板，如图所示：

软件环境

项目	内容
操作系统	Bianbu 3.0+
编译标准	C99 / C++14
基础工具	cmake、build-essential
基础依赖	pthreads、libm
控制依赖	components/peripherals/motor

权限设置

SO-101 夹爪通常通过串口设备节点（如 /dev/ttyACM0）与系统通信。运行硬件测试前，请确保当前用户具备串口访问权限，例如：

sudo chmod 666 /dev/ttyACM0

首次上电前需确认舵机连线、电源容量与夹爪机构状态正常，避免因卡滞或过载导致硬件损伤。

构建编译

进入模块代码目录，执行基础构建：

cd components/control/grasp
mkdir build && cd build
cmake ..
make -j$(nproc)

默认会生成 libgrasp.so 以及基础测试程序 test_grasp。

若需启用 SO-101 夹爪硬件测试程序，可执行：

cd components/control/grasp
rm -rf build && mkdir build && cd build
cmake .. -DGRASP_BUILD_HW_TEST=ON
make -j$(nproc)

典型产物包括：

产物	说明
build/libgrasp.so	夹爪控制共享库
build/test_grasp	dummy 驱动单元测试程序
build/test_hw_so101_gripper	SO-101 夹爪硬件测试程序（需打开 GRASP_BUILD_HW_TEST）

构建参数说明：

• GRASP_BUILD_TESTS=ON：启用基础单元测试；
• GRASP_BUILD_HW_TEST=ON：编译 SO-101 夹爪硬件测试程序；
• 若 motor 组件头文件或库未找到，SO-101 夹爪驱动与硬件测试能力将自动关闭。

3. 示例使用

3.1 示例一：基础框架与 dummy 驱动验证

前置：见 §2。该示例无需连接真实夹爪硬件。

步骤 1：进入模块目录并编译测试程序

cd components/control/grasp
rm -rf build && mkdir build && cd build
cmake ..
make -j$(nproc)

运行成功后，将生成 libgrasp.so 与 test_grasp：

➜  build git:(robot-dev) ✗ ll             
total 472K
-rw-r--r-- 1 root root  18K Apr 18 16:11 CMakeCache.txt
drwxr-xr-x 7 root root 4.0K Apr 18 16:11 CMakeFiles
-rw-r--r-- 1 root root  616 Apr 18 16:11 CTestTestfile.cmake
-rw-r--r-- 1 root root  12K Apr 18 16:11 Makefile
-rw-r--r-- 1 root root 3.4K Apr 18 16:11 cmake_install.cmake
-rwxr-xr-x 1 root root 223K Apr 18 16:11 libgrasp.so
drwxr-xr-x 3 root root 4.0K Apr 18 16:11 motor
-rwxr-xr-x 1 root root 200K Apr 18 16:11 test_grasp

步骤 2：运行 dummy 驱动单元测试

./test_grasp

预期现象：终端输出驱动注册信息和单元测试结果，表示基础框架、参数校验与状态流转逻辑正常。

[GRASP] Registered driver: dummy
[GRASP] Registered driver: so101_gripper
=== Grasp Module Unit Tests ===
[test] alloc/free (dummy driver) ... OK
[test] invalid driver ... [GRASP] Driver not found: nonexistent_driver
[GRASP] No driver found: nonexistent_driver
OK
[test] null params ... OK
[test] execute grab ... OK
[test] execute release ... OK
[test] set_position ... OK
[test] set_position bounds ... OK
[test] tick state transition ... OK
[test] get_feedback ... OK
=== All grasp tests PASSED ===

该示例可用于验证：

• 驱动注册机制正常；
• grasp_alloc()、grasp_execute()、grasp_set_position() 等基础接口可正常调用；
• dummy 驱动下的状态机与反馈逻辑工作正常。

3.2 示例二：SO-101 夹爪硬件控制验证

前置：见 §2。将 SO-101 夹爪通过串口连接到 K3 开发板，并确认串口节点可访问。

步骤 1：编译硬件测试程序

cd components/control/grasp
rm -rf build && mkdir build && cd build
cmake .. -DGRASP_BUILD_HW_TEST=ON
make -j$(nproc)

若依赖完整，构建完成后可看到 test_hw_so101_gripper 可执行文件：

-rwxr-xr-x 1 root root  19K Apr 18 16:12 test_hw_so101_gripper

步骤 2：确认串口权限，假设设备节点为 /dev/ttyACM0

sudo chmod 666 /dev/ttyACM0

步骤 3：启动硬件测试程序

./test_hw_so101_gripper

如需指定串口、波特率或舵机 ID，可使用：

./test_hw_so101_gripper --port /dev/ttyACM0 --baud 1000000 --id 6

程序启动后会显示当前配置，并进入交互式菜单，可执行完全打开、完全闭合、中间位置、抓取测试、释放测试、状态查看与校准等操作。

初始化 SO-101 夹爪...
	串口: /dev/ttyACM0
	波特率: 1000000
	舵机 ID: 6
夹爪初始化成功!

========== SO-101 夹爪硬件测试 ==========
1. 完全打开 (position = 1.0)
2. 完全闭合 (position = 0.0)
3. 中间位置 (position = 0.5)
4. 手动输入位置 (0.0 ~ 1.0)
5. 抓取测试 (GRAB 命令)
6. 释放测试 (RELEASE 命令)
7. 放松测试 (RELAX 命令)
8. 查看当前状态和反馈
9. 校准夹爪 (学习实际边界)
0. 退出

首次使用时建议优先执行校准流程，使夹爪学习实际打开与闭合边界。校准完成后，数据会保存到 ./config/so101_gripper_calibration.json，后续启动时自动加载。

[!NOTE]

硬件测试时请确保夹爪周围无障碍物，并避免长时间堵转或夹持过载，以免触发舵机保护或损伤机构。

4. 应用开发

对外 API 或接口形态

• 公共头文件：components/control/grasp/include/grasp.h
• 动态库产物：libgrasp.so
• 常用接口包括：
  • grasp_alloc() / grasp_free()：用于夹爪实例的创建与释放
  • grasp_execute()：用于执行 GRAB / RELEASE / RELAX 控制命令
  • grasp_set_position()：用于归一化位置控制
  • grasp_get_state()：用于获取夹爪高级状态
  • grasp_get_feedback()：用于获取当前位置与负载反馈
  • grasp_tick()：用于周期性推进状态机
  • grasp_calibrate()：用于执行交互式校准流程

调用方式与注意点

• 可通过 grasp_alloc("so101_gripper", &cfg) 创建夹爪实例；其中 cfg 可配置串口路径、波特率、舵机 ID 以及抓取参数；
• 对于仅做逻辑验证的场景，可使用 grasp_alloc("dummy", NULL) 创建 dummy 驱动实例；
• grasp_execute() 适合执行全开、全闭和放松等高层动作；若需要控制具体开合度，建议使用 grasp_set_position()；
• grasp_tick(dev, dt_s) 建议以固定频率调用，用于持续发送目标命令、刷新反馈并完成 HOLDING / EMPTY 状态判断；
• grasp_get_feedback() 返回的位置为归一化值 [0.0, 1.0]，负载值为底层舵机反馈原始量；
• 首次接入硬件时建议先执行 grasp_calibrate()，使位置映射与实际机构边界一致；
• 夹爪实例释放前会自动执行 RELAX，但在应用退出或异常恢复流程中，仍建议显式调用 grasp_stop() 或 grasp_execute(dev, GRASP_CMD_RELAX, 0.0f)；
• 运行硬件控制时需重点关注串口权限、舵机供电能力与过载保护状态。

参考 demo 或示例路径

components/control/grasp/test/test_grasp.c              # 基础框架与 dummy 驱动单元测试示例
components/control/grasp/test/test_hw_so101_gripper.c  # SO-101 夹爪硬件交互测试示例
components/control/grasp/test/HW_TEST_GUIDE.md         # 硬件测试流程与校准说明
components/control/grasp/README.md                     # 模块说明、构建方法与接口参考文档

5. 调试指南

• 可先运行 test_grasp 验证驱动注册、参数检查与基础状态机逻辑，再连接真实硬件运行 test_hw_so101_gripper，按“软件 → 硬件”顺序逐层排查；
• 若编译日志中出现 motor headers not found 或硬件测试程序未生成，说明 motor 组件未被正确发现，需检查 components/peripherals/motor 是否已编译并可被 CMake 检测；
• 若 grasp_alloc("so101_gripper", ...) 失败，可重点检查串口节点、串口权限、波特率、舵机 ID 以及舵机供电状态；
• 若夹爪动作异常或位置不准，建议先运行校准流程，确认 ./config/so101_gripper_calibration.json 已正确生成且内容有效；
• 若负载检测不准确，可检查 hold_threshold 设置是否合适，并结合实际夹持物体调整阈值；
• 与硬件/驱动同事联调时，建议同步提供以下信息：
  • 串口设备节点与权限状态；
  • 舵机 ID、波特率、电源规格；
  • 是否启用了 GRASP_BUILD_HW_TEST；
  • 测试程序输出日志；
  • 校准文件是否存在，以及校准得到的开合 ticks 范围。

6. 常见问题

现象	可能原因	处理
编译时未生成 test_hw_so101_gripper	motor 组件头文件或库未找到，硬件驱动被自动关闭	检查 components/peripherals/motor 是否已编译，必要时通过 MOTOR_INCLUDE_PATH / MOTOR_LIB_PATH 指定路径
grasp_alloc("so101_gripper", ...) 返回空	串口不可访问、舵机未上电、舵机 ID 或波特率不匹配	检查 /dev/ttyACM* 或 /dev/ttyUSB* 节点，开放权限，确认默认 1Mbaud 与舵机 ID=6 配置正确
夹爪移动到错误位置	未校准，或当前硬件开合范围与默认映射不一致	运行 test_hw_so101_gripper 的校准流程，生成并保存校准文件
抓取后未进入 HOLDING 状态	负载阈值过高，或夹持物体过轻	调整 hold_threshold 参数，并结合实际负载重新验证
校准失败或范围异常	手动移动位置不准确，或打开/闭合位置差值过小	重新执行校准，确保完全打开与完全闭合位置明显不同，且范围大于最小有效阈值