开合控制调整：切向触发只看 Y 轴，三挡位只控制释放、触发固定灵敏参数；自适应负载改看切向 X；恢复原灵敏档张开逻辑与排查前夹持力参数。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
cheng
Binary file not shown.
|
Before Width: | Height: | Size: 2.5 MiB After Width: | Height: | Size: 2.5 MiB |
+12
-6
@@ -6,7 +6,7 @@
|
||||
"sensor_fps": 30.0,
|
||||
"motion_threshold": 0.0,
|
||||
"sensor_cpu": false,
|
||||
"port": "COM10",
|
||||
"port": "COM5",
|
||||
"slave_id": 1,
|
||||
"baudrate": 115200,
|
||||
"serial_timeout": 0.2,
|
||||
@@ -29,8 +29,9 @@
|
||||
"baseline_seconds": 1.0,
|
||||
"filter_alpha": 0.42,
|
||||
"shear_filter_alpha": 0.52,
|
||||
"trigger_normal_force": 0.06,
|
||||
"trigger_shear_force": 0.06,
|
||||
"shear_control_axes": "y",
|
||||
"trigger_normal_force": 0.12,
|
||||
"trigger_shear_force": 0.12,
|
||||
"trigger_clear_stable_seconds": 0.15,
|
||||
"open_wait_stable_samples": 8,
|
||||
"open_wait_stable_range_n": 0.025,
|
||||
@@ -42,7 +43,7 @@
|
||||
"force_ramp_step": 1,
|
||||
"force_ramp_interval": 0.1,
|
||||
"grip_settle_seconds": 0.2,
|
||||
"grip_behavior_mode": "stable",
|
||||
"grip_behavior_mode": "sensitive",
|
||||
"adaptive_grip_enabled": true,
|
||||
"adaptive_shear_start_force": 0.1,
|
||||
"adaptive_shear_full_force": 0.6,
|
||||
@@ -72,10 +73,15 @@
|
||||
"release_require_slope_settle": true,
|
||||
"release_slope_hold_seconds": 0.45,
|
||||
"release_settle_normal_only": true,
|
||||
"release_shear_oscillation_open": true,
|
||||
"release_shear_oscillation_window_seconds": 0.4,
|
||||
"release_shear_oscillation_min_flips": 3,
|
||||
"release_shear_oscillation_amplitude": 0.15,
|
||||
"release_shear_oscillation_confirm_samples": 2,
|
||||
"release_side_shear_enabled": true,
|
||||
"release_side_normal_enabled": false,
|
||||
"release_side_normal_enabled": true,
|
||||
"release_shear_onset_enabled": true,
|
||||
"release_normal_onset_enabled": false,
|
||||
"release_normal_onset_enabled": true,
|
||||
"rearm_seconds": 0.55,
|
||||
"recover_normal_force": 0.02,
|
||||
"recover_shear_force": 0.02,
|
||||
|
||||
@@ -1,15 +1,17 @@
|
||||
# 释放控制与回放验证
|
||||
|
||||
## 行为模式（迅速开合 / 夹持稳定）
|
||||
## 行为模式（灵敏 / 较稳定 / 十分稳定）
|
||||
|
||||
设置页「行为模式」与主界面「迅速开合 / 夹持稳定」按钮共用 `grip_behavior_mode`：
|
||||
设置里可切换三套开合预设（`grip_behavior_mode`），切换后只改写**释放/开合**相关参数；
|
||||
闭合触发（`trigger_*`、等待再武装稳定门槛）一律固定为灵敏档。
|
||||
|
||||
| 模式 | 目标 | 典型倾向 |
|
||||
| --- | --- | --- |
|
||||
| `rapid`（迅速开合） | 跟手开合 | `release_min_hold=0`、更低侧触/确认门槛、更快再武装、关闭自适应加力 |
|
||||
| `stable`（夹持稳定） | 夹得住、少误开 | 更长接触保护、更高确认帧数与速率门槛、开启自适应；**长历史斜率趋零后再允许侧触/监视** |
|
||||
| 模式 | 含义 |
|
||||
| --- | --- |
|
||||
| `sensitive`（灵敏） | 当前默认调参，跟手、更容易开合 |
|
||||
| `stable`（较稳定） | 明显提高释放门槛与确认，少误开 |
|
||||
| `firm`（十分稳定） | 约高一个数量级释放门槛，需故意大力开合 |
|
||||
|
||||
预设定义见 `gripper/config/behavior_profiles.py`。切换模式会改写释放/滤波/爬力相关字段；可在设置里再微调后保存。运行中切换会即时写入 live `config` 并重置 `ReleaseWatch`。
|
||||
预设见 `gripper/config/behavior_profiles.py`。旧配置里的 `rapid` /「迅速开合」映射为 `sensitive`。
|
||||
|
||||
## 会话录制（优化张开用）
|
||||
|
||||
@@ -38,7 +40,7 @@
|
||||
具体处理：
|
||||
|
||||
1. 闭合阶段保留最近几帧，接触时继承各通道的趋势，不把接触帧当作静止起点。
|
||||
2. 每个通道保留可配置长度的历史（`release_curve_history`；rapid 约 6 点，stable 约 30 点）。
|
||||
2. 每个通道保留可配置长度的历史（`release_curve_history`，stable 约 30 点）。
|
||||
**起始检测**仍只用最近约 6 点做局部二次拟合；**夹稳斜率**用更长窗口（`release_slope_window_seconds`）
|
||||
做一次拟合，得到 `fit_slope`（N/s）。夹住加力过程表现为该斜率逐渐变小并趋近于 0。
|
||||
3. 自然减速、变平、相近速度的斜率变号是曲线延续；它们不因累计位移达到门槛而启动释放候选。
|
||||
@@ -52,12 +54,15 @@
|
||||
后续自然变化。已达幅度的候选若明显回落则取消，记录 `settling-turn`；回落判定容许
|
||||
当前噪声范围内的波动，避免真实触碰后的轻微抖动被误拒绝。
|
||||
同一套起始判断覆盖 `side_touch`、`touch`、`sudden` 和弱路径，避免其他分支绕过。
|
||||
7. **`release_require_slope_settle`（stable 默认开）**：仅看法向通道长窗 `|fit_slope|`
|
||||
7. **`release_require_slope_settle`（默认开）**：仅看法向通道长窗 `|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 仍法向+切向都能开。
|
||||
8. **通道开关**：默认法向与切向均允许 onset 与侧触；可用
|
||||
`release_*_onset_enabled` / `release_side_*_enabled` 单独关闭。夹稳判定仍可只看法向
|
||||
（`release_settle_normal_only`）。
|
||||
9. **切向振荡旁路（默认开）**：短窗内切向反复换向且峰峰值够大时，走
|
||||
`shear_oscillation`，**不要求**已夹稳闩锁，可在 `force-settling` 期间直接确认开合。
|
||||
|
||||
没有增加固定夹稳等待，也没有恢复“四通道同时安静”的全局条件。以下是幅度与确认路径：
|
||||
|
||||
@@ -66,6 +71,7 @@
|
||||
| 即时侧触 `side_touch` | 继承接触前左右力差曲线，检测偏离该曲线的新侧触 | 默认两次左右均更新的传感器帧；不等 0.28s 夹稳计时 |
|
||||
| 普通轻触 `touch` | 出现新起始变化后，实际位移和相对原曲线的偏移均达到 0.04N | 默认两帧；不要求瞬时速率达到 1.6N/s |
|
||||
| 突变 `sudden` / 弱路径 `slow_external` | 新起始变化叠加大速率 / 大幅偏移 | 默认两帧 / 五帧 |
|
||||
| 切向振荡 `shear_oscillation` | 切向短窗内多次换向 + 峰峰值 | 默认两帧；绕过斜率夹稳门控 |
|
||||
|
||||
即时侧触使用左减右的法向/切向力差的变化，抵消两侧共同压紧或负载变化。命令排队、
|
||||
执行及短响应尾部内，法向差分仍受保护，切向差分继续检测。普通共同法向卸载仍经过
|
||||
@@ -113,17 +119,22 @@
|
||||
| `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_curve_history` | 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_require_slope_settle` | true | 是否在侧触与退出 settle 前要求斜率趋零 |
|
||||
| `release_slope_hold_seconds` | 0.45s | 斜率达标后还需保持多久（滤掉短刺） |
|
||||
| `release_settle_normal_only` | true | 夹稳只看法向，不看切向 |
|
||||
| `release_shear_oscillation_open` | true | 切向振荡时不待夹稳也开合 |
|
||||
| `release_shear_oscillation_window_seconds` | 0.4 | 切向振荡统计窗口 |
|
||||
| `release_shear_oscillation_min_flips` | 3 | 窗口内最少换向次数 |
|
||||
| `release_shear_oscillation_amplitude` | 0.15 N | 切向峰峰值门槛 |
|
||||
| `release_shear_oscillation_confirm_samples` | 2 | 振荡确认帧数 |
|
||||
| `release_side_shear_enabled` | true | 是否允许切向左右差侧触 |
|
||||
| `release_side_normal_enabled` | true / stable false | 是否允许法向左右差侧触 |
|
||||
| `release_side_normal_enabled` | true | 是否允许法向左右差侧触 |
|
||||
| `release_shear_onset_enabled` | true | 是否允许切向触发普通释放路径 |
|
||||
| `release_normal_onset_enabled` | true / stable false | 是否允许法向触发普通释放路径 |
|
||||
| `release_normal_onset_enabled` | true | 是否允许法向触发普通释放路径 |
|
||||
|
||||
N/N/s 适用于当前标定配置。缓存帧不重复滤波或投票，提高控制频率不能代替真实传感器帧。
|
||||
`grip_settle_seconds` 只控制外围状态进入 `hold_check` 的最早时间。
|
||||
|
||||
+194
-122
@@ -1,31 +1,33 @@
|
||||
"""Grip behavior presets: extreme rapid open/close vs stable hold."""
|
||||
"""Grip behavior presets: sensitive / stable / firm (open/release only)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
|
||||
|
||||
# rapid = 迅速开合：优先跟手释放 / 再武装
|
||||
# stable = 夹持稳定：优先防误开、夹得住
|
||||
GRIP_BEHAVIOR_MODES = ("rapid", "stable")
|
||||
# sensitive = 灵敏：跟手释放，当前默认调参
|
||||
# stable = 较稳定：原十分稳定档，明显更难误开
|
||||
# firm = 十分稳定：约高一个数量级门槛，故意开合才响应
|
||||
GRIP_BEHAVIOR_MODES = ("sensitive", "stable", "firm")
|
||||
|
||||
GRIP_BEHAVIOR_LABELS = {
|
||||
"rapid": "迅速开合",
|
||||
"stable": "夹持稳定",
|
||||
"sensitive": "灵敏",
|
||||
"stable": "较稳定",
|
||||
"firm": "十分稳定",
|
||||
}
|
||||
|
||||
# Keys owned by a behavior profile. Selecting a mode rewrites these fields.
|
||||
GRIP_BEHAVIOR_KEYS = (
|
||||
"filter_alpha",
|
||||
"shear_filter_alpha",
|
||||
# Close/re-arm trigger always uses the sensitive values, regardless of mode.
|
||||
GRIP_TRIGGER_KEYS = (
|
||||
"trigger_normal_force",
|
||||
"trigger_shear_force",
|
||||
"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",
|
||||
)
|
||||
|
||||
# Keys rewritten by the behavior mode. Trigger params are not included —
|
||||
# they stay locked to the sensitive profile via GRIP_TRIGGER_KEYS.
|
||||
GRIP_BEHAVIOR_KEYS = (
|
||||
"release_open_speed",
|
||||
"release_open_accel",
|
||||
"release_open_decel",
|
||||
@@ -51,6 +53,11 @@ GRIP_BEHAVIOR_KEYS = (
|
||||
"release_require_slope_settle",
|
||||
"release_slope_hold_seconds",
|
||||
"release_settle_normal_only",
|
||||
"release_shear_oscillation_open",
|
||||
"release_shear_oscillation_window_seconds",
|
||||
"release_shear_oscillation_min_flips",
|
||||
"release_shear_oscillation_amplitude",
|
||||
"release_shear_oscillation_confirm_samples",
|
||||
"release_side_shear_enabled",
|
||||
"release_side_normal_enabled",
|
||||
"release_shear_onset_enabled",
|
||||
@@ -59,136 +66,201 @@ GRIP_BEHAVIOR_KEYS = (
|
||||
"recover_stable_seconds",
|
||||
)
|
||||
|
||||
_SENSITIVE_TRIGGER = {
|
||||
# Close trigger must stay above force-display noise (~0.05N).
|
||||
"trigger_normal_force": 0.12,
|
||||
"trigger_shear_force": 0.12,
|
||||
"open_wait_stable_samples": 8,
|
||||
"open_wait_stable_range_n": 0.025,
|
||||
"open_wait_stable_trend_n": 0.015,
|
||||
}
|
||||
|
||||
_SENSITIVE_PROFILE = {
|
||||
**_SENSITIVE_TRIGGER,
|
||||
"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,
|
||||
"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,
|
||||
# Intentional rub/twist: oscillating shear opens even before plateau latch.
|
||||
"release_shear_oscillation_open": True,
|
||||
"release_shear_oscillation_window_seconds": 0.4,
|
||||
"release_shear_oscillation_min_flips": 3,
|
||||
"release_shear_oscillation_amplitude": 0.15,
|
||||
"release_shear_oscillation_confirm_samples": 2,
|
||||
"release_side_shear_enabled": True,
|
||||
"release_side_normal_enabled": True,
|
||||
"release_shear_onset_enabled": True,
|
||||
"release_normal_onset_enabled": True,
|
||||
"rearm_seconds": 0.55,
|
||||
"recover_stable_seconds": 0.55,
|
||||
}
|
||||
|
||||
_STABLE_PROFILE = {
|
||||
"release_open_speed": 45,
|
||||
"release_open_accel": 60,
|
||||
"release_open_decel": 60,
|
||||
"release_arm_delay_seconds": 0.55,
|
||||
"release_min_hold_seconds": 1.2,
|
||||
"release_settle_seconds": 0.22,
|
||||
"release_min_change": 0.16,
|
||||
"release_touch_window_seconds": 0.22,
|
||||
"release_side_touch_change": 0.24,
|
||||
"release_confirm_window_seconds": 0.28,
|
||||
"release_sample_timeout_seconds": 0.3,
|
||||
"release_confirm_samples": 8,
|
||||
"release_slow_confirm_samples": 12,
|
||||
"release_normal_rate": 4.0,
|
||||
"release_shear_rate": 3.6,
|
||||
"release_slow_rate": 0.14,
|
||||
"release_normal_change": 0.7,
|
||||
"release_shear_change": 0.6,
|
||||
"release_curve_history": 42,
|
||||
"release_slope_window_seconds": 1.2,
|
||||
"release_slope_settle_rate": 0.12,
|
||||
"release_slope_min_points": 14,
|
||||
"release_require_slope_settle": True,
|
||||
"release_slope_hold_seconds": 0.9,
|
||||
"release_settle_normal_only": True,
|
||||
"release_shear_oscillation_open": True,
|
||||
"release_shear_oscillation_window_seconds": 0.6,
|
||||
"release_shear_oscillation_min_flips": 5,
|
||||
"release_shear_oscillation_amplitude": 0.3,
|
||||
"release_shear_oscillation_confirm_samples": 4,
|
||||
"release_side_shear_enabled": True,
|
||||
"release_side_normal_enabled": True,
|
||||
"release_shear_onset_enabled": True,
|
||||
"release_normal_onset_enabled": True,
|
||||
"rearm_seconds": 0.9,
|
||||
"recover_stable_seconds": 0.9,
|
||||
}
|
||||
|
||||
# Roughly ~10x harder to trip open than sensitive; intentional open needs clear force.
|
||||
_FIRM_PROFILE = {
|
||||
"release_open_speed": 35,
|
||||
"release_open_accel": 50,
|
||||
"release_open_decel": 50,
|
||||
"release_arm_delay_seconds": 1.2,
|
||||
"release_min_hold_seconds": 3.0,
|
||||
"release_settle_seconds": 0.5,
|
||||
"release_min_change": 0.8,
|
||||
"release_touch_window_seconds": 0.35,
|
||||
"release_side_touch_change": 1.2,
|
||||
"release_confirm_window_seconds": 0.5,
|
||||
"release_sample_timeout_seconds": 0.4,
|
||||
"release_confirm_samples": 12,
|
||||
"release_slow_confirm_samples": 18,
|
||||
"release_normal_rate": 20.0,
|
||||
"release_shear_rate": 18.0,
|
||||
"release_slow_rate": 0.08,
|
||||
"release_normal_change": 4.0,
|
||||
"release_shear_change": 3.5,
|
||||
"release_curve_history": 60,
|
||||
"release_slope_window_seconds": 2.0,
|
||||
"release_slope_settle_rate": 0.06,
|
||||
"release_slope_min_points": 20,
|
||||
"release_require_slope_settle": True,
|
||||
"release_slope_hold_seconds": 2.0,
|
||||
"release_settle_normal_only": True,
|
||||
# Keep path available, but require large rub/twist evidence.
|
||||
"release_shear_oscillation_open": True,
|
||||
"release_shear_oscillation_window_seconds": 0.8,
|
||||
"release_shear_oscillation_min_flips": 6,
|
||||
"release_shear_oscillation_amplitude": 1.5,
|
||||
"release_shear_oscillation_confirm_samples": 6,
|
||||
"release_side_shear_enabled": True,
|
||||
"release_side_normal_enabled": True,
|
||||
"release_shear_onset_enabled": True,
|
||||
"release_normal_onset_enabled": True,
|
||||
"rearm_seconds": 1.5,
|
||||
"recover_stable_seconds": 1.5,
|
||||
}
|
||||
|
||||
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,
|
||||
},
|
||||
"sensitive": _SENSITIVE_PROFILE,
|
||||
"stable": _STABLE_PROFILE,
|
||||
"firm": _FIRM_PROFILE,
|
||||
}
|
||||
|
||||
|
||||
def normalize_grip_behavior_mode(mode) -> str:
|
||||
"""Resolve aliases to sensitive / stable / firm; default sensitive."""
|
||||
text = str(mode or "").strip().lower()
|
||||
aliases = {
|
||||
"fast": "rapid",
|
||||
"quick": "rapid",
|
||||
"snap": "rapid",
|
||||
"迅速": "rapid",
|
||||
"迅速开合": "rapid",
|
||||
"hold": "stable",
|
||||
"firm": "stable",
|
||||
"secure": "stable",
|
||||
"fast": "sensitive",
|
||||
"quick": "sensitive",
|
||||
"snap": "sensitive",
|
||||
"rapid": "sensitive",
|
||||
"responsive": "sensitive",
|
||||
"灵敏": "sensitive",
|
||||
"灵敏开合": "sensitive",
|
||||
"迅速": "sensitive",
|
||||
"迅速开合": "sensitive",
|
||||
"sensitive": "sensitive",
|
||||
"稳定": "stable",
|
||||
"较稳定": "stable",
|
||||
"夹持稳定": "stable",
|
||||
"stable": "stable",
|
||||
"hold": "firm",
|
||||
"firm": "firm",
|
||||
"secure": "firm",
|
||||
"very_stable": "firm",
|
||||
"verystable": "firm",
|
||||
"十分稳定": "firm",
|
||||
"极度稳定": "firm",
|
||||
}
|
||||
text = aliases.get(text, text)
|
||||
if text in aliases:
|
||||
return aliases[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 "sensitive"
|
||||
return text
|
||||
|
||||
|
||||
def grip_behavior_profile(mode: str) -> dict:
|
||||
def _merged_profile(mode: str) -> dict:
|
||||
"""Release/open params from *mode*, trigger params always from sensitive."""
|
||||
mode = normalize_grip_behavior_mode(mode)
|
||||
return copy.deepcopy(GRIP_BEHAVIOR_PROFILES[mode])
|
||||
profile = {
|
||||
key: copy.deepcopy(GRIP_BEHAVIOR_PROFILES[mode][key])
|
||||
for key in GRIP_BEHAVIOR_KEYS
|
||||
if key in GRIP_BEHAVIOR_PROFILES[mode]
|
||||
}
|
||||
for key in GRIP_TRIGGER_KEYS:
|
||||
profile[key] = copy.deepcopy(_SENSITIVE_TRIGGER[key])
|
||||
return profile
|
||||
|
||||
|
||||
def apply_grip_behavior_profile(target, mode: str) -> str:
|
||||
"""Write profile fields onto a config object or dict. Returns normalized mode."""
|
||||
def grip_behavior_profile(mode: str = "sensitive") -> dict:
|
||||
return _merged_profile(mode)
|
||||
|
||||
|
||||
def apply_grip_behavior_profile(target, mode: str = "sensitive") -> str:
|
||||
mode = normalize_grip_behavior_mode(mode)
|
||||
profile = GRIP_BEHAVIOR_PROFILES[mode]
|
||||
profile = _merged_profile(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):
|
||||
|
||||
@@ -68,6 +68,8 @@ class Demo02Config:
|
||||
baseline_seconds: float = 1.0
|
||||
filter_alpha: float = 0.55
|
||||
shear_filter_alpha: float = 0.65
|
||||
# Shear magnitude used for trigger/adaptive/release: "xy" | "x" | "y".
|
||||
shear_control_axes: str = "y"
|
||||
|
||||
trigger_normal_force: float = 0.04
|
||||
trigger_shear_force: float = 0.04
|
||||
@@ -82,7 +84,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"
|
||||
grip_behavior_mode: str = "sensitive"
|
||||
adaptive_grip_enabled: bool = True
|
||||
adaptive_shear_start_force: float = 0.10
|
||||
adaptive_shear_full_force: float = 0.60
|
||||
@@ -112,6 +114,11 @@ class Demo02Config:
|
||||
release_require_slope_settle: bool = False
|
||||
release_slope_hold_seconds: float = 0.0
|
||||
release_settle_normal_only: bool = True
|
||||
release_shear_oscillation_open: bool = False
|
||||
release_shear_oscillation_window_seconds: float = 0.4
|
||||
release_shear_oscillation_min_flips: int = 3
|
||||
release_shear_oscillation_amplitude: float = 0.15
|
||||
release_shear_oscillation_confirm_samples: int = 2
|
||||
release_side_shear_enabled: bool = True
|
||||
release_side_normal_enabled: bool = True
|
||||
release_shear_onset_enabled: bool = True
|
||||
@@ -184,6 +191,12 @@ class Demo02Config:
|
||||
add("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds))
|
||||
add("--filter-alpha", type=float, default=get("filter_alpha", cls.filter_alpha))
|
||||
add("--shear-filter-alpha", type=float, default=get("shear_filter_alpha", cls.shear_filter_alpha))
|
||||
add(
|
||||
"--shear-control-axes",
|
||||
type=str,
|
||||
choices=["xy", "x", "y"],
|
||||
default=str(get("shear_control_axes", cls.shear_control_axes)).strip().lower() or "y",
|
||||
)
|
||||
|
||||
add("--trigger-normal-force", type=float, default=get("trigger_normal_force", cls.trigger_normal_force))
|
||||
add("--trigger-shear-force", type=float, default=get("trigger_shear_force", cls.trigger_shear_force))
|
||||
@@ -202,7 +215,7 @@ class Demo02Config:
|
||||
add(
|
||||
"--grip-behavior-mode",
|
||||
type=str,
|
||||
choices=["rapid", "stable"],
|
||||
choices=["sensitive", "stable", "firm"],
|
||||
default=normalize_grip_behavior_mode(
|
||||
get("grip_behavior_mode", cls.grip_behavior_mode)
|
||||
),
|
||||
@@ -241,6 +254,17 @@ class Demo02Config:
|
||||
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-shear-oscillation-open", dest="release_shear_oscillation_open", action="store_true",
|
||||
default=get("release_shear_oscillation_open", cls.release_shear_oscillation_open))
|
||||
add("--no-release-shear-oscillation-open", dest="release_shear_oscillation_open", action="store_false")
|
||||
add("--release-shear-oscillation-window-seconds", type=float,
|
||||
default=get("release_shear_oscillation_window_seconds", cls.release_shear_oscillation_window_seconds))
|
||||
add("--release-shear-oscillation-min-flips", type=int,
|
||||
default=get("release_shear_oscillation_min_flips", cls.release_shear_oscillation_min_flips))
|
||||
add("--release-shear-oscillation-amplitude", type=float,
|
||||
default=get("release_shear_oscillation_amplitude", cls.release_shear_oscillation_amplitude))
|
||||
add("--release-shear-oscillation-confirm-samples", type=int,
|
||||
default=get("release_shear_oscillation_confirm_samples", cls.release_shear_oscillation_confirm_samples))
|
||||
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")
|
||||
|
||||
@@ -26,7 +26,7 @@ _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):
|
||||
def __init__(self, gripper, position_poll_hz: float = 30.0):
|
||||
self.gripper = gripper
|
||||
self.position_poll_hz = max(1.0, float(position_poll_hz))
|
||||
self._stop_event = threading.Event()
|
||||
@@ -40,6 +40,10 @@ class ActuatorWorker:
|
||||
self._motion_completed_at: Optional[float] = None
|
||||
self._wake = threading.Event()
|
||||
self._busy = threading.Event()
|
||||
# After a motion command, poll faster so the 3D view tracks travel.
|
||||
self._boost_poll_until = 0.0
|
||||
self._boost_poll_hz = 50.0
|
||||
self._boost_poll_seconds = 2.5
|
||||
|
||||
self._state_lock = threading.Lock()
|
||||
self._position: Optional[int] = None
|
||||
@@ -225,7 +229,6 @@ class ActuatorWorker:
|
||||
return
|
||||
|
||||
self._started.set()
|
||||
poll_interval = 1.0 / self.position_poll_hz
|
||||
next_poll = time.perf_counter()
|
||||
self._perf_last_log = time.perf_counter()
|
||||
|
||||
@@ -251,7 +254,7 @@ class ActuatorWorker:
|
||||
now = time.perf_counter()
|
||||
if now >= next_poll:
|
||||
self._poll_position()
|
||||
next_poll = now + poll_interval
|
||||
next_poll = now + self._current_poll_interval(now)
|
||||
self._maybe_log_perf(now)
|
||||
continue
|
||||
|
||||
@@ -259,6 +262,16 @@ class ActuatorWorker:
|
||||
self._wake.wait(timeout=wait_s)
|
||||
self._wake.clear()
|
||||
|
||||
def _current_poll_interval(self, now: float) -> float:
|
||||
if now < self._boost_poll_until:
|
||||
return 1.0 / self._boost_poll_hz
|
||||
return 1.0 / self.position_poll_hz
|
||||
|
||||
def _note_motion_command(self) -> None:
|
||||
"""Prefer measured travel over commanded target for position cache."""
|
||||
self._boost_poll_until = time.perf_counter() + self._boost_poll_seconds
|
||||
self._poll_position()
|
||||
|
||||
def _execute(self, cmd: GripCommand) -> None:
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
@@ -270,7 +283,7 @@ class ActuatorWorker:
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._update_position(self.gripper.last_commanded_position)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "close":
|
||||
self.gripper.close(
|
||||
cmd.force_pct,
|
||||
@@ -279,7 +292,7 @@ class ActuatorWorker:
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._update_position(self.gripper.last_commanded_position)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "move":
|
||||
self.gripper.move(
|
||||
cmd.position,
|
||||
@@ -289,7 +302,7 @@ class ActuatorWorker:
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._update_position(cmd.position)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "set_force":
|
||||
self.gripper.set_force(cmd.force_pct)
|
||||
elif cmd.kind == "hold":
|
||||
@@ -303,9 +316,10 @@ class ActuatorWorker:
|
||||
self._hold_pos = hold_pos
|
||||
if hold_pos is not None:
|
||||
self._position = int(hold_pos)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "final_open":
|
||||
self.gripper.final_open_if_needed()
|
||||
self._update_position(self.gripper.last_commanded_position)
|
||||
self._note_motion_command()
|
||||
else:
|
||||
self._set_error(f"unknown command kind: {cmd.kind}")
|
||||
except Exception as exc:
|
||||
|
||||
@@ -78,7 +78,7 @@ class SideTriggerGripController:
|
||||
self._control_mode = mode
|
||||
|
||||
def set_grip_behavior_mode(self, mode):
|
||||
"""Apply rapid/stable preset onto live config and reset release watch."""
|
||||
"""Apply sensitive/stable/firm grip preset onto live config and reset release watch."""
|
||||
applied = self.config.apply_grip_behavior_mode(mode)
|
||||
# Filters should match the new profile immediately.
|
||||
try:
|
||||
@@ -112,6 +112,7 @@ class SideTriggerGripController:
|
||||
"control_hz",
|
||||
"filter_alpha",
|
||||
"shear_filter_alpha",
|
||||
"shear_control_axes",
|
||||
"trigger_normal_force",
|
||||
"trigger_shear_force",
|
||||
"grip_normal_force",
|
||||
@@ -143,7 +144,7 @@ class SideTriggerGripController:
|
||||
def run(self):
|
||||
self._stop_event.clear()
|
||||
self.reader.start()
|
||||
self.actuator = ActuatorWorker(self.gripper, position_poll_hz=10.0)
|
||||
self.actuator = ActuatorWorker(self.gripper, position_poll_hz=30.0)
|
||||
|
||||
try:
|
||||
self.actuator.start()
|
||||
@@ -812,6 +813,11 @@ class SideTriggerGripController:
|
||||
return stable
|
||||
|
||||
def _open_wait_delta_triggered(self, sample, stable_values):
|
||||
"""Arm close only when a channel rises enough above its quiet baseline.
|
||||
|
||||
Stable mode previously used 0.06N which sits inside typical optical-flow
|
||||
residual noise and caused spontaneous side-trigger-close.
|
||||
"""
|
||||
thresholds = {
|
||||
"left_normal": self.config.trigger_normal_force,
|
||||
"right_normal": self.config.trigger_normal_force,
|
||||
@@ -844,6 +850,16 @@ class SideTriggerGripController:
|
||||
<= int(self.config.open_position_tolerance)
|
||||
)
|
||||
|
||||
def _load_shear(self, feedback):
|
||||
"""Payload/gravity estimate uses shear X; trigger may stay on Y."""
|
||||
left = abs(float(getattr(feedback, "left_shear_x", 0.0) or 0.0))
|
||||
right = abs(float(getattr(feedback, "right_shear_x", 0.0) or 0.0))
|
||||
magnitude = max(left, right)
|
||||
if magnitude > 0.0:
|
||||
return magnitude
|
||||
# Magnitude-mode fallback (no per-axis components).
|
||||
return max(0.0, float(getattr(feedback, "max_shear", 0.0) or 0.0))
|
||||
|
||||
def _adaptive_force_target(self, feedback):
|
||||
config = self.config
|
||||
if not config.adaptive_grip_enabled:
|
||||
@@ -864,7 +880,7 @@ class SideTriggerGripController:
|
||||
|
||||
shear_start = max(0.0, float(config.adaptive_shear_start_force))
|
||||
shear_full = max(shear_start + 1e-6, float(config.adaptive_shear_full_force))
|
||||
shear = max(0.0, float(feedback.max_shear))
|
||||
shear = self._load_shear(feedback)
|
||||
if shear <= shear_start:
|
||||
return force_min
|
||||
if shear >= shear_full:
|
||||
@@ -1108,7 +1124,7 @@ class SideTriggerGripController:
|
||||
"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"),
|
||||
"grip_behavior_mode": getattr(self.config, "grip_behavior_mode", "sensitive"),
|
||||
"speed_pct": speed_pct,
|
||||
"gripper_position": gripper_position,
|
||||
"open_pos": self.config.open_pos,
|
||||
|
||||
@@ -4,8 +4,33 @@ import time
|
||||
from ..common.filters import ExponentialFilter
|
||||
|
||||
|
||||
def shear_magnitude(sample):
|
||||
return (float(sample["fshearx"]) ** 2 + float(sample["fsheary"]) ** 2) ** 0.5
|
||||
def _normalize_shear_axes(axes):
|
||||
value = str(axes or "xy").strip().lower()
|
||||
if value in {"y", "fy", "sheary", "fsheary"}:
|
||||
return "y"
|
||||
if value in {"x", "fx", "shearx", "fshearx"}:
|
||||
return "x"
|
||||
return "xy"
|
||||
|
||||
|
||||
def shear_magnitude(sample, axes="xy"):
|
||||
axes = _normalize_shear_axes(axes)
|
||||
shear_x = float(sample["fshearx"])
|
||||
shear_y = float(sample["fsheary"])
|
||||
if axes == "y":
|
||||
return abs(shear_y)
|
||||
if axes == "x":
|
||||
return abs(shear_x)
|
||||
return (shear_x ** 2 + shear_y ** 2) ** 0.5
|
||||
|
||||
|
||||
def combine_shear_components(shear_x, shear_y, axes="xy"):
|
||||
axes = _normalize_shear_axes(axes)
|
||||
if axes == "y":
|
||||
return abs(float(shear_y))
|
||||
if axes == "x":
|
||||
return abs(float(shear_x))
|
||||
return (float(shear_x) ** 2 + float(shear_y) ** 2) ** 0.5
|
||||
|
||||
|
||||
def _component_shear_enabled(converter):
|
||||
@@ -70,12 +95,16 @@ class TactileFeedbackProcessor:
|
||||
return filter_.update(value)
|
||||
return filter_.value
|
||||
|
||||
def _shear_axes(self):
|
||||
return _normalize_shear_axes(getattr(self.config, "shear_control_axes", "xy"))
|
||||
|
||||
def _convert_shear(self, sample):
|
||||
axes = self._shear_axes()
|
||||
if _component_shear_enabled(self.shear_converter):
|
||||
shear_x = self.shear_converter.convert_component(sample["fshearx"])
|
||||
shear_y = self.shear_converter.convert_component(sample["fsheary"])
|
||||
return (shear_x ** 2 + shear_y ** 2) ** 0.5, shear_x, shear_y
|
||||
return self.shear_converter.convert(shear_magnitude(sample)), 0.0, 0.0
|
||||
return combine_shear_components(shear_x, shear_y, axes), shear_x, shear_y
|
||||
return self.shear_converter.convert(shear_magnitude(sample, axes)), 0.0, 0.0
|
||||
|
||||
def calibrate_baseline(self, reader):
|
||||
seconds = self.config.baseline_seconds
|
||||
@@ -133,6 +162,7 @@ class TactileFeedbackProcessor:
|
||||
0.0,
|
||||
self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal,
|
||||
)
|
||||
axes = self._shear_axes()
|
||||
if _component_shear_enabled(self.shear_converter):
|
||||
_, raw_left_shear_x, raw_left_shear_y = self._convert_shear(left)
|
||||
_, raw_right_shear_x, raw_right_shear_y = self._convert_shear(right)
|
||||
@@ -140,17 +170,17 @@ class TactileFeedbackProcessor:
|
||||
left_shear_y = raw_left_shear_y - self.baseline.left_shear_y
|
||||
right_shear_x = raw_right_shear_x - self.baseline.right_shear_x
|
||||
right_shear_y = raw_right_shear_y - self.baseline.right_shear_y
|
||||
left_shear = (left_shear_x ** 2 + left_shear_y ** 2) ** 0.5
|
||||
right_shear = (right_shear_x ** 2 + right_shear_y ** 2) ** 0.5
|
||||
left_shear = combine_shear_components(left_shear_x, left_shear_y, axes)
|
||||
right_shear = combine_shear_components(right_shear_x, right_shear_y, axes)
|
||||
else:
|
||||
left_shear_x = left_shear_y = right_shear_x = right_shear_y = 0.0
|
||||
left_shear = max(
|
||||
0.0,
|
||||
self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear,
|
||||
self.shear_converter.convert(shear_magnitude(left, axes)) - self.baseline.left_shear,
|
||||
)
|
||||
right_shear = max(
|
||||
0.0,
|
||||
self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear,
|
||||
self.shear_converter.convert(shear_magnitude(right, axes)) - self.baseline.right_shear,
|
||||
)
|
||||
|
||||
left_timestamp, right_timestamp = left.get("timestamp"), right.get("timestamp")
|
||||
|
||||
@@ -241,6 +241,8 @@ class ReleaseWatch:
|
||||
self._tails = {}
|
||||
self._watch_since = None
|
||||
self._candidate = None
|
||||
self._osc_count = 0
|
||||
self._osc_channel = ""
|
||||
self._last_fresh = {}
|
||||
self._rates = {}
|
||||
self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
|
||||
@@ -330,6 +332,74 @@ class ReleaseWatch:
|
||||
return True
|
||||
return False
|
||||
|
||||
def _shear_oscillation(self, now):
|
||||
"""Detect repeated shear direction flips with enough peak-to-peak swing.
|
||||
|
||||
Intentional open/close often shows as left/right shear oscillating while
|
||||
normals stay flat and the grip has not yet latched a plateau. Returns
|
||||
the strongest oscillating shear channel name, or "".
|
||||
"""
|
||||
if not getattr(self.config, "release_shear_oscillation_open", False):
|
||||
return ""
|
||||
if not getattr(self.config, "release_shear_onset_enabled", True):
|
||||
return ""
|
||||
window = max(0.12, float(getattr(self.config, "release_shear_oscillation_window_seconds", 0.4) or 0.4))
|
||||
min_flips = max(2, int(getattr(self.config, "release_shear_oscillation_min_flips", 3) or 3))
|
||||
amplitude = max(0.04, float(getattr(self.config, "release_shear_oscillation_amplitude", 0.15) or 0.15))
|
||||
step_floor = max(0.02, amplitude * 0.2)
|
||||
best_key, best_flips, best_span = "", 0, 0.0
|
||||
cutoff = now - window
|
||||
for key in CHANNELS[2:]:
|
||||
points = [(t, v) for t, v in self._curves[key].history if t >= cutoff]
|
||||
if len(points) < min_flips + 2:
|
||||
continue
|
||||
values = [v for _, v in points]
|
||||
span = max(values) - min(values)
|
||||
if span + 1e-9 < amplitude:
|
||||
continue
|
||||
flips = 0
|
||||
prev_sign = 0
|
||||
for i in range(1, len(points)):
|
||||
delta = points[i][1] - points[i - 1][1]
|
||||
if abs(delta) < step_floor:
|
||||
continue
|
||||
sign = 1 if delta > 0 else -1
|
||||
if prev_sign and sign != prev_sign:
|
||||
flips += 1
|
||||
prev_sign = sign
|
||||
if flips >= min_flips and (flips > best_flips or (flips == best_flips and span > best_span)):
|
||||
best_key, best_flips, best_span = key, flips, span
|
||||
return best_key
|
||||
|
||||
def _shear_oscillation_release(self, now, metrics):
|
||||
"""Confirm oscillating shear and release without waiting for plateau."""
|
||||
channel = self._shear_oscillation(now)
|
||||
if not channel:
|
||||
self._osc_count = 0
|
||||
self._osc_channel = ""
|
||||
return None
|
||||
needed = max(2, int(getattr(self.config, "release_shear_oscillation_confirm_samples", 2) or 2))
|
||||
if self._osc_channel != channel:
|
||||
self._osc_channel = channel
|
||||
self._osc_count = 1
|
||||
else:
|
||||
self._osc_count += 1
|
||||
released = self._osc_count >= needed
|
||||
if released:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
else:
|
||||
self.phase = ReleasePhase.CONFIRMING
|
||||
self._candidate = None
|
||||
return ReleaseDecision(
|
||||
self.phase,
|
||||
"shear_oscillation",
|
||||
release=released,
|
||||
confirm_count=self._osc_count,
|
||||
confirm_needed=needed,
|
||||
channel=channel,
|
||||
**dict(metrics, curve_motion="shear_oscillation"),
|
||||
)
|
||||
|
||||
def _protect(self, reason, values, **metrics):
|
||||
self.phase = (
|
||||
ReleasePhase.SELF_SETTLE if self._grip_settled
|
||||
@@ -337,6 +407,8 @@ class ReleaseWatch:
|
||||
)
|
||||
self._reference.update(values)
|
||||
self._candidate = None
|
||||
self._osc_count = 0
|
||||
self._osc_channel = ""
|
||||
self._watch_since = None
|
||||
self._settle_seen.clear()
|
||||
for curve in self._curves.values():
|
||||
@@ -398,6 +470,11 @@ class ReleaseWatch:
|
||||
"settling_turn" if "settling_turn" in motions else
|
||||
"settling_flatten" if "settling_flatten" in motions else "curve_follow")
|
||||
|
||||
# Shear oscillation = intentional open/close: bypass plateau / settle wait.
|
||||
osc_decision = self._shear_oscillation_release(now, metrics)
|
||||
if osc_decision is not None:
|
||||
return osc_decision
|
||||
|
||||
self_tail = (
|
||||
motion_completed_at is not None
|
||||
and now - motion_completed_at < max(0.0, cfg.release_arm_delay_seconds)
|
||||
|
||||
@@ -3,10 +3,13 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import contextlib
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import subprocess
|
||||
import sys
|
||||
from dataclasses import dataclass
|
||||
from dataclasses import dataclass, replace
|
||||
|
||||
import cv2
|
||||
|
||||
@@ -15,6 +18,12 @@ import cv2
|
||||
# prevents a search from spending seconds probing empty DirectShow devices.
|
||||
DEFAULT_MAX_CAMERA_INDEX = 3
|
||||
|
||||
# Gripper tactile sensors on this project: SunplusIT UVC "USB Camera".
|
||||
SENSOR_FRIENDLY_NAME = "USB Camera"
|
||||
SENSOR_VENDOR_NAME = "SunplusIT Inc"
|
||||
SENSOR_NAME_NEEDLE = "usb camera"
|
||||
SENSOR_VENDOR_NEEDLES = ("sunplusit", "1bcf", "vid_1bcf")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraInfo:
|
||||
@@ -23,6 +32,11 @@ class CameraInfo:
|
||||
backend_api: int
|
||||
width: int
|
||||
height: int
|
||||
friendly_name: str = ""
|
||||
manufacturer: str = ""
|
||||
vid_pid: str = ""
|
||||
location: str = ""
|
||||
instance_id: str = ""
|
||||
|
||||
@property
|
||||
def source(self):
|
||||
@@ -31,9 +45,15 @@ class CameraInfo:
|
||||
return str(self.index)
|
||||
|
||||
def label(self):
|
||||
device = self.source if sys.platform.startswith("linux") else f"相机 {self.index}"
|
||||
device = self.source if sys.platform.startswith("linux") else f"ID {self.index}"
|
||||
name = self.friendly_name or "Camera"
|
||||
vendor = self.manufacturer or self.vid_pid
|
||||
meta = f"{name}"
|
||||
if vendor:
|
||||
meta = f"{meta} / {vendor}"
|
||||
resolution = f" {self.width}x{self.height}" if self.width and self.height else ""
|
||||
return f"{device}{resolution} [{self.backend_name}]"
|
||||
where = f" {self.location}" if self.location else ""
|
||||
return f"{device} {meta}{resolution}{where} [{self.backend_name}]"
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
@@ -80,10 +100,11 @@ def _suppress_opencv_noise():
|
||||
def _backend_candidates():
|
||||
candidates = []
|
||||
if sys.platform == "win32":
|
||||
if hasattr(cv2, "CAP_DSHOW"):
|
||||
candidates.append(("DSHOW", cv2.CAP_DSHOW))
|
||||
# MSMF currently opens the SunplusIT sensors more reliably than DSHOW.
|
||||
if hasattr(cv2, "CAP_MSMF"):
|
||||
candidates.append(("MSMF", cv2.CAP_MSMF))
|
||||
if hasattr(cv2, "CAP_DSHOW"):
|
||||
candidates.append(("DSHOW", cv2.CAP_DSHOW))
|
||||
elif sys.platform.startswith("linux") and hasattr(cv2, "CAP_V4L2"):
|
||||
candidates.append(("V4L2", cv2.CAP_V4L2))
|
||||
return candidates or [("ANY", cv2.CAP_ANY)]
|
||||
@@ -113,16 +134,122 @@ def _candidate_indices(max_index):
|
||||
return list(range(max_index + 1))
|
||||
|
||||
|
||||
def _parse_vid_pid(text):
|
||||
match = re.search(r"VID_([0-9A-Fa-f]{4}).*PID_([0-9A-Fa-f]{4})", str(text or ""))
|
||||
if not match:
|
||||
match = re.search(r"\b([0-9A-Fa-f]{4}):([0-9A-Fa-f]{4})\b", str(text or ""))
|
||||
if not match:
|
||||
return ""
|
||||
return f"{match.group(1).upper()}:{match.group(2).upper()}"
|
||||
|
||||
|
||||
def _is_sensor_device(friendly_name="", manufacturer="", hardware_ids="", bus_reported=""):
|
||||
blob = " ".join(
|
||||
str(part or "") for part in (friendly_name, manufacturer, hardware_ids, bus_reported)
|
||||
).lower()
|
||||
name_ok = SENSOR_NAME_NEEDLE in str(friendly_name or bus_reported or "").lower()
|
||||
vendor_ok = any(needle in blob for needle in SENSOR_VENDOR_NEEDLES)
|
||||
return name_ok and vendor_ok
|
||||
|
||||
|
||||
def list_windows_sensor_devices():
|
||||
"""Return Status=OK Windows Camera devices matching the tactile sensors."""
|
||||
if sys.platform != "win32":
|
||||
return []
|
||||
# Keep this query narrow and property-light: startup calls it on the UI thread
|
||||
# via a short timer, so multi-second usbipd / full-property scans freeze the GUI.
|
||||
script = r"""
|
||||
$ErrorActionPreference = 'SilentlyContinue'
|
||||
$items = @()
|
||||
Get-PnpDevice -Class Camera -Status OK |
|
||||
Where-Object {
|
||||
$_.FriendlyName -eq 'USB Camera' -and
|
||||
$_.InstanceId -like '*VID_1BCF&PID_2281*'
|
||||
} |
|
||||
ForEach-Object {
|
||||
$id = $_.InstanceId
|
||||
$loc = (Get-PnpDeviceProperty -InstanceId $id -KeyName 'DEVPKEY_Device_LocationInfo').Data
|
||||
$addr = (Get-PnpDeviceProperty -InstanceId $id -KeyName 'DEVPKEY_Device_Address').Data
|
||||
$items += [pscustomobject]@{
|
||||
FriendlyName = [string]$_.FriendlyName
|
||||
Location = [string]$loc
|
||||
Address = [string]$addr
|
||||
InstanceId = [string]$id
|
||||
Status = [string]$_.Status
|
||||
}
|
||||
}
|
||||
$items | ConvertTo-Json -Depth 4 -Compress
|
||||
"""
|
||||
try:
|
||||
raw = subprocess.check_output(
|
||||
["powershell", "-NoProfile", "-ExecutionPolicy", "Bypass", "-Command", script],
|
||||
text=True,
|
||||
encoding="utf-8",
|
||||
errors="replace",
|
||||
timeout=20,
|
||||
)
|
||||
devices = json.loads(raw) if raw.strip() else []
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
if isinstance(devices, dict):
|
||||
devices = [devices]
|
||||
|
||||
matched = []
|
||||
for item in devices:
|
||||
friendly = str(item.get("FriendlyName") or SENSOR_FRIENDLY_NAME)
|
||||
instance_id = str(item.get("InstanceId") or "")
|
||||
if SENSOR_NAME_NEEDLE not in friendly.lower():
|
||||
continue
|
||||
if "VID_1BCF" not in instance_id.upper() or "PID_2281" not in instance_id.upper():
|
||||
continue
|
||||
address = item.get("Address")
|
||||
try:
|
||||
address_value = int(str(address))
|
||||
except (TypeError, ValueError):
|
||||
address_value = 10**9
|
||||
matched.append(
|
||||
{
|
||||
"friendly_name": friendly or SENSOR_FRIENDLY_NAME,
|
||||
"manufacturer": SENSOR_VENDOR_NAME,
|
||||
"vid_pid": "1BCF:2281",
|
||||
"location": str(item.get("Location") or ""),
|
||||
"instance_id": instance_id,
|
||||
"address": address_value,
|
||||
}
|
||||
)
|
||||
matched.sort(key=lambda row: (row["address"], row["location"], row["instance_id"]))
|
||||
return matched
|
||||
|
||||
|
||||
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]
|
||||
sensors = list_windows_sensor_devices()
|
||||
if sensors:
|
||||
return [
|
||||
CameraInfo(
|
||||
index=index,
|
||||
backend_name=backend_name,
|
||||
backend_api=backend_api,
|
||||
width=0,
|
||||
height=0,
|
||||
friendly_name=str(sensor.get("friendly_name") or SENSOR_FRIENDLY_NAME),
|
||||
manufacturer=str(sensor.get("manufacturer") or SENSOR_VENDOR_NAME),
|
||||
vid_pid=str(sensor.get("vid_pid") or ""),
|
||||
location=str(sensor.get("location") or ""),
|
||||
instance_id=str(sensor.get("instance_id") or ""),
|
||||
)
|
||||
for index, sensor in enumerate(sensors)
|
||||
if index <= max_index
|
||||
]
|
||||
return [
|
||||
CameraInfo(index, backend_name, backend_api, 0, 0)
|
||||
for index in _candidate_indices(max_index)
|
||||
]
|
||||
|
||||
|
||||
def _try_open(index, backend_name, backend_api):
|
||||
def _try_open(index, backend_name, backend_api, meta=None):
|
||||
capture = cv2.VideoCapture(index, backend_api)
|
||||
try:
|
||||
if not capture.isOpened():
|
||||
@@ -136,7 +263,17 @@ def _try_open(index, backend_name, backend_api):
|
||||
height = height or int(frame_height)
|
||||
elif width <= 0 or height <= 0:
|
||||
return None
|
||||
return CameraInfo(index, backend_name, backend_api, width, height)
|
||||
info = CameraInfo(index, backend_name, backend_api, width, height)
|
||||
if meta:
|
||||
info = replace(
|
||||
info,
|
||||
friendly_name=str(meta.get("friendly_name") or ""),
|
||||
manufacturer=str(meta.get("manufacturer") or ""),
|
||||
vid_pid=str(meta.get("vid_pid") or ""),
|
||||
location=str(meta.get("location") or ""),
|
||||
instance_id=str(meta.get("instance_id") or ""),
|
||||
)
|
||||
return info
|
||||
finally:
|
||||
capture.release()
|
||||
|
||||
@@ -170,3 +307,65 @@ def list_cameras(
|
||||
seen_indices.add(index)
|
||||
found.sort(key=lambda camera: camera.index)
|
||||
return found
|
||||
|
||||
|
||||
def list_sensor_cameras(
|
||||
max_index=DEFAULT_MAX_CAMERA_INDEX,
|
||||
skip_indices=None,
|
||||
fallback_backends=True,
|
||||
):
|
||||
"""Return OpenCV IDs for SunplusIT ``USB Camera`` tactile sensors only."""
|
||||
sensors = list_windows_sensor_devices()
|
||||
if sys.platform == "win32" and not sensors:
|
||||
return []
|
||||
|
||||
backends = _backend_candidates()
|
||||
if not fallback_backends:
|
||||
backends = backends[:1]
|
||||
|
||||
skip = set()
|
||||
for source in skip_indices or ():
|
||||
index = camera_source_index(source)
|
||||
if index is not None:
|
||||
skip.add(index)
|
||||
|
||||
found = []
|
||||
with _suppress_opencv_noise():
|
||||
for backend_name, backend_api in backends:
|
||||
# Prefer default IDs 0/1 first, then any remaining openable indices.
|
||||
preferred = list(range(min(2, max_index + 1))) + [
|
||||
index for index in _candidate_indices(max_index) if index >= 2
|
||||
]
|
||||
for index in preferred:
|
||||
if index in skip or any(camera.index == index for camera in found):
|
||||
continue
|
||||
meta = sensors[len(found)] if len(found) < len(sensors) else {
|
||||
"friendly_name": SENSOR_FRIENDLY_NAME,
|
||||
"manufacturer": SENSOR_VENDOR_NAME,
|
||||
"vid_pid": "1BCF:2281",
|
||||
}
|
||||
info = _try_open(index, backend_name, backend_api, meta=meta)
|
||||
if info is None:
|
||||
continue
|
||||
found.append(info)
|
||||
if sensors and len(found) >= len(sensors):
|
||||
break
|
||||
if sensors and len(found) >= len(sensors):
|
||||
break
|
||||
if found:
|
||||
break
|
||||
found.sort(key=lambda camera: camera.index)
|
||||
return found
|
||||
|
||||
|
||||
def default_sensor_camera_sources(cameras=None):
|
||||
"""Prefer OpenCV IDs 0/1 when both sensor cameras are available."""
|
||||
cameras = list(cameras if cameras is not None else list_sensor_cameras())
|
||||
sources = [camera.source for camera in cameras]
|
||||
if "0" in sources and "1" in sources:
|
||||
return "0", "1"
|
||||
if len(sources) >= 2:
|
||||
return sources[0], sources[1]
|
||||
if len(sources) == 1:
|
||||
return sources[0], ""
|
||||
return "", ""
|
||||
|
||||
+470
-329
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user