完善释放控制：稳定/迅速行为预设、力曲线夹稳门控与会话记录。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
cheng
@@ -11,8 +11,8 @@
|
||||
5. 闭合过程中检测到物体后，切到低力并保持当前位置。
|
||||
6. 像 01 一样进入 `gripping`，先慢慢加力到基础 `HOLD_FORCE`。
|
||||
7. 进入 `hold_check` 后，根据切向力估计物体负载；切向力越大，目标夹紧力越高，并按小步进继续补力。
|
||||
8. 自适应补力停止并稳定 `RELEASE_ARM_DELAY_SECONDS` 后，重新记录释放参考力，才开始检测人取物松开。
|
||||
9. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
|
||||
8. 接触起立即检测新的侧触；侧触释放不等待固定夹稳计时。普通法向变化另有短保护，无需等爬力完成。
|
||||
9. 新外力偏离既有曲线且持续，经新帧确认后张开到 `OPEN_POS`；原曲线变平、斜率变号和自然回落继续跟随，不靠累计位移释放。
|
||||
10. 张开后进入 `open_recover`，保持张开并等待恢复条件。
|
||||
11. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，继续休息至少 `REARM_SECONDS`。
|
||||
12. 休息结束后才回到 `open_wait` 并重新武装；休息期间不管有没有物体扫过，都不会闭合。
|
||||
@@ -153,14 +153,14 @@ python -m gripper.visualizer --windowed
|
||||
|
||||
- `trigger_normal_force`: 单侧法向力触发阈值。
|
||||
- `trigger_shear_force`: 单侧切向力触发阈值。
|
||||
- `trigger_clear_stable_seconds`: 张开恢复后，触发力必须清掉并稳定多久，才允许下一次扫过触发。
|
||||
- `trigger_clear_stable_seconds`: 旧配置兼容字段；当前稳定基线增量触发不使用它。
|
||||
- `grip_normal_force`: 夹住时左右接触阈值。
|
||||
- `grip_requires_both`: 是否必须左右都接触才算夹住。
|
||||
- `grip_start_force`: 检测到物体后先切到的低力。
|
||||
- `force_ramp_step`: 慢慢加力时每次增加多少 force_pct。
|
||||
- `force_ramp_interval`: 慢慢加力时两次加力的间隔。
|
||||
- `hold_force`: 慢慢加力到这个值后进入 `hold_check`。
|
||||
- `grip_settle_seconds`: 夹住后等待稳定多久再记录参考力。
|
||||
- `grip_settle_seconds`: `gripping` 进入 `hold_check` 的最早时间，不控制释放保护。
|
||||
- `adaptive_grip_enabled`: 是否启用按切向力估计负载并自适应补夹紧力。
|
||||
- `adaptive_shear_start_force`: 切向力低于这个值时，认为是轻物体，只使用最小目标力。
|
||||
- `adaptive_shear_full_force`: 切向力达到这个值时，认为负载较大，目标力加到最大目标力。
|
||||
@@ -168,9 +168,19 @@ python -m gripper.visualizer --windowed
|
||||
- `adaptive_force_max`: 自适应夹持的最大目标力；重物夹不住优先加这个，但不要超过 `force_max`。
|
||||
- `adaptive_force_step`: 自适应补力时每次增加多少 `force_pct`。
|
||||
- `adaptive_force_interval`: 自适应补力两次之间的间隔，越大越柔和。
|
||||
- `release_arm_delay_seconds`: 自适应补力停止后，等待多久再启用“人取物松开”检测。
|
||||
- `release_normal_change`: 夹住后法向力变化超过它就松开。
|
||||
- `release_shear_change`: 夹住后切向力变化超过它就松开。
|
||||
- `release_arm_delay_seconds`: 自运动实际命令执行完成后的短保护，默认 0.12s，不是旧式总释放延时。
|
||||
- `release_min_hold_seconds`: 普通法向接触保护，默认 0.28s；即时侧触路径不等待它。
|
||||
- `release_settle_seconds`: 脉冲拒绝/断流后的短恢复保护，默认 0.06s；不再要求全局持续安静。
|
||||
- `release_normal_rate` / `release_shear_rate`: 突变释放的速率门槛，默认 1.6 N/s。
|
||||
- `release_min_change`: 普通突变/轻触的最小净变化，默认 0.04 N。
|
||||
- `release_side_touch_change`: 即时侧触的左右力差变化，默认 0.06 N。
|
||||
- `release_touch_window_seconds`: 新外力起始证据有效期，默认 0.2s；不是等待时间。
|
||||
- `release_confirm_samples`: 突变确认新帧数，默认 2，包含首帧；平台偏移也能确认。
|
||||
- `release_slow_rate`: 慢变日志分类速率，默认 0.4 N/s；参考跟随由曲线延续决定，不门控就绪。
|
||||
- `release_normal_change` / `release_shear_change`: 中速外力累积的弱路径阈值，默认 0.28 N。
|
||||
- `release_slow_confirm_samples`: 弱路径确认新帧数，默认 5。
|
||||
- `release_confirm_window_seconds`: 释放候选最长有效期，默认 0.2s。
|
||||
- `release_sample_timeout_seconds`: 采样断流后重新等待平稳的门槛，默认 0.2s。
|
||||
- `release_open_speed`: 检测到松开后，张开到 `open_pos` 使用的速度。
|
||||
- `release_open_accel`: release 张开的加速度。
|
||||
- `release_open_decel`: release 张开的减速度。
|
||||
@@ -191,7 +201,7 @@ python -m gripper.visualizer --windowed
|
||||
- `maxN`: 左右法向力最大值，单位 N。
|
||||
- `shear`: 左右切向力最大值，单位 N。
|
||||
- `trigger`: 是否满足单侧触发条件。
|
||||
- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 且是新的一次触发上升沿才会闭合。
|
||||
- `armed`: 是否已经获得稳定通道基线；当前值相对该基线增长到触发阈值才会闭合。
|
||||
- `grip_contact`: 是否满足夹住接触条件。
|
||||
- `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。
|
||||
- `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。
|
||||
@@ -199,7 +209,8 @@ python -m gripper.visualizer --windowed
|
||||
- `hold_pos`: 夹住后保持的位置。
|
||||
- `force_pct`: 当前夹爪目标力百分比。
|
||||
- `target`: 当前根据切向力计算出来的自适应目标力。
|
||||
- `release_armed`: 是否已经启用释放检测；`0` 表示还在补力/等待稳定，`1` 表示法向或切向变化会触发张开。
|
||||
- `release_armed`: 普通监视或候选确认；独立即时侧触是否运行还需看 `side_watch`。
|
||||
- `side_watch`: 即时侧触监视，普通法向保护期间也可以为 `1`。
|
||||
- `action`: 当前动作。
|
||||
|
||||
## 可视化展示
|
||||
@@ -210,7 +221,7 @@ python -m gripper.visualizer --windowed
|
||||
- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。
|
||||
- 当前 `force_pct`、自适应目标力、速度 `speed_pct`、`hold_pos`。
|
||||
- 顶部有 `重启` 按钮，会先停止当前控制线程，释放相机与夹爪后再重新启动。
|
||||
- 当前 `release_armed`，用于判断系统是在补夹紧力，还是已经开始检测人取物松开。
|
||||
- 当前 `release_armed` 及 `release_side_watch`，分别表示普通释放与即时侧触检测。
|
||||
- 折线图带图例，默认固定 y 轴为 `0~3 N`，范围在 `visualizer.py` 顶部的 `PLOT_FORCE_Y_MIN_N / PLOT_FORCE_Y_MAX_N` 调整。
|
||||
- 左右传感器 FPS、接触标志、光流均值/最大值。
|
||||
- 中间 3D 夹爪使用 `crt_ctag2f90d_gripper_visualization` 里的完整 STL/URDF 实体面片模型；启动时会预缓存 41 个开合档位，运行中只按实际位置切换缓存，避免拖慢控制延迟。
|
||||
@@ -220,7 +231,7 @@ python -m gripper.visualizer --windowed
|
||||
- `open_wait`: 夹爪张开，等待任意单侧触发。
|
||||
- `closing`: 单侧触发后正在闭合，等待检测到物体。
|
||||
- `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。
|
||||
- `hold_check`: 已夹住；先根据切向力自适应补夹紧力，补力稳定后再监测法向/切向变化；变化超过阈值会直接张开。
|
||||
- `hold_check`: 已达到基础保持力；每 tick 先监视释放，再决定是否补力；候选确认优先于补力。
|
||||
- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。
|
||||
|
||||
## 常见动作说明
|
||||
@@ -228,14 +239,35 @@ python -m gripper.visualizer --windowed
|
||||
- `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。
|
||||
- `adaptive-grip-ramp`: 切向力显示负载较大，正在按小步进增加夹紧力。
|
||||
- `adaptive-grip-wait`: 已经需要更高目标力，但还没到下一次加力时间。
|
||||
- `release-arm-delay`: 补力刚停止，正在等待力稳定，暂时不触发松开。
|
||||
- `release-reference-armed`: 已重新记录释放参考力，后续法向/切向变化会触发张开。
|
||||
- `release-grip-settle`: 普通法向接触保护，独立即时侧触仍可检测。
|
||||
- `release-self-settle`: 普通法向自运动保护，独立即时切向侧触仍可检测。
|
||||
- `release-ready`: 夹稳通过，开始监视释放。
|
||||
- `release-sudden`: 突变外力候选及确认。
|
||||
- `release-touch`: 新的曲线起始变化及轻推偏移确认，不要求大瞬时速率。
|
||||
- `release-side_touch`: 即时侧触确认，不等固定夹稳时间。
|
||||
- `release-slow_internal`: 缓慢变化跟随参考。
|
||||
- `release-slow_external`: 新起始变化后达到弱路径幅度，经较长确认。
|
||||
- `release-transient-rejected`: 候选回弹或方向改变，丢弃此次证据。
|
||||
- `release-settling-turn` / `curve=settling_turn`: 平稳拐点，继续夹持。
|
||||
- `release-sample-gap`: 传感器断流，丢弃旧证据，等待新帧后恢复监视。
|
||||
- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。
|
||||
- `open-ready-position`: 夹爪已经回到初始点位，并且休息时间结束，重新进入等待下一次扫过触发；此时会直接武装，但残余触发保持不变不会自动闭合。
|
||||
- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。
|
||||
- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。
|
||||
- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。
|
||||
- `open-wait-trigger-held`: 已经武装，但当前触发不是新上升沿，等待力先降下去再下一次扫过。
|
||||
- `open-ready-position`: 夹爪已经回到初始点位，并且休息时间结束，重新进入等待下一次扫过触发；此时重新采集稳定基线，残余力保持不变不会自动闭合。
|
||||
- `open-wait-armed`: 已锁定稳定通道基线，下一次增量侧触可以闭合。
|
||||
- `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。
|
||||
- `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定和休息时间结束。
|
||||
- `open-ready`: 力已经清空并稳定，并且休息时间结束，重新进入等待下一次扫过触发。
|
||||
|
||||
## 释放状态设计与验证
|
||||
|
||||
[释放控制说明](docs/RELEASE_CONTROL.zh-CN.md) 包含独立 FSM、参数语义、曲线判据、
|
||||
回放命令及实机验收步骤。每次自运动稳定后会保留至少约 100ms 的观察窗口；基础爬力
|
||||
和自适应补力间隔因此可能比配置更长。侧触路径从接触就开始运行，普通法向与两侧共同变化仍有短保护。
|
||||
|
||||
```powershell
|
||||
python -m unittest discover -s tests -v
|
||||
```
|
||||
|
||||
软件回放验证稳定后外力至释放决定不超过 200ms；相机、串口和机械延迟需实机测量。
|
||||
|
||||
所有释放路径都要求新的曲线起始变化。闭合前的趋势会带入接触阶段，避免把原曲线的拐点当新触碰。
|
||||
每次自动释放后保存 `logs/release-last.json`，包含此前最多 120 帧控制快照与判据，用于复核实机误开。
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 2.7 MiB After Width: | Height: | Size: 2.5 MiB |
+39
-15
@@ -1,12 +1,12 @@
|
||||
{
|
||||
"video1": "2",
|
||||
"video1": "0",
|
||||
"video2": "1",
|
||||
"sensor_config1": "./config/ddjx01_q6a.json",
|
||||
"sensor_config2": "./config/ddjx01_q6a.json",
|
||||
"sensor_fps": 30.0,
|
||||
"motion_threshold": 0.0,
|
||||
"sensor_cpu": false,
|
||||
"port": "COM9",
|
||||
"port": "COM10",
|
||||
"slave_id": 1,
|
||||
"baudrate": 115200,
|
||||
"serial_timeout": 0.2,
|
||||
@@ -15,7 +15,7 @@
|
||||
"speed": 55,
|
||||
"accel": 70,
|
||||
"decel": 70,
|
||||
"release_open_speed": 50,
|
||||
"release_open_speed": 55,
|
||||
"release_open_accel": 70,
|
||||
"release_open_decel": 70,
|
||||
"open_force": 25,
|
||||
@@ -27,13 +27,13 @@
|
||||
"force_max": 99,
|
||||
"control_hz": 30.0,
|
||||
"baseline_seconds": 1.0,
|
||||
"filter_alpha": 0.55,
|
||||
"shear_filter_alpha": 0.65,
|
||||
"trigger_normal_force": 0.1,
|
||||
"trigger_shear_force": 0.1,
|
||||
"filter_alpha": 0.42,
|
||||
"shear_filter_alpha": 0.52,
|
||||
"trigger_normal_force": 0.06,
|
||||
"trigger_shear_force": 0.06,
|
||||
"trigger_clear_stable_seconds": 0.15,
|
||||
"open_wait_stable_samples": 12,
|
||||
"open_wait_stable_range_n": 0.01,
|
||||
"open_wait_stable_samples": 8,
|
||||
"open_wait_stable_range_n": 0.025,
|
||||
"open_wait_stable_trend_n": 0.015,
|
||||
"grip_normal_force": 0.2,
|
||||
"grip_requires_both": true,
|
||||
@@ -41,21 +41,45 @@
|
||||
"closing_command_interval": 0.2,
|
||||
"force_ramp_step": 1,
|
||||
"force_ramp_interval": 0.1,
|
||||
"grip_settle_seconds": 0.15,
|
||||
"grip_settle_seconds": 0.2,
|
||||
"grip_behavior_mode": "stable",
|
||||
"adaptive_grip_enabled": true,
|
||||
"adaptive_shear_start_force": 0.1,
|
||||
"adaptive_shear_full_force": 0.6,
|
||||
"adaptive_force_min": 33,
|
||||
"adaptive_force_max": 80,
|
||||
"adaptive_force_step": 5,
|
||||
"adaptive_force_interval": 0.03,
|
||||
"release_arm_delay_seconds": 1.2,
|
||||
"release_normal_change": 0.35,
|
||||
"adaptive_force_interval": 0.08,
|
||||
"release_arm_delay_seconds": 0.28,
|
||||
"release_min_hold_seconds": 0.65,
|
||||
"release_settle_seconds": 0.12,
|
||||
"release_min_change": 0.08,
|
||||
"release_touch_window_seconds": 0.14,
|
||||
"release_side_touch_change": 0.12,
|
||||
"release_confirm_window_seconds": 0.14,
|
||||
"release_sample_timeout_seconds": 0.2,
|
||||
"release_confirm_samples": 4,
|
||||
"release_slow_confirm_samples": 8,
|
||||
"release_normal_rate": 2.6,
|
||||
"release_shear_rate": 2.4,
|
||||
"release_slow_rate": 0.22,
|
||||
"release_normal_change": 0.42,
|
||||
"release_shear_change": 0.35,
|
||||
"rearm_seconds": 0.4,
|
||||
"release_curve_history": 30,
|
||||
"release_slope_window_seconds": 0.8,
|
||||
"release_slope_settle_rate": 0.22,
|
||||
"release_slope_min_points": 10,
|
||||
"release_require_slope_settle": true,
|
||||
"release_slope_hold_seconds": 0.45,
|
||||
"release_settle_normal_only": true,
|
||||
"release_side_shear_enabled": true,
|
||||
"release_side_normal_enabled": false,
|
||||
"release_shear_onset_enabled": true,
|
||||
"release_normal_onset_enabled": false,
|
||||
"rearm_seconds": 0.55,
|
||||
"recover_normal_force": 0.02,
|
||||
"recover_shear_force": 0.02,
|
||||
"recover_stable_seconds": 0.4,
|
||||
"recover_stable_seconds": 0.55,
|
||||
"open_position_recover_enabled": true,
|
||||
"open_position_tolerance": 100,
|
||||
"open_position_stable_seconds": 0.1,
|
||||
|
||||
@@ -397,8 +397,17 @@ FPS 文本约每秒更新一次。短时间红点可能来自相机启动、重
|
||||
| `hold-reference-armed` | 已记录夹持参考值 |
|
||||
| `adaptive-grip-ramp` | 正在自适应增加夹持力 |
|
||||
| `adaptive-grip-wait` | 等待下一次自适应加力 |
|
||||
| `release-arm-delay` | 等待释放检测稳定 |
|
||||
| `release-reference-armed` | 已重新记录释放参考值 |
|
||||
| `release-grip-settle` | 普通法向接触保护，即时侧触仍可检测 |
|
||||
| `release-self-settle` | 普通法向自运动保护，即时切向侧触仍可检测 |
|
||||
| `release-ready` | 释放检测已就绪 |
|
||||
| `release-sudden` | 突变外力新帧确认 |
|
||||
| `release-touch` | 新的曲线起始变化及轻推偏移确认 |
|
||||
| `release-side_touch` | 接触后即可识别的即时侧触确认 |
|
||||
| `release-slow_internal` | 慢变跟随参考 |
|
||||
| `release-slow_external` | 新起始变化后达到弱路径幅度，经较长确认 |
|
||||
| `release-transient-rejected` | 脉冲回弹，丢弃候选 |
|
||||
| `release-settling-turn` | 平稳曲线拐点或候选偏移回落，继续夹持 |
|
||||
| `release-sample-gap` | 采样断流，丢弃旧证据，等待新帧后恢复监视 |
|
||||
| `hold-check` | 正在监测保持状态 |
|
||||
| `force-change-release-open` | 检测到取物力变化并执行张开 |
|
||||
| `open-recover-position` | 正在等待张开位置恢复 |
|
||||
@@ -562,9 +571,23 @@ FPS 文本约每秒更新一次。短时间红点可能来自相机启动、重
|
||||
| `adaptive_force_max` | `80` | 自适应目标力上限 |
|
||||
| `adaptive_force_step` | `1` | 自适应每次增加的力比例 |
|
||||
| `adaptive_force_interval` | `0.05` | 自适应两次加力间隔 |
|
||||
| `release_arm_delay_seconds` | `0.8` | 停止加力后启用释放检测前的等待时间 |
|
||||
| `release_normal_change` | `0.2` | 相对释放参考值的法向变化阈值 |
|
||||
| `release_shear_change` | `0.2` | 相对释放参考值的切向变化阈值 |
|
||||
| `release_arm_delay_seconds` | `0.12` | 实际自运动命令执行完成后的短保护（秒），不是总释放延时 |
|
||||
| `release_min_hold_seconds` | `0.28` | 普通法向接触保护（秒），即时侧触不等待 |
|
||||
| `release_settle_seconds` | `0.06` | 脉冲/断流后的短恢复保护（秒） |
|
||||
| `release_normal_rate` / `release_shear_rate` | `1.6` | 突变速率门槛（N/s） |
|
||||
| `release_min_change` | `0.04` | 普通突变/轻触最小净变化（N） |
|
||||
| `release_side_touch_change` | `0.06` | 即时侧触的左右力差变化（N） |
|
||||
| `release_touch_window_seconds` | `0.2` | 新外力起始证据有效期（秒），非等待时间 |
|
||||
| `release_confirm_samples` | `2` | 突变确认新帧数，运行时至少 2 帧 |
|
||||
| `release_slow_rate` | `0.4` | 慢变日志分类速率（N/s），曲线跟随与就绪不由它门控 |
|
||||
| `release_normal_change` / `release_shear_change` | `0.28` | 中速外力累积的弱路径幅度（N） |
|
||||
| `release_slow_confirm_samples` | `5` | 弱路径确认新帧数 |
|
||||
| `release_confirm_window_seconds` | `0.2` | 确认有效窗口（秒） |
|
||||
| `release_sample_timeout_seconds` | `0.2` | 采样断流保护门槛（秒） |
|
||||
|
||||
每次自运动稳定后保留至少约 100ms 观察时间，候选确认优先于补力，因此实际加力
|
||||
间隔可能长于配置。`gripping` 期间也可以释放，无需等全部爬力完成。缓存帧不重复
|
||||
参与确认。详细曲线策略、保护窗口的取舍及实机验收见 [释放控制说明](RELEASE_CONTROL.zh-CN.md)。
|
||||
|
||||
### 14.8 恢复
|
||||
|
||||
@@ -747,3 +770,6 @@ Windows 日志连续出现 `CvCapture_MSMF::grabFrame ... can't grab frame`，
|
||||
---
|
||||
|
||||
本版本为快速初版，后续版本可补充带编号的主界面截图、设备接线图、参数调试示例和现场验收记录表。
|
||||
|
||||
释放检测先识别相对原有曲线的新起始变化；自然减速、变平和拐点不因累计位移够大就开爪。
|
||||
实际自动释放前的逐帧数据保存在 `logs/release-last.json`，源码和打包启动均适用。
|
||||
|
||||
@@ -0,0 +1,171 @@
|
||||
# 释放控制与回放验证
|
||||
|
||||
## 行为模式（迅速开合 / 夹持稳定）
|
||||
|
||||
设置页「行为模式」与主界面「迅速开合 / 夹持稳定」按钮共用 `grip_behavior_mode`：
|
||||
|
||||
| 模式 | 目标 | 典型倾向 |
|
||||
| --- | --- | --- |
|
||||
| `rapid`（迅速开合） | 跟手开合 | `release_min_hold=0`、更低侧触/确认门槛、更快再武装、关闭自适应加力 |
|
||||
| `stable`（夹持稳定） | 夹得住、少误开 | 更长接触保护、更高确认帧数与速率门槛、开启自适应；**长历史斜率趋零后再允许侧触/监视** |
|
||||
|
||||
预设定义见 `gripper/config/behavior_profiles.py`。切换模式会改写释放/滤波/爬力相关字段；可在设置里再微调后保存。运行中切换会即时写入 live `config` 并重置 `ReleaseWatch`。
|
||||
|
||||
## 会话录制（优化张开用）
|
||||
|
||||
主界面「录制数据」可开关。开启后每个控制 tick 追加写入
|
||||
`logs/sessions/session-YYYYMMDD-HHMMSS.jsonl`：
|
||||
|
||||
- 滤波后力 `feedback`、原始左右样本 `raw_left` / `raw_right`
|
||||
- 状态机 `state` / `action`、行为模式、力指令
|
||||
- 释放判定 `release`（phase/reason/confirm/rates…）
|
||||
- 执行器 `motion_pending` / `motion_completed_at`
|
||||
|
||||
文件为 JSONL：首行 `session_header`（含当时 release 相关配置），末行 `session_footer`。
|
||||
停止控制或再点按钮会结束录制。另有短窗快照 `logs/release-last.json`（仅最近一次释放前后）。
|
||||
|
||||
---
|
||||
|
||||
## 平稳拐点继续跟随，新外力才释放
|
||||
|
||||
`ReleaseWatch` 位于 `gripper/control/release_watch.py`，消费力数据和执行器状态，返回
|
||||
释放决定及是否允许补力；`controller.py` 通过 `ActuatorWorker` 发令。外围主循环保留原结构。
|
||||
|
||||
当前释放路径共用 `force_curve.py` 的曲线判断。旧的“短窗口净变化够大、两帧仍有偏移”
|
||||
会把缓升变平或自然回落当成侧触；现在仅有幅度、速率或左右力差都不足以释放，必须
|
||||
先出现相对已有曲线的新起始变化，再检查幅度和持续性。
|
||||
|
||||
具体处理：
|
||||
|
||||
1. 闭合阶段保留最近几帧，接触时继承各通道的趋势，不把接触帧当作静止起点。
|
||||
2. 每个通道保留可配置长度的历史（`release_curve_history`；rapid 约 6 点，stable 约 30 点）。
|
||||
**起始检测**仍只用最近约 6 点做局部二次拟合；**夹稳斜率**用更长窗口（`release_slope_window_seconds`）
|
||||
做一次拟合，得到 `fit_slope`（N/s）。夹住加力过程表现为该斜率逐渐变小并趋近于 0。
|
||||
3. 自然减速、变平、相近速度的斜率变号是曲线延续；它们不因累计位移达到门槛而启动释放候选。
|
||||
4. 新的偏离方向须与实际变化方向一致，并超过小幅噪声门槛（当前为
|
||||
`min(release_min_change, release_side_touch_change)` 的 20%）。真正的起始变化
|
||||
会被短暂保留，允许轻推在后续几帧达到幅度门槛。拟合残差及近期残差中位数用于估计
|
||||
噪声，预测误差容许范围随外推距离扩大；新偏离还必须超过该范围，不能把噪声外推成外力。
|
||||
5. 候选必须同时具有实际测得的位移和相对原曲线的偏移，避免曲线外推把小噪声放大成
|
||||
虚假的大变化。确认时扣除原曲线的继续变化，因此拐点附近新的触碰仍可检测。
|
||||
6. 尚未达到幅度的小变化一旦变平，或偏移落回预测误差范围，就丢弃起始证据，不能续接
|
||||
后续自然变化。已达幅度的候选若明显回落则取消，记录 `settling-turn`；回落判定容许
|
||||
当前噪声范围内的波动，避免真实触碰后的轻微抖动被误拒绝。
|
||||
同一套起始判断覆盖 `side_touch`、`touch`、`sudden` 和弱路径，避免其他分支绕过。
|
||||
7. **`release_require_slope_settle`（stable 默认开）**：仅看法向通道长窗 `|fit_slope|`
|
||||
（`release_settle_normal_only`）低于 `release_slope_settle_rate`，并连续保持
|
||||
`release_slope_hold_seconds`，**首次**达到后闩锁为已夹稳。之后自适应加力或主动
|
||||
张开使斜率再升高，**不会**撤销释放武装。
|
||||
8. **stable 干净分工**：夹稳只看法向；张开只走切向（`release_normal_onset_enabled=false`、
|
||||
`release_side_normal_enabled=false`，切向 onset/侧触开启）。rapid 仍法向+切向都能开。
|
||||
|
||||
没有增加固定夹稳等待，也没有恢复“四通道同时安静”的全局条件。以下是幅度与确认路径：
|
||||
|
||||
| 路径 | 检测 | 确认 |
|
||||
| --- | --- | --- |
|
||||
| 即时侧触 `side_touch` | 继承接触前左右力差曲线，检测偏离该曲线的新侧触 | 默认两次左右均更新的传感器帧；不等 0.28s 夹稳计时 |
|
||||
| 普通轻触 `touch` | 出现新起始变化后，实际位移和相对原曲线的偏移均达到 0.04N | 默认两帧；不要求瞬时速率达到 1.6N/s |
|
||||
| 突变 `sudden` / 弱路径 `slow_external` | 新起始变化叠加大速率 / 大幅偏移 | 默认两帧 / 五帧 |
|
||||
|
||||
即时侧触使用左减右的法向/切向力差的变化，抵消两侧共同压紧或负载变化。命令排队、
|
||||
执行及短响应尾部内，法向差分仍受保护，切向差分继续检测。普通共同法向卸载仍经过
|
||||
接触与自运动保护，不能把它与即时切向侧触混为一谈。
|
||||
|
||||
差分投票要求左右都提供新帧，避免相机异步更新制造虚假左右差。观察窗口不是必须
|
||||
等待的时间：出现新起始变化并达到幅度后立即确认。确认要求同一通道、同方向的
|
||||
新偏移持续存在；平稳拐点、单帧脉冲、回弹、换边不能拼票。
|
||||
|
||||
差分不能完美区分真实触碰与持续的单侧机械扰动。实机需特别检查单侧摩擦明显的物体；
|
||||
两帧确认和共同受力抵消并不等于所有机械扰动都能被识别。
|
||||
|
||||
## 状态与补力
|
||||
|
||||
| 状态 | 行为 |
|
||||
| --- | --- |
|
||||
| `disarmed` | 没有夹持周期，不检测释放 |
|
||||
| `grip_settle` | 普通法向接触保护，独立即时侧触路径已运行 |
|
||||
| `self_settle` | 普通法向自运动/瞬态恢复保护，即时切向侧触继续运行 |
|
||||
| `watching` | 突变、轻触和弱路径均可检测；慢漂移跟随参考 |
|
||||
| `confirming` | 候选确认优先，暂停补力 |
|
||||
| `released` | 一次性输出释放效果，进入张开恢复 |
|
||||
|
||||
保护计时结束且获得左右新帧，即恢复普通监视；不再要求四通道同时安静。脉冲拒绝或
|
||||
断流后的小恢复窗同样有界，不会因噪声反复延长。即时侧触确认独立于这些普通保护状态。
|
||||
|
||||
`gripping` 和 `hold_check` 每 tick 先检测释放，再考虑补力。恢复普通监视后至少保留
|
||||
`max(0.1s, release_confirm_samples/control_hz)` 观察时间再补力。轻推初期也暂缓补力，
|
||||
防止新命令覆盖取物信号；低速蠕变不会永久禁止补力。配置的补力间隔仍是最短间隔。
|
||||
|
||||
## 参数
|
||||
|
||||
| 参数 | 默认及当前 JSON | 语义 |
|
||||
| --- | ---: | --- |
|
||||
| `release_side_touch_change` | 0.06 N | 即时侧触的左右力差变化门槛 |
|
||||
| `release_touch_window_seconds` | 0.2s | 新外力起始证据有效期；不是额外等待 |
|
||||
| `release_min_change` | 0.04 N | 普通突变/轻触路径的最小净变化 |
|
||||
| `release_confirm_samples` | 2 | 突变/轻触/侧触确认帧数，含首帧；至少两帧 |
|
||||
| `release_min_hold_seconds` | 0.28s | 普通法向接触保护；不阻塞即时侧触 |
|
||||
| `release_arm_delay_seconds` | 0.12s | 实际命令执行完成后的普通法向保护；即时切向侧触仍运行 |
|
||||
| `release_settle_seconds` | 0.06s | 脉冲拒绝/断流后的短恢复保护；不再表示全局安静时间 |
|
||||
| `release_normal_rate` / `release_shear_rate` | 1.6 N/s | 突变路径门槛；轻触不必跨过它 |
|
||||
| `release_slow_rate` | 0.4 N/s | 慢变日志分类阈值；参考跟随由曲线延续决定，不门控就绪 |
|
||||
| `release_normal_change` / `release_shear_change` | 0.28 N | 新外力起始之后的大幅变化弱路径门槛，旧曲线累计变化不能绕过起始判断 |
|
||||
| `release_slow_confirm_samples` | 5 | 弱路径确认帧数，不少于突变路径 |
|
||||
| `release_confirm_window_seconds` | 0.2s | 候选最长确认时间；超时丢弃 |
|
||||
| `release_sample_timeout_seconds` | 0.2s | 任一侧断流或时间戳间隔过大时，丢弃已有证据 |
|
||||
| `release_curve_history` | 6 / stable 30 | 力曲线保留点数；onset 仍用最近约 6 点，斜率门控用更长窗 |
|
||||
| `release_slope_window_seconds` | 0.8s | 夹稳斜率估计时间窗 |
|
||||
| `release_slope_settle_rate` | 0.2 N/s | `|fit_slope|` 低于此值才视为加力趋稳 |
|
||||
| `release_slope_min_points` | 8 | 斜率门控生效前最少历史点数 |
|
||||
| `release_require_slope_settle` | false / stable true | 是否在侧触与退出 settle 前要求斜率趋零 |
|
||||
| `release_slope_hold_seconds` | 0 / stable 0.45s | 斜率达标后还需保持多久（滤掉短刺） |
|
||||
| `release_settle_normal_only` | true | 夹稳只看法向，不看切向 |
|
||||
| `release_side_shear_enabled` | true | 是否允许切向左右差侧触 |
|
||||
| `release_side_normal_enabled` | true / stable false | 是否允许法向左右差侧触 |
|
||||
| `release_shear_onset_enabled` | true | 是否允许切向触发普通释放路径 |
|
||||
| `release_normal_onset_enabled` | true / stable false | 是否允许法向触发普通释放路径 |
|
||||
|
||||
N/N/s 适用于当前标定配置。缓存帧不重复滤波或投票，提高控制频率不能代替真实传感器帧。
|
||||
`grip_settle_seconds` 只控制外围状态进入 `hold_check` 的最早时间。
|
||||
`trigger_clear_stable_seconds` 是未使用的兼容字段，开爪等待仍采用稳定基线增量。
|
||||
|
||||
## 日志
|
||||
|
||||
- `release-side_touch`：接触/自运动保护期间检测到侧触，正在确认。
|
||||
- `release-touch`：检测到新的轻推起始及持续偏移，不需要很大的瞬时速率。
|
||||
- `release-settling-turn` / `curve=settling_turn`：平稳拐点或候选回落，继续夹持。
|
||||
- `curve=settling_flatten`：原曲线自然减速、变平。
|
||||
- `force-settling` / `curve=force_settling`：长历史斜率尚未趋零，暂不侧触、不进 watching。
|
||||
- `channel` / `innovation`：候选通道及起始时偏离曲线预测的量。
|
||||
- `release-sudden` / `release-slow_external`：突变或弱路径确认。
|
||||
- `release-grip-settle` / `release-self-settle`：普通法向保护；`side_watch=1` 表示即时侧触仍运行。
|
||||
- `release-slow_internal`：慢变跟随参考，不累积无限漂移。
|
||||
- `release-transient-rejected` / `release-confirm-expired`：脉冲回弹或确认过期，未释放。
|
||||
- `release-sample-gap` / `release-waiting-sample`：断流保护或等待新帧。
|
||||
|
||||
`release_armed` 表示普通监视或已有侧触候选；不能单凭其为 0 判断侧触关闭。
|
||||
新增 `side_watch` 日志和 `release_side_watch` UI 状态单独表示即时侧触。
|
||||
状态/原因变化立即记录，重复日志约 0.2s 节流。
|
||||
|
||||
## 验证
|
||||
|
||||
```powershell
|
||||
conda activate py311
|
||||
python -m unittest discover -s tests -v
|
||||
```
|
||||
|
||||
回放先复现了旧版在没有突变的余弦缓升、变平、回落中误开的问题，再验证修正后不释放。
|
||||
覆盖 20/30Hz、单侧差分与双侧曲线、接触前已有上升趋势、大速率自然曲线、弱路径绕过、
|
||||
有界随机噪声，以及在上升、峰顶、回落阶段叠加一次真实新触碰。额外覆盖 60 组带噪声的
|
||||
平滑曲线和 60 组在带噪声曲线上叠加新触碰的回放，防止外推放大噪声或确认误拒绝。
|
||||
原有即时侧触和轻推
|
||||
响应回归保留，但软件回放不能证明实机已不误开。
|
||||
|
||||
每次实际自动释放后，会将释放前最多 120 个控制 tick 的逐帧力值、左右传感器时间戳、
|
||||
状态、起始判据及参数保存到 `logs/release-last.json`（源码启动也有效，覆盖上一次记录）。
|
||||
写文件在张开命令提交后进行，诊断不能阻塞该次发令。该文件用于复核真实拐点和误开原因，
|
||||
无需根据界面里被节流的几行日志猜测。
|
||||
|
||||
这些时间只覆盖软件决定，不包含相机处理、串口排队和机械动作。脉冲、回弹、慢漂移、
|
||||
断流、手动命令、闭合超时和当前 JSON 参数回放仍参与回归。实机应检查接触后立即侧触、
|
||||
持续噪声下轻推、长时间夹持误开，以及不同单侧摩擦条件下自运动是否被误识别为侧触。
|
||||
@@ -0,0 +1,196 @@
|
||||
"""Grip behavior presets: extreme rapid open/close vs stable hold."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
|
||||
|
||||
# rapid = 迅速开合：优先跟手释放 / 再武装
|
||||
# stable = 夹持稳定：优先防误开、夹得住
|
||||
GRIP_BEHAVIOR_MODES = ("rapid", "stable")
|
||||
|
||||
GRIP_BEHAVIOR_LABELS = {
|
||||
"rapid": "迅速开合",
|
||||
"stable": "夹持稳定",
|
||||
}
|
||||
|
||||
# Keys owned by a behavior profile. Selecting a mode rewrites these fields.
|
||||
GRIP_BEHAVIOR_KEYS = (
|
||||
"filter_alpha",
|
||||
"shear_filter_alpha",
|
||||
"open_wait_stable_samples",
|
||||
"open_wait_stable_range_n",
|
||||
"open_wait_stable_trend_n",
|
||||
"grip_settle_seconds",
|
||||
"force_ramp_step",
|
||||
"force_ramp_interval",
|
||||
"adaptive_grip_enabled",
|
||||
"adaptive_force_interval",
|
||||
"release_open_speed",
|
||||
"release_open_accel",
|
||||
"release_open_decel",
|
||||
"release_arm_delay_seconds",
|
||||
"release_min_hold_seconds",
|
||||
"release_settle_seconds",
|
||||
"release_min_change",
|
||||
"release_touch_window_seconds",
|
||||
"release_side_touch_change",
|
||||
"release_confirm_window_seconds",
|
||||
"release_sample_timeout_seconds",
|
||||
"release_confirm_samples",
|
||||
"release_slow_confirm_samples",
|
||||
"release_normal_rate",
|
||||
"release_shear_rate",
|
||||
"release_slow_rate",
|
||||
"release_normal_change",
|
||||
"release_shear_change",
|
||||
"release_curve_history",
|
||||
"release_slope_window_seconds",
|
||||
"release_slope_settle_rate",
|
||||
"release_slope_min_points",
|
||||
"release_require_slope_settle",
|
||||
"release_slope_hold_seconds",
|
||||
"release_settle_normal_only",
|
||||
"release_side_shear_enabled",
|
||||
"release_side_normal_enabled",
|
||||
"release_shear_onset_enabled",
|
||||
"release_normal_onset_enabled",
|
||||
"rearm_seconds",
|
||||
"recover_stable_seconds",
|
||||
)
|
||||
|
||||
GRIP_BEHAVIOR_PROFILES = {
|
||||
"rapid": {
|
||||
"filter_alpha": 0.72,
|
||||
"shear_filter_alpha": 0.78,
|
||||
"open_wait_stable_samples": 3,
|
||||
"open_wait_stable_range_n": 0.04,
|
||||
"open_wait_stable_trend_n": 0.03,
|
||||
"grip_settle_seconds": 0.05,
|
||||
"force_ramp_step": 3,
|
||||
"force_ramp_interval": 0.04,
|
||||
"adaptive_grip_enabled": False,
|
||||
"adaptive_force_interval": 0.05,
|
||||
"release_open_speed": 85,
|
||||
"release_open_accel": 90,
|
||||
"release_open_decel": 90,
|
||||
# Almost no normal-contact lock; rely on curve/side-touch rejection.
|
||||
"release_arm_delay_seconds": 0.04,
|
||||
"release_min_hold_seconds": 0.0,
|
||||
"release_settle_seconds": 0.03,
|
||||
"release_min_change": 0.03,
|
||||
"release_touch_window_seconds": 0.28,
|
||||
"release_side_touch_change": 0.04,
|
||||
"release_confirm_window_seconds": 0.28,
|
||||
"release_sample_timeout_seconds": 0.25,
|
||||
"release_confirm_samples": 2,
|
||||
"release_slow_confirm_samples": 3,
|
||||
"release_normal_rate": 1.0,
|
||||
"release_shear_rate": 1.0,
|
||||
"release_slow_rate": 0.55,
|
||||
"release_normal_change": 0.14,
|
||||
"release_shear_change": 0.14,
|
||||
"release_curve_history": 6,
|
||||
"release_slope_window_seconds": 0.35,
|
||||
"release_slope_settle_rate": 0.8,
|
||||
"release_slope_min_points": 4,
|
||||
"release_require_slope_settle": False,
|
||||
"release_slope_hold_seconds": 0.0,
|
||||
"release_settle_normal_only": False,
|
||||
"release_side_shear_enabled": True,
|
||||
"release_side_normal_enabled": True,
|
||||
"release_shear_onset_enabled": True,
|
||||
"release_normal_onset_enabled": True,
|
||||
"rearm_seconds": 0.12,
|
||||
"recover_stable_seconds": 0.12,
|
||||
},
|
||||
"stable": {
|
||||
"filter_alpha": 0.42,
|
||||
"shear_filter_alpha": 0.52,
|
||||
"open_wait_stable_samples": 8,
|
||||
"open_wait_stable_range_n": 0.025,
|
||||
"open_wait_stable_trend_n": 0.015,
|
||||
"grip_settle_seconds": 0.2,
|
||||
"force_ramp_step": 1,
|
||||
"force_ramp_interval": 0.1,
|
||||
"adaptive_grip_enabled": True,
|
||||
"adaptive_force_interval": 0.08,
|
||||
"release_open_speed": 55,
|
||||
"release_open_accel": 70,
|
||||
"release_open_decel": 70,
|
||||
"release_arm_delay_seconds": 0.28,
|
||||
"release_min_hold_seconds": 0.65,
|
||||
"release_settle_seconds": 0.12,
|
||||
"release_min_change": 0.08,
|
||||
"release_touch_window_seconds": 0.14,
|
||||
"release_side_touch_change": 0.12,
|
||||
"release_confirm_window_seconds": 0.14,
|
||||
"release_sample_timeout_seconds": 0.2,
|
||||
"release_confirm_samples": 4,
|
||||
"release_slow_confirm_samples": 8,
|
||||
"release_normal_rate": 2.6,
|
||||
"release_shear_rate": 2.4,
|
||||
"release_slow_rate": 0.22,
|
||||
"release_normal_change": 0.42,
|
||||
"release_shear_change": 0.35,
|
||||
# Clean split: settle on normal plateau; open only via shear.
|
||||
"release_curve_history": 30,
|
||||
"release_slope_window_seconds": 0.8,
|
||||
"release_slope_settle_rate": 0.22,
|
||||
"release_slope_min_points": 10,
|
||||
"release_require_slope_settle": True,
|
||||
"release_slope_hold_seconds": 0.45,
|
||||
"release_settle_normal_only": True,
|
||||
"release_side_shear_enabled": True,
|
||||
"release_side_normal_enabled": False,
|
||||
"release_shear_onset_enabled": True,
|
||||
"release_normal_onset_enabled": False,
|
||||
"rearm_seconds": 0.55,
|
||||
"recover_stable_seconds": 0.55,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def normalize_grip_behavior_mode(mode) -> str:
|
||||
text = str(mode or "").strip().lower()
|
||||
aliases = {
|
||||
"fast": "rapid",
|
||||
"quick": "rapid",
|
||||
"snap": "rapid",
|
||||
"迅速": "rapid",
|
||||
"迅速开合": "rapid",
|
||||
"hold": "stable",
|
||||
"firm": "stable",
|
||||
"secure": "stable",
|
||||
"稳定": "stable",
|
||||
"夹持稳定": "stable",
|
||||
}
|
||||
text = aliases.get(text, text)
|
||||
if text not in GRIP_BEHAVIOR_PROFILES:
|
||||
raise ValueError(
|
||||
f"unsupported grip_behavior_mode: {mode!r}; "
|
||||
f"expected one of {list(GRIP_BEHAVIOR_PROFILES)}"
|
||||
)
|
||||
return text
|
||||
|
||||
|
||||
def grip_behavior_profile(mode: str) -> dict:
|
||||
mode = normalize_grip_behavior_mode(mode)
|
||||
return copy.deepcopy(GRIP_BEHAVIOR_PROFILES[mode])
|
||||
|
||||
|
||||
def apply_grip_behavior_profile(target, mode: str) -> str:
|
||||
"""Write profile fields onto a config object or dict. Returns normalized mode."""
|
||||
mode = normalize_grip_behavior_mode(mode)
|
||||
profile = GRIP_BEHAVIOR_PROFILES[mode]
|
||||
if isinstance(target, dict):
|
||||
target["grip_behavior_mode"] = mode
|
||||
for key, value in profile.items():
|
||||
target[key] = copy.deepcopy(value)
|
||||
return mode
|
||||
|
||||
setattr(target, "grip_behavior_mode", mode)
|
||||
for key, value in profile.items():
|
||||
if hasattr(target, key):
|
||||
setattr(target, key, copy.deepcopy(value))
|
||||
return mode
|
||||
@@ -13,6 +13,10 @@ from .rotation import (
|
||||
DEFAULT_ROTATION2,
|
||||
normalize_rotation_config,
|
||||
)
|
||||
from .behavior_profiles import (
|
||||
apply_grip_behavior_profile,
|
||||
normalize_grip_behavior_mode,
|
||||
)
|
||||
|
||||
|
||||
DEFAULT_CONTROL_CONFIG = str(
|
||||
@@ -78,6 +82,7 @@ class Demo02Config:
|
||||
force_ramp_step: int = 1
|
||||
force_ramp_interval: float = 0.1
|
||||
grip_settle_seconds: float = 0.1
|
||||
grip_behavior_mode: str = "stable"
|
||||
adaptive_grip_enabled: bool = True
|
||||
adaptive_shear_start_force: float = 0.10
|
||||
adaptive_shear_full_force: float = 0.60
|
||||
@@ -85,9 +90,32 @@ class Demo02Config:
|
||||
adaptive_force_max: int = 45
|
||||
adaptive_force_step: int = 1
|
||||
adaptive_force_interval: float = 0.15
|
||||
release_arm_delay_seconds: float = 0.8
|
||||
release_normal_change: float = 0.03
|
||||
release_shear_change: float = 0.03
|
||||
release_arm_delay_seconds: float = 0.12
|
||||
release_min_hold_seconds: float = 0.28
|
||||
release_settle_seconds: float = 0.06
|
||||
release_min_change: float = 0.04
|
||||
release_touch_window_seconds: float = 0.2
|
||||
release_side_touch_change: float = 0.06
|
||||
release_confirm_window_seconds: float = 0.2
|
||||
release_sample_timeout_seconds: float = 0.2
|
||||
release_confirm_samples: int = 2
|
||||
release_slow_confirm_samples: int = 5
|
||||
release_normal_rate: float = 1.6
|
||||
release_shear_rate: float = 1.6
|
||||
release_slow_rate: float = 0.4
|
||||
release_normal_change: float = 0.28
|
||||
release_shear_change: float = 0.28
|
||||
release_curve_history: int = 6
|
||||
release_slope_window_seconds: float = 0.8
|
||||
release_slope_settle_rate: float = 0.2
|
||||
release_slope_min_points: int = 8
|
||||
release_require_slope_settle: bool = False
|
||||
release_slope_hold_seconds: float = 0.0
|
||||
release_settle_normal_only: bool = True
|
||||
release_side_shear_enabled: bool = True
|
||||
release_side_normal_enabled: bool = True
|
||||
release_shear_onset_enabled: bool = True
|
||||
release_normal_onset_enabled: bool = True
|
||||
rearm_seconds: float = 0.5
|
||||
recover_normal_force: float = 0.02
|
||||
recover_shear_force: float = 0.02
|
||||
@@ -171,6 +199,14 @@ class Demo02Config:
|
||||
add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step))
|
||||
add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval))
|
||||
add("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds))
|
||||
add(
|
||||
"--grip-behavior-mode",
|
||||
type=str,
|
||||
choices=["rapid", "stable"],
|
||||
default=normalize_grip_behavior_mode(
|
||||
get("grip_behavior_mode", cls.grip_behavior_mode)
|
||||
),
|
||||
)
|
||||
add("--enable-adaptive-grip", dest="adaptive_grip_enabled", action="store_true", default=get("adaptive_grip_enabled", cls.adaptive_grip_enabled))
|
||||
add("--disable-adaptive-grip", dest="adaptive_grip_enabled", action="store_false")
|
||||
add("--adaptive-shear-start-force", type=float, default=get("adaptive_shear_start_force", cls.adaptive_shear_start_force))
|
||||
@@ -180,8 +216,43 @@ class Demo02Config:
|
||||
add("--adaptive-force-step", type=int, default=get("adaptive_force_step", cls.adaptive_force_step))
|
||||
add("--adaptive-force-interval", type=float, default=get("adaptive_force_interval", cls.adaptive_force_interval))
|
||||
add("--release-arm-delay-seconds", type=float, default=get("release_arm_delay_seconds", cls.release_arm_delay_seconds))
|
||||
add("--release-min-hold-seconds", type=float, default=get("release_min_hold_seconds", cls.release_min_hold_seconds))
|
||||
add("--release-settle-seconds", type=float, default=get("release_settle_seconds", cls.release_settle_seconds))
|
||||
add("--release-min-change", type=float, default=get("release_min_change", cls.release_min_change))
|
||||
add("--release-touch-window-seconds", type=float, default=get("release_touch_window_seconds", cls.release_touch_window_seconds))
|
||||
add("--release-side-touch-change", type=float, default=get("release_side_touch_change", cls.release_side_touch_change))
|
||||
add("--release-confirm-window-seconds", type=float, default=get("release_confirm_window_seconds", cls.release_confirm_window_seconds))
|
||||
add("--release-sample-timeout-seconds", type=float, default=get("release_sample_timeout_seconds", cls.release_sample_timeout_seconds))
|
||||
add("--release-confirm-samples", type=int, default=get("release_confirm_samples", cls.release_confirm_samples))
|
||||
add("--release-slow-confirm-samples", type=int, default=get("release_slow_confirm_samples", cls.release_slow_confirm_samples))
|
||||
add("--release-normal-rate", type=float, default=get("release_normal_rate", cls.release_normal_rate))
|
||||
add("--release-shear-rate", type=float, default=get("release_shear_rate", cls.release_shear_rate))
|
||||
add("--release-slow-rate", type=float, default=get("release_slow_rate", cls.release_slow_rate))
|
||||
add("--release-normal-change", type=float, default=get("release_normal_change", cls.release_normal_change))
|
||||
add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
|
||||
add("--release-curve-history", type=int, default=get("release_curve_history", cls.release_curve_history))
|
||||
add("--release-slope-window-seconds", type=float, default=get("release_slope_window_seconds", cls.release_slope_window_seconds))
|
||||
add("--release-slope-settle-rate", type=float, default=get("release_slope_settle_rate", cls.release_slope_settle_rate))
|
||||
add("--release-slope-min-points", type=int, default=get("release_slope_min_points", cls.release_slope_min_points))
|
||||
add("--release-require-slope-settle", dest="release_require_slope_settle", action="store_true",
|
||||
default=get("release_require_slope_settle", cls.release_require_slope_settle))
|
||||
add("--no-release-require-slope-settle", dest="release_require_slope_settle", action="store_false")
|
||||
add("--release-slope-hold-seconds", type=float, default=get("release_slope_hold_seconds", cls.release_slope_hold_seconds))
|
||||
add("--release-settle-normal-only", dest="release_settle_normal_only", action="store_true",
|
||||
default=get("release_settle_normal_only", cls.release_settle_normal_only))
|
||||
add("--release-settle-all-channels", dest="release_settle_normal_only", action="store_false")
|
||||
add("--release-side-shear-enabled", dest="release_side_shear_enabled", action="store_true",
|
||||
default=get("release_side_shear_enabled", cls.release_side_shear_enabled))
|
||||
add("--no-release-side-shear", dest="release_side_shear_enabled", action="store_false")
|
||||
add("--release-side-normal-enabled", dest="release_side_normal_enabled", action="store_true",
|
||||
default=get("release_side_normal_enabled", cls.release_side_normal_enabled))
|
||||
add("--no-release-side-normal", dest="release_side_normal_enabled", action="store_false")
|
||||
add("--release-shear-onset-enabled", dest="release_shear_onset_enabled", action="store_true",
|
||||
default=get("release_shear_onset_enabled", cls.release_shear_onset_enabled))
|
||||
add("--no-release-shear-onset", dest="release_shear_onset_enabled", action="store_false")
|
||||
add("--release-normal-onset-enabled", dest="release_normal_onset_enabled", action="store_true",
|
||||
default=get("release_normal_onset_enabled", cls.release_normal_onset_enabled))
|
||||
add("--no-release-normal-onset", dest="release_normal_onset_enabled", action="store_false")
|
||||
add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds))
|
||||
add("--recover-normal-force", type=float, default=get("recover_normal_force", cls.recover_normal_force))
|
||||
add("--recover-shear-force", type=float, default=get("recover_shear_force", cls.recover_shear_force))
|
||||
@@ -208,4 +279,10 @@ class Demo02Config:
|
||||
values["shear_force_calibration"] = copy.deepcopy(
|
||||
config_data.get("shear_force_calibration", DEFAULT_SHEAR_FORCE_CALIBRATION)
|
||||
)
|
||||
values["grip_behavior_mode"] = normalize_grip_behavior_mode(
|
||||
values.get("grip_behavior_mode", cls.grip_behavior_mode)
|
||||
)
|
||||
return cls(**values)
|
||||
|
||||
def apply_grip_behavior_mode(self, mode: str) -> str:
|
||||
return apply_grip_behavior_profile(self, mode)
|
||||
|
||||
@@ -1,5 +1,11 @@
|
||||
from .controller import SideTriggerGripController
|
||||
from .feedback import TactileFeedbackProcessor
|
||||
from .actuator import ActuatorWorker, GripCommand
|
||||
|
||||
|
||||
__all__ = ["SideTriggerGripController", "TactileFeedbackProcessor"]
|
||||
__all__ = [
|
||||
"SideTriggerGripController",
|
||||
"TactileFeedbackProcessor",
|
||||
"ActuatorWorker",
|
||||
"GripCommand",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,362 @@
|
||||
"""Serial-owning actuator worker: command queue + position cache."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GripCommand:
|
||||
kind: str
|
||||
force_pct: Optional[int] = None
|
||||
position: Optional[int] = None
|
||||
label: str = ""
|
||||
speed_pct: Optional[int] = None
|
||||
accel: Optional[int] = None
|
||||
decel: Optional[int] = None
|
||||
|
||||
|
||||
_MOTION_KINDS = frozenset({"open", "close", "move", "hold"})
|
||||
|
||||
|
||||
class ActuatorWorker:
|
||||
"""Runs all GripperClient I/O on one thread; control loop only submits."""
|
||||
|
||||
def __init__(self, gripper, position_poll_hz: float = 10.0):
|
||||
self.gripper = gripper
|
||||
self.position_poll_hz = max(1.0, float(position_poll_hz))
|
||||
self._stop_event = threading.Event()
|
||||
self._started = threading.Event()
|
||||
self._start_error: Optional[BaseException] = None
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
|
||||
self._cmd_lock = threading.Lock()
|
||||
self._commands: deque[GripCommand] = deque()
|
||||
self._active_command: Optional[GripCommand] = None
|
||||
self._motion_completed_at: Optional[float] = None
|
||||
self._wake = threading.Event()
|
||||
self._busy = threading.Event()
|
||||
|
||||
self._state_lock = threading.Lock()
|
||||
self._position: Optional[int] = None
|
||||
self._hold_pos: Optional[int] = None
|
||||
self._last_error: Optional[str] = None
|
||||
self._cmd_count = 0
|
||||
self._cmd_total_s = 0.0
|
||||
self._cmd_max_s = 0.0
|
||||
self._pos_count = 0
|
||||
self._pos_total_s = 0.0
|
||||
self._pos_max_s = 0.0
|
||||
self._perf_last_log = 0.0
|
||||
|
||||
@property
|
||||
def position(self) -> Optional[int]:
|
||||
with self._state_lock:
|
||||
return self._position
|
||||
|
||||
@property
|
||||
def hold_pos(self) -> Optional[int]:
|
||||
with self._state_lock:
|
||||
return self._hold_pos
|
||||
|
||||
@property
|
||||
def last_error(self) -> Optional[str]:
|
||||
with self._state_lock:
|
||||
return self._last_error
|
||||
|
||||
@property
|
||||
def motion_status(self):
|
||||
"""Atomic queue/in-flight snapshot and actual I/O completion time."""
|
||||
with self._cmd_lock:
|
||||
return bool(self._commands) or self._active_command is not None, self._motion_completed_at
|
||||
|
||||
def start(self, timeout: float = 10.0) -> None:
|
||||
if self._thread is not None and self._thread.is_alive():
|
||||
return
|
||||
self._stop_event.clear()
|
||||
self._started.clear()
|
||||
self._start_error = None
|
||||
self._thread = threading.Thread(
|
||||
target=self._run,
|
||||
name="gripper-actuator",
|
||||
daemon=True,
|
||||
)
|
||||
self._thread.start()
|
||||
if not self._started.wait(timeout=timeout):
|
||||
raise TimeoutError("actuator worker failed to start in time")
|
||||
if self._start_error is not None:
|
||||
raise RuntimeError(f"actuator worker failed to connect: {self._start_error}")
|
||||
|
||||
def stop(self, timeout: float = 3.0) -> None:
|
||||
self._stop_event.set()
|
||||
self._wake.set()
|
||||
thread = self._thread
|
||||
if thread is not None:
|
||||
thread.join(timeout=timeout)
|
||||
self._thread = None
|
||||
|
||||
def submit(self, command: GripCommand) -> None:
|
||||
with self._cmd_lock:
|
||||
if command.kind in _MOTION_KINDS:
|
||||
self._commands = deque(
|
||||
item for item in self._commands
|
||||
if item.kind not in _MOTION_KINDS and item.kind != "set_force"
|
||||
)
|
||||
elif command.kind == "set_force":
|
||||
self._commands = deque(
|
||||
item for item in self._commands if item.kind != "set_force"
|
||||
)
|
||||
self._commands.append(command)
|
||||
self._wake.set()
|
||||
|
||||
def open(
|
||||
self,
|
||||
force_pct: int,
|
||||
label: str = "open",
|
||||
speed_pct=None,
|
||||
accel=None,
|
||||
decel=None,
|
||||
) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="open",
|
||||
force_pct=int(force_pct),
|
||||
label=label,
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
)
|
||||
)
|
||||
|
||||
def close(
|
||||
self,
|
||||
force_pct: int,
|
||||
label: str = "close",
|
||||
speed_pct=None,
|
||||
accel=None,
|
||||
decel=None,
|
||||
) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="close",
|
||||
force_pct=int(force_pct),
|
||||
label=label,
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
)
|
||||
)
|
||||
|
||||
def move(
|
||||
self,
|
||||
position: int,
|
||||
force_pct: int,
|
||||
label: str = "move",
|
||||
speed_pct=None,
|
||||
accel=None,
|
||||
decel=None,
|
||||
) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="move",
|
||||
position=int(position),
|
||||
force_pct=int(force_pct),
|
||||
label=label,
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
)
|
||||
)
|
||||
|
||||
def set_force(self, force_pct: int) -> None:
|
||||
self.submit(GripCommand(kind="set_force", force_pct=int(force_pct)))
|
||||
|
||||
def hold(self, force_pct: int, speed_pct=None, accel=None, decel=None) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="hold",
|
||||
force_pct=int(force_pct),
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
label="clamp-hold",
|
||||
)
|
||||
)
|
||||
|
||||
def final_open_if_needed(self) -> None:
|
||||
self.submit(GripCommand(kind="final_open"))
|
||||
self.wait_idle(timeout=5.0)
|
||||
|
||||
def wait_idle(self, timeout: float = 5.0) -> bool:
|
||||
deadline = time.perf_counter() + timeout
|
||||
while time.perf_counter() < deadline:
|
||||
pending, _ = self.motion_status
|
||||
if not pending:
|
||||
return True
|
||||
time.sleep(0.01)
|
||||
return False
|
||||
|
||||
def _pop_command(self) -> Optional[GripCommand]:
|
||||
with self._cmd_lock:
|
||||
if not self._commands:
|
||||
return None
|
||||
self._active_command = self._commands.popleft()
|
||||
return self._active_command
|
||||
|
||||
def _set_error(self, message: str) -> None:
|
||||
with self._state_lock:
|
||||
self._last_error = message
|
||||
|
||||
def _update_position(self, position: Optional[int]) -> None:
|
||||
with self._state_lock:
|
||||
if position is not None:
|
||||
self._position = int(position)
|
||||
|
||||
def _run(self) -> None:
|
||||
try:
|
||||
self.gripper.connect()
|
||||
except BaseException as exc:
|
||||
self._start_error = exc
|
||||
self._started.set()
|
||||
return
|
||||
|
||||
self._started.set()
|
||||
poll_interval = 1.0 / self.position_poll_hz
|
||||
next_poll = time.perf_counter()
|
||||
self._perf_last_log = time.perf_counter()
|
||||
|
||||
# Prime position cache once after connect.
|
||||
self._poll_position()
|
||||
|
||||
while not self._stop_event.is_set():
|
||||
cmd = self._pop_command()
|
||||
if cmd is not None:
|
||||
self._busy.set()
|
||||
try:
|
||||
self._execute(cmd)
|
||||
finally:
|
||||
with self._cmd_lock:
|
||||
self._motion_completed_at = time.perf_counter()
|
||||
self._active_command = None
|
||||
still_pending = bool(self._commands)
|
||||
if not still_pending:
|
||||
self._busy.clear()
|
||||
continue
|
||||
|
||||
self._busy.clear()
|
||||
now = time.perf_counter()
|
||||
if now >= next_poll:
|
||||
self._poll_position()
|
||||
next_poll = now + poll_interval
|
||||
self._maybe_log_perf(now)
|
||||
continue
|
||||
|
||||
wait_s = min(0.02, max(0.0, next_poll - now))
|
||||
self._wake.wait(timeout=wait_s)
|
||||
self._wake.clear()
|
||||
|
||||
def _execute(self, cmd: GripCommand) -> None:
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
if cmd.kind == "open":
|
||||
self.gripper.open(
|
||||
cmd.force_pct,
|
||||
cmd.label or "open",
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._update_position(self.gripper.last_commanded_position)
|
||||
elif cmd.kind == "close":
|
||||
self.gripper.close(
|
||||
cmd.force_pct,
|
||||
cmd.label or "close",
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._update_position(self.gripper.last_commanded_position)
|
||||
elif cmd.kind == "move":
|
||||
self.gripper.move(
|
||||
cmd.position,
|
||||
cmd.force_pct,
|
||||
cmd.label or "move",
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._update_position(cmd.position)
|
||||
elif cmd.kind == "set_force":
|
||||
self.gripper.set_force(cmd.force_pct)
|
||||
elif cmd.kind == "hold":
|
||||
hold_pos = self.gripper.hold_current_position(
|
||||
cmd.force_pct,
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
with self._state_lock:
|
||||
self._hold_pos = hold_pos
|
||||
if hold_pos is not None:
|
||||
self._position = int(hold_pos)
|
||||
elif cmd.kind == "final_open":
|
||||
self.gripper.final_open_if_needed()
|
||||
self._update_position(self.gripper.last_commanded_position)
|
||||
else:
|
||||
self._set_error(f"unknown command kind: {cmd.kind}")
|
||||
except Exception as exc:
|
||||
self._set_error(f"{cmd.kind} failed: {exc}")
|
||||
print(f"[actuator] {cmd.kind} failed: {exc}")
|
||||
finally:
|
||||
dt = time.perf_counter() - t0
|
||||
with self._state_lock:
|
||||
self._cmd_count += 1
|
||||
self._cmd_total_s += dt
|
||||
self._cmd_max_s = max(self._cmd_max_s, dt)
|
||||
|
||||
def _poll_position(self) -> None:
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
position = self.gripper.read_position()
|
||||
self._update_position(position)
|
||||
except Exception as exc:
|
||||
self._set_error(f"read_position failed: {exc}")
|
||||
finally:
|
||||
dt = time.perf_counter() - t0
|
||||
with self._state_lock:
|
||||
self._pos_count += 1
|
||||
self._pos_total_s += dt
|
||||
self._pos_max_s = max(self._pos_max_s, dt)
|
||||
|
||||
def _maybe_log_perf(self, now: float) -> None:
|
||||
if now - self._perf_last_log < 1.0:
|
||||
return
|
||||
with self._state_lock:
|
||||
cmd_count = self._cmd_count
|
||||
cmd_total = self._cmd_total_s
|
||||
cmd_max = self._cmd_max_s
|
||||
pos_count = self._pos_count
|
||||
pos_total = self._pos_total_s
|
||||
pos_max = self._pos_max_s
|
||||
position = self._position
|
||||
self._cmd_count = 0
|
||||
self._cmd_total_s = 0.0
|
||||
self._cmd_max_s = 0.0
|
||||
self._pos_count = 0
|
||||
self._pos_total_s = 0.0
|
||||
self._pos_max_s = 0.0
|
||||
self._perf_last_log = now
|
||||
if cmd_count == 0 and pos_count == 0:
|
||||
return
|
||||
cmd_avg = (cmd_total / cmd_count * 1000.0) if cmd_count else 0.0
|
||||
pos_avg = (pos_total / pos_count * 1000.0) if pos_count else 0.0
|
||||
print(
|
||||
"[perf] actuator "
|
||||
f"cmd={cmd_count} avg={cmd_avg:.2f}ms max={cmd_max * 1000.0:.2f}ms "
|
||||
f"pos_read={pos_count} avg={pos_avg:.2f}ms max={pos_max * 1000.0:.2f}ms "
|
||||
f"pos={position}"
|
||||
)
|
||||
+383
-193
@@ -1,8 +1,14 @@
|
||||
import time
|
||||
import threading
|
||||
import json
|
||||
from collections import deque
|
||||
from dataclasses import asdict
|
||||
|
||||
from ..common.filters import clamp
|
||||
from ..common.paths import REPO_ROOT
|
||||
from .actuator import ActuatorWorker
|
||||
from .release_watch import ReleaseWatch
|
||||
from .session_recorder import SessionRecorder
|
||||
|
||||
|
||||
class SideTriggerGripController:
|
||||
@@ -21,11 +27,20 @@ class SideTriggerGripController:
|
||||
self.normal_unit = feedback_processor.normal_converter.unit
|
||||
self.shear_unit = feedback_processor.shear_converter.unit
|
||||
self.status_callback = status_callback
|
||||
self.actuator = None
|
||||
self.release_watch = ReleaseWatch(config)
|
||||
self._release_decision = None
|
||||
self._force_trace = deque(maxlen=120)
|
||||
self.release_trace_path = REPO_ROOT / "logs" / "release-last.json"
|
||||
self.session_recorder = SessionRecorder(REPO_ROOT / "logs" / "sessions")
|
||||
self._stop_event = threading.Event()
|
||||
self._manual_lock = threading.Lock()
|
||||
self._manual_commands = deque()
|
||||
self._mode_lock = threading.Lock()
|
||||
self._control_mode = "smart"
|
||||
self._last_log_time = 0.0
|
||||
self._log_interval_s = 0.2
|
||||
self._last_log_key = None
|
||||
|
||||
def stop(self):
|
||||
self._stop_event.set()
|
||||
@@ -62,6 +77,59 @@ class SideTriggerGripController:
|
||||
with self._mode_lock:
|
||||
self._control_mode = mode
|
||||
|
||||
def set_grip_behavior_mode(self, mode):
|
||||
"""Apply rapid/stable preset onto live config and reset release watch."""
|
||||
applied = self.config.apply_grip_behavior_mode(mode)
|
||||
# Filters should match the new profile immediately.
|
||||
try:
|
||||
self.feedback_processor.reset_filters()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self.release_watch.reset()
|
||||
except Exception:
|
||||
pass
|
||||
print(
|
||||
f"[behavior] grip_behavior_mode={applied} "
|
||||
f"min_hold={self.config.release_min_hold_seconds:.2f}s "
|
||||
f"side_touch={self.config.release_side_touch_change:.3f} "
|
||||
f"confirm={self.config.release_confirm_samples} "
|
||||
f"curve_hist={getattr(self.config, 'release_curve_history', 6)} "
|
||||
f"slope_settle={getattr(self.config, 'release_require_slope_settle', False)}"
|
||||
)
|
||||
return applied
|
||||
|
||||
def start_session_recording(self, note=""):
|
||||
meta = {
|
||||
"note": note,
|
||||
"grip_behavior_mode": getattr(self.config, "grip_behavior_mode", None),
|
||||
"control_mode": self._get_control_mode(),
|
||||
"config": {
|
||||
key: getattr(self.config, key)
|
||||
for key in dir(self.config)
|
||||
if key.startswith("release_")
|
||||
or key in {
|
||||
"control_hz",
|
||||
"filter_alpha",
|
||||
"shear_filter_alpha",
|
||||
"trigger_normal_force",
|
||||
"trigger_shear_force",
|
||||
"grip_normal_force",
|
||||
"grip_requires_both",
|
||||
"grip_behavior_mode",
|
||||
"adaptive_grip_enabled",
|
||||
}
|
||||
},
|
||||
}
|
||||
return self.session_recorder.start(meta=meta)
|
||||
|
||||
def stop_session_recording(self):
|
||||
return self.session_recorder.stop()
|
||||
|
||||
@property
|
||||
def session_recording(self):
|
||||
return self.session_recorder.enabled
|
||||
|
||||
def _get_control_mode(self):
|
||||
with self._mode_lock:
|
||||
return self._control_mode
|
||||
@@ -75,16 +143,18 @@ class SideTriggerGripController:
|
||||
def run(self):
|
||||
self._stop_event.clear()
|
||||
self.reader.start()
|
||||
self.actuator = ActuatorWorker(self.gripper, position_poll_hz=10.0)
|
||||
|
||||
try:
|
||||
self.gripper.connect()
|
||||
self.actuator.start()
|
||||
open_force = int(clamp(
|
||||
self.config.open_force,
|
||||
self.config.force_min,
|
||||
self.config.force_max,
|
||||
))
|
||||
print("demo02 start: open gripper and wait for side touch")
|
||||
self.gripper.open(open_force, "startup-open")
|
||||
print("control loop uses async actuator thread (serial I/O off control tick)")
|
||||
self.actuator.open(open_force, "startup-open")
|
||||
|
||||
self._print_thresholds()
|
||||
print(
|
||||
@@ -104,7 +174,16 @@ class SideTriggerGripController:
|
||||
except KeyboardInterrupt:
|
||||
print("\nStopping demo02.")
|
||||
finally:
|
||||
self.gripper.final_open_if_needed()
|
||||
try:
|
||||
self.stop_session_recording()
|
||||
except Exception:
|
||||
pass
|
||||
if self.actuator is not None:
|
||||
try:
|
||||
self.actuator.final_open_if_needed()
|
||||
finally:
|
||||
self.actuator.stop()
|
||||
self.actuator = None
|
||||
self.reader.stop()
|
||||
|
||||
def _print_thresholds(self):
|
||||
@@ -117,8 +196,11 @@ class SideTriggerGripController:
|
||||
"Grip/release thresholds: "
|
||||
f"grip_normal={self.config.grip_normal_force:.3f}{self.normal_unit}, "
|
||||
f"requires_both={int(self.config.grip_requires_both)}, "
|
||||
f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, "
|
||||
f"release_shear_change={self.config.release_shear_change:.3f}{self.shear_unit}"
|
||||
f"release_abs_n={self.config.release_normal_change:.3f}{self.normal_unit}, "
|
||||
f"release_abs_s={self.config.release_shear_change:.3f}{self.shear_unit}, "
|
||||
f"release_rate_n={getattr(self.config, 'release_normal_rate', 1.8):.2f}/s, "
|
||||
f"release_rate_s={getattr(self.config, 'release_shear_rate', 1.8):.2f}/s, "
|
||||
f"slow_rate={getattr(self.config, 'release_slow_rate', 0.35):.2f}/s"
|
||||
)
|
||||
print(
|
||||
"Adaptive grip: "
|
||||
@@ -128,7 +210,10 @@ class SideTriggerGripController:
|
||||
f"force={self.config.adaptive_force_min}-{self.config.adaptive_force_max}, "
|
||||
f"step={self.config.adaptive_force_step}, "
|
||||
f"interval={self.config.adaptive_force_interval:.2f}s, "
|
||||
f"release_arm_delay={self.config.release_arm_delay_seconds:.2f}s"
|
||||
f"self_force_cooldown={self.config.release_arm_delay_seconds:.2f}s, "
|
||||
f"min_hold={getattr(self.config, 'release_min_hold_seconds', 0.28):.2f}s, "
|
||||
f"curve_settle={self.config.release_settle_seconds:.2f}s, "
|
||||
f"release_confirm={getattr(self.config, 'release_confirm_samples', 2)}"
|
||||
)
|
||||
print(
|
||||
"Release open: "
|
||||
@@ -136,6 +221,7 @@ class SideTriggerGripController:
|
||||
f"accel={self.config.release_open_accel}, "
|
||||
f"decel={self.config.release_open_decel}"
|
||||
)
|
||||
print("Release: side-touch watch starts at contact; normal clamp protection never waits for global quiet")
|
||||
print(
|
||||
"Recover: "
|
||||
f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, "
|
||||
@@ -157,66 +243,79 @@ class SideTriggerGripController:
|
||||
last_open_position_ready = False
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
last_trigger = False
|
||||
open_wait_samples = []
|
||||
open_wait_stable_values = {}
|
||||
force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
|
||||
adaptive_target_force = int(clamp(config.hold_force, config.force_min, config.force_max))
|
||||
release_armed = False
|
||||
release_watch = self.release_watch
|
||||
release_watch.reset()
|
||||
recent_feedback = deque(maxlen=6)
|
||||
recent_pair_stamps = None
|
||||
hold_pos = None
|
||||
reference = None
|
||||
current_position = None
|
||||
last_position_status_time = 0.0
|
||||
position_perf = {
|
||||
"last_log": time.perf_counter(),
|
||||
"count": 0,
|
||||
"total": 0.0,
|
||||
"max": 0.0,
|
||||
}
|
||||
|
||||
def update_current_position(now, force=False):
|
||||
nonlocal current_position, last_position_status_time, position_perf
|
||||
if force or now - last_position_status_time >= 0.1:
|
||||
last_position_status_time = now
|
||||
t0 = time.perf_counter()
|
||||
current_position = self.gripper.read_position()
|
||||
dt = time.perf_counter() - t0
|
||||
position_perf["count"] += 1
|
||||
position_perf["total"] += dt
|
||||
position_perf["max"] = max(position_perf["max"], dt)
|
||||
if now - position_perf["last_log"] >= 1.0:
|
||||
avg_ms = (
|
||||
position_perf["total"] / position_perf["count"] * 1000.0
|
||||
if position_perf["count"]
|
||||
else 0.0
|
||||
)
|
||||
print(
|
||||
"[perf] position_read "
|
||||
f"count={position_perf['count']} "
|
||||
f"avg={avg_ms:.2f}ms "
|
||||
f"max={position_perf['max'] * 1000.0:.2f}ms "
|
||||
f"pos={current_position}"
|
||||
)
|
||||
position_perf = {
|
||||
"last_log": now,
|
||||
"count": 0,
|
||||
"total": 0.0,
|
||||
"max": 0.0,
|
||||
}
|
||||
return current_position
|
||||
actuator = self.actuator
|
||||
|
||||
def reset_open_wait_stability():
|
||||
nonlocal open_wait_samples, open_wait_stable_values
|
||||
open_wait_samples = []
|
||||
open_wait_stable_values = {}
|
||||
|
||||
def current_position():
|
||||
return None if actuator is None else actuator.position
|
||||
|
||||
def refresh_hold_pos():
|
||||
nonlocal hold_pos
|
||||
if actuator is None:
|
||||
return hold_pos
|
||||
cached = actuator.hold_pos
|
||||
if cached is not None:
|
||||
hold_pos = cached
|
||||
return hold_pos
|
||||
|
||||
def reset_release_watch():
|
||||
release_watch.reset()
|
||||
|
||||
def try_release_watch(now_ts, feedback_now):
|
||||
# The release FSM returns effects; only this adapter submits I/O.
|
||||
nonlocal state, state_start, force_pct, hold_pos
|
||||
nonlocal last_open_position_check_time, last_open_position
|
||||
nonlocal last_open_position_ready, recover_quiet_since, recover_position_since
|
||||
pending, completed_at = actuator.motion_status
|
||||
decision = release_watch.step(
|
||||
now_ts, feedback_now,
|
||||
motion_pending=pending, motion_completed_at=completed_at,
|
||||
)
|
||||
self._release_decision = decision
|
||||
action = decision.action
|
||||
if decision.release:
|
||||
force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
|
||||
actuator.open(
|
||||
force_pct, "force-change-release-open",
|
||||
speed_pct=config.release_open_speed,
|
||||
accel=config.release_open_accel, decel=config.release_open_decel,
|
||||
)
|
||||
self.feedback_processor.reset_filters()
|
||||
state = "open_recover"
|
||||
state_start = now_ts
|
||||
last_open_position_check_time = 0.0
|
||||
last_open_position = None
|
||||
last_open_position_ready = False
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
reset_open_wait_stability()
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
action = f"force-change-release-open({decision.reason})"
|
||||
return decision.release, action, decision.normal_change, decision.shear_change
|
||||
|
||||
while not self._stop_event.is_set():
|
||||
tick_start = time.perf_counter()
|
||||
now = time.perf_counter()
|
||||
manual_action = None
|
||||
self._release_decision = None
|
||||
control_mode = self._get_control_mode()
|
||||
if control_mode == "manual":
|
||||
release_watch.suspend()
|
||||
|
||||
manual_payload = self._pop_manual_command()
|
||||
if isinstance(manual_payload, dict):
|
||||
@@ -235,7 +334,7 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.open(force_pct, "manual-open", speed_pct=manual_speed_pct)
|
||||
actuator.open(force_pct, "manual-open", speed_pct=manual_speed_pct)
|
||||
self.feedback_processor.reset_filters()
|
||||
state = "open_recover"
|
||||
state_start = now
|
||||
@@ -244,12 +343,10 @@ class SideTriggerGripController:
|
||||
last_open_position_ready = False
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
manual_action = "manual-open"
|
||||
elif manual_command == "close":
|
||||
force_pct = int(clamp(
|
||||
@@ -257,7 +354,7 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.close(force_pct, "manual-close", speed_pct=manual_speed_pct)
|
||||
actuator.close(force_pct, "manual-close", speed_pct=manual_speed_pct)
|
||||
state = "manual_close"
|
||||
state_start = now
|
||||
last_force_ramp_time = now
|
||||
@@ -266,12 +363,10 @@ class SideTriggerGripController:
|
||||
last_open_position_ready = False
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
manual_action = "manual-close"
|
||||
elif manual_command == "hold":
|
||||
force_pct = int(clamp(
|
||||
@@ -279,30 +374,24 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.set_force(force_pct)
|
||||
hold_pos = self.gripper.hold_current_position(
|
||||
force_pct,
|
||||
speed_pct=manual_speed_pct,
|
||||
)
|
||||
actuator.set_force(force_pct)
|
||||
actuator.hold(force_pct, speed_pct=manual_speed_pct)
|
||||
hold_pos = refresh_hold_pos() or current_position()
|
||||
state = "manual_hold"
|
||||
state_start = now
|
||||
last_force_ramp_time = now
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
reference = None
|
||||
reset_release_watch()
|
||||
manual_action = "manual-hold"
|
||||
elif manual_command == "release":
|
||||
if state == "manual_hold":
|
||||
state = "open_wait"
|
||||
state_start = now
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
manual_action = "manual-release"
|
||||
elif manual_command == "move_position":
|
||||
target_min = min(int(config.open_pos), int(config.close_pos))
|
||||
@@ -317,7 +406,7 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.move(
|
||||
actuator.move(
|
||||
target_position,
|
||||
force_pct,
|
||||
"manual-move-position",
|
||||
@@ -331,36 +420,37 @@ class SideTriggerGripController:
|
||||
last_open_position_ready = False
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
current_position = target_position
|
||||
manual_action = "manual-move-position"
|
||||
elif manual_command == "restart_left_sensor":
|
||||
if self.reader.restart_sensor(0):
|
||||
release_watch.suspend()
|
||||
reset_open_wait_stability()
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
manual_action = "left-sensor-restart"
|
||||
else:
|
||||
manual_action = "left-sensor-restart-failed"
|
||||
elif manual_command == "restart_right_sensor":
|
||||
if self.reader.restart_sensor(1):
|
||||
release_watch.suspend()
|
||||
reset_open_wait_stability()
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
manual_action = "right-sensor-restart"
|
||||
else:
|
||||
manual_action = "right-sensor-restart-failed"
|
||||
|
||||
feedback = self.feedback_processor.read(self.reader, timeout=0.3)
|
||||
# Non-blocking sample read: never stall the FSM on sensor wait.
|
||||
feedback = self.feedback_processor.read(self.reader, timeout=0.0)
|
||||
if feedback is None:
|
||||
release_watch.suspend()
|
||||
recent_feedback.clear()
|
||||
recent_pair_stamps = None
|
||||
action = manual_action or "waiting-for-sensor-samples"
|
||||
position_for_status = update_current_position(now)
|
||||
print(f"state={state} action={action}")
|
||||
position_for_status = current_position()
|
||||
self._log_sparse(f"state={state} action={action}", now)
|
||||
self._emit_status(
|
||||
{
|
||||
"timestamp": time.time(),
|
||||
@@ -369,7 +459,9 @@ class SideTriggerGripController:
|
||||
"force_pct": force_pct,
|
||||
"adaptive_target_force": adaptive_target_force,
|
||||
"adaptive_grip_enabled": config.adaptive_grip_enabled,
|
||||
"release_armed": release_armed,
|
||||
"release_armed": release_watch.armed,
|
||||
"release_phase": release_watch.phase.value,
|
||||
"release_reason": "sample-gap",
|
||||
"manual_hold_active": state == "manual_hold",
|
||||
"control_mode": control_mode,
|
||||
"speed_pct": config.speed,
|
||||
@@ -380,9 +472,19 @@ class SideTriggerGripController:
|
||||
"shear_unit": self.shear_unit,
|
||||
}
|
||||
)
|
||||
time.sleep(0.05)
|
||||
sleep_time = interval - (time.perf_counter() - tick_start)
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
continue
|
||||
|
||||
pair_stamps = (feedback.left_timestamp, feedback.right_timestamp)
|
||||
if (recent_pair_stamps is None or None in pair_stamps
|
||||
or all(a != b for a, b in zip(pair_stamps, recent_pair_stamps))):
|
||||
if recent_feedback and now - recent_feedback[-1][0] > config.release_sample_timeout_seconds:
|
||||
recent_feedback.clear()
|
||||
recent_feedback.append((now, feedback))
|
||||
recent_pair_stamps = pair_stamps
|
||||
|
||||
trigger = self._side_triggered(feedback)
|
||||
grip_contact = self._grip_contact(feedback)
|
||||
normal_change = 0.0
|
||||
@@ -397,14 +499,17 @@ class SideTriggerGripController:
|
||||
open_wait_sample = self._open_wait_sample(feedback)
|
||||
self._append_open_wait_sample(open_wait_samples, open_wait_sample)
|
||||
stable_values = self._open_wait_stable_values(open_wait_samples)
|
||||
trigger = self._open_wait_delta_triggered(
|
||||
delta_trigger = self._open_wait_delta_triggered(
|
||||
open_wait_sample,
|
||||
open_wait_stable_values,
|
||||
)
|
||||
if not trigger:
|
||||
open_wait_stable_values.update(stable_values)
|
||||
# Lock each channel's first stable reference. Continuously
|
||||
# replacing it makes the reference follow a slow touch.
|
||||
for key, value in stable_values.items():
|
||||
open_wait_stable_values.setdefault(key, value)
|
||||
|
||||
trigger_armed = bool(open_wait_stable_values)
|
||||
trigger = trigger_armed and delta_trigger
|
||||
if not trigger_armed:
|
||||
action = "open-wait-stabilizing"
|
||||
elif not trigger:
|
||||
@@ -415,7 +520,7 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.close(force_pct, "side-trigger-close")
|
||||
actuator.close(force_pct, "side-trigger-close")
|
||||
state = "closing"
|
||||
state_start = now
|
||||
last_force_ramp_time = now
|
||||
@@ -424,13 +529,11 @@ class SideTriggerGripController:
|
||||
last_open_position_ready = False
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = False
|
||||
reset_open_wait_stability()
|
||||
adaptive_target_force = self._adaptive_force_target(feedback)
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "side-trigger-close"
|
||||
|
||||
elif state == "closing":
|
||||
@@ -440,14 +543,14 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.set_force(force_pct)
|
||||
hold_pos = self.gripper.hold_current_position(force_pct)
|
||||
actuator.set_force(force_pct)
|
||||
actuator.hold(force_pct)
|
||||
hold_pos = refresh_hold_pos() or current_position()
|
||||
state = "gripping"
|
||||
state_start = now
|
||||
last_force_ramp_time = now
|
||||
adaptive_target_force = self._adaptive_force_target(feedback)
|
||||
release_armed = False
|
||||
reference = None
|
||||
release_watch.contact(now, feedback, history=recent_feedback)
|
||||
action = "object-detected-low-force"
|
||||
elif now - state_start >= config.close_timeout_seconds:
|
||||
force_pct = int(clamp(
|
||||
@@ -455,7 +558,7 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.open(force_pct, "close-timeout-open")
|
||||
actuator.open(force_pct, "close-timeout-open")
|
||||
self.feedback_processor.reset_filters()
|
||||
state = "open_recover"
|
||||
state_start = now
|
||||
@@ -465,24 +568,31 @@ class SideTriggerGripController:
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "close-timeout-open"
|
||||
else:
|
||||
if (
|
||||
config.closing_command_interval > 0
|
||||
and now - last_force_ramp_time >= config.closing_command_interval
|
||||
):
|
||||
self.gripper.close(force_pct, "closing-continue")
|
||||
actuator.close(force_pct, "closing-continue")
|
||||
last_force_ramp_time = now
|
||||
action = "closing-continue"
|
||||
else:
|
||||
action = "closing-to-grip"
|
||||
|
||||
elif state == "gripping":
|
||||
if (
|
||||
now - last_force_ramp_time >= config.force_ramp_interval
|
||||
refresh_hold_pos()
|
||||
released, release_action, normal_change, shear_change = try_release_watch(
|
||||
now,
|
||||
feedback,
|
||||
)
|
||||
if released:
|
||||
action = release_action
|
||||
elif (
|
||||
self._release_decision.allow_force_update
|
||||
and now - last_force_ramp_time >= config.force_ramp_interval
|
||||
and force_pct < config.hold_force
|
||||
):
|
||||
force_pct = int(clamp(
|
||||
@@ -490,32 +600,37 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
config.hold_force,
|
||||
))
|
||||
self.gripper.set_force(force_pct)
|
||||
actuator.set_force(force_pct)
|
||||
last_force_ramp_time = now
|
||||
action = "grip-ramp-up"
|
||||
action = f"{release_action}; grip-ramp-up"
|
||||
elif (
|
||||
force_pct >= config.hold_force
|
||||
and now - state_start >= config.grip_settle_seconds
|
||||
):
|
||||
reference = self._make_reference(feedback)
|
||||
# Keep release watch continuous; do not re-lock on enter.
|
||||
state = "hold_check"
|
||||
state_start = now
|
||||
adaptive_target_force = self._adaptive_force_target(feedback)
|
||||
release_armed = False
|
||||
action = "hold-reference-armed"
|
||||
action = release_action or "enter-hold-check"
|
||||
else:
|
||||
action = "grip-hold"
|
||||
action = release_action or "grip-hold"
|
||||
|
||||
elif state == "hold_check":
|
||||
refresh_hold_pos()
|
||||
adaptive_target_force = self._adaptive_force_target(feedback)
|
||||
should_adapt_force = (
|
||||
config.adaptive_grip_enabled
|
||||
and not release_armed
|
||||
and force_pct < adaptive_target_force
|
||||
)
|
||||
|
||||
# Always inspect external force first, even while the target
|
||||
# asks for more clamp force. Confirmation takes priority.
|
||||
released, release_action, normal_change, shear_change = try_release_watch(now, feedback)
|
||||
action = release_action
|
||||
if (
|
||||
should_adapt_force
|
||||
not released
|
||||
and self._release_decision.allow_force_update
|
||||
and should_adapt_force
|
||||
and now - last_force_ramp_time >= config.adaptive_force_interval
|
||||
):
|
||||
force_pct = int(clamp(
|
||||
@@ -523,59 +638,9 @@ class SideTriggerGripController:
|
||||
config.force_min,
|
||||
adaptive_target_force,
|
||||
))
|
||||
self.gripper.set_force(force_pct)
|
||||
actuator.set_force(force_pct)
|
||||
last_force_ramp_time = now
|
||||
state_start = now
|
||||
reference = self._make_reference(feedback)
|
||||
release_armed = False
|
||||
action = "adaptive-grip-ramp"
|
||||
elif should_adapt_force:
|
||||
release_armed = False
|
||||
action = "adaptive-grip-wait"
|
||||
elif (
|
||||
not release_armed
|
||||
and now - max(state_start, last_force_ramp_time)
|
||||
< config.release_arm_delay_seconds
|
||||
):
|
||||
action = "release-arm-delay"
|
||||
elif not release_armed:
|
||||
reference = self._make_reference(feedback)
|
||||
release_armed = True
|
||||
action = "release-reference-armed"
|
||||
else:
|
||||
normal_change, shear_change = self._release_changes(feedback, reference)
|
||||
should_release = (
|
||||
normal_change >= config.release_normal_change
|
||||
or shear_change >= config.release_shear_change
|
||||
)
|
||||
if should_release:
|
||||
force_pct = int(clamp(
|
||||
config.open_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.open(
|
||||
force_pct,
|
||||
"force-change-release-open",
|
||||
speed_pct=config.release_open_speed,
|
||||
accel=config.release_open_accel,
|
||||
decel=config.release_open_decel,
|
||||
)
|
||||
self.feedback_processor.reset_filters()
|
||||
state = "open_recover"
|
||||
state_start = now
|
||||
last_open_position_check_time = 0.0
|
||||
last_open_position = None
|
||||
last_open_position_ready = False
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
reset_open_wait_stability()
|
||||
release_armed = False
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "force-change-release-open"
|
||||
else:
|
||||
action = "hold-check"
|
||||
action = f"{release_action}; adaptive-grip-ramp"
|
||||
|
||||
elif state == "open_recover":
|
||||
current_pos = last_open_position
|
||||
@@ -586,7 +651,8 @@ class SideTriggerGripController:
|
||||
>= config.open_position_check_interval
|
||||
):
|
||||
last_open_position_check_time = now
|
||||
current_pos = self.gripper.read_position()
|
||||
# Position is polled by actuator thread; FSM only reads cache.
|
||||
current_pos = current_position()
|
||||
position_ready = self._open_position_ready(current_pos)
|
||||
last_open_position = current_pos
|
||||
last_open_position_ready = position_ready
|
||||
@@ -606,13 +672,10 @@ class SideTriggerGripController:
|
||||
state_start = now
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = True
|
||||
reset_open_wait_stability()
|
||||
last_trigger = trigger
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = f"open-ready-position pos={current_pos}"
|
||||
else:
|
||||
action = f"open-recover-position-stabilizing pos={current_pos}"
|
||||
@@ -629,13 +692,10 @@ class SideTriggerGripController:
|
||||
state_start = now
|
||||
recover_quiet_since = None
|
||||
recover_position_since = None
|
||||
trigger_clear_since = None
|
||||
trigger_armed = True
|
||||
reset_open_wait_stability()
|
||||
last_trigger = trigger
|
||||
release_armed = False
|
||||
reset_release_watch()
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "open-ready"
|
||||
else:
|
||||
action = "open-recover-stabilizing"
|
||||
@@ -645,6 +705,7 @@ class SideTriggerGripController:
|
||||
action = "open-recover-wait-force-clear"
|
||||
|
||||
elif state == "manual_hold":
|
||||
refresh_hold_pos()
|
||||
action = "manual-hold"
|
||||
|
||||
elif state == "manual_close":
|
||||
@@ -653,6 +714,23 @@ class SideTriggerGripController:
|
||||
elif state == "manual_position":
|
||||
action = "manual-position"
|
||||
|
||||
self._record_force_trace(now, feedback, state, force_pct, action)
|
||||
self._record_session_tick(
|
||||
now=now,
|
||||
feedback=feedback,
|
||||
state=state,
|
||||
force_pct=force_pct,
|
||||
adaptive_target_force=adaptive_target_force,
|
||||
action=action,
|
||||
trigger=trigger,
|
||||
trigger_armed=trigger_armed,
|
||||
grip_contact=grip_contact,
|
||||
normal_change=normal_change,
|
||||
shear_change=shear_change,
|
||||
control_mode=control_mode,
|
||||
gripper_position=current_position(),
|
||||
hold_pos=hold_pos,
|
||||
)
|
||||
self._log_tick(
|
||||
feedback=feedback,
|
||||
state=state,
|
||||
@@ -663,11 +741,12 @@ class SideTriggerGripController:
|
||||
hold_pos=hold_pos,
|
||||
force_pct=force_pct,
|
||||
adaptive_target_force=adaptive_target_force,
|
||||
release_armed=release_armed,
|
||||
release_armed=release_watch.armed,
|
||||
action=action,
|
||||
trigger_armed=trigger_armed,
|
||||
now=now,
|
||||
)
|
||||
position_for_status = update_current_position(now)
|
||||
position_for_status = current_position()
|
||||
self._emit_status(
|
||||
self._make_status(
|
||||
feedback=feedback,
|
||||
@@ -679,14 +758,13 @@ class SideTriggerGripController:
|
||||
hold_pos=hold_pos,
|
||||
force_pct=force_pct,
|
||||
adaptive_target_force=adaptive_target_force,
|
||||
release_armed=release_armed,
|
||||
release_armed=release_watch.armed,
|
||||
action=action,
|
||||
trigger_armed=trigger_armed,
|
||||
control_mode=control_mode,
|
||||
gripper_position=position_for_status,
|
||||
)
|
||||
)
|
||||
last_trigger = trigger
|
||||
|
||||
sleep_time = interval - (time.perf_counter() - tick_start)
|
||||
if sleep_time > 0:
|
||||
@@ -796,29 +874,118 @@ class SideTriggerGripController:
|
||||
target = round(force_min + ratio * (force_max - force_min))
|
||||
return int(clamp(target, force_min, force_max))
|
||||
|
||||
def _make_reference(self, feedback):
|
||||
return {
|
||||
"left_normal": feedback.left_normal,
|
||||
"right_normal": feedback.right_normal,
|
||||
"left_shear": feedback.left_shear,
|
||||
"right_shear": feedback.right_shear,
|
||||
"max_shear": feedback.max_shear,
|
||||
}
|
||||
def _log_sparse(self, message, now=None):
|
||||
now = time.perf_counter() if now is None else now
|
||||
if now - self._last_log_time < self._log_interval_s:
|
||||
return
|
||||
self._last_log_time = now
|
||||
print(message)
|
||||
|
||||
def _release_changes(self, feedback, reference):
|
||||
if reference is None:
|
||||
return 0.0, 0.0
|
||||
normal_change = max(
|
||||
abs(feedback.left_normal - reference["left_normal"]),
|
||||
abs(feedback.right_normal - reference["right_normal"]),
|
||||
)
|
||||
shear_change = abs(feedback.max_shear - reference["max_shear"])
|
||||
shear_change = max(
|
||||
shear_change,
|
||||
abs(feedback.left_shear - reference["left_shear"]),
|
||||
abs(feedback.right_shear - reference["right_shear"]),
|
||||
)
|
||||
return normal_change, shear_change
|
||||
def _record_force_trace(self, now, feedback, state, force_pct, action):
|
||||
"""Keep the actual pre-release curve, including cached-frame timestamps.
|
||||
|
||||
File I/O happens only after a release command has already been submitted.
|
||||
The bounded snapshot replaces the previous event, even in source runs.
|
||||
"""
|
||||
decision = self._release_decision
|
||||
self._force_trace.append({
|
||||
"time": now, "state": state, "force_pct": force_pct, "action": action,
|
||||
"feedback": asdict(feedback),
|
||||
"release": None if decision is None else asdict(decision),
|
||||
})
|
||||
if decision is None or not decision.release:
|
||||
return
|
||||
path = self.release_trace_path
|
||||
try:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
payload = {
|
||||
"captured_at": time.time(),
|
||||
"config": {key: value for key, value in vars(self.config).items()
|
||||
if key.startswith("release_") or key in {
|
||||
"control_hz", "filter_alpha", "shear_filter_alpha"}},
|
||||
"frames": list(self._force_trace),
|
||||
}
|
||||
temporary = path.with_suffix(".tmp")
|
||||
temporary.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
temporary.replace(path)
|
||||
print(f"[release-trace] {path}")
|
||||
except (OSError, TypeError, ValueError) as exc:
|
||||
print(f"[release-trace] save failed: {exc}")
|
||||
|
||||
def _record_session_tick(
|
||||
self,
|
||||
now,
|
||||
feedback,
|
||||
state,
|
||||
force_pct,
|
||||
adaptive_target_force,
|
||||
action,
|
||||
trigger,
|
||||
trigger_armed,
|
||||
grip_contact,
|
||||
normal_change,
|
||||
shear_change,
|
||||
control_mode,
|
||||
gripper_position,
|
||||
hold_pos,
|
||||
):
|
||||
if not self.session_recorder.enabled:
|
||||
return
|
||||
decision = self._release_decision
|
||||
pending = completed_at = None
|
||||
if self.actuator is not None:
|
||||
try:
|
||||
pending, completed_at = self.actuator.motion_status
|
||||
except Exception:
|
||||
pending, completed_at = None, None
|
||||
|
||||
left_sample = right_sample = None
|
||||
try:
|
||||
if len(self.reader.latest) >= 2:
|
||||
left_sample = self.reader.latest[0]
|
||||
right_sample = self.reader.latest[1]
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _raw_force(sample):
|
||||
if not isinstance(sample, dict):
|
||||
return None
|
||||
return {
|
||||
"fnormal": sample.get("fnormal"),
|
||||
"fshearx": sample.get("fshearx"),
|
||||
"fsheary": sample.get("fsheary"),
|
||||
"fps": sample.get("fps"),
|
||||
"timestamp": sample.get("timestamp"),
|
||||
}
|
||||
|
||||
self.session_recorder.record({
|
||||
"t": now,
|
||||
"wall": time.time(),
|
||||
"state": state,
|
||||
"action": action,
|
||||
"control_mode": control_mode,
|
||||
"grip_behavior_mode": getattr(self.config, "grip_behavior_mode", None),
|
||||
"force_pct": force_pct,
|
||||
"adaptive_target_force": adaptive_target_force,
|
||||
"hold_pos": hold_pos,
|
||||
"gripper_position": gripper_position,
|
||||
"trigger": bool(trigger),
|
||||
"trigger_armed": bool(trigger_armed),
|
||||
"grip_contact": bool(grip_contact),
|
||||
"normal_change": normal_change,
|
||||
"shear_change": shear_change,
|
||||
"feedback": asdict(feedback),
|
||||
"raw_left": _raw_force(left_sample),
|
||||
"raw_right": _raw_force(right_sample),
|
||||
"release": None if decision is None else asdict(decision),
|
||||
"release_phase": (
|
||||
None if decision is None else getattr(decision.phase, "value", decision.phase)
|
||||
),
|
||||
"release_armed": bool(self.release_watch.armed),
|
||||
"side_watch": bool(self.release_watch.side_watch_active),
|
||||
"motion_pending": pending,
|
||||
"motion_completed_at": completed_at,
|
||||
})
|
||||
|
||||
def _log_tick(
|
||||
self,
|
||||
@@ -834,7 +1001,21 @@ class SideTriggerGripController:
|
||||
release_armed,
|
||||
action,
|
||||
trigger_armed,
|
||||
now=None,
|
||||
):
|
||||
now = time.perf_counter() if now is None else now
|
||||
decision = self._release_decision
|
||||
release_reason = decision.reason if decision is not None else ""
|
||||
log_key = (state, release_reason, action.split(" ", 1)[0])
|
||||
if (release_reason == "waiting-sample" and self._last_log_key is not None
|
||||
and self._last_log_key[0] == state):
|
||||
# Camera/control rates differ: cached ticks are not state changes.
|
||||
log_key = self._last_log_key
|
||||
if log_key == self._last_log_key and now - self._last_log_time < self._log_interval_s:
|
||||
return
|
||||
self._last_log_key = log_key
|
||||
self._last_log_time = now
|
||||
|
||||
left_fps = 0.0
|
||||
right_fps = 0.0
|
||||
try:
|
||||
@@ -858,6 +1039,7 @@ class SideTriggerGripController:
|
||||
f"state={state} hold_pos={hold_pos} "
|
||||
f"force_pct={force_pct:3d} target={adaptive_target_force:3d} "
|
||||
f"release_armed={int(release_armed)} action={action}"
|
||||
f" side_watch={int(self.release_watch.side_watch_active)}"
|
||||
)
|
||||
|
||||
def _make_status(
|
||||
@@ -878,7 +1060,7 @@ class SideTriggerGripController:
|
||||
gripper_position=None,
|
||||
):
|
||||
speed_pct = self.config.speed
|
||||
if action == "force-change-release-open":
|
||||
if action.startswith("force-change-release-open"):
|
||||
speed_pct = self.config.release_open_speed
|
||||
|
||||
left_sample = None
|
||||
@@ -917,8 +1099,16 @@ class SideTriggerGripController:
|
||||
"adaptive_target_force": adaptive_target_force,
|
||||
"adaptive_grip_enabled": self.config.adaptive_grip_enabled,
|
||||
"release_armed": bool(release_armed),
|
||||
"release_phase": self._release_decision.phase.value if self._release_decision else self.release_watch.phase.value,
|
||||
"release_side_watch": self.release_watch.side_watch_active,
|
||||
"release_reason": self._release_decision.reason if self._release_decision else "",
|
||||
"release_normal_rate": self._release_decision.normal_rate if self._release_decision else 0.0,
|
||||
"release_shear_rate": self._release_decision.shear_rate if self._release_decision else 0.0,
|
||||
"release_curve_motion": self._release_decision.curve_motion if self._release_decision else "",
|
||||
"release_channel": self._release_decision.channel if self._release_decision else "",
|
||||
"manual_hold_active": state == "manual_hold",
|
||||
"control_mode": control_mode,
|
||||
"grip_behavior_mode": getattr(self.config, "grip_behavior_mode", "stable"),
|
||||
"speed_pct": speed_pct,
|
||||
"gripper_position": gripper_position,
|
||||
"open_pos": self.config.open_pos,
|
||||
|
||||
@@ -39,6 +39,8 @@ class ForceFeedback:
|
||||
left_shear_y: float = 0.0
|
||||
right_shear_x: float = 0.0
|
||||
right_shear_y: float = 0.0
|
||||
left_timestamp: float | None = None
|
||||
right_timestamp: float | None = None
|
||||
|
||||
|
||||
class TactileFeedbackProcessor:
|
||||
@@ -50,6 +52,7 @@ class TactileFeedbackProcessor:
|
||||
self.reset_filters()
|
||||
|
||||
def reset_filters(self):
|
||||
self._sample_timestamps = {}
|
||||
self.left_normal_filter = ExponentialFilter(self.config.filter_alpha)
|
||||
self.right_normal_filter = ExponentialFilter(self.config.filter_alpha)
|
||||
self.left_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
|
||||
@@ -59,6 +62,14 @@ class TactileFeedbackProcessor:
|
||||
self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha)
|
||||
self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha)
|
||||
|
||||
def _filter_sample(self, key, value, timestamp):
|
||||
"""A cached camera frame must not advance the filter a second time."""
|
||||
filter_ = getattr(self, f"{key}_filter")
|
||||
if timestamp is None or self._sample_timestamps.get(key) != timestamp:
|
||||
self._sample_timestamps[key] = timestamp
|
||||
return filter_.update(value)
|
||||
return filter_.value
|
||||
|
||||
def _convert_shear(self, sample):
|
||||
if _component_shear_enabled(self.shear_converter):
|
||||
shear_x = self.shear_converter.convert_component(sample["fshearx"])
|
||||
@@ -142,14 +153,15 @@ class TactileFeedbackProcessor:
|
||||
self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear,
|
||||
)
|
||||
|
||||
left_normal = self.left_normal_filter.update(left_force)
|
||||
right_normal = self.right_normal_filter.update(right_force)
|
||||
left_shear_filtered = self.left_shear_filter.update(left_shear)
|
||||
right_shear_filtered = self.right_shear_filter.update(right_shear)
|
||||
left_shear_x_filtered = self.left_shear_x_filter.update(left_shear_x)
|
||||
left_shear_y_filtered = self.left_shear_y_filter.update(left_shear_y)
|
||||
right_shear_x_filtered = self.right_shear_x_filter.update(right_shear_x)
|
||||
right_shear_y_filtered = self.right_shear_y_filter.update(right_shear_y)
|
||||
left_timestamp, right_timestamp = left.get("timestamp"), right.get("timestamp")
|
||||
left_normal = self._filter_sample("left_normal", left_force, left_timestamp)
|
||||
right_normal = self._filter_sample("right_normal", right_force, right_timestamp)
|
||||
left_shear_filtered = self._filter_sample("left_shear", left_shear, left_timestamp)
|
||||
right_shear_filtered = self._filter_sample("right_shear", right_shear, right_timestamp)
|
||||
left_shear_x_filtered = self._filter_sample("left_shear_x", left_shear_x, left_timestamp)
|
||||
left_shear_y_filtered = self._filter_sample("left_shear_y", left_shear_y, left_timestamp)
|
||||
right_shear_x_filtered = self._filter_sample("right_shear_x", right_shear_x, right_timestamp)
|
||||
right_shear_y_filtered = self._filter_sample("right_shear_y", right_shear_y, right_timestamp)
|
||||
|
||||
return ForceFeedback(
|
||||
left_normal=left_normal,
|
||||
@@ -165,4 +177,6 @@ class TactileFeedbackProcessor:
|
||||
left_shear_y=left_shear_y_filtered,
|
||||
right_shear_x=right_shear_x_filtered,
|
||||
right_shear_y=right_shear_y_filtered,
|
||||
left_timestamp=left_timestamp,
|
||||
right_timestamp=right_timestamp,
|
||||
)
|
||||
|
||||
@@ -0,0 +1,211 @@
|
||||
"""Recognize a new force onset, rather than accumulated settling movement."""
|
||||
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
from statistics import median
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _CurveModel:
|
||||
time: float
|
||||
scale: float
|
||||
coefficients: tuple
|
||||
covariance: tuple
|
||||
noise: float
|
||||
|
||||
def terms_at(self, stamp):
|
||||
x = (stamp - self.time) / self.scale
|
||||
return tuple(x ** i for i in range(len(self.coefficients)))
|
||||
|
||||
def predict(self, stamp):
|
||||
return sum(c * x for c, x in zip(self.coefficients, self.terms_at(stamp)))
|
||||
|
||||
def uncertainty(self, stamp):
|
||||
terms = self.terms_at(stamp)
|
||||
variance = sum(a * b * self.covariance[i][j]
|
||||
for i, a in enumerate(terms) for j, b in enumerate(terms))
|
||||
# Include the new sample's noise, and widen the corridor with forecast
|
||||
# distance. A tiny fitting error must not grow into release evidence.
|
||||
return 4.0 * self.noise * math.sqrt(1.0 + max(0.0, variance))
|
||||
|
||||
def change_uncertainty(self, before, after):
|
||||
terms = tuple(b - a for a, b in zip(self.terms_at(before), self.terms_at(after)))
|
||||
variance = sum(a * b * self.covariance[i][j]
|
||||
for i, a in enumerate(terms) for j, b in enumerate(terms))
|
||||
return 4.0 * self.noise * math.sqrt(2.0 + max(0.0, variance))
|
||||
|
||||
@property
|
||||
def slope(self):
|
||||
"""Physical dy/dt from the linear term of the scaled polynomial."""
|
||||
if len(self.coefficients) < 2 or self.scale <= 0:
|
||||
return 0.0
|
||||
return self.coefficients[1] / self.scale
|
||||
|
||||
|
||||
def _fit_curve(history, degree, noise_floor):
|
||||
"""Small least-squares fit with its prediction uncertainty (no I/O)."""
|
||||
stamp = history[-1][0]
|
||||
scale = max(stamp - history[0][0], 1e-6)
|
||||
rows = [tuple(((t - stamp) / scale) ** i for i in range(degree + 1))
|
||||
for t, _ in history]
|
||||
size = degree + 1
|
||||
matrix = [[sum(row[i] * row[j] for row in rows) for j in range(size)]
|
||||
+ [float(i == j) for j in range(size)] for i in range(size)]
|
||||
# Invert the at-most 3x3 normal matrix. Sensor times have already been
|
||||
# checked for duplicates/restarts before samples enter the history.
|
||||
for column in range(size):
|
||||
pivot = max(range(column, size), key=lambda row: abs(matrix[row][column]))
|
||||
matrix[column], matrix[pivot] = matrix[pivot], matrix[column]
|
||||
divisor = matrix[column][column]
|
||||
if abs(divisor) < 1e-12:
|
||||
return _fit_curve(history, degree - 1, noise_floor)
|
||||
matrix[column] = [value / divisor for value in matrix[column]]
|
||||
for row in range(size):
|
||||
if row != column:
|
||||
multiplier = matrix[row][column]
|
||||
matrix[row] = [a - multiplier * b for a, b in zip(matrix[row], matrix[column])]
|
||||
covariance = tuple(tuple(row[size:]) for row in matrix)
|
||||
target = [sum(row[i] * value for row, (_, value) in zip(rows, history)) for i in range(size)]
|
||||
coefficients = tuple(sum(a * b for a, b in zip(row, target)) for row in covariance)
|
||||
error = sum((value - sum(c * x for c, x in zip(coefficients, row))) ** 2
|
||||
for row, (_, value) in zip(rows, history))
|
||||
noise = max(noise_floor, math.sqrt(error / max(1, len(history) - size)))
|
||||
return _CurveModel(stamp, scale, coefficients, covariance, noise)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ForceOnset:
|
||||
time: float
|
||||
origin: float
|
||||
direction: float
|
||||
innovation: float
|
||||
model: _CurveModel
|
||||
|
||||
def baseline_at(self, stamp):
|
||||
return self.model.predict(stamp)
|
||||
|
||||
def significant_at(self, stamp, value):
|
||||
return abs(value - self.baseline_at(stamp)) > self.model.uncertainty(stamp)
|
||||
|
||||
|
||||
class ForceCurve:
|
||||
"""Local polynomial continuation plus signed slope/turn classification.
|
||||
|
||||
A short recent window still drives onset detection. A longer retained history
|
||||
estimates the settling slope (grip force ramp decaying toward zero).
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
history = max(6, int(getattr(config, "release_curve_history", 6) or 6))
|
||||
self.history = deque(maxlen=history)
|
||||
self._noise_history = deque(maxlen=6)
|
||||
self.onset = None
|
||||
self.rate = 0.0
|
||||
self.delta = 0.0
|
||||
self.innovation = 0.0
|
||||
self.motion = "curve_follow"
|
||||
self.fit_slope = 0.0
|
||||
|
||||
def discard_onset(self):
|
||||
self.onset = None
|
||||
|
||||
def _onset_history(self):
|
||||
"""Keep onset detection local even when the retained history is long."""
|
||||
onset_points = max(3, min(6, len(self.history)))
|
||||
if len(self.history) <= onset_points:
|
||||
return list(self.history)
|
||||
return list(self.history)[-onset_points:]
|
||||
|
||||
def window_slope(self, now=None, window_s=None):
|
||||
"""Linear least-squares slope over a longer settling window (N/s)."""
|
||||
if len(self.history) < 2:
|
||||
return 0.0
|
||||
window_s = float(
|
||||
getattr(self.config, "release_slope_window_seconds", 0.8)
|
||||
if window_s is None
|
||||
else window_s
|
||||
)
|
||||
now = self.history[-1][0] if now is None else now
|
||||
cutoff = now - max(0.05, window_s)
|
||||
points = [(t, v) for t, v in self.history if t >= cutoff]
|
||||
if len(points) < 2:
|
||||
points = list(self.history)
|
||||
floor = min(self.config.release_min_change, self.config.release_side_touch_change)
|
||||
model = _fit_curve(points, min(1, len(points) - 1), max(1e-6, floor * 0.025))
|
||||
return model.slope
|
||||
|
||||
def observe(self, stamp, value):
|
||||
if self.history:
|
||||
if stamp == self.history[-1][0]:
|
||||
return
|
||||
if stamp < self.history[-1][0]:
|
||||
self.history.clear()
|
||||
self._noise_history.clear()
|
||||
self.onset = None
|
||||
self.rate = self.delta = self.innovation = 0.0
|
||||
self.fit_slope = 0.0
|
||||
self.motion = "curve_follow"
|
||||
if not self.history:
|
||||
self.history.append((stamp, value))
|
||||
return
|
||||
previous_time, previous_value = self.history[-1]
|
||||
dt = stamp - previous_time
|
||||
self.delta = value - previous_value
|
||||
self.rate = self.delta / dt
|
||||
previous_rate = 0.0
|
||||
prior_rates = []
|
||||
if len(self.history) >= 2:
|
||||
older_time, older_value = self.history[-2]
|
||||
previous_rate = (previous_value - older_value) / (previous_time - older_time)
|
||||
prior_rates.append(previous_rate)
|
||||
if len(self.history) >= 3:
|
||||
oldest_time, oldest_value = self.history[-3]
|
||||
older_rate = (older_value - oldest_value) / (older_time - oldest_time)
|
||||
prior_rates.append(older_rate)
|
||||
floor = min(self.config.release_min_change, self.config.release_side_touch_change)
|
||||
noise_floor = floor * 0.025
|
||||
onset_history = self._onset_history()
|
||||
model = _fit_curve(onset_history, min(2, len(onset_history) - 1), noise_floor)
|
||||
self._noise_history.append(model.noise)
|
||||
model = replace(model, noise=max(model.noise, median(self._noise_history)))
|
||||
self.innovation = value - model.predict(stamp)
|
||||
# Append first so the settling slope includes the new sample.
|
||||
self.history.append((stamp, value))
|
||||
self.fit_slope = self.window_slope(stamp)
|
||||
# A sign reversal of comparable speed is a turn, not another impulse.
|
||||
turning = (previous_rate * self.rate < 0
|
||||
and abs(self.rate) <= 1.5 * max((abs(r) for r in prior_rates), default=0.0))
|
||||
decelerating = (previous_rate * self.rate >= 0
|
||||
and abs(self.rate) <= abs(previous_rate) + 1e-9)
|
||||
onset = self.onset
|
||||
if onset is not None:
|
||||
if self.delta * onset.direction < -floor * 0.1:
|
||||
self.onset = None
|
||||
turning = True
|
||||
elif not onset.significant_at(stamp, value):
|
||||
# Losing evidence before reaching the amplitude threshold is
|
||||
# not permission to accumulate later, unrelated curve travel.
|
||||
self.onset = None
|
||||
elif ((value - onset.origin) * onset.direction < floor
|
||||
and self.delta * onset.direction < floor * 0.1):
|
||||
# A sub-threshold onset which has already flattened is over.
|
||||
# Do not let the forecast alone grow a small bump into a later
|
||||
# candidate while the real signal resumes its natural curve.
|
||||
self.onset = None
|
||||
elif stamp - onset.time > self.config.release_touch_window_seconds:
|
||||
self.onset = None
|
||||
if turning:
|
||||
self.motion = "settling_turn"
|
||||
elif decelerating and abs(previous_rate) > 1e-9:
|
||||
self.motion = "settling_flatten"
|
||||
|
||||
if (self.onset is None and not turning and not decelerating
|
||||
and abs(self.innovation) + 1e-9 >= floor * 0.2
|
||||
and abs(self.innovation) > model.uncertainty(stamp)
|
||||
and self.innovation * self.delta > 0):
|
||||
self.onset = ForceOnset(stamp, previous_value,
|
||||
1.0 if self.innovation > 0 else -1.0, self.innovation,
|
||||
model)
|
||||
self.motion = "new_onset"
|
||||
@@ -0,0 +1,598 @@
|
||||
"""Release FSM. No clock, serial I/O or callbacks: step() returns a decision."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
import math
|
||||
|
||||
from .force_curve import ForceCurve, ForceOnset
|
||||
|
||||
|
||||
CHANNELS = ("left_normal", "right_normal", "left_shear", "right_shear")
|
||||
|
||||
|
||||
class ReleasePhase(str, Enum):
|
||||
DISARMED = "disarmed"
|
||||
GRIP_SETTLE = "grip_settle"
|
||||
SELF_SETTLE = "self_settle"
|
||||
WATCHING = "watching"
|
||||
CONFIRMING = "confirming"
|
||||
RELEASED = "released"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ReleaseDecision:
|
||||
phase: ReleasePhase
|
||||
reason: str
|
||||
release: bool = False
|
||||
allow_force_update: bool = False
|
||||
normal_change: float = 0.0
|
||||
shear_change: float = 0.0
|
||||
normal_rate: float = 0.0
|
||||
shear_rate: float = 0.0
|
||||
confirm_count: int = 0
|
||||
confirm_needed: int = 0
|
||||
channel: str = ""
|
||||
innovation: float = 0.0
|
||||
curve_motion: str = ""
|
||||
|
||||
@property
|
||||
def action(self):
|
||||
return (
|
||||
f"release-{self.reason} phase={self.phase.value} "
|
||||
f"confirm={self.confirm_count}/{self.confirm_needed} "
|
||||
f"rN={self.normal_rate:.2f} rS={self.shear_rate:.2f}"
|
||||
f" channel={self.channel or '-'} innovation={self.innovation:.4f} curve={self.curve_motion or '-'}"
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Candidate:
|
||||
kind: str
|
||||
channel: str
|
||||
origin: float
|
||||
direction: float
|
||||
peak: float
|
||||
started_at: float
|
||||
needed: int
|
||||
floor: float
|
||||
count: int = 1
|
||||
onset: ForceOnset | None = None
|
||||
last_displacement: float = 0.0
|
||||
last_sample_time: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Rebound:
|
||||
origin: float
|
||||
direction: float
|
||||
remaining: float
|
||||
|
||||
|
||||
def _returning_tail(tails, key, value, floor):
|
||||
"""Suppress only a rejected pulse returning towards its original baseline.
|
||||
|
||||
This is channel-local and ends on return or renewed movement. It does not
|
||||
wait for unrelated channels to become quiet or impose a new hold timer.
|
||||
"""
|
||||
tail = tails.get(key)
|
||||
if tail is None:
|
||||
return False
|
||||
remaining = (value - tail.origin) * tail.direction
|
||||
if abs(remaining) <= floor * 0.5:
|
||||
del tails[key]
|
||||
return True
|
||||
if remaining < 0 or remaining > tail.remaining + floor * 0.25:
|
||||
del tails[key]
|
||||
return False
|
||||
tail.remaining = min(tail.remaining, remaining)
|
||||
return True
|
||||
|
||||
|
||||
class _SideTouch:
|
||||
"""Observe a new lateral touch even during clamp protection.
|
||||
|
||||
Bilateral common-mode force is cancelled. A vote needs new frames from
|
||||
both sides so asynchronous camera updates cannot fabricate a difference.
|
||||
Normal force stays masked during an actual force command and its tail;
|
||||
differential shear remains available without a contact timer.
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.last_pair = None
|
||||
self.seen = set()
|
||||
self.candidate = None
|
||||
self.recover_until = 0.0
|
||||
self.tails = {}
|
||||
self.curves = {index: ForceCurve(self.config) for index in (1, 2)}
|
||||
|
||||
def seed(self, now, values):
|
||||
self.last_pair = (now, values["left_normal"] - values["right_normal"],
|
||||
values["left_shear"] - values["right_shear"])
|
||||
for index in (1, 2):
|
||||
if not self.curves[index].history:
|
||||
self.curves[index].observe(now, self.last_pair[index])
|
||||
|
||||
def prime(self, now, values):
|
||||
for index, field in ((1, "normal"), (2, "shear")):
|
||||
self.curves[index].observe(now, values[f"left_{field}"] - values[f"right_{field}"])
|
||||
self.curves[index].discard_onset()
|
||||
|
||||
def step(self, now, values, fresh, enabled, normal_masked, sample_time,
|
||||
onset_allowed=True):
|
||||
self.seen.update(fresh)
|
||||
if not all(key in self.seen for key in CHANNELS):
|
||||
return None
|
||||
self.seen.clear()
|
||||
sample = (sample_time, values["left_normal"] - values["right_normal"],
|
||||
values["left_shear"] - values["right_shear"])
|
||||
for index in (1, 2):
|
||||
self.curves[index].observe(sample_time, sample[index])
|
||||
if normal_masked:
|
||||
self.curves[1].discard_onset()
|
||||
if not enabled or now < self.recover_until:
|
||||
self.seed(sample_time, values)
|
||||
self.candidate = None
|
||||
for curve in self.curves.values():
|
||||
curve.discard_onset()
|
||||
return None
|
||||
self.last_pair = sample
|
||||
# Stable grip: keep learning the long ramp curve, but do not arm side
|
||||
# touch while the settling slope is still decaying toward zero.
|
||||
if not onset_allowed:
|
||||
self.candidate = None
|
||||
for curve in self.curves.values():
|
||||
curve.discard_onset()
|
||||
return None
|
||||
# Drop leftover candidates that the profile has disabled.
|
||||
if self.candidate is not None:
|
||||
if (self.candidate.channel == "side_shear"
|
||||
and not getattr(self.config, "release_side_shear_enabled", True)):
|
||||
self.candidate = None
|
||||
self.curves[2].discard_onset()
|
||||
elif (self.candidate.channel == "side_normal"
|
||||
and not getattr(self.config, "release_side_normal_enabled", True)):
|
||||
self.candidate = None
|
||||
self.curves[1].discard_onset()
|
||||
returning = {index for index in (1, 2) if _returning_tail(
|
||||
self.tails, index, sample[index], self.config.release_side_touch_change)}
|
||||
if returning:
|
||||
for index in returning:
|
||||
self.curves[index].discard_onset()
|
||||
candidate = self.candidate
|
||||
if candidate is not None:
|
||||
index = 1 if candidate.channel == "side_normal" else 2
|
||||
baseline = candidate.onset.baseline_at(sample_time)
|
||||
delta = (sample[index] - baseline) * candidate.direction
|
||||
return_margin = max(self.config.release_min_change * 0.1,
|
||||
candidate.onset.model.change_uncertainty(candidate.last_sample_time, sample_time))
|
||||
turning = delta < candidate.last_displacement - return_margin
|
||||
if (now - candidate.started_at > self.config.release_confirm_window_seconds
|
||||
or delta + 1e-9 < max(candidate.floor, 0.6 * candidate.peak)
|
||||
or not candidate.onset.significant_at(sample_time, sample[index])
|
||||
or (normal_masked and index == 1) or turning):
|
||||
self.candidate = None
|
||||
self.tails[index] = _Rebound(baseline, candidate.direction, delta)
|
||||
self.recover_until = now + self.config.release_settle_seconds
|
||||
self.seed(sample_time, values)
|
||||
self.curves[index].discard_onset()
|
||||
return "settling-turn" if turning else "rejected"
|
||||
candidate.peak = max(candidate.peak, delta)
|
||||
candidate.last_displacement = delta
|
||||
candidate.last_sample_time = sample_time
|
||||
candidate.count += 1
|
||||
else:
|
||||
shear_ok = getattr(self.config, "release_side_shear_enabled", True)
|
||||
normal_ok = getattr(self.config, "release_side_normal_enabled", True)
|
||||
channels = []
|
||||
if shear_ok:
|
||||
channels.append((2, "side_shear"))
|
||||
if normal_ok:
|
||||
channels.append((1, "side_normal"))
|
||||
for index, channel in channels:
|
||||
if index in returning or (normal_masked and index == 1):
|
||||
continue
|
||||
onset = self.curves[index].onset
|
||||
if onset is None:
|
||||
continue
|
||||
delta = sample[index] - onset.baseline_at(sample_time)
|
||||
if (abs(delta) + 1e-9 >= self.config.release_side_touch_change
|
||||
and (sample[index] - onset.origin) * onset.direction + 1e-9 >= self.config.release_side_touch_change
|
||||
and onset.significant_at(sample_time, sample[index])
|
||||
and delta * onset.direction > 0):
|
||||
candidate = _Candidate("side_touch", channel, onset.origin,
|
||||
1.0 if delta > 0 else -1.0, abs(delta), now,
|
||||
max(2, self.config.release_confirm_samples),
|
||||
self.config.release_side_touch_change,
|
||||
onset=onset, last_displacement=abs(delta), last_sample_time=sample_time)
|
||||
self.candidate = candidate
|
||||
break
|
||||
return candidate
|
||||
|
||||
|
||||
class ReleaseWatch:
|
||||
"""Immediate side touch + bounded clamp protection + persistent displacement.
|
||||
|
||||
A force command masks normal changes through execution and a short tail.
|
||||
Lateral shear remains observable; global quiet never gates release.
|
||||
New force commands require an observation window, so repeated adaptive
|
||||
commands cannot continuously re-arm a timer or erase release evidence.
|
||||
Sensor timestamps are optional for replay; production supplies both sides.
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self._side_touch = _SideTouch(config)
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.phase = ReleasePhase.DISARMED
|
||||
self._contact_at = None
|
||||
self._grip_settled = False
|
||||
self._slope_settle_since = None
|
||||
self._plateau_latched = False
|
||||
self._reference = {}
|
||||
self._previous = {}
|
||||
self._recovery_until = 0.0
|
||||
self._settle_seen = set()
|
||||
self._tails = {}
|
||||
self._watch_since = None
|
||||
self._candidate = None
|
||||
self._last_fresh = {}
|
||||
self._rates = {}
|
||||
self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
|
||||
self._side_touch.reset()
|
||||
|
||||
@property
|
||||
def armed(self):
|
||||
return (self.phase in {ReleasePhase.WATCHING, ReleasePhase.CONFIRMING}
|
||||
or self._side_touch.candidate is not None)
|
||||
|
||||
@property
|
||||
def side_watch_active(self):
|
||||
return (self.phase not in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}
|
||||
and self._side_touch.last_pair is not None and len(self._previous) == len(CHANNELS))
|
||||
|
||||
def contact(self, now, feedback=None, history=()):
|
||||
self.reset()
|
||||
self._contact_at = now
|
||||
self.phase = ReleasePhase.GRIP_SETTLE
|
||||
for frame_time, frame in history:
|
||||
values = {key: float(getattr(frame, key)) for key in CHANNELS}
|
||||
for key, value in values.items():
|
||||
stamp = getattr(frame, f"{key.split('_', 1)[0]}_timestamp", None)
|
||||
self._curves[key].observe(frame_time if stamp is None else stamp, value)
|
||||
self._curves[key].discard_onset()
|
||||
self._side_touch.prime(self._pair_time(frame_time, frame), values)
|
||||
if feedback is not None:
|
||||
self._side_touch.seed(self._pair_time(now, feedback), {key: float(getattr(feedback, key)) for key in CHANNELS})
|
||||
for key in CHANNELS:
|
||||
stamp = getattr(feedback, f"{key.split('_', 1)[0]}_timestamp", None)
|
||||
self._curves[key].observe(now if stamp is None else stamp, float(getattr(feedback, key)))
|
||||
self._curves[key].discard_onset()
|
||||
|
||||
@staticmethod
|
||||
def _pair_time(now, feedback):
|
||||
stamps = (getattr(feedback, "left_timestamp", None), getattr(feedback, "right_timestamp", None))
|
||||
return now if None in stamps else max(stamps)
|
||||
|
||||
def suspend(self):
|
||||
"""Discard evidence during missing feedback, sensor restart or manual mode."""
|
||||
if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
|
||||
return
|
||||
self._protect("sample-gap", {})
|
||||
self._side_touch.reset()
|
||||
self._reference.clear()
|
||||
self._previous.clear()
|
||||
self._rates.clear()
|
||||
self._last_fresh.clear()
|
||||
self._tails.clear()
|
||||
self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
|
||||
|
||||
def _normal_slopes_live_ok(self, now):
|
||||
"""Live check: normal long-window slopes are near zero long enough."""
|
||||
if not getattr(self.config, "release_require_slope_settle", False):
|
||||
return True
|
||||
threshold = max(0.0, float(getattr(self.config, "release_slope_settle_rate", 0.2) or 0.2))
|
||||
min_points = max(3, int(getattr(self.config, "release_slope_min_points", 8) or 8))
|
||||
hold = max(0.0, float(getattr(self.config, "release_slope_hold_seconds", 0.0) or 0.0))
|
||||
normal_only = getattr(self.config, "release_settle_normal_only", True)
|
||||
keys = CHANNELS[:2] if normal_only else CHANNELS
|
||||
for key in keys:
|
||||
curve = self._curves[key]
|
||||
if len(curve.history) < min_points or abs(curve.fit_slope) > threshold:
|
||||
self._slope_settle_since = None
|
||||
return False
|
||||
if self._slope_settle_since is None:
|
||||
self._slope_settle_since = now
|
||||
if hold > 0 and now - self._slope_settle_since < hold:
|
||||
return False
|
||||
return True
|
||||
|
||||
def _force_settled(self, now):
|
||||
"""Release-armed after the first relative normal plateau (latched).
|
||||
|
||||
The live slope near zero is only required to *enter* the armed state.
|
||||
Once latched, further adaptive 加力 or an intentional open push may make
|
||||
the slope non-zero again — that must not revoke release permission, or
|
||||
the open gesture is mistaken for "still settling".
|
||||
"""
|
||||
if not getattr(self.config, "release_require_slope_settle", False):
|
||||
self._plateau_latched = True
|
||||
return True
|
||||
if self._plateau_latched:
|
||||
return True
|
||||
if self._normal_slopes_live_ok(now):
|
||||
self._plateau_latched = True
|
||||
return True
|
||||
return False
|
||||
|
||||
def _protect(self, reason, values, **metrics):
|
||||
self.phase = (
|
||||
ReleasePhase.SELF_SETTLE if self._grip_settled
|
||||
else ReleasePhase.GRIP_SETTLE
|
||||
)
|
||||
self._reference.update(values)
|
||||
self._candidate = None
|
||||
self._watch_since = None
|
||||
self._settle_seen.clear()
|
||||
for curve in self._curves.values():
|
||||
curve.discard_onset()
|
||||
return ReleaseDecision(self.phase, reason, **metrics)
|
||||
|
||||
def step(self, now, feedback, *, motion_pending=False, motion_completed_at=None):
|
||||
if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
|
||||
return ReleaseDecision(self.phase, self.phase.value)
|
||||
|
||||
cfg = self.config
|
||||
timeout = max(0.001, cfg.release_sample_timeout_seconds)
|
||||
values = {key: float(getattr(feedback, key)) for key in CHANNELS}
|
||||
if not all(math.isfinite(value) for value in values.values()):
|
||||
self._previous.clear()
|
||||
self._rates.clear()
|
||||
self._side_touch.reset()
|
||||
self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
|
||||
self._recovery_until = now + cfg.release_settle_seconds
|
||||
return self._protect("invalid-sample", {})
|
||||
|
||||
rates, fresh = {}, set()
|
||||
discontinuity = False
|
||||
for key, value in values.items():
|
||||
side = key.split("_", 1)[0]
|
||||
stamp = getattr(feedback, f"{side}_timestamp", None)
|
||||
stamp = now if stamp is None else stamp
|
||||
previous = self._previous.get(key)
|
||||
if previous is not None and stamp == previous[0]:
|
||||
continue
|
||||
fresh.add(key)
|
||||
self._last_fresh[key] = now
|
||||
if previous is not None:
|
||||
dt = stamp - previous[0]
|
||||
if 0 < dt <= timeout:
|
||||
rates[key] = (value - previous[1]) / dt
|
||||
else:
|
||||
discontinuity = True
|
||||
self._previous[key] = (stamp, value)
|
||||
self._rates.update(rates)
|
||||
|
||||
normal_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[:2]), default=0.0)
|
||||
shear_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[2:]), default=0.0)
|
||||
normal_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[:2])
|
||||
shear_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[2:])
|
||||
metrics = dict(normal_change=normal_change, shear_change=shear_change,
|
||||
normal_rate=normal_rate, shear_rate=shear_rate)
|
||||
stale = any(now - self._last_fresh.get(k, now) > timeout for k in CHANNELS)
|
||||
if stale or discontinuity:
|
||||
self._side_touch.reset()
|
||||
self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
|
||||
self._recovery_until = now + cfg.release_settle_seconds
|
||||
return self._protect("sample-gap", values, **metrics)
|
||||
|
||||
for key in fresh:
|
||||
self._curves[key].observe(self._previous[key][0], values[key])
|
||||
motions = {curve.motion for curve in self._curves.values()}
|
||||
metrics["curve_motion"] = ("new_onset" if "new_onset" in motions else
|
||||
"settling_turn" if "settling_turn" in motions else
|
||||
"settling_flatten" if "settling_flatten" in motions else "curve_follow")
|
||||
|
||||
self_tail = (
|
||||
motion_completed_at is not None
|
||||
and now - motion_completed_at < max(0.0, cfg.release_arm_delay_seconds)
|
||||
)
|
||||
contact_guard = now - self._contact_at < max(0.0, cfg.release_min_hold_seconds)
|
||||
protected = motion_pending or self_tail or contact_guard or self.phase in {
|
||||
ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE,
|
||||
}
|
||||
force_settled = self._force_settled(now)
|
||||
if not force_settled:
|
||||
metrics["curve_motion"] = "force_settling"
|
||||
side = self._side_touch.step(now, values, fresh,
|
||||
enabled=protected or self._side_touch.candidate is not None,
|
||||
normal_masked=motion_pending or self_tail,
|
||||
sample_time=self._pair_time(now, feedback),
|
||||
onset_allowed=force_settled)
|
||||
if side in ("rejected", "settling-turn"):
|
||||
reason = "settling-turn" if side == "settling-turn" else "transient-rejected"
|
||||
return self._cancel(now, values, reason, metrics, reset_side=False)
|
||||
if side is not None:
|
||||
self._candidate = None
|
||||
released = side.count >= side.needed
|
||||
if released:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
return ReleaseDecision(ReleasePhase.RELEASED if released else ReleasePhase.CONFIRMING,
|
||||
"side_touch", release=released, confirm_count=side.count,
|
||||
confirm_needed=side.needed, channel=side.channel,
|
||||
innovation=side.onset.innovation,
|
||||
**dict(metrics, curve_motion="onset_confirm"))
|
||||
|
||||
slope_gate = getattr(cfg, "release_require_slope_settle", False)
|
||||
# Contact min-hold always clamps. Self-motion clamps until the first
|
||||
# normal plateau is latched (stable) — or always when slope gate is off.
|
||||
if contact_guard or ((motion_pending or self_tail) and (not slope_gate or not force_settled)):
|
||||
reason = ("force-settling" if slope_gate and not force_settled
|
||||
else "self-settle" if self._grip_settled else "grip-settle")
|
||||
return self._protect(reason, values, **metrics)
|
||||
if slope_gate and force_settled and (motion_pending or self_tail):
|
||||
# Adaptive 加力 after plateau: keep watching for intentional open.
|
||||
if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
|
||||
self._grip_settled = True
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
if self._watch_since is None:
|
||||
self._watch_since = now
|
||||
for curve in self._curves.values():
|
||||
curve.discard_onset()
|
||||
|
||||
if not fresh:
|
||||
return ReleaseDecision(self.phase, "waiting-sample", **metrics)
|
||||
|
||||
# Quiet is diagnostic only. No global "all four rates quiet" arming
|
||||
# gate: even small continuous camera noise can keep that gate shut.
|
||||
quiet = (
|
||||
len(self._rates) == len(CHANNELS)
|
||||
and max(abs(rate) for rate in self._rates.values()) <= cfg.release_slow_rate
|
||||
)
|
||||
if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
|
||||
self._settle_seen.update(fresh)
|
||||
self._reference.update(values)
|
||||
if now < self._recovery_until or len(self._settle_seen) < len(CHANNELS):
|
||||
reason = "self-settle" if self._grip_settled else "grip-settle"
|
||||
return ReleaseDecision(self.phase, reason, **metrics)
|
||||
# Stable mode waits until the long-history slope has decayed near
|
||||
# zero before ordinary watching / release paths arm.
|
||||
if not force_settled:
|
||||
return ReleaseDecision(self.phase, "force-settling", **metrics)
|
||||
self._grip_settled = True
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
self._watch_since = now
|
||||
for curve in self._curves.values():
|
||||
curve.discard_onset()
|
||||
return ReleaseDecision(self.phase, "ready", **metrics)
|
||||
returning = {key for key in fresh if _returning_tail(
|
||||
self._tails, key, values[key], cfg.release_min_change)}
|
||||
for key in returning:
|
||||
self._reference[key] = values[key]
|
||||
self._curves[key].discard_onset()
|
||||
|
||||
# Drop watching candidates on channel families the profile disabled.
|
||||
if (self._candidate is not None
|
||||
and self._candidate.channel.endswith("normal")
|
||||
and not getattr(cfg, "release_normal_onset_enabled", True)):
|
||||
self._candidate = None
|
||||
if self._grip_settled:
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
if (self._candidate is not None
|
||||
and self._candidate.channel.endswith("shear")
|
||||
and not getattr(cfg, "release_shear_onset_enabled", True)):
|
||||
self._candidate = None
|
||||
if self._grip_settled:
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
|
||||
candidate = self._candidate
|
||||
if candidate is not None:
|
||||
if now - candidate.started_at > cfg.release_confirm_window_seconds:
|
||||
return self._cancel(now, values, "confirm-expired", metrics)
|
||||
if candidate.channel not in fresh:
|
||||
return ReleaseDecision(self.phase, "waiting-sample", **metrics)
|
||||
stamp = self._previous[candidate.channel][0]
|
||||
displacement = (values[candidate.channel] - candidate.onset.baseline_at(stamp)) * candidate.direction
|
||||
return_margin = max(cfg.release_min_change * 0.1,
|
||||
candidate.onset.model.change_uncertainty(candidate.last_sample_time, stamp))
|
||||
if displacement < candidate.last_displacement - return_margin:
|
||||
return self._cancel(now, values, "settling-turn", metrics)
|
||||
# A spike returning to baseline, reversal, or channel swap is not a
|
||||
# second vote. A step that stays displaced is, even with dF/dt=0.
|
||||
if displacement + 1e-9 < max(candidate.floor, 0.6 * candidate.peak):
|
||||
return self._cancel(now, values, "transient-rejected", metrics)
|
||||
if not candidate.onset.significant_at(stamp, values[candidate.channel]):
|
||||
return self._cancel(now, values, "transient-rejected", metrics)
|
||||
candidate.peak = max(candidate.peak, displacement)
|
||||
candidate.last_displacement = displacement
|
||||
candidate.last_sample_time = stamp
|
||||
candidate.count += 1
|
||||
else:
|
||||
choices = []
|
||||
for key in fresh:
|
||||
if key in returning:
|
||||
continue
|
||||
is_normal = key.endswith("normal")
|
||||
if is_normal and not getattr(cfg, "release_normal_onset_enabled", True):
|
||||
continue
|
||||
if (not is_normal
|
||||
and not getattr(cfg, "release_shear_onset_enabled", True)):
|
||||
continue
|
||||
onset = self._curves[key].onset
|
||||
if onset is None or not onset.significant_at(self._previous[key][0], values[key]):
|
||||
continue
|
||||
rate_limit = cfg.release_normal_rate if is_normal else cfg.release_shear_rate
|
||||
weak_limit = cfg.release_normal_change if is_normal else cfg.release_shear_change
|
||||
delta = values[key] - onset.baseline_at(self._previous[key][0])
|
||||
rate = rates.get(key, 0.0)
|
||||
kind = None
|
||||
origin = onset.origin
|
||||
measured = (values[key] - origin) * onset.direction
|
||||
if (abs(rate) >= rate_limit and abs(delta) >= cfg.release_min_change
|
||||
and measured + 1e-9 >= cfg.release_min_change and rate * delta > 0):
|
||||
kind, needed, floor = "sudden", max(2, cfg.release_confirm_samples), cfg.release_min_change
|
||||
elif (abs(delta) + 1e-9 >= cfg.release_min_change
|
||||
and measured + 1e-9 >= cfg.release_min_change and delta * onset.direction > 0):
|
||||
kind, needed, floor = "touch", max(2, cfg.release_confirm_samples), cfg.release_min_change
|
||||
elif abs(delta) >= weak_limit and measured >= weak_limit:
|
||||
kind = "slow_external"
|
||||
needed = max(2, cfg.release_confirm_samples, cfg.release_slow_confirm_samples)
|
||||
floor = weak_limit
|
||||
if kind:
|
||||
choices.append((kind == "sudden", abs(delta), key, kind, needed, floor, delta, origin, onset))
|
||||
if choices:
|
||||
_, _, key, kind, needed, floor, delta, origin, onset = max(choices)
|
||||
candidate = _Candidate(kind, key, origin,
|
||||
1.0 if delta > 0 else -1.0, abs(delta), now, needed, floor,
|
||||
onset=onset, last_displacement=abs(delta),
|
||||
last_sample_time=self._previous[key][0])
|
||||
self._candidate = candidate
|
||||
self.phase = ReleasePhase.CONFIRMING
|
||||
|
||||
if candidate is not None:
|
||||
released = candidate.count >= candidate.needed
|
||||
if released:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
return ReleaseDecision(self.phase, candidate.kind, release=released,
|
||||
confirm_count=candidate.count,
|
||||
confirm_needed=candidate.needed, channel=candidate.channel,
|
||||
innovation=candidate.onset.innovation,
|
||||
**dict(metrics, curve_motion="onset_confirm"))
|
||||
|
||||
# Follow curve continuation, including large smooth turns. Displacement
|
||||
# without a new onset must never become a weak-path release later.
|
||||
for key in fresh:
|
||||
if self._curves[key].onset is None:
|
||||
self._reference[key] = values[key]
|
||||
observation = max(0.1, max(2, cfg.release_confirm_samples) / max(1.0, cfg.control_hz))
|
||||
# Ordinary noise must not prevent clamp force from reaching its target.
|
||||
# Meaningful coherent movement reserves the next frame for release.
|
||||
touch_pending = any(
|
||||
self._curves[key].onset is not None
|
||||
and abs(values[key] - self._curves[key].onset.baseline_at(self._previous[key][0])) >= cfg.release_min_change * 0.5
|
||||
for key in fresh
|
||||
)
|
||||
allow_force = (not touch_pending and self._watch_since is not None
|
||||
and now - self._watch_since >= observation)
|
||||
reason = "settling-turn" if metrics["curve_motion"] == "settling_turn" else "slow_internal" if quiet else "watch"
|
||||
return ReleaseDecision(self.phase, reason,
|
||||
allow_force_update=allow_force, **metrics)
|
||||
|
||||
def _cancel(self, now, values, reason, metrics, *, reset_side=True):
|
||||
# Wait for the rejected pulse's tail; otherwise its falling edge can
|
||||
# become a fresh negative-direction candidate and release on rebound.
|
||||
self._recovery_until = now + self.config.release_settle_seconds
|
||||
if self._candidate is not None:
|
||||
candidate = self._candidate
|
||||
baseline = candidate.onset.baseline_at(self._previous[candidate.channel][0])
|
||||
remaining = (values[candidate.channel] - baseline) * candidate.direction
|
||||
self._tails[candidate.channel] = _Rebound(baseline, candidate.direction, remaining)
|
||||
if reset_side:
|
||||
self._side_touch.reset()
|
||||
self._side_touch.recover_until = self._recovery_until
|
||||
return self._protect(reason, values, **metrics)
|
||||
@@ -0,0 +1,124 @@
|
||||
"""Session recorder for grip/release tuning.
|
||||
|
||||
Writes one JSON object per control tick (JSONL). Enable from UI or config; keep
|
||||
I/O off the hot path when disabled.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import asdict, is_dataclass
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def _jsonable(value):
|
||||
if value is None or isinstance(value, (bool, int, float, str)):
|
||||
return value
|
||||
if is_dataclass(value):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (list, tuple)):
|
||||
return [_jsonable(item) for item in value]
|
||||
if hasattr(value, "value") and not callable(value):
|
||||
# Enum-like
|
||||
try:
|
||||
return value.value
|
||||
except Exception:
|
||||
pass
|
||||
return str(value)
|
||||
|
||||
|
||||
class SessionRecorder:
|
||||
def __init__(self, root: Path):
|
||||
self.root = Path(root)
|
||||
self._lock = threading.Lock()
|
||||
self._enabled = False
|
||||
self._path = None
|
||||
self._file = None
|
||||
self._frames = 0
|
||||
self._started_at = None
|
||||
self._meta = {}
|
||||
|
||||
@property
|
||||
def enabled(self) -> bool:
|
||||
with self._lock:
|
||||
return self._enabled
|
||||
|
||||
@property
|
||||
def path(self):
|
||||
with self._lock:
|
||||
return self._path
|
||||
|
||||
@property
|
||||
def frames(self) -> int:
|
||||
with self._lock:
|
||||
return self._frames
|
||||
|
||||
def start(self, meta=None) -> Path:
|
||||
with self._lock:
|
||||
if self._enabled:
|
||||
return self._path
|
||||
self.root.mkdir(parents=True, exist_ok=True)
|
||||
stamp = datetime.now().strftime("%Y%m%d-%H%M%S")
|
||||
path = self.root / f"session-{stamp}.jsonl"
|
||||
handle = path.open("w", encoding="utf-8")
|
||||
header = {
|
||||
"type": "session_header",
|
||||
"started_at": time.time(),
|
||||
"started_at_iso": datetime.now().isoformat(timespec="seconds"),
|
||||
"meta": _jsonable(meta or {}),
|
||||
}
|
||||
handle.write(json.dumps(header, ensure_ascii=False) + "\n")
|
||||
handle.flush()
|
||||
self._file = handle
|
||||
self._path = path
|
||||
self._enabled = True
|
||||
self._frames = 0
|
||||
self._started_at = time.perf_counter()
|
||||
self._meta = dict(meta or {})
|
||||
print(f"[session-record] start {path}")
|
||||
return path
|
||||
|
||||
def stop(self) -> Path | None:
|
||||
with self._lock:
|
||||
if not self._enabled:
|
||||
return None
|
||||
path = self._path
|
||||
frames = self._frames
|
||||
started = self._started_at
|
||||
try:
|
||||
footer = {
|
||||
"type": "session_footer",
|
||||
"stopped_at": time.time(),
|
||||
"frames": frames,
|
||||
"duration_s": (
|
||||
None if started is None else time.perf_counter() - started
|
||||
),
|
||||
}
|
||||
if self._file is not None:
|
||||
self._file.write(json.dumps(footer, ensure_ascii=False) + "\n")
|
||||
self._file.flush()
|
||||
self._file.close()
|
||||
finally:
|
||||
self._file = None
|
||||
self._enabled = False
|
||||
self._path = None
|
||||
self._frames = 0
|
||||
self._started_at = None
|
||||
print(f"[session-record] stop {path} frames={frames}")
|
||||
return path
|
||||
|
||||
def record(self, frame: dict) -> None:
|
||||
with self._lock:
|
||||
if not self._enabled or self._file is None:
|
||||
return
|
||||
payload = {"type": "tick", **_jsonable(frame)}
|
||||
self._file.write(json.dumps(payload, ensure_ascii=False) + "\n")
|
||||
self._frames += 1
|
||||
# Flush periodically so a crash still keeps most of the capture.
|
||||
if self._frames % 30 == 0:
|
||||
self._file.flush()
|
||||
@@ -32,7 +32,8 @@ class CameraInfo:
|
||||
|
||||
def label(self):
|
||||
device = self.source if sys.platform.startswith("linux") else f"相机 {self.index}"
|
||||
return f"{device} {self.width}x{self.height} [{self.backend_name}]"
|
||||
resolution = f" {self.width}x{self.height}" if self.width and self.height else ""
|
||||
return f"{device}{resolution} [{self.backend_name}]"
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
@@ -112,6 +113,15 @@ def _candidate_indices(max_index):
|
||||
return list(range(max_index + 1))
|
||||
|
||||
|
||||
def list_camera_ids(max_index=DEFAULT_MAX_CAMERA_INDEX):
|
||||
"""List selectable camera IDs without opening devices or reading frames."""
|
||||
backend_name, backend_api = _backend_candidates()[0]
|
||||
return [
|
||||
CameraInfo(index, backend_name, backend_api, 0, 0)
|
||||
for index in _candidate_indices(max_index)
|
||||
]
|
||||
|
||||
|
||||
def _try_open(index, backend_name, backend_api):
|
||||
capture = cv2.VideoCapture(index, backend_api)
|
||||
try:
|
||||
|
||||
+489
-37
@@ -98,7 +98,7 @@ else:
|
||||
|
||||
from ..app import build_controller
|
||||
from ..common.paths import PACKAGE_DIR, REPO_ROOT
|
||||
from ..sensors.camera_discovery import camera_source_index, list_cameras
|
||||
from ..sensors.camera_discovery import list_camera_ids
|
||||
|
||||
|
||||
PLOT_POINTS = 180
|
||||
@@ -208,8 +208,21 @@ ACTION_TEXT = {
|
||||
"hold-reference-armed": "记录夹持基准",
|
||||
"adaptive-grip-ramp": "自适应加力",
|
||||
"adaptive-grip-wait": "等待自适应加力",
|
||||
"release-arm-delay": "释放检测延时",
|
||||
"release-reference-armed": "释放检测就绪",
|
||||
"release-grip-settle": "法向夹稳保护，侧触可释放",
|
||||
"release-self-settle": "自运动保护，侧触可释放",
|
||||
"release-ready": "释放检测就绪",
|
||||
"release-sudden": "突变外力确认",
|
||||
"release-touch": "轻触变化确认",
|
||||
"release-side_touch": "即时侧触确认",
|
||||
"release-slow_external": "缓推外力确认",
|
||||
"release-slow_internal": "慢变跟随",
|
||||
"release-watch": "监视外力变化",
|
||||
"release-waiting-sample": "等待新传感器帧",
|
||||
"release-sample-gap": "采样断流保护",
|
||||
"release-invalid-sample": "传感器数据无效",
|
||||
"release-transient-rejected": "短脉冲已拒绝",
|
||||
"release-settling-turn": "平稳拐点，继续夹持",
|
||||
"release-confirm-expired": "外力确认已过期",
|
||||
"hold-check": "保持检测",
|
||||
"force-change-release-open": "力变化张开",
|
||||
"open-recover-position": "张开位置恢复",
|
||||
@@ -238,7 +251,7 @@ def label_with_cn(value, mapping):
|
||||
cn = mapping.get(value)
|
||||
if cn is None:
|
||||
for prefix, text in mapping.items():
|
||||
if value.startswith(f"{prefix} "):
|
||||
if value.startswith((f"{prefix} ", f"{prefix}(")):
|
||||
cn = text
|
||||
break
|
||||
if cn is None:
|
||||
@@ -2282,18 +2295,14 @@ class CameraDiscoveryDialog(QDialog):
|
||||
self._scan_id += 1
|
||||
scan_id = self._scan_id
|
||||
self._scan_result = None
|
||||
self.status_label.setText("正在快速搜索摄像头（编号 0-3）...")
|
||||
self.status_label.setText("正在读取摄像头 ID...")
|
||||
current_sources = self.viewer.camera_sources()
|
||||
skip_indices = set()
|
||||
for side, combo in self.camera_combos.items():
|
||||
combo.clear()
|
||||
current = current_sources.get(side, "")
|
||||
if current:
|
||||
combo.addItem(f"当前配置 {current}（可能正在使用）", current)
|
||||
combo.setEnabled(True)
|
||||
current_index = camera_source_index(current)
|
||||
if current_index is not None:
|
||||
skip_indices.add(current_index)
|
||||
else:
|
||||
combo.setEnabled(False)
|
||||
self.refresh_button.setEnabled(False)
|
||||
@@ -2305,7 +2314,7 @@ class CameraDiscoveryDialog(QDialog):
|
||||
try:
|
||||
self._scan_result = (
|
||||
scan_id,
|
||||
list_cameras(skip_indices=skip_indices),
|
||||
list_camera_ids(),
|
||||
None,
|
||||
)
|
||||
except Exception as exc:
|
||||
@@ -2339,7 +2348,7 @@ class CameraDiscoveryDialog(QDialog):
|
||||
if error:
|
||||
self.status_label.setText(f"搜索失败：{error}")
|
||||
elif cameras:
|
||||
self.status_label.setText(f"找到 {len(cameras)} 个可用摄像头")
|
||||
self.status_label.setText(f"已列出 {len(cameras)} 个摄像头 ID（未占用设备）")
|
||||
else:
|
||||
self.status_label.setText("未发现空闲摄像头；正在使用的当前配置仍可保留")
|
||||
has_both = all(combo.count() > 0 for combo in self.camera_combos.values())
|
||||
@@ -2358,6 +2367,12 @@ class CameraDiscoveryDialog(QDialog):
|
||||
|
||||
class VisualSettingsDialog(QDialog):
|
||||
CONFIG_GROUPS = [
|
||||
(
|
||||
"行为模式",
|
||||
[
|
||||
"grip_behavior_mode",
|
||||
],
|
||||
),
|
||||
(
|
||||
"相机 / 传感器",
|
||||
[
|
||||
@@ -2440,8 +2455,31 @@ class VisualSettingsDialog(QDialog):
|
||||
"adaptive_force_step",
|
||||
"adaptive_force_interval",
|
||||
"release_arm_delay_seconds",
|
||||
"release_min_hold_seconds",
|
||||
"release_settle_seconds",
|
||||
"release_min_change",
|
||||
"release_touch_window_seconds",
|
||||
"release_side_touch_change",
|
||||
"release_confirm_window_seconds",
|
||||
"release_sample_timeout_seconds",
|
||||
"release_confirm_samples",
|
||||
"release_slow_confirm_samples",
|
||||
"release_normal_rate",
|
||||
"release_shear_rate",
|
||||
"release_slow_rate",
|
||||
"release_normal_change",
|
||||
"release_shear_change",
|
||||
"release_curve_history",
|
||||
"release_slope_window_seconds",
|
||||
"release_slope_settle_rate",
|
||||
"release_slope_min_points",
|
||||
"release_require_slope_settle",
|
||||
"release_slope_hold_seconds",
|
||||
"release_settle_normal_only",
|
||||
"release_side_shear_enabled",
|
||||
"release_side_normal_enabled",
|
||||
"release_shear_onset_enabled",
|
||||
"release_normal_onset_enabled",
|
||||
],
|
||||
),
|
||||
(
|
||||
@@ -2470,7 +2508,7 @@ class VisualSettingsDialog(QDialog):
|
||||
"motion_threshold": "运动阈值",
|
||||
"sensor_cpu": "传感器 CPU 模式",
|
||||
"port": "串口号",
|
||||
"slave_id": "从站 ID",
|
||||
"slave_id": "夹爪从站 ID",
|
||||
"baudrate": "波特率",
|
||||
"serial_timeout": "串口超时",
|
||||
"open_pos": "张开位置",
|
||||
@@ -2497,7 +2535,7 @@ class VisualSettingsDialog(QDialog):
|
||||
"trigger_force_update": "触发力更新",
|
||||
"trigger_normal_force": "法向触发阈值",
|
||||
"trigger_shear_force": "切向触发阈值",
|
||||
"trigger_clear_stable_seconds": "触发清除稳定时间",
|
||||
"trigger_clear_stable_seconds": "旧触发清除时间（当前未使用）",
|
||||
"open_wait_stable_samples": "等待稳定样本数",
|
||||
"open_wait_stable_range_n": "等待稳定范围",
|
||||
"open_wait_stable_trend_n": "等待稳定趋势",
|
||||
@@ -2508,6 +2546,7 @@ class VisualSettingsDialog(QDialog):
|
||||
"force_ramp_step": "加力步进",
|
||||
"force_ramp_interval": "加力间隔",
|
||||
"grip_settle_seconds": "夹持稳定时间",
|
||||
"grip_behavior_mode": "夹持行为模式",
|
||||
"adaptive_grip_enabled": "启用自适应夹持",
|
||||
"adaptive_shear_start_force": "自适应起始切向力",
|
||||
"adaptive_shear_full_force": "自适应满量程切向力",
|
||||
@@ -2515,9 +2554,32 @@ class VisualSettingsDialog(QDialog):
|
||||
"adaptive_force_max": "自适应最大力",
|
||||
"adaptive_force_step": "自适应加力步进",
|
||||
"adaptive_force_interval": "自适应加力间隔",
|
||||
"release_arm_delay_seconds": "释放检测延时",
|
||||
"release_normal_change": "释放法向变化",
|
||||
"release_shear_change": "释放切向变化",
|
||||
"release_arm_delay_seconds": "自运动执行后保护时间 (s)",
|
||||
"release_min_hold_seconds": "接触法向保护 (s，侧触不等待)",
|
||||
"release_settle_seconds": "脉冲/断流恢复保护上限 (s)",
|
||||
"release_min_change": "突变/轻触释放最小力变化",
|
||||
"release_touch_window_seconds": "新外力起始证据有效期 (s，非等待时间)",
|
||||
"release_side_touch_change": "即时侧触左右力差变化",
|
||||
"release_confirm_window_seconds": "释放确认有效窗口 (s)",
|
||||
"release_sample_timeout_seconds": "释放采样断流保护 (s)",
|
||||
"release_confirm_samples": "突变释放确认帧数",
|
||||
"release_slow_confirm_samples": "缓推外力确认新帧数",
|
||||
"release_normal_rate": "释放法向突变速率",
|
||||
"release_shear_rate": "释放切向突变速率",
|
||||
"release_slow_rate": "慢变日志分类速率 (N/s)",
|
||||
"release_normal_change": "慢外力法向阈值",
|
||||
"release_shear_change": "慢外力切向阈值",
|
||||
"release_curve_history": "力曲线历史点数（稳定模式宜更长）",
|
||||
"release_slope_window_seconds": "夹稳斜率估计窗口 (s)",
|
||||
"release_slope_settle_rate": "夹稳斜率门限 (N/s，趋近 0)",
|
||||
"release_slope_min_points": "判定夹稳前最少历史点数",
|
||||
"release_require_slope_settle": "释放前要求法向斜率趋零（夹持稳定）",
|
||||
"release_slope_hold_seconds": "斜率趋零后还需保持多久才算夹稳 (s)",
|
||||
"release_settle_normal_only": "夹稳判定仅看法向（忽略切向）",
|
||||
"release_side_shear_enabled": "允许切向左右差侧触张开",
|
||||
"release_side_normal_enabled": "允许法向左右差侧触张开",
|
||||
"release_shear_onset_enabled": "允许切向通道触发普通释放",
|
||||
"release_normal_onset_enabled": "允许法向通道触发普通释放",
|
||||
"rearm_seconds": "重新武装休息时间",
|
||||
"recover_normal_force": "恢复法向阈值",
|
||||
"recover_shear_force": "恢复切向阈值",
|
||||
@@ -2571,6 +2633,8 @@ class VisualSettingsDialog(QDialog):
|
||||
self.config_data = {}
|
||||
except Exception:
|
||||
self.config_data = {}
|
||||
if "grip_behavior_mode" not in self.config_data:
|
||||
self.config_data["grip_behavior_mode"] = "stable"
|
||||
|
||||
def _build_ui(self):
|
||||
outer = QVBoxLayout(self)
|
||||
@@ -2748,6 +2812,29 @@ class VisualSettingsDialog(QDialog):
|
||||
return scroll
|
||||
|
||||
def _make_control(self, key, value):
|
||||
if key == "grip_behavior_mode":
|
||||
from gripper.config.behavior_profiles import (
|
||||
GRIP_BEHAVIOR_LABELS,
|
||||
GRIP_BEHAVIOR_MODES,
|
||||
normalize_grip_behavior_mode,
|
||||
)
|
||||
|
||||
control = QComboBox()
|
||||
try:
|
||||
current = normalize_grip_behavior_mode(value)
|
||||
except Exception:
|
||||
current = "stable"
|
||||
for mode in GRIP_BEHAVIOR_MODES:
|
||||
control.addItem(f"{GRIP_BEHAVIOR_LABELS[mode]} ({mode})", mode)
|
||||
index = control.findData(current)
|
||||
control.setCurrentIndex(max(0, index))
|
||||
control.setToolTip(
|
||||
"迅速开合：极端跟手释放/再武装；夹持稳定：极端防误开、优先夹得住。"
|
||||
"切换后会立刻填入对应预设参数。"
|
||||
)
|
||||
control.currentIndexChanged.connect(self._on_behavior_mode_changed)
|
||||
self.controls[key] = control
|
||||
return control
|
||||
if key == "port":
|
||||
container = QWidget()
|
||||
layout = QHBoxLayout(container)
|
||||
@@ -2763,7 +2850,11 @@ class VisualSettingsDialog(QDialog):
|
||||
layout.addWidget(search_button)
|
||||
self.controls[key] = control
|
||||
return container
|
||||
if isinstance(value, bool):
|
||||
if key == "slave_id":
|
||||
control = QSpinBox()
|
||||
control.setRange(1, 247)
|
||||
control.setValue(max(1, min(247, int(value))))
|
||||
elif isinstance(value, bool):
|
||||
control = QCheckBox()
|
||||
control.setChecked(value)
|
||||
elif isinstance(value, (dict, list)):
|
||||
@@ -2785,6 +2876,8 @@ class VisualSettingsDialog(QDialog):
|
||||
if control.currentIndex() >= 0 and selected is not None:
|
||||
return str(selected)
|
||||
return control.currentText().strip()
|
||||
if isinstance(control, QSpinBox):
|
||||
return int(control.value())
|
||||
if isinstance(original, bool):
|
||||
return bool(control.isChecked())
|
||||
if isinstance(original, (dict, list)):
|
||||
@@ -2809,6 +2902,8 @@ class VisualSettingsDialog(QDialog):
|
||||
return
|
||||
if isinstance(control, QCheckBox):
|
||||
control.setChecked(bool(value))
|
||||
elif isinstance(control, QSpinBox):
|
||||
control.setValue(int(value))
|
||||
elif isinstance(control, QComboBox):
|
||||
text = "" if value is None else str(value)
|
||||
index = control.findData(text)
|
||||
@@ -2821,6 +2916,26 @@ class VisualSettingsDialog(QDialog):
|
||||
elif isinstance(control, QLineEdit):
|
||||
control.setText("null" if value is None else str(value))
|
||||
|
||||
def _on_behavior_mode_changed(self, _index=None):
|
||||
control = self.controls.get("grip_behavior_mode")
|
||||
if not isinstance(control, QComboBox):
|
||||
return
|
||||
mode = control.currentData()
|
||||
if not mode:
|
||||
return
|
||||
from gripper.config.behavior_profiles import grip_behavior_profile
|
||||
|
||||
try:
|
||||
profile = grip_behavior_profile(mode)
|
||||
except Exception as exc:
|
||||
QMessageBox.warning(self, "模式无效", str(exc))
|
||||
return
|
||||
for key, value in profile.items():
|
||||
if key in self.controls:
|
||||
self._set_control_value(key, value)
|
||||
self.config_data[key] = value
|
||||
self.config_data["grip_behavior_mode"] = mode
|
||||
|
||||
def _search_serial_ports(self):
|
||||
control = self.controls.get("port")
|
||||
if not isinstance(control, QComboBox):
|
||||
@@ -2830,7 +2945,15 @@ class VisualSettingsDialog(QDialog):
|
||||
QMessageBox.warning(self, "搜索失败", "PySerial 串口枚举模块未安装。")
|
||||
return
|
||||
try:
|
||||
ports = list(serial_list_ports.comports())
|
||||
ports = sorted(
|
||||
serial_list_ports.comports(),
|
||||
key=lambda port: (
|
||||
0
|
||||
if "USB" in f"{getattr(port, 'description', '')} {getattr(port, 'hwid', '')}".upper()
|
||||
else 1,
|
||||
str(port.device),
|
||||
),
|
||||
)
|
||||
except Exception as exc:
|
||||
QMessageBox.warning(self, "搜索失败", f"无法枚举串口:\n{exc}")
|
||||
return
|
||||
@@ -2845,13 +2968,15 @@ class VisualSettingsDialog(QDialog):
|
||||
selected_index = control.count() - 1
|
||||
if current and selected_index < 0:
|
||||
control.addItem(f"{current} [当前配置]", current)
|
||||
selected_index = control.count() - 1
|
||||
if selected_index < 0 and ports:
|
||||
selected_index = 0
|
||||
if selected_index >= 0:
|
||||
control.setCurrentIndex(selected_index)
|
||||
if not ports:
|
||||
QMessageBox.information(self, "搜索串口", "未发现可用串口，已保留当前配置。")
|
||||
return
|
||||
control.showPopup()
|
||||
control.setToolTip(f"找到 {len(ports)} 个串口")
|
||||
QTimer.singleShot(50, control.showPopup)
|
||||
|
||||
def _reload(self):
|
||||
self._load_config()
|
||||
@@ -2884,10 +3009,19 @@ class VisualSettingsDialog(QDialog):
|
||||
return
|
||||
|
||||
self.config_data = updated
|
||||
self.viewer.sync_main_device_controls(
|
||||
updated.get("port", ""),
|
||||
updated.get("slave_id", 1),
|
||||
)
|
||||
applied_live = self.viewer.apply_runtime_config(updated)
|
||||
if applied_live:
|
||||
tip = "配置已写入，并已对当前运行中的控制即时生效。"
|
||||
else:
|
||||
tip = "配置已写入。若控制未运行，启动后将使用新参数。"
|
||||
QMessageBox.information(
|
||||
self,
|
||||
"已保存",
|
||||
f"配置已写入 gripper.json，画面旋转已写入:\n{layout_path}\n重启控制程序后完整生效。",
|
||||
f"{tip}\n画面旋转已写入:\n{layout_path}",
|
||||
)
|
||||
|
||||
def _adjust(self, side, action):
|
||||
@@ -3148,11 +3282,13 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
dialog.show()
|
||||
dialog.raise_()
|
||||
|
||||
def _update_camera_argv(self, config_key, source):
|
||||
def _update_config_argv(self, config_key, source):
|
||||
args = list(self.argv or [])
|
||||
option_names = {
|
||||
"video1": ("--video1", "-v1"),
|
||||
"video2": ("--video2", "-v2"),
|
||||
"port": ("--port",),
|
||||
"slave_id": ("--slave-id",),
|
||||
}[config_key]
|
||||
for index, arg in enumerate(args):
|
||||
if arg in option_names and index + 1 < len(args):
|
||||
@@ -3196,8 +3332,8 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
QMessageBox.warning(self, "保存失败", f"无法保存摄像头配置:\n{exc}")
|
||||
return False
|
||||
|
||||
self._update_camera_argv("video1", left_source)
|
||||
self._update_camera_argv("video2", right_source)
|
||||
self._update_config_argv("video1", left_source)
|
||||
self._update_config_argv("video2", right_source)
|
||||
if self.visual_settings_dialog is not None:
|
||||
for config_key, source in (("video1", left_source), ("video2", right_source)):
|
||||
self.visual_settings_dialog.config_data[config_key] = source
|
||||
@@ -3210,6 +3346,120 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
self.restart_control()
|
||||
return True
|
||||
|
||||
def _load_serial_device_config(self):
|
||||
port = ""
|
||||
slave_id = 1
|
||||
try:
|
||||
with open(self._control_config_path(), "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
port = str(data.get("port", port))
|
||||
slave_id = int(data.get("slave_id", slave_id))
|
||||
except Exception:
|
||||
pass
|
||||
return port, max(1, min(247, slave_id))
|
||||
|
||||
def sync_main_device_controls(self, port, slave_id):
|
||||
if not hasattr(self, "main_port_combo"):
|
||||
return
|
||||
port = str(port)
|
||||
index = self.main_port_combo.findData(port)
|
||||
if index < 0:
|
||||
self.main_port_combo.addItem(port, port)
|
||||
index = self.main_port_combo.count() - 1
|
||||
self.main_port_combo.setCurrentIndex(index)
|
||||
self.main_slave_id_spin.setValue(max(1, min(247, int(slave_id))))
|
||||
|
||||
@staticmethod
|
||||
def _combo_source_value(combo):
|
||||
selected = combo.currentData()
|
||||
if combo.currentIndex() >= 0 and selected is not None:
|
||||
return str(selected)
|
||||
return combo.currentText().strip()
|
||||
|
||||
def search_main_serial_ports(self):
|
||||
current = self._combo_source_value(self.main_port_combo)
|
||||
if serial_list_ports is None:
|
||||
QMessageBox.warning(self, "搜索失败", "PySerial 串口枚举模块未安装。")
|
||||
return
|
||||
try:
|
||||
ports = sorted(
|
||||
serial_list_ports.comports(),
|
||||
key=lambda port: (
|
||||
0
|
||||
if "USB" in f"{getattr(port, 'description', '')} {getattr(port, 'hwid', '')}".upper()
|
||||
else 1,
|
||||
str(port.device),
|
||||
),
|
||||
)
|
||||
except Exception as exc:
|
||||
QMessageBox.warning(self, "搜索失败", f"无法枚举串口:\n{exc}")
|
||||
return
|
||||
|
||||
self.main_port_combo.clear()
|
||||
selected_index = -1
|
||||
for port in ports:
|
||||
device = str(port.device)
|
||||
description = str(port.description or "未知设备")
|
||||
self.main_port_combo.addItem(f"{device} [{description}]", device)
|
||||
if device == current:
|
||||
selected_index = self.main_port_combo.count() - 1
|
||||
if current and selected_index < 0:
|
||||
self.main_port_combo.addItem(f"{current} [当前配置]", current)
|
||||
if selected_index < 0 and ports:
|
||||
selected_index = 0
|
||||
if selected_index >= 0:
|
||||
self.main_port_combo.setCurrentIndex(selected_index)
|
||||
if not ports:
|
||||
QMessageBox.information(self, "搜索串口", "未发现可用串口，已保留当前配置。")
|
||||
return
|
||||
self.main_port_combo.setToolTip(f"找到 {len(ports)} 个串口")
|
||||
QTimer.singleShot(50, self.main_port_combo.showPopup)
|
||||
|
||||
def apply_main_device_config(self):
|
||||
port = self._combo_source_value(self.main_port_combo)
|
||||
slave_id = int(self.main_slave_id_spin.value())
|
||||
if not port:
|
||||
QMessageBox.warning(self, "配置无效", "请选择或输入夹爪串口。")
|
||||
return False
|
||||
|
||||
config_path = self._control_config_path()
|
||||
temp_path = config_path.with_suffix(config_path.suffix + ".tmp")
|
||||
try:
|
||||
with open(config_path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("配置文件根节点不是 JSON 对象")
|
||||
if str(data.get("port", "")) == port and int(data.get("slave_id", 1)) == slave_id:
|
||||
return True
|
||||
data["port"] = port
|
||||
data["slave_id"] = slave_id
|
||||
with open(temp_path, "w", encoding="utf-8") as f:
|
||||
json.dump(data, f, ensure_ascii=False, indent=2)
|
||||
f.write("\n")
|
||||
os.replace(temp_path, config_path)
|
||||
except Exception as exc:
|
||||
try:
|
||||
if temp_path.exists():
|
||||
temp_path.unlink()
|
||||
except OSError:
|
||||
pass
|
||||
QMessageBox.warning(self, "保存失败", f"无法保存夹爪设备配置:\n{exc}")
|
||||
return False
|
||||
|
||||
self._update_config_argv("port", port)
|
||||
self._update_config_argv("slave_id", str(slave_id))
|
||||
if self.visual_settings_dialog is not None:
|
||||
for config_key, value in (("port", port), ("slave_id", slave_id)):
|
||||
self.visual_settings_dialog.config_data[config_key] = value
|
||||
self.visual_settings_dialog._set_control_value(config_key, value)
|
||||
QMessageBox.information(
|
||||
self,
|
||||
"夹爪设备已更新",
|
||||
f"串口: {port}\n夹爪从站 ID: {slave_id}\n控制程序将自动重启。",
|
||||
)
|
||||
self.restart_control()
|
||||
return True
|
||||
|
||||
def _load_position_config(self):
|
||||
open_pos = 0
|
||||
close_pos = 9000
|
||||
@@ -3377,6 +3627,36 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
self.smart_mode_button.setChecked(True)
|
||||
self.smart_mode_button.clicked.connect(lambda: self.set_control_mode("smart"))
|
||||
self.manual_mode_button.clicked.connect(lambda: self.set_control_mode("manual"))
|
||||
|
||||
self.rapid_behavior_button = AnimatedButton("迅速开合")
|
||||
self.rapid_behavior_button.setObjectName("ControlModeButton")
|
||||
self.rapid_behavior_button.setCheckable(True)
|
||||
self.rapid_behavior_button.setToolTip("极端跟手：接触后尽快可释放，优先开合速度")
|
||||
self.stable_behavior_button = AnimatedButton("夹持稳定")
|
||||
self.stable_behavior_button.setObjectName("ControlModeButton")
|
||||
self.stable_behavior_button.setCheckable(True)
|
||||
self.stable_behavior_button.setToolTip("极端稳夹：加长保护与确认，优先防误开")
|
||||
self.behavior_button_group = QButtonGroup(self)
|
||||
self.behavior_button_group.setExclusive(True)
|
||||
self.behavior_button_group.addButton(self.rapid_behavior_button)
|
||||
self.behavior_button_group.addButton(self.stable_behavior_button)
|
||||
self.stable_behavior_button.setChecked(True)
|
||||
self.rapid_behavior_button.clicked.connect(
|
||||
lambda: self.set_grip_behavior_mode("rapid")
|
||||
)
|
||||
self.stable_behavior_button.clicked.connect(
|
||||
lambda: self.set_grip_behavior_mode("stable")
|
||||
)
|
||||
self._sync_behavior_buttons(self._load_grip_behavior_mode())
|
||||
|
||||
self.record_button = AnimatedButton("录制数据")
|
||||
self.record_button.setObjectName("ControlModeButton")
|
||||
self.record_button.setCheckable(True)
|
||||
self.record_button.setToolTip(
|
||||
"录制控制过程中的力/状态/释放判定到 logs/sessions/*.jsonl，便于后续优化张开检测"
|
||||
)
|
||||
self.record_button.clicked.connect(self.toggle_session_recording)
|
||||
|
||||
open_pos, close_pos, position_min, position_max = self._load_position_config()
|
||||
self.gripper_open_pos = open_pos
|
||||
self.gripper_close_pos = close_pos
|
||||
@@ -3390,10 +3670,13 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
self.position_control.slider.sliderReleased.connect(self.move_to_slider_position)
|
||||
self.position_control.minus_button.clicked.connect(self.move_to_slider_position)
|
||||
self.position_control.plus_button.clicked.connect(self.move_to_slider_position)
|
||||
gripper_layout.addWidget(gripper_title, 0, 0, 1, 2, Qt.AlignLeft | Qt.AlignTop)
|
||||
gripper_layout.addWidget(gripper_title, 0, 0, 1, 1, Qt.AlignLeft | Qt.AlignTop)
|
||||
gripper_layout.addWidget(self.rapid_behavior_button, 0, 1)
|
||||
gripper_layout.addWidget(self.stable_behavior_button, 0, 2)
|
||||
gripper_layout.addWidget(self.manual_mode_button, 0, 3)
|
||||
gripper_layout.addWidget(self.smart_mode_button, 0, 4)
|
||||
gripper_layout.addWidget(self.speed_control, 1, 0, 1, 4)
|
||||
gripper_layout.addWidget(self.speed_control, 1, 0, 1, 3)
|
||||
gripper_layout.addWidget(self.record_button, 1, 3)
|
||||
gripper_layout.addWidget(self.force_control, 2, 0, 1, 4)
|
||||
gripper_layout.addWidget(self.position_control, 3, 0, 1, 4)
|
||||
gripper_layout.addWidget(self.manual_hold_button, 1, 4)
|
||||
@@ -3418,6 +3701,29 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
sensor_title.setObjectName("SectionTitle")
|
||||
sensor_title.setAlignment(Qt.AlignLeft | Qt.AlignTop)
|
||||
sensor_panel.set_drag_handle(sensor_title)
|
||||
port, slave_id = self._load_serial_device_config()
|
||||
port_label = QLabel("串口")
|
||||
port_label.setObjectName("MetricLabel")
|
||||
self.main_port_combo = QComboBox()
|
||||
self.main_port_combo.setEditable(True)
|
||||
self.main_port_combo.addItem(port, port)
|
||||
self.main_port_combo.setMinimumWidth(96)
|
||||
self.main_port_combo.setToolTip("夹爪串口")
|
||||
self.main_port_search_button = TopIconButton("search", "搜索夹爪串口")
|
||||
self.main_port_search_button.setFixedSize(26, 26)
|
||||
self.main_port_search_button.setIconSize(QSize(18, 18))
|
||||
self.main_port_search_button.clicked.connect(self.search_main_serial_ports)
|
||||
slave_id_label = QLabel("夹爪从站 ID")
|
||||
slave_id_label.setObjectName("MetricLabel")
|
||||
self.main_slave_id_spin = QSpinBox()
|
||||
self.main_slave_id_spin.setRange(1, 247)
|
||||
self.main_slave_id_spin.setValue(slave_id)
|
||||
self.main_slave_id_spin.setToolTip("Modbus 从站 ID（1-247）")
|
||||
self.main_slave_id_spin.setMaximumWidth(72)
|
||||
self.main_device_apply_button = AnimatedButton("应用")
|
||||
self.main_device_apply_button.setObjectName("ManualButton")
|
||||
self.main_device_apply_button.setToolTip("保存串口和夹爪从站 ID，并重启控制程序")
|
||||
self.main_device_apply_button.clicked.connect(self.apply_main_device_config)
|
||||
self.left_sensor_reset_button = TopIconButton("restart", "重启左传感器")
|
||||
self.left_sensor_reset_button.setEnabled(False)
|
||||
self.left_sensor_reset_button.clicked.connect(
|
||||
@@ -3429,10 +3735,16 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
lambda: self.send_manual_command("restart_right_sensor")
|
||||
)
|
||||
sensor_layout.addWidget(sensor_title, 0, 0, 1, 8, Qt.AlignLeft | Qt.AlignTop)
|
||||
sensor_layout.addWidget(port_label, 1, 0)
|
||||
sensor_layout.addWidget(self.main_port_combo, 1, 1, 1, 2)
|
||||
sensor_layout.addWidget(self.main_port_search_button, 1, 3)
|
||||
sensor_layout.addWidget(slave_id_label, 1, 4)
|
||||
sensor_layout.addWidget(self.main_slave_id_spin, 1, 5)
|
||||
sensor_layout.addWidget(self.main_device_apply_button, 1, 6, 1, 2)
|
||||
self.left_sensor_dot = StatusDot("左传感器", self.left_sensor_reset_button)
|
||||
self.right_sensor_dot = StatusDot("右传感器", self.right_sensor_reset_button)
|
||||
sensor_layout.addWidget(self.left_sensor_dot, 1, 0, 1, 4)
|
||||
sensor_layout.addWidget(self.right_sensor_dot, 1, 4, 1, 4)
|
||||
sensor_layout.addWidget(self.left_sensor_dot, 2, 0, 1, 4)
|
||||
sensor_layout.addWidget(self.right_sensor_dot, 2, 4, 1, 4)
|
||||
self.left_force = MetricBox("左法向力", "--")
|
||||
self.right_force = MetricBox("右法向力", "--")
|
||||
self.left_shear_box = MetricBox("左切向力", "--")
|
||||
@@ -3440,18 +3752,18 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
self.force_pct_box = MetricBox("夹爪当前力比例（%）", "--")
|
||||
self.target_force_box = MetricBox("夹爪目标力比例（%）", "--")
|
||||
self.speed_box = MetricBox("速度", "--")
|
||||
sensor_layout.addWidget(self.left_force, 2, 0, 1, 2)
|
||||
sensor_layout.addWidget(self.right_force, 2, 2, 1, 2)
|
||||
sensor_layout.addWidget(self.left_shear_box, 2, 4, 1, 2)
|
||||
sensor_layout.addWidget(self.right_shear_box, 2, 6, 1, 2)
|
||||
sensor_layout.addWidget(self.force_pct_box, 3, 0, 1, 3)
|
||||
sensor_layout.addWidget(self.target_force_box, 3, 3, 1, 3)
|
||||
sensor_layout.addWidget(self.speed_box, 3, 6, 1, 2)
|
||||
sensor_layout.addWidget(self.left_force, 3, 0, 1, 2)
|
||||
sensor_layout.addWidget(self.right_force, 3, 2, 1, 2)
|
||||
sensor_layout.addWidget(self.left_shear_box, 3, 4, 1, 2)
|
||||
sensor_layout.addWidget(self.right_shear_box, 3, 6, 1, 2)
|
||||
sensor_layout.addWidget(self.force_pct_box, 4, 0, 1, 3)
|
||||
sensor_layout.addWidget(self.target_force_box, 4, 3, 1, 3)
|
||||
sensor_layout.addWidget(self.speed_box, 4, 6, 1, 2)
|
||||
for col in range(8):
|
||||
sensor_layout.setColumnStretch(col, 1)
|
||||
for row in range(4):
|
||||
for row in range(5):
|
||||
sensor_layout.setRowStretch(row, 0)
|
||||
sensor_layout.setRowStretch(4, 1)
|
||||
sensor_layout.setRowStretch(5, 1)
|
||||
self._set_button_state(self.button_state)
|
||||
self.sensor_control_panel = sensor_panel
|
||||
self.gripper_stage.add_floating_control_panel(plot_panel, "plot")
|
||||
@@ -3620,6 +3932,20 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
self.manual_mode_button.setEnabled(mode_enabled)
|
||||
if hasattr(self, "smart_mode_button"):
|
||||
self.smart_mode_button.setEnabled(mode_enabled)
|
||||
if hasattr(self, "rapid_behavior_button"):
|
||||
self.rapid_behavior_button.setEnabled(True)
|
||||
if hasattr(self, "stable_behavior_button"):
|
||||
self.stable_behavior_button.setEnabled(True)
|
||||
if hasattr(self, "record_button"):
|
||||
recording = (
|
||||
self.controller is not None
|
||||
and getattr(self.controller, "session_recording", False)
|
||||
)
|
||||
self.record_button.setEnabled(state == "running")
|
||||
self.record_button.blockSignals(True)
|
||||
self.record_button.setChecked(bool(recording))
|
||||
self.record_button.setText("录制中…" if recording else "录制数据")
|
||||
self.record_button.blockSignals(False)
|
||||
if hasattr(self, "left_sensor_reset_button"):
|
||||
self.left_sensor_reset_button.setEnabled(sensor_restart_enabled)
|
||||
if hasattr(self, "right_sensor_reset_button"):
|
||||
@@ -3644,6 +3970,130 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
self.manual_mode_button.blockSignals(False)
|
||||
self._set_button_state(self.button_state)
|
||||
|
||||
def _load_grip_behavior_mode(self):
|
||||
from gripper.config.behavior_profiles import normalize_grip_behavior_mode
|
||||
|
||||
path = REPO_ROOT / "config" / "gripper.json"
|
||||
try:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
return normalize_grip_behavior_mode(data.get("grip_behavior_mode", "stable"))
|
||||
except Exception:
|
||||
return "stable"
|
||||
|
||||
def _sync_behavior_buttons(self, mode):
|
||||
mode = "rapid" if mode == "rapid" else "stable"
|
||||
if hasattr(self, "rapid_behavior_button"):
|
||||
self.rapid_behavior_button.blockSignals(True)
|
||||
self.stable_behavior_button.blockSignals(True)
|
||||
self.rapid_behavior_button.setChecked(mode == "rapid")
|
||||
self.stable_behavior_button.setChecked(mode == "stable")
|
||||
self.rapid_behavior_button.blockSignals(False)
|
||||
self.stable_behavior_button.blockSignals(False)
|
||||
|
||||
def set_grip_behavior_mode(self, mode, persist=True):
|
||||
from gripper.config.behavior_profiles import (
|
||||
apply_grip_behavior_profile,
|
||||
normalize_grip_behavior_mode,
|
||||
)
|
||||
|
||||
try:
|
||||
mode = normalize_grip_behavior_mode(mode)
|
||||
except Exception as exc:
|
||||
QMessageBox.warning(self, "模式无效", str(exc))
|
||||
return None
|
||||
|
||||
self._sync_behavior_buttons(mode)
|
||||
if self.controller is not None:
|
||||
try:
|
||||
self.controller.set_grip_behavior_mode(mode)
|
||||
except Exception as exc:
|
||||
print(f"set_grip_behavior_mode failed: {exc}")
|
||||
|
||||
if persist:
|
||||
path = REPO_ROOT / "config" / "gripper.json"
|
||||
try:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
if not isinstance(data, dict):
|
||||
data = {}
|
||||
apply_grip_behavior_profile(data, mode)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
json.dump(data, f, ensure_ascii=False, indent=2)
|
||||
f.write("\n")
|
||||
except Exception as exc:
|
||||
print(f"persist grip_behavior_mode failed: {exc}")
|
||||
|
||||
dialog = getattr(self, "visual_settings_dialog", None)
|
||||
if dialog is not None and hasattr(dialog, "controls"):
|
||||
if "grip_behavior_mode" in dialog.controls:
|
||||
dialog.controls["grip_behavior_mode"].blockSignals(True)
|
||||
dialog._set_control_value("grip_behavior_mode", mode)
|
||||
dialog.controls["grip_behavior_mode"].blockSignals(False)
|
||||
dialog._on_behavior_mode_changed()
|
||||
return mode
|
||||
|
||||
def toggle_session_recording(self):
|
||||
if self.controller is None:
|
||||
self.record_button.blockSignals(True)
|
||||
self.record_button.setChecked(False)
|
||||
self.record_button.blockSignals(False)
|
||||
QMessageBox.information(self, "录制数据", "请先启动控制程序再录制。")
|
||||
return
|
||||
try:
|
||||
if self.controller.session_recording:
|
||||
path = self.controller.stop_session_recording()
|
||||
self.record_button.setText("录制数据")
|
||||
self.record_button.setChecked(False)
|
||||
if path is not None:
|
||||
QMessageBox.information(
|
||||
self,
|
||||
"录制已停止",
|
||||
f"会话数据已保存到:\n{path}",
|
||||
)
|
||||
else:
|
||||
path = self.controller.start_session_recording(note="ui")
|
||||
self.record_button.setText("录制中…")
|
||||
self.record_button.setChecked(True)
|
||||
QMessageBox.information(
|
||||
self,
|
||||
"开始录制",
|
||||
f"正在写入:\n{path}\n再点一次按钮结束录制。",
|
||||
)
|
||||
except Exception as exc:
|
||||
self.record_button.blockSignals(True)
|
||||
self.record_button.setChecked(False)
|
||||
self.record_button.setText("录制数据")
|
||||
self.record_button.blockSignals(False)
|
||||
QMessageBox.warning(self, "录制失败", str(exc))
|
||||
|
||||
def apply_runtime_config(self, updated):
|
||||
"""Push saved JSON fields into the live controller config when possible."""
|
||||
if self.controller is None or not hasattr(self.controller, "config"):
|
||||
return False
|
||||
config = self.controller.config
|
||||
mode = updated.get("grip_behavior_mode")
|
||||
if mode:
|
||||
try:
|
||||
self.controller.set_grip_behavior_mode(mode)
|
||||
self._sync_behavior_buttons(mode)
|
||||
except Exception as exc:
|
||||
print(f"apply grip_behavior_mode failed: {exc}")
|
||||
|
||||
for key, value in updated.items():
|
||||
if key == "grip_behavior_mode":
|
||||
continue
|
||||
if hasattr(config, key):
|
||||
try:
|
||||
setattr(config, key, value)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self.controller.feedback_processor.reset_filters()
|
||||
except Exception:
|
||||
pass
|
||||
return True
|
||||
|
||||
def open_visual_settings(self):
|
||||
if self.visual_settings_dialog is None:
|
||||
self.visual_settings_dialog = VisualSettingsDialog(self)
|
||||
@@ -3804,6 +4254,8 @@ class GripperDemo02Viewer(QMainWindow):
|
||||
)
|
||||
try:
|
||||
self.controller.set_control_mode(self.control_mode)
|
||||
behavior = getattr(self.controller.config, "grip_behavior_mode", "stable")
|
||||
self._sync_behavior_buttons(behavior)
|
||||
except Exception as exc:
|
||||
print(f"control mode init failed: {exc}")
|
||||
self.control_thread = threading.Thread(
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
import unittest
|
||||
|
||||
from gripper.config.behavior_profiles import (
|
||||
GRIP_BEHAVIOR_PROFILES,
|
||||
apply_grip_behavior_profile,
|
||||
normalize_grip_behavior_mode,
|
||||
)
|
||||
from gripper.config.settings import Demo02Config
|
||||
|
||||
|
||||
class BehaviorProfileTests(unittest.TestCase):
|
||||
def test_normalize_aliases(self):
|
||||
self.assertEqual(normalize_grip_behavior_mode("迅速开合"), "rapid")
|
||||
self.assertEqual(normalize_grip_behavior_mode("夹持稳定"), "stable")
|
||||
|
||||
def test_apply_to_config_object(self):
|
||||
config = Demo02Config()
|
||||
apply_grip_behavior_profile(config, "rapid")
|
||||
self.assertEqual(config.grip_behavior_mode, "rapid")
|
||||
self.assertEqual(config.release_min_hold_seconds, 0.0)
|
||||
self.assertFalse(config.adaptive_grip_enabled)
|
||||
|
||||
apply_grip_behavior_profile(config, "stable")
|
||||
self.assertEqual(config.grip_behavior_mode, "stable")
|
||||
self.assertGreater(config.release_min_hold_seconds, 0.5)
|
||||
self.assertTrue(config.adaptive_grip_enabled)
|
||||
self.assertTrue(config.release_require_slope_settle)
|
||||
self.assertTrue(config.release_settle_normal_only)
|
||||
self.assertTrue(config.release_side_shear_enabled)
|
||||
self.assertFalse(config.release_side_normal_enabled)
|
||||
self.assertTrue(config.release_shear_onset_enabled)
|
||||
self.assertFalse(config.release_normal_onset_enabled)
|
||||
self.assertGreater(config.release_slope_hold_seconds, 0.2)
|
||||
self.assertGreaterEqual(config.release_curve_history, 24)
|
||||
self.assertGreater(
|
||||
config.release_confirm_samples,
|
||||
GRIP_BEHAVIOR_PROFILES["rapid"]["release_confirm_samples"],
|
||||
)
|
||||
|
||||
apply_grip_behavior_profile(config, "rapid")
|
||||
self.assertFalse(config.release_require_slope_settle)
|
||||
self.assertTrue(config.release_side_shear_enabled)
|
||||
self.assertTrue(config.release_side_normal_enabled)
|
||||
self.assertTrue(config.release_shear_onset_enabled)
|
||||
self.assertTrue(config.release_normal_onset_enabled)
|
||||
self.assertLessEqual(config.release_curve_history, 8)
|
||||
|
||||
def test_apply_to_dict(self):
|
||||
data = {"port": "COM1"}
|
||||
apply_grip_behavior_profile(data, "rapid")
|
||||
self.assertEqual(data["grip_behavior_mode"], "rapid")
|
||||
self.assertEqual(data["release_side_touch_change"], 0.04)
|
||||
self.assertEqual(data["port"], "COM1")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,273 @@
|
||||
import contextlib
|
||||
import io
|
||||
import threading
|
||||
import time
|
||||
import tempfile
|
||||
import json
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
import unittest
|
||||
from unittest.mock import patch
|
||||
|
||||
from gripper.config.settings import Demo02Config
|
||||
from gripper.control.actuator import ActuatorWorker
|
||||
from gripper.control.controller import SideTriggerGripController
|
||||
from gripper.control.feedback import TactileFeedbackProcessor
|
||||
from test_release_watch import feedback
|
||||
|
||||
|
||||
class Clock:
|
||||
now = 10.0
|
||||
|
||||
def sleep(self, seconds):
|
||||
self.now += seconds
|
||||
|
||||
|
||||
class ReplayActuator:
|
||||
position = 0
|
||||
hold_pos = None
|
||||
completed_at = None
|
||||
|
||||
def __init__(self, clock):
|
||||
self.clock = clock
|
||||
self.commands = []
|
||||
self.force_steps = 0
|
||||
|
||||
@property
|
||||
def motion_status(self):
|
||||
return False, self.completed_at
|
||||
|
||||
def record(self, kind, *args, **kwargs):
|
||||
self.commands.append((self.clock.now, kind, args, kwargs))
|
||||
self.completed_at = self.clock.now
|
||||
|
||||
def open(self, *args, **kwargs):
|
||||
self.record("open", *args, **kwargs)
|
||||
self.position = 0
|
||||
|
||||
def close(self, *args, **kwargs):
|
||||
self.record("close", *args, **kwargs)
|
||||
self.position = 500
|
||||
|
||||
def hold(self, *args, **kwargs):
|
||||
self.record("hold", *args, **kwargs)
|
||||
self.hold_pos = self.position
|
||||
|
||||
def set_force(self, *args, **kwargs):
|
||||
self.record("set_force", *args, **kwargs)
|
||||
self.force_steps += 1
|
||||
|
||||
def move(self, position, *args, **kwargs):
|
||||
self.record("move", position, *args, **kwargs)
|
||||
self.position = position
|
||||
|
||||
|
||||
class ControllerReplayTests(unittest.TestCase):
|
||||
def run_replay(self, curve, *, config=None, duration=1.5, manual=None):
|
||||
config = config or Demo02Config(open_wait_stable_samples=2, grip_start_force=30,
|
||||
hold_force=30, adaptive_force_min=30)
|
||||
clock = Clock()
|
||||
actuator = ReplayActuator(clock)
|
||||
reader = SimpleNamespace(latest=[])
|
||||
processor = SimpleNamespace(
|
||||
normal_converter=SimpleNamespace(unit="N"), shear_converter=SimpleNamespace(unit="N"),
|
||||
reset_filters=lambda: None,
|
||||
read=lambda *_args, **_kwargs: curve(clock.now - 10, actuator),
|
||||
)
|
||||
statuses = []
|
||||
controller = SideTriggerGripController(config, reader, None, processor)
|
||||
controller.actuator = actuator
|
||||
trace_directory = tempfile.TemporaryDirectory()
|
||||
self.addCleanup(trace_directory.cleanup)
|
||||
controller.release_trace_path = Path(trace_directory.name) / "release-last.json"
|
||||
|
||||
def on_status(status):
|
||||
statuses.append((clock.now - 10, status))
|
||||
if manual:
|
||||
manual(controller, len(statuses))
|
||||
if clock.now - 10 >= duration:
|
||||
controller.stop()
|
||||
|
||||
controller.status_callback = on_status
|
||||
if manual:
|
||||
controller.set_control_mode("manual")
|
||||
manual(controller, 0)
|
||||
output = io.StringIO()
|
||||
with patch("gripper.control.controller.time.perf_counter", side_effect=lambda: clock.now), \
|
||||
patch("gripper.control.controller.time.sleep", side_effect=clock.sleep), \
|
||||
contextlib.redirect_stdout(output):
|
||||
controller._run_event_monitor()
|
||||
self.last_release_trace = (json.loads(controller.release_trace_path.read_text(encoding="utf-8"))
|
||||
if controller.release_trace_path.exists() else None)
|
||||
return actuator.commands, statuses, output.getvalue()
|
||||
|
||||
@staticmethod
|
||||
def contact_curve(t, *, shear=0.05, normal=0.5):
|
||||
if t < 0.1:
|
||||
return feedback(normal=0, shear=0, stamp=t)
|
||||
if t < 0.14:
|
||||
return feedback(normal=0.2, right_normal=0, shear=0, stamp=t)
|
||||
return feedback(normal=normal, shear=shear, stamp=t)
|
||||
|
||||
def test_release_wins_over_adaptive_force_request(self):
|
||||
commands, statuses, logs = self.run_replay(
|
||||
lambda t, _: self.contact_curve(t, shear=0.35 if t >= 0.8 else 0.05))
|
||||
releases = [entry for entry in commands if entry[1] == "open"]
|
||||
self.assertEqual(len(releases), 1)
|
||||
self.assertIn("force-change-release-open", releases[0][2])
|
||||
self.assertLessEqual(releases[0][0] - 10 - 0.8, 0.2)
|
||||
self.assertEqual(len([c for c in commands if c[1] == "set_force"]), 1)
|
||||
self.assertIn("release-sudden", logs, "short confirmation must survive log throttling")
|
||||
released_status = next(s for _, s in statuses if s["action"].startswith("force-change-release"))
|
||||
self.assertEqual(released_status["speed_pct"], Demo02Config.release_open_speed)
|
||||
|
||||
def test_release_in_gripping_before_ramp_finishes(self):
|
||||
config = Demo02Config(open_wait_stable_samples=2, grip_start_force=15, hold_force=50)
|
||||
commands, statuses, _ = self.run_replay(
|
||||
lambda t, _: self.contact_curve(t, shear=0.35 if t >= 0.5 else 0.05), config=config)
|
||||
self.assertTrue(any(c[1] == "open" for c in commands))
|
||||
pre_release = [s["state"] for t, s in statuses if t < 0.5]
|
||||
self.assertIn("gripping", pre_release)
|
||||
self.assertNotIn("hold_check", pre_release)
|
||||
|
||||
def test_side_touch_opens_during_initial_contact_guard(self):
|
||||
def curve(t, _):
|
||||
sample = self.contact_curve(t)
|
||||
if 0.2 <= t < 0.4:
|
||||
sample.left_shear = 0.15
|
||||
sample.max_shear = 0.15
|
||||
sample.shear_diff = 0.1
|
||||
return sample
|
||||
|
||||
commands, statuses, _ = self.run_replay(curve, duration=0.5)
|
||||
contact_time = next(t for t, s in statuses if s["action"] == "object-detected-low-force")
|
||||
opens = [c for c in commands if c[1] == "open"]
|
||||
self.assertEqual(len(opens), 1)
|
||||
self.assertLess(opens[0][0] - 10 - contact_time, 0.1)
|
||||
self.assertTrue(any(s["action"] == "force-change-release-open(side_touch)" for _, s in statuses))
|
||||
trace = self.last_release_trace
|
||||
self.assertIsNotNone(trace)
|
||||
self.assertEqual(trace["frames"][-1]["release"]["channel"], "side_shear")
|
||||
self.assertTrue(trace["frames"][-1]["release"]["release"])
|
||||
self.assertGreater(len(trace["frames"]), 2)
|
||||
self.assertIn("release_min_change", trace["config"])
|
||||
|
||||
def test_contact_and_adaptive_spikes_do_not_open(self):
|
||||
def curve(t, actuator):
|
||||
return self.contact_curve(t, normal=0.5 + actuator.force_steps * 0.2, shear=0.6)
|
||||
commands, _, logs = self.run_replay(curve, duration=3)
|
||||
self.assertFalse(any(c[1] == "open" for c in commands))
|
||||
self.assertGreater(len([c for c in commands if c[1] == "set_force"]), 3)
|
||||
self.assertIn("release-grip-settle", logs)
|
||||
self.assertIn("release-self-settle", logs)
|
||||
self.assertIn("release-slow_internal", logs)
|
||||
|
||||
def test_manual_command_paths_remain_manual(self):
|
||||
sequence = {0: ("close", {}), 3: ("hold", {}), 6: ("release", {}),
|
||||
9: ("move_position", {"position": 123}), 12: ("open", {})}
|
||||
|
||||
def manual(controller, count):
|
||||
if count in sequence:
|
||||
command, args = sequence[count]
|
||||
controller.request_manual_command(command, **args)
|
||||
|
||||
commands, statuses, _ = self.run_replay(
|
||||
lambda t, _: feedback(normal=0.5 if round(t * 30) % 2 else 0, shear=t, stamp=t),
|
||||
manual=manual, duration=0.7)
|
||||
self.assertEqual([c[1] for c in commands], ["close", "set_force", "hold", "move", "open"])
|
||||
self.assertFalse(any(s["release_armed"] for _, s in statuses))
|
||||
|
||||
def test_close_timeout_still_opens(self):
|
||||
config = Demo02Config(open_wait_stable_samples=2, close_timeout_seconds=0.3)
|
||||
commands, _, _ = self.run_replay(
|
||||
lambda t, _: feedback(normal=0 if t < 0.1 else 0.2, right_normal=0, shear=0, stamp=t),
|
||||
config=config, duration=0.8)
|
||||
self.assertTrue(any(c[1] == "open" and c[2][1] == "close-timeout-open" for c in commands))
|
||||
|
||||
def test_missing_feedback_cancels_pending_release(self):
|
||||
def curve(t, _):
|
||||
if 0.82 <= t < 1.0:
|
||||
return None
|
||||
return self.contact_curve(t, shear=0.35 if t >= 0.8 else 0.05)
|
||||
commands, statuses, _ = self.run_replay(curve)
|
||||
self.assertFalse(any(c[1] == "open" for c in commands))
|
||||
gaps = [s for _, s in statuses if s["action"] == "waiting-for-sensor-samples"]
|
||||
self.assertTrue(gaps)
|
||||
self.assertTrue(all(not s["release_armed"] for s in gaps))
|
||||
|
||||
def test_current_json_ramp_and_release(self):
|
||||
config = Demo02Config.from_args([])
|
||||
|
||||
def curve(t, actuator):
|
||||
if t < 0.4:
|
||||
return feedback(normal=0, shear=0, stamp=t)
|
||||
if t < 0.5:
|
||||
return feedback(normal=0.3, right_normal=0, shear=0, stamp=t)
|
||||
shear = 0.4 if t >= 4 else 0.05
|
||||
return feedback(normal=0.5 + actuator.force_steps * 0.1, shear=shear, stamp=t)
|
||||
|
||||
commands, _, _ = self.run_replay(curve, config=config, duration=4.4)
|
||||
releases = [c for c in commands if c[1] == "open"]
|
||||
self.assertEqual(len(releases), 1)
|
||||
self.assertGreaterEqual(releases[0][0] - 10, 4)
|
||||
self.assertLessEqual(releases[0][0] - 10 - 4, 0.2)
|
||||
|
||||
|
||||
class FeedbackFreshnessTests(unittest.TestCase):
|
||||
def test_only_fresh_side_advances_its_filter(self):
|
||||
converter = SimpleNamespace(convert=float)
|
||||
processor = TactileFeedbackProcessor(Demo02Config(filter_alpha=0.5), converter, converter)
|
||||
samples = [dict(fnormal=0, fshearx=0, fsheary=0, timestamp=1.0) for _ in range(2)]
|
||||
reader = SimpleNamespace(get_samples=lambda **_: samples)
|
||||
processor.read(reader)
|
||||
samples[0] = dict(fnormal=1, fshearx=0, fsheary=0, timestamp=2.0)
|
||||
first = processor.read(reader)
|
||||
again = processor.read(reader)
|
||||
self.assertEqual(first.left_normal, 0.5)
|
||||
self.assertEqual(again.left_normal, first.left_normal)
|
||||
samples[1] = dict(fnormal=1, fshearx=0, fsheary=0, timestamp=2.1)
|
||||
other = processor.read(reader)
|
||||
self.assertEqual(other.left_normal, 0.5)
|
||||
self.assertEqual(other.right_normal, 0.5)
|
||||
processor.reset_filters()
|
||||
self.assertEqual(processor.read(reader).left_normal, 1)
|
||||
|
||||
|
||||
class WorkerMotionTests(unittest.TestCase):
|
||||
def test_open_drops_queued_grip_force(self):
|
||||
worker = ActuatorWorker(None)
|
||||
worker.set_force(80)
|
||||
worker.hold(80)
|
||||
worker.set_force(90)
|
||||
worker.open(20)
|
||||
self.assertEqual([c.kind for c in worker._commands], ["open"])
|
||||
|
||||
def test_motion_snapshot_covers_queue_and_slow_actual_io(self):
|
||||
entered, finish = threading.Event(), threading.Event()
|
||||
|
||||
def set_force(_):
|
||||
entered.set()
|
||||
finish.wait(timeout=2)
|
||||
|
||||
gripper = SimpleNamespace(connect=lambda: None, read_position=lambda: 0, set_force=set_force)
|
||||
worker = ActuatorWorker(gripper)
|
||||
worker.set_force(30)
|
||||
self.assertEqual(worker.motion_status, (True, None))
|
||||
worker.start()
|
||||
try:
|
||||
self.assertTrue(entered.wait(timeout=1))
|
||||
self.assertEqual(worker.motion_status, (True, None))
|
||||
self.assertFalse(worker.wait_idle(timeout=0.02))
|
||||
before_completion = time.perf_counter()
|
||||
finish.set()
|
||||
self.assertTrue(worker.wait_idle(timeout=1))
|
||||
pending, completed = worker.motion_status
|
||||
self.assertFalse(pending)
|
||||
self.assertGreaterEqual(completed, before_completion)
|
||||
finally:
|
||||
finish.set()
|
||||
worker.stop()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,302 @@
|
||||
import math
|
||||
import random
|
||||
import unittest
|
||||
|
||||
from gripper.config.settings import Demo02Config
|
||||
from gripper.control.release_watch import ReleaseWatch
|
||||
from test_release_watch import feedback
|
||||
|
||||
|
||||
class SettlingCurveTests(unittest.TestCase):
|
||||
def watch(self):
|
||||
cfg = Demo02Config()
|
||||
watch = ReleaseWatch(cfg)
|
||||
watch.contact(0, feedback())
|
||||
for i in range(1, 16):
|
||||
watch.step(i / 30, feedback(stamp=i / 30))
|
||||
return watch
|
||||
|
||||
def test_smooth_rise_flatten_and_return_are_not_a_new_touch(self):
|
||||
for side_only in (False, True):
|
||||
for hz in (20, 30):
|
||||
with self.subTest(side_only=side_only, hz=hz):
|
||||
watch = self.watch()
|
||||
for i in range(1, 2 * hz + 1):
|
||||
t = i / hz
|
||||
value = 0.05 + 0.08 * (1 - math.cos(2 * math.pi * min(t, 1.6) / 1.6))
|
||||
sample = feedback(shear=value, right_shear=0.05 if side_only else value, stamp=0.5 + t)
|
||||
result = watch.step(0.5 + t, sample, motion_pending=side_only)
|
||||
self.assertFalse(result.release, f"false release at t={t:.3f}: {result}")
|
||||
|
||||
def test_new_touch_on_top_of_smooth_settling_still_releases(self):
|
||||
for touch_at in (0.3, 0.8, 1.1):
|
||||
for side_only in (False, True):
|
||||
with self.subTest(touch_at=touch_at, side_only=side_only):
|
||||
watch = self.watch()
|
||||
released_at = None
|
||||
for i in range(1, 48):
|
||||
t = i / 30
|
||||
value = 0.05 + 0.08 * (1 - math.cos(2 * math.pi * t / 1.6))
|
||||
if t + 1e-9 >= touch_at:
|
||||
value += 0.1
|
||||
sample = feedback(shear=value, right_shear=0.05 if side_only else value, stamp=0.5 + t)
|
||||
result = watch.step(0.5 + t, sample, motion_pending=side_only)
|
||||
if result.release:
|
||||
released_at = t
|
||||
break
|
||||
self.assertIsNotNone(released_at)
|
||||
self.assertGreaterEqual(released_at + 1e-9, touch_at)
|
||||
self.assertLessEqual(released_at - touch_at, 0.2)
|
||||
|
||||
def test_large_smooth_change_cannot_bypass_shape_gate_via_weak_path(self):
|
||||
watch = self.watch()
|
||||
watch.config.release_min_change = 0.5
|
||||
for i in range(1, 61):
|
||||
t = i / 30
|
||||
value = 0.05 + 0.6 * (1 - math.cos(math.pi * min(t, 1.5) / 1.5))
|
||||
result = watch.step(0.5 + t, feedback(shear=value, stamp=0.5 + t))
|
||||
self.assertFalse(result.release)
|
||||
|
||||
def test_contact_inherits_the_already_rising_curve(self):
|
||||
watch = ReleaseWatch(Demo02Config())
|
||||
history = [(i / 30, feedback(shear=value, right_shear=0.05, stamp=i / 30))
|
||||
for i, value in enumerate((0.05, 0.08, 0.11))]
|
||||
watch.contact(2 / 30, history[-1][1], history=history)
|
||||
results = []
|
||||
for i, value in enumerate((0.135, 0.153, 0.165, 0.17, 0.168, 0.16, 0.15, 0.14), 3):
|
||||
result = watch.step(i / 30, feedback(shear=value, right_shear=0.05, stamp=i / 30), motion_pending=True)
|
||||
self.assertFalse(result.release)
|
||||
results.append(result)
|
||||
self.assertTrue(any(r.curve_motion == "settling_turn" for r in results))
|
||||
|
||||
def test_high_rate_existing_curve_does_not_bypass_onset_gate(self):
|
||||
for side_only in (False, True):
|
||||
with self.subTest(side_only=side_only):
|
||||
watch = ReleaseWatch(Demo02Config())
|
||||
|
||||
def sample_at(t):
|
||||
value = 0.3 + 4.5 * t - 4.0 * t * t
|
||||
return feedback(shear=value, right_shear=0.05 if side_only else value, stamp=t)
|
||||
|
||||
history = [(i / 30, sample_at(i / 30)) for i in range(3)]
|
||||
watch.contact(2 / 30, history[-1][1], history=history)
|
||||
for i in range(3, 27):
|
||||
result = watch.step(i / 30, sample_at(i / 30))
|
||||
self.assertFalse(result.release)
|
||||
|
||||
def test_side_curve_uses_sensor_time_despite_control_tick_jitter(self):
|
||||
watch = self.watch()
|
||||
receipt = 10.0
|
||||
for i in range(1, 49):
|
||||
t = i / 30
|
||||
receipt += 0.018 if i % 2 else 0.0486666667
|
||||
value = 0.05 + 0.08 * (1 - math.cos(2 * math.pi * t / 1.6))
|
||||
sample = feedback(shear=value, right_shear=0.05, stamp=1000 + t)
|
||||
result = watch.step(receipt, sample, motion_pending=True)
|
||||
self.assertFalse(result.release)
|
||||
|
||||
def test_predictor_cannot_invent_displacement_from_bounded_noise(self):
|
||||
for protected in (False, True):
|
||||
with self.subTest(protected=protected):
|
||||
watch = self.watch()
|
||||
rng = random.Random(17)
|
||||
for i in range(1, 301):
|
||||
t = 0.5 + i / 30
|
||||
sample = feedback(normal=0.5 + rng.uniform(-0.012, 0.012),
|
||||
right_normal=0.5 + rng.uniform(-0.012, 0.012),
|
||||
shear=0.05 + rng.uniform(-0.012, 0.012),
|
||||
right_shear=0.05 + rng.uniform(-0.012, 0.012), stamp=t)
|
||||
result = watch.step(t, sample, motion_pending=protected)
|
||||
self.assertFalse(result.release)
|
||||
|
||||
@staticmethod
|
||||
def noisy_curve(noise, side_only, seed, touch_at=None):
|
||||
rng = random.Random(seed)
|
||||
for i in range(1, 61):
|
||||
t = i / 30
|
||||
value = 0.05 + 0.08 * (1 - math.cos(2 * math.pi * min(t, 1.6) / 1.6))
|
||||
offset = 0.1 if touch_at is not None and t + 1e-9 >= touch_at else 0.0
|
||||
left = value + rng.uniform(-noise, noise) + offset
|
||||
right = 0.05 if side_only else value + rng.uniform(-noise, noise) + offset
|
||||
yield t, feedback(shear=left, right_shear=right, stamp=0.5 + t)
|
||||
|
||||
def test_noise_on_a_smooth_curve_cannot_be_amplified_into_a_touch(self):
|
||||
# Both values move well beyond the amplitude floor over time, but the
|
||||
# only departures from the settling curve are tiny bounded noise.
|
||||
# Three-point extrapolation previously falsely released 56/60 cases.
|
||||
for noise in (0.002, 0.004, 0.006):
|
||||
for side_only in (False, True):
|
||||
for seed in range(10):
|
||||
with self.subTest(noise=noise, side_only=side_only, seed=seed):
|
||||
watch = self.watch()
|
||||
for t, sample in self.noisy_curve(noise, side_only, seed):
|
||||
result = watch.step(0.5 + t, sample, motion_pending=side_only)
|
||||
self.assertFalse(result.release, f"false release at {t:.3f}: {result}")
|
||||
|
||||
def test_noise_near_a_turn_does_not_cancel_a_real_touch(self):
|
||||
for touch_at in (0.3, 0.8, 1.1):
|
||||
for side_only in (False, True):
|
||||
for seed in range(10):
|
||||
with self.subTest(touch_at=touch_at, side_only=side_only, seed=seed):
|
||||
watch = self.watch()
|
||||
released_at = None
|
||||
for t, sample in self.noisy_curve(0.006, side_only, seed, touch_at):
|
||||
result = watch.step(0.5 + t, sample, motion_pending=side_only)
|
||||
if result.release:
|
||||
released_at = t
|
||||
break
|
||||
self.assertIsNotNone(released_at)
|
||||
self.assertGreaterEqual(released_at + 1e-9, touch_at)
|
||||
self.assertLessEqual(released_at - touch_at, 0.2)
|
||||
|
||||
|
||||
class LongHistorySlopeTests(unittest.TestCase):
|
||||
def test_force_curve_fit_slope_decays_as_ramp_flattens(self):
|
||||
from gripper.control.force_curve import ForceCurve
|
||||
|
||||
cfg = Demo02Config(
|
||||
release_curve_history=30,
|
||||
release_slope_window_seconds=0.8,
|
||||
release_min_change=0.04,
|
||||
release_side_touch_change=0.06,
|
||||
)
|
||||
curve = ForceCurve(cfg)
|
||||
rising = []
|
||||
for i in range(1, 36):
|
||||
t = i / 30
|
||||
# Fast early climb then flatten: slope should shrink.
|
||||
value = 0.5 + 0.4 * (1 - math.exp(-4.0 * t))
|
||||
curve.observe(t, value)
|
||||
rising.append(abs(curve.fit_slope))
|
||||
self.assertGreaterEqual(len(curve.history), 30)
|
||||
self.assertGreater(max(rising[:10]), rising[-1])
|
||||
self.assertLess(rising[-1], 0.25)
|
||||
|
||||
def test_stable_mode_blocks_shear_side_touch_before_plateau(self):
|
||||
cfg = Demo02Config()
|
||||
cfg.apply_grip_behavior_mode("stable")
|
||||
cfg.release_confirm_samples = 2
|
||||
watch = ReleaseWatch(cfg)
|
||||
watch.contact(0.0, feedback())
|
||||
for i in range(1, 25):
|
||||
t = i / 30
|
||||
normal = 0.5 + 0.8 * (1 - math.exp(-3.0 * t))
|
||||
sample = feedback(normal=normal, right_normal=normal * 0.95,
|
||||
shear=0.05 + 0.02 * i, right_shear=0.05, stamp=t)
|
||||
result = watch.step(t, sample, motion_pending=True)
|
||||
self.assertFalse(result.release)
|
||||
self.assertTrue(result.reason in {"force-settling", "grip-settle", "self-settle"})
|
||||
|
||||
def test_stable_mode_open_works_during_adaptive_force_after_plateau(self):
|
||||
cfg = Demo02Config()
|
||||
cfg.apply_grip_behavior_mode("stable")
|
||||
cfg.release_confirm_samples = 2
|
||||
cfg.release_min_hold_seconds = 0.05
|
||||
watch = ReleaseWatch(cfg)
|
||||
watch.contact(0.0, feedback())
|
||||
for i in range(1, 55):
|
||||
t = i / 30
|
||||
sample = feedback(normal=0.9, right_normal=0.9, shear=0.05, right_shear=0.05, stamp=t)
|
||||
self.assertFalse(watch.step(t, sample).release)
|
||||
self.assertTrue(watch._plateau_latched)
|
||||
# Adaptive 加力 is still pending, but intentional shear open must work.
|
||||
released = False
|
||||
for i in range(55, 70):
|
||||
t = i / 30
|
||||
sample = feedback(normal=0.95, right_normal=0.95,
|
||||
shear=0.35, right_shear=0.05, stamp=t)
|
||||
result = watch.step(t, sample, motion_pending=True)
|
||||
if result.release:
|
||||
released = True
|
||||
break
|
||||
self.assertTrue(released)
|
||||
|
||||
def test_stable_mode_ignores_normal_open_after_plateau(self):
|
||||
cfg = Demo02Config()
|
||||
cfg.apply_grip_behavior_mode("stable")
|
||||
cfg.release_confirm_samples = 2
|
||||
cfg.release_min_hold_seconds = 0.05
|
||||
watch = ReleaseWatch(cfg)
|
||||
watch.contact(0.0, feedback())
|
||||
for i in range(1, 55):
|
||||
t = i / 30
|
||||
sample = feedback(normal=0.9, right_normal=0.9, shear=0.05, right_shear=0.05, stamp=t)
|
||||
self.assertFalse(watch.step(t, sample).release)
|
||||
for i in range(55, 70):
|
||||
t = i / 30
|
||||
sample = feedback(normal=1.25, right_normal=0.9, shear=0.05, right_shear=0.05, stamp=t)
|
||||
result = watch.step(t, sample)
|
||||
self.assertFalse(result.release, f"normal false open: {result}")
|
||||
|
||||
def test_stable_mode_allows_shear_open_after_plateau(self):
|
||||
cfg = Demo02Config()
|
||||
cfg.apply_grip_behavior_mode("stable")
|
||||
cfg.release_confirm_samples = 2
|
||||
cfg.release_min_hold_seconds = 0.05
|
||||
watch = ReleaseWatch(cfg)
|
||||
watch.contact(0.0, feedback())
|
||||
for i in range(1, 55):
|
||||
t = i / 30
|
||||
sample = feedback(normal=0.9, right_normal=0.9, shear=0.05, right_shear=0.05, stamp=t)
|
||||
self.assertFalse(watch.step(t, sample).release)
|
||||
released = False
|
||||
for i in range(55, 70):
|
||||
t = i / 30
|
||||
sample = feedback(normal=0.9, right_normal=0.9,
|
||||
shear=0.35, right_shear=0.05, stamp=t)
|
||||
result = watch.step(t, sample)
|
||||
if result.release:
|
||||
released = True
|
||||
break
|
||||
self.assertTrue(released)
|
||||
|
||||
def test_intentional_open_after_plateau_not_revoked_by_rising_slope(self):
|
||||
cfg = Demo02Config()
|
||||
cfg.apply_grip_behavior_mode("stable")
|
||||
cfg.release_confirm_samples = 2
|
||||
cfg.release_min_hold_seconds = 0.05
|
||||
watch = ReleaseWatch(cfg)
|
||||
watch.contact(0.0, feedback())
|
||||
for i in range(1, 55):
|
||||
t = i / 30
|
||||
watch.step(t, feedback(normal=0.9, right_normal=0.9, stamp=t))
|
||||
self.assertTrue(watch._plateau_latched)
|
||||
released = False
|
||||
for i in range(55, 75):
|
||||
t = i / 30
|
||||
n = 0.9 + 0.05 * (i - 54)
|
||||
self.assertTrue(watch._force_settled(t))
|
||||
result = watch.step(
|
||||
t,
|
||||
feedback(normal=n, right_normal=0.9,
|
||||
shear=0.05 + 0.03 * (i - 54), right_shear=0.05, stamp=t),
|
||||
motion_pending=True,
|
||||
)
|
||||
if result.release:
|
||||
released = True
|
||||
break
|
||||
self.assertTrue(released)
|
||||
|
||||
def test_short_normal_spike_does_not_count_as_settled_plateau(self):
|
||||
cfg = Demo02Config()
|
||||
cfg.apply_grip_behavior_mode("stable")
|
||||
cfg.release_confirm_samples = 2
|
||||
cfg.release_min_hold_seconds = 0.0
|
||||
cfg.release_slope_hold_seconds = 0.4
|
||||
watch = ReleaseWatch(cfg)
|
||||
watch.contact(0.0, feedback())
|
||||
for i in range(1, 12):
|
||||
t = i / 30
|
||||
n = 0.9 if i >= 6 else 0.1 + 0.15 * i
|
||||
sample = feedback(normal=n, right_normal=n, stamp=t)
|
||||
watch.step(t, sample, motion_pending=True)
|
||||
for i in range(12, 20):
|
||||
t = i / 30
|
||||
sample = feedback(normal=1.2, right_normal=0.9, stamp=t)
|
||||
result = watch.step(t, sample, motion_pending=True)
|
||||
self.assertFalse(result.release)
|
||||
self.assertIn(result.reason, {"force-settling", "grip-settle", "self-settle"})
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,347 @@
|
||||
import unittest
|
||||
|
||||
from gripper.common.filters import ExponentialFilter
|
||||
from gripper.config.settings import Demo02Config
|
||||
from gripper.control.feedback import ForceFeedback
|
||||
from gripper.control.release_watch import ReleasePhase, ReleaseWatch
|
||||
|
||||
|
||||
def feedback(normal=0.5, shear=0.05, *, right_normal=None, right_shear=None, stamp=None):
|
||||
rn = normal if right_normal is None else right_normal
|
||||
rs = shear if right_shear is None else right_shear
|
||||
return ForceFeedback(normal, rn, min(normal, rn), max(normal, rn), normal - rn,
|
||||
shear, rs, max(shear, rs), shear - rs,
|
||||
left_timestamp=stamp, right_timestamp=stamp)
|
||||
|
||||
|
||||
class ReleaseReplayTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.config = Demo02Config()
|
||||
self.watch = ReleaseWatch(self.config)
|
||||
self.now = 0.0
|
||||
self.watch.contact(self.now)
|
||||
|
||||
def tick(self, **kwargs):
|
||||
self.now += 1 / 30
|
||||
motion = {k: kwargs.pop(k) for k in tuple(kwargs) if k.startswith("motion_")}
|
||||
kwargs.setdefault("stamp", self.now)
|
||||
return self.watch.step(self.now, feedback(**kwargs), **motion)
|
||||
|
||||
def ready(self):
|
||||
for _ in range(15):
|
||||
result = self.tick()
|
||||
self.assertFalse(result.release)
|
||||
self.assertEqual(self.watch.phase, ReleasePhase.WATCHING)
|
||||
return result
|
||||
|
||||
def test_contact_spikes_and_ringing_do_not_release(self):
|
||||
for i in range(20):
|
||||
result = self.tick(normal=0.5 + (0.3 if i % 2 else 0))
|
||||
self.assertFalse(result.release)
|
||||
if self.now < self.config.release_min_hold_seconds:
|
||||
self.assertEqual(result.reason, "grip-settle")
|
||||
self.ready()
|
||||
|
||||
def test_contact_minimum_is_not_restarted_by_self_motion(self):
|
||||
for i in range(15):
|
||||
result = self.tick(motion_completed_at=0.06)
|
||||
if self.now < self.config.release_min_hold_seconds:
|
||||
self.assertFalse(self.watch.armed)
|
||||
self.assertTrue(self.watch.armed)
|
||||
|
||||
def test_step_then_plateau_confirms_without_second_rate_spike(self):
|
||||
self.ready()
|
||||
result = self.tick(shear=0.2)
|
||||
self.assertEqual(result.reason, "sudden")
|
||||
self.assertFalse(result.release)
|
||||
result = self.tick(shear=0.2)
|
||||
self.assertTrue(result.release)
|
||||
self.assertEqual(result.shear_rate, 0.0)
|
||||
self.assertFalse(self.tick(shear=0.2).release, "release effect must be emitted once")
|
||||
|
||||
def test_normal_unloading_and_either_side_shear(self):
|
||||
for changes in ({"normal": 0.3}, {"right_normal": 0.3},
|
||||
{"shear": 0.25}, {"right_shear": 0.25}):
|
||||
with self.subTest(changes=changes):
|
||||
self.setUp()
|
||||
self.ready()
|
||||
self.tick(**changes)
|
||||
self.assertTrue(self.tick(**changes).release)
|
||||
|
||||
def test_single_frame_pulse_and_filtered_falling_edge_do_not_release(self):
|
||||
for alpha in (1.0, 0.55, 0.65):
|
||||
with self.subTest(alpha=alpha):
|
||||
self.setUp()
|
||||
self.ready()
|
||||
filt = ExponentialFilter(alpha)
|
||||
filt.update(0.05)
|
||||
for raw in [0.85] + [0.05] * 20:
|
||||
result = self.tick(shear=filt.update(raw))
|
||||
self.assertFalse(result.release)
|
||||
self.assertTrue(self.watch.armed)
|
||||
|
||||
def test_reversal_or_channel_swap_cannot_combine_votes(self):
|
||||
self.ready()
|
||||
self.tick(shear=0.3, right_shear=0.05)
|
||||
result = self.tick(shear=0.05, right_shear=0.3)
|
||||
self.assertFalse(result.release)
|
||||
self.assertEqual(result.reason, "settling-turn")
|
||||
|
||||
def test_slow_internal_drift_never_accumulates_into_release(self):
|
||||
self.ready()
|
||||
for i in range(1, 601):
|
||||
result = self.tick(normal=0.5 + i * 0.005, shear=0.05 + i * 0.003)
|
||||
self.assertFalse(result.release)
|
||||
self.assertEqual(result.reason, "slow_internal")
|
||||
|
||||
def test_medium_rate_push_uses_separate_longer_confirmation(self):
|
||||
# Exercise the legacy weak path independently of the new touch path.
|
||||
self.config.release_min_change = 0.4
|
||||
self.ready()
|
||||
counts = []
|
||||
# A new, large but sub-sudden-rate onset followed by a plateau.
|
||||
# A large displacement accumulated along a smooth curve is no longer
|
||||
# sufficient evidence for this path.
|
||||
self.now += 0.19
|
||||
result = self.watch.step(self.now, feedback(shear=0.34, stamp=self.now))
|
||||
counts.append(result.confirm_count)
|
||||
for i in range(10):
|
||||
result = self.tick(shear=0.34)
|
||||
if result.confirm_count:
|
||||
counts.append(result.confirm_count)
|
||||
if result.release:
|
||||
break
|
||||
self.assertTrue(result.release)
|
||||
self.assertEqual(result.reason, "slow_external")
|
||||
self.assertEqual(counts, [1, 2, 3, 4, 5])
|
||||
|
||||
def test_noise_floor_rejects_tiny_high_rate_change(self):
|
||||
self.ready()
|
||||
for i in range(10):
|
||||
self.now += 0.001
|
||||
result = self.watch.step(self.now, feedback(shear=0.05 + (0.01 if i % 2 else 0), stamp=self.now))
|
||||
self.assertFalse(result.release)
|
||||
self.assertNotEqual(result.phase, ReleasePhase.CONFIRMING)
|
||||
|
||||
def test_queued_delayed_command_and_tail_are_protected(self):
|
||||
self.ready()
|
||||
for i in range(12):
|
||||
result = self.tick(normal=0.5 + i * 0.1, motion_pending=True)
|
||||
self.assertEqual(result.reason, "self-settle")
|
||||
self.assertFalse(result.release)
|
||||
completed = self.now
|
||||
for i in range(4):
|
||||
result = self.tick(normal=1.8, motion_completed_at=completed)
|
||||
self.assertFalse(result.release)
|
||||
self.assertFalse(result.allow_force_update)
|
||||
for _ in range(10):
|
||||
result = self.tick(normal=1.8, motion_completed_at=completed)
|
||||
self.assertFalse(result.release)
|
||||
self.assertTrue(result.allow_force_update)
|
||||
self.tick(normal=1.8, shear=0.3, motion_completed_at=completed)
|
||||
self.assertTrue(self.tick(normal=1.8, shear=0.3, motion_completed_at=completed).release)
|
||||
|
||||
def test_repeated_self_steps_leave_observation_window(self):
|
||||
self.ready()
|
||||
completed = None
|
||||
force = 0.5
|
||||
commands, command_times = 0, []
|
||||
for _ in range(100):
|
||||
result = self.tick(normal=force, motion_completed_at=completed)
|
||||
self.assertFalse(result.release)
|
||||
if result.allow_force_update:
|
||||
commands += 1
|
||||
command_times.append(self.now)
|
||||
force += 0.15
|
||||
completed = self.now
|
||||
self.assertGreater(commands, 3)
|
||||
self.assertTrue(all(b - a >= 0.2 for a, b in zip(command_times, command_times[1:])))
|
||||
|
||||
def test_cached_frames_and_opposite_side_updates_do_not_confirm(self):
|
||||
self.ready()
|
||||
result = self.tick(shear=0.3, right_shear=0.05)
|
||||
stamp = self.now
|
||||
for i in range(1, 5):
|
||||
self.now += 1 / 30
|
||||
sample = feedback(shear=0.3, right_shear=0.05, stamp=stamp)
|
||||
sample.right_timestamp = self.now
|
||||
result = self.watch.step(self.now, sample)
|
||||
self.assertFalse(result.release)
|
||||
self.assertEqual(result.phase, ReleasePhase.CONFIRMING)
|
||||
|
||||
def test_stale_gap_and_timestamp_restart_discard_candidate(self):
|
||||
for restarted in (False, True):
|
||||
with self.subTest(restarted=restarted):
|
||||
self.setUp()
|
||||
self.ready()
|
||||
self.tick(shear=0.3)
|
||||
self.now += 0.4
|
||||
result = self.watch.step(self.now, feedback(shear=0.3, stamp=0.0 if restarted else self.now))
|
||||
self.assertFalse(result.release)
|
||||
self.assertEqual(result.reason, "sample-gap")
|
||||
for _ in range(15):
|
||||
self.assertFalse(self.tick(shear=0.3).release)
|
||||
|
||||
def test_expired_candidate_cannot_release_on_late_second_frame(self):
|
||||
self.config.release_confirm_window_seconds = 0.05
|
||||
self.ready()
|
||||
self.tick(shear=0.3)
|
||||
self.now += 0.08
|
||||
result = self.watch.step(self.now, feedback(shear=0.3, stamp=self.now))
|
||||
self.assertFalse(result.release)
|
||||
self.assertEqual(result.reason, "confirm-expired")
|
||||
|
||||
def test_filtered_release_latency_at_30hz_and_60hz_control(self):
|
||||
for control_hz in (30, 60):
|
||||
for alpha in (0.55, 0.65):
|
||||
with self.subTest(control_hz=control_hz, alpha=alpha):
|
||||
self.setUp()
|
||||
self.config.control_hz = control_hz
|
||||
self.ready()
|
||||
filt = ExponentialFilter(alpha)
|
||||
filt.update(0.05)
|
||||
onset = self.now
|
||||
result = None
|
||||
for frame in range(6):
|
||||
stamp = self.now + 1 / 30
|
||||
filtered = filt.update(0.35)
|
||||
for _ in range(control_hz // 30):
|
||||
self.now += 1 / control_hz
|
||||
result = self.watch.step(self.now, feedback(shear=filtered, stamp=stamp))
|
||||
if result.release:
|
||||
break
|
||||
if result.release:
|
||||
break
|
||||
self.assertTrue(result.release)
|
||||
self.assertLessEqual(self.now - onset, 0.2)
|
||||
|
||||
def test_reset_discards_previous_grip(self):
|
||||
self.ready()
|
||||
self.tick(shear=0.3)
|
||||
self.watch.reset()
|
||||
self.assertFalse(self.tick(shear=0.3).release)
|
||||
self.assertEqual(self.watch.phase, ReleasePhase.DISARMED)
|
||||
self.watch.contact(self.now)
|
||||
self.assertEqual(self.tick(shear=0.3).reason, "grip-settle")
|
||||
|
||||
def test_suspend_discards_candidate_until_fresh_curve_settles(self):
|
||||
self.ready()
|
||||
self.tick(shear=0.3)
|
||||
self.watch.suspend()
|
||||
self.assertFalse(self.watch.armed)
|
||||
for _ in range(15):
|
||||
self.assertFalse(self.tick(shear=0.3).release)
|
||||
self.assertTrue(self.watch.armed)
|
||||
self.tick(shear=0.5)
|
||||
self.assertTrue(self.tick(shear=0.5).release)
|
||||
|
||||
def test_stopped_side_cannot_supply_quiet_curve_from_duplicate_frames(self):
|
||||
self.ready()
|
||||
self.tick(normal=0.8, motion_pending=True)
|
||||
stamp = self.now
|
||||
for i in range(1, 20):
|
||||
self.now += 1 / 30
|
||||
sample = feedback(normal=0.8, stamp=stamp)
|
||||
sample.right_timestamp = self.now
|
||||
result = self.watch.step(self.now, sample)
|
||||
self.assertFalse(result.release)
|
||||
self.assertFalse(self.watch.armed)
|
||||
|
||||
def test_small_continuous_noise_cannot_lock_contact_protection(self):
|
||||
for i in range(30):
|
||||
result = self.tick(normal=0.5 + (0.012 if i % 2 else -0.012))
|
||||
self.assertFalse(result.release)
|
||||
self.assertTrue(self.watch.armed)
|
||||
self.tick(normal=0.512, shear=0.13)
|
||||
self.assertTrue(self.tick(normal=0.488, shear=0.13).release)
|
||||
|
||||
def test_light_filtered_touch_below_sudden_rate_releases(self):
|
||||
self.ready()
|
||||
filt = ExponentialFilter(0.65)
|
||||
filt.update(0.05)
|
||||
onset = self.now
|
||||
for _ in range(6):
|
||||
result = self.tick(shear=filt.update(0.12))
|
||||
if result.release:
|
||||
break
|
||||
self.assertTrue(result.release)
|
||||
self.assertLessEqual(self.now - onset, 0.2 + 1e-9)
|
||||
|
||||
def test_gentle_push_is_not_absorbed_as_slow_reference_drift(self):
|
||||
self.ready()
|
||||
onset = self.now
|
||||
for i in range(1, 7):
|
||||
result = self.tick(shear=0.05 + 0.01 * i)
|
||||
if result.release:
|
||||
break
|
||||
self.assertTrue(result.release)
|
||||
self.assertLessEqual(self.now - onset, 0.2 + 1e-9)
|
||||
|
||||
def test_noise_after_force_command_does_not_extend_protection_forever(self):
|
||||
self.ready()
|
||||
completed = self.now
|
||||
for i in range(15):
|
||||
result = self.tick(normal=0.7 + (0.012 if i % 2 else -0.012),
|
||||
motion_completed_at=completed)
|
||||
self.assertFalse(result.release)
|
||||
self.assertTrue(self.watch.armed)
|
||||
|
||||
def test_side_touch_does_not_wait_for_contact_timer_or_serial(self):
|
||||
self.watch.contact(self.now, feedback())
|
||||
first = self.tick(shear=0.15, right_shear=0.05, motion_pending=True)
|
||||
self.assertEqual(first.reason, "side_touch")
|
||||
self.assertFalse(first.release)
|
||||
second = self.tick(shear=0.15, right_shear=0.05, motion_pending=True)
|
||||
self.assertTrue(second.release)
|
||||
self.assertLess(self.now, 0.1)
|
||||
self.assertLess(self.now, self.config.release_min_hold_seconds)
|
||||
|
||||
def test_bilateral_contact_shear_is_not_early_side_touch(self):
|
||||
self.watch.contact(self.now, feedback())
|
||||
for i in range(10):
|
||||
result = self.tick(normal=0.5 + i * 0.1, shear=0.05 + i * 0.06,
|
||||
motion_pending=True)
|
||||
self.assertFalse(result.release)
|
||||
|
||||
def test_normal_self_offset_is_not_replayed_after_cooldown(self):
|
||||
self.ready()
|
||||
completed = self.now
|
||||
for _ in range(15):
|
||||
result = self.tick(normal=0.8, right_normal=0.5, motion_completed_at=completed)
|
||||
self.assertFalse(result.release)
|
||||
|
||||
def test_early_side_touch_requires_fresh_both_sides(self):
|
||||
self.watch.contact(self.now, feedback())
|
||||
self.tick(shear=0.15, right_shear=0.05, motion_pending=True)
|
||||
stamp = self.now
|
||||
for _ in range(4):
|
||||
self.now += 1 / 30
|
||||
sample = feedback(shear=0.15, right_shear=0.05, stamp=stamp)
|
||||
sample.left_timestamp = self.now
|
||||
result = self.watch.step(self.now, sample, motion_pending=True)
|
||||
self.assertFalse(result.release)
|
||||
|
||||
def test_early_filtered_contact_pulse_does_not_open(self):
|
||||
for alpha in (0.55, 0.65):
|
||||
for peak in (0.85, 2.05, 8.05):
|
||||
with self.subTest(alpha=alpha, peak=peak):
|
||||
self.setUp()
|
||||
self.watch.contact(self.now, feedback())
|
||||
filt = ExponentialFilter(alpha)
|
||||
filt.update(0.05)
|
||||
for raw in [peak] + [0.05] * 20:
|
||||
result = self.tick(shear=filt.update(raw), right_shear=0.05, motion_pending=True)
|
||||
self.assertFalse(result.release)
|
||||
|
||||
def test_slow_creep_does_not_permanently_prevent_force_updates(self):
|
||||
self.ready()
|
||||
allowed = 0
|
||||
for i in range(1, 60):
|
||||
result = self.tick(normal=0.5 + 0.005 * i)
|
||||
self.assertFalse(result.release)
|
||||
allowed += result.allow_force_update
|
||||
self.assertGreater(allowed, 40)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user