diff --git a/README.md b/README.md index 00c13ee..03e7a11 100644 --- a/README.md +++ b/README.md @@ -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 帧控制快照与判据，用于复核实机误开。 diff --git a/config/compound_eye.png b/config/compound_eye.png index ba0ea4a..159c5d2 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/config/gripper.json b/config/gripper.json index f8d73a0..23c4dda 100644 --- a/config/gripper.json +++ b/config/gripper.json @@ -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, diff --git a/docs/GUI_USER_MANUAL.zh-CN.md b/docs/GUI_USER_MANUAL.zh-CN.md index 31b6c22..e4cc0c4 100644 --- a/docs/GUI_USER_MANUAL.zh-CN.md +++ b/docs/GUI_USER_MANUAL.zh-CN.md @@ -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`，源码和打包启动均适用。 diff --git a/docs/RELEASE_CONTROL.zh-CN.md b/docs/RELEASE_CONTROL.zh-CN.md new file mode 100644 index 0000000..c132ca8 --- /dev/null +++ b/docs/RELEASE_CONTROL.zh-CN.md @@ -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 参数回放仍参与回归。实机应检查接触后立即侧触、 +持续噪声下轻推、长时间夹持误开，以及不同单侧摩擦条件下自运动是否被误识别为侧触。 diff --git a/gripper/config/behavior_profiles.py b/gripper/config/behavior_profiles.py new file mode 100644 index 0000000..50bb849 --- /dev/null +++ b/gripper/config/behavior_profiles.py @@ -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 diff --git a/gripper/config/settings.py b/gripper/config/settings.py index a98b1a1..a689f2b 100644 --- a/gripper/config/settings.py +++ b/gripper/config/settings.py @@ -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) diff --git a/gripper/control/__init__.py b/gripper/control/__init__.py index ffc49c0..0f55c53 100644 --- a/gripper/control/__init__.py +++ b/gripper/control/__init__.py @@ -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", +] diff --git a/gripper/control/actuator.py b/gripper/control/actuator.py new file mode 100644 index 0000000..9b14b75 --- /dev/null +++ b/gripper/control/actuator.py @@ -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}" + ) diff --git a/gripper/control/controller.py b/gripper/control/controller.py index 3bf8588..6a2f247 100644 --- a/gripper/control/controller.py +++ b/gripper/control/controller.py @@ -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, diff --git a/gripper/control/feedback.py b/gripper/control/feedback.py index e3dd8b2..1e7a33e 100644 --- a/gripper/control/feedback.py +++ b/gripper/control/feedback.py @@ -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, ) diff --git a/gripper/control/force_curve.py b/gripper/control/force_curve.py new file mode 100644 index 0000000..fe65ad6 --- /dev/null +++ b/gripper/control/force_curve.py @@ -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" diff --git a/gripper/control/release_watch.py b/gripper/control/release_watch.py new file mode 100644 index 0000000..e88a55b --- /dev/null +++ b/gripper/control/release_watch.py @@ -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) diff --git a/gripper/control/session_recorder.py b/gripper/control/session_recorder.py new file mode 100644 index 0000000..7abce28 --- /dev/null +++ b/gripper/control/session_recorder.py @@ -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() diff --git a/gripper/sensors/camera_discovery.py b/gripper/sensors/camera_discovery.py index 53f2b94..0491abd 100644 --- a/gripper/sensors/camera_discovery.py +++ b/gripper/sensors/camera_discovery.py @@ -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: diff --git a/gripper/ui/visualizer.py b/gripper/ui/visualizer.py index 2624e26..cf2791a 100644 --- a/gripper/ui/visualizer.py +++ b/gripper/ui/visualizer.py @@ -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( diff --git a/tests/test_behavior_profiles.py b/tests/test_behavior_profiles.py new file mode 100644 index 0000000..562b757 --- /dev/null +++ b/tests/test_behavior_profiles.py @@ -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() diff --git a/tests/test_control_integration.py b/tests/test_control_integration.py new file mode 100644 index 0000000..f853b48 --- /dev/null +++ b/tests/test_control_integration.py @@ -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() diff --git a/tests/test_release_curves.py b/tests/test_release_curves.py new file mode 100644 index 0000000..4faa9ad --- /dev/null +++ b/tests/test_release_curves.py @@ -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() \ No newline at end of file diff --git a/tests/test_release_watch.py b/tests/test_release_watch.py new file mode 100644 index 0000000..2be6cbe --- /dev/null +++ b/tests/test_release_watch.py @@ -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()