# Gripper Control

当前实际控制链：

1. `gripper_control/main.py`
   程序入口，负责组装配置、触觉读取、夹爪硬件和控制器。

2. `gripper_control/config.py`
   读取 Python 配置文件和命令行参数，生成 `ControlConfig`。

3. `gripper_control/force_reader.py`
   启动左右两个触觉传感器进程，读取 `FNORMAL`、`FSHEARX`、`FSHEARY`。

4. `gripper_control/calibration.py`
   把原始力值映射成 N。

5. `gripper_control/feedback.py`
   做基线扣除、滤波，输出控制器需要的力反馈。

6. `gripper_control/hardware.py`
   封装夹爪 SDK 的 `temp_move`、`set_target_force`、`read_real_position`。

7. `gripper_control/controller.py`
   定时开合、检测物体、慢慢加力、稳定夹持、空夹恢复、人取物松手的状态机。

## 运行

从项目根目录运行：

```powershell
conda activate py311
python -m gripper_control.main
```

旧入口仍然可用：

```powershell
python examples\gripper_timed_cycle_control.py
```

默认配置文件：

```text
gripper_control\config\gripper_timed_cycle_control.py
```

只测试逻辑、不打开串口：

```powershell
python -m gripper_control.main --dry-run
```

## 日志字段

程序运行时主要有两类输出：周期状态日志和事件日志。

### 周期状态日志

示例：

```text
t=38.83s L=0.024N R=0.067N min=0.024N max=0.067N diff=-0.043N both=0 object=0 grip_contact=0 empty_for=0.4s shear=0.113N d_shear=-0.013N shear_stable=1 state=hold_check gripped=1 hold_pos=9000 force_pct=15 action=hold-check
```

字段说明：

- `t`: 当前 close phase 已经运行的时间，单位秒。
- `L`: 左侧触觉传感器标定后的法向力，单位 N。
- `R`: 右侧触觉传感器标定后的法向力，单位 N。
- `min`: 左右两侧法向力中的较小值，单位 N。
- `max`: 左右两侧法向力中的较大值，单位 N。
- `diff`: `L - R`，左右法向力差值，单位 N；负数表示右侧更大。
- `both`: 左右两侧是否都超过 `BOTH_CONTACT_FORCE`；`1` 是，`0` 否。
- `object`: 是否确认检测到物体；默认需要达到 `OBJECT_FORCE` 且 `both=1`。
- `grip_contact`: 夹持后是否仍然认为有接触；默认等于 `both`。
- `empty_for`: 夹持/保持状态下，接触连续消失的时间；达到 `EMPTY_RELEASE_SECONDS` 后恢复定时开合。
- `shear`: 标定后的切向力，单位 N；由 `sqrt(FSHEARX^2 + FSHEARY^2)` 映射得到。
- `d_shear`: 本次循环相对上一次循环的切向力变化量，单位 N；负数表示切向力下降。
- `shear_stable`: 切向力是否进入稳定计时；`1` 表示已经超过 `SHEAR_HOLD_FORCE` 且变化小于 `SHEAR_STABLE_DELTA`。
- `state`: 控制器当前状态。
- `gripped`: 是否已经进入过夹持状态；`1` 是，`0` 否。
- `hold_pos`: 夹爪保持位置；通常是检测到物体时读取到的当前位置。
- `force_pct`: 当前发送给夹爪的目标力百分比。
- `action`: 当前这一步执行或等待的动作。

### 事件日志

示例：

```text
L=0.013N R=0.223N min=0.013N max=0.223N force_pct=10 action=empty-grip-resume-cycle
```

这类日志只在关键事件发生时打印，所以字段更少。

- `L/R/min/max`: 事件发生时的左右法向力和最小/最大法向力，单位 N。
- `force_pct`: 事件发生后设置的夹爪目标力百分比。
- `action=empty-grip-resume-cycle`: 已经进入夹持/保持状态，但接触连续消失超过 `EMPTY_RELEASE_SECONDS`，程序判断为空夹，退出当前夹持状态，回到定时开合循环。下一轮会先执行 open phase。

### 常见状态

- `closing`: 正在按定时循环闭合，还没确认检测到物体。
- `gripping`: 已检测到物体，夹爪保持当前位置，并从低力慢慢加力。
- `hold_check`: 切向力已经达到最低要求并稳定，夹爪进入保持/人取物检测状态。

### 常见 action

- `closing`: 继续闭合等待物体。
- `close-continue`: 未检测到物体，重复发送闭合命令。
- `object-detected-low-force`: 检测到物体，停止定时开合，并把夹爪力切到 `GRIP_START_FORCE`。
- `grip-ramp-up`: 夹住后慢慢增加 `force_pct`。
- `wait-shear-stable`: 切向力已超过 `SHEAR_HOLD_FORCE`，正在等待稳定。
- `shear-stable-hold`: 切向力稳定时间达到 `SHEAR_STABLE_SECONDS`，进入 `hold_check`。
- `hold-check`: 保持夹住，等待人取物触发。
- `hold-check-armed`: 进入 `hold_check` 后已经过了 `HUMAN_HOLD_SECONDS`，开始监测人取物。
- `human-release-open`: 检测到人取物造成的切向力变化，夹爪张开。
- `empty-grip-resume-cycle`: 夹持后接触消失，判定为空夹，回到定时开合。
- `emergency-open`: 超过 `EMERGENCY_TOUCH_FORCE` 且启用 emergency open 后，夹爪张开。