diff --git a/config/compound_eye.png b/config/compound_eye.png index 159c5d2..e019068 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 23c4dda..f13840a 100644 --- a/config/gripper.json +++ b/config/gripper.json @@ -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, diff --git a/docs/RELEASE_CONTROL.zh-CN.md b/docs/RELEASE_CONTROL.zh-CN.md index c132ca8..ee5dee4 100644 --- a/docs/RELEASE_CONTROL.zh-CN.md +++ b/docs/RELEASE_CONTROL.zh-CN.md @@ -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` 的最早时间。 diff --git a/gripper/config/behavior_profiles.py b/gripper/config/behavior_profiles.py index 50bb849..f10c496 100644 --- a/gripper/config/behavior_profiles.py +++ b/gripper/config/behavior_profiles.py @@ -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): diff --git a/gripper/config/settings.py b/gripper/config/settings.py index a689f2b..7dde9c8 100644 --- a/gripper/config/settings.py +++ b/gripper/config/settings.py @@ -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") diff --git a/gripper/control/actuator.py b/gripper/control/actuator.py index 9b14b75..4e64b69 100644 --- a/gripper/control/actuator.py +++ b/gripper/control/actuator.py @@ -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: diff --git a/gripper/control/controller.py b/gripper/control/controller.py index 6a2f247..77b4eee 100644 --- a/gripper/control/controller.py +++ b/gripper/control/controller.py @@ -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, diff --git a/gripper/control/feedback.py b/gripper/control/feedback.py index 1e7a33e..ea2e9ea 100644 --- a/gripper/control/feedback.py +++ b/gripper/control/feedback.py @@ -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") diff --git a/gripper/control/release_watch.py b/gripper/control/release_watch.py index e88a55b..4ecae7b 100644 --- a/gripper/control/release_watch.py +++ b/gripper/control/release_watch.py @@ -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) diff --git a/gripper/sensors/camera_discovery.py b/gripper/sensors/camera_discovery.py index 0491abd..a430811 100644 --- a/gripper/sensors/camera_discovery.py +++ b/gripper/sensors/camera_discovery.py @@ -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 "", "" diff --git a/gripper/ui/visualizer.py b/gripper/ui/visualizer.py index cf2791a..e2fc89e 100644 --- a/gripper/ui/visualizer.py +++ b/gripper/ui/visualizer.py @@ -41,7 +41,7 @@ _fix_qt_plugin_path() _QT_IMPORT_ERROR = None try: - from PyQt5.QtCore import QEasingCurve, QEvent, QPointF, QRect, QRectF, QSize, QPropertyAnimation, Qt, QTimer + from PyQt5.QtCore import QEasingCurve, QEvent, QPoint, QPointF, QRect, QRectF, QSize, QPropertyAnimation, Qt, QTimer from PyQt5.QtGui import QColor, QFont, QIcon, QImage, QLinearGradient, QPainter, QPen, QPixmap, QPolygonF, QRadialGradient, QVector3D from PyQt5.QtWidgets import ( QApplication, @@ -73,7 +73,7 @@ try: ) except Exception as exc: _QT_IMPORT_ERROR = exc - Qt = QTimer = QPropertyAnimation = QEasingCurve = QEvent = QPointF = QRect = QRectF = QSize = QFont = QIcon = QImage = QPixmap = QApplication = None + Qt = QTimer = QPropertyAnimation = QEasingCurve = QEvent = QPoint = QPointF = QRect = QRectF = QSize = QFont = QIcon = QImage = QPixmap = QApplication = None QColor = QLinearGradient = QPainter = QPen = QPolygonF = QRadialGradient = QVector3D = None QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizeGrip = QSizePolicy = QSplitter = None QVBoxLayout = QWidget = None @@ -98,12 +98,17 @@ else: from ..app import build_controller from ..common.paths import PACKAGE_DIR, REPO_ROOT -from ..sensors.camera_discovery import list_camera_ids +from ..sensors.camera_discovery import ( + default_sensor_camera_sources, + list_camera_ids, +) PLOT_POINTS = 180 -PLOT_Y_DEFAULT_MIN_N = 0.0 -PLOT_Y_DEFAULT_MAX_N = 1.0 +PLOT_Y_FIXED_MIN_N = -5.0 +PLOT_Y_FIXED_MAX_N = 5.0 +PLOT_Y_NORMAL_MIN_N = 0.0 +PLOT_Y_NORMAL_MAX_N = 5.0 PLOT_FONT_FAMILY = "Microsoft YaHei" PLOT_TITLE_FONT_SIZE = 9 PLOT_AXIS_FONT_SIZE = 8 @@ -112,6 +117,16 @@ CURVE_COLOR = "#536dce" PLOT_SMOOTH_ALPHA = 0.28 GUI_LAYOUT_VERSION = 1 GUI_LAYOUT_PATH = REPO_ROOT / "config" / "gui_layout.json" +SIGNED_FORCE_PLOT_KEYS = frozenset({ + "left_shear_x", + "left_shear_y", + "right_shear_x", + "right_shear_y", +}) +NORMAL_FORCE_PLOT_KEYS = frozenset({ + "left_normal", + "right_normal", +}) def cv2_to_pixmap(img_bgr): @@ -154,8 +169,11 @@ def force_display_value(value): return value -def plot_force_value(value): - return max(0.0, force_display_value(value)) +def plot_force_value(value, *, signed=False): + value = force_display_value(value) + if signed: + return value + return max(0.0, value) def smooth_plot_values(values, alpha=PLOT_SMOOTH_ALPHA): @@ -259,6 +277,15 @@ def label_with_cn(value, mapping): return f"{value}（{cn}）" +def shorten_status_text(text, max_len=22): + text = str(text or "--").strip() + if len(text) <= max_len: + return text + if max_len <= 1: + return text[:max_len] + return text[: max_len - 1] + "…" + + STYLE = """ QMainWindow { background-color: #f5f7fa; @@ -1274,7 +1301,12 @@ class FloatingPanel(QFrame): class GripperStage(QFrame): - MODEL_OPEN_BINS = 40 + # Display refresh target (~60 FPS). Measured position still arrives slower; + # we interpolate open_ratio between samples so the mesh does not step. + GL_ANIM_INTERVAL_MS = 16 + GL_RATIO_EPS = 2e-4 + # Approach factor per anim tick (~80 ms time constant at 60 FPS). + GL_SMOOTH_ALPHA = 0.22 _mesh_cache = None _urdf_cache = None _mesh_link_cache = None @@ -1295,17 +1327,18 @@ class GripperStage(QFrame): self._gl_faces = {} self._gl_item = None self._combined_faces = None + self._combined_colors = None + self._local_parts = [] # [(name, verts Nx3 float32), ...] self._mesh_dict = {} self._mesh_face_counts = {} - self._vertex_cache = {} - self._mesh_cache_prewarmed = False - self._last_gl_open_bin = None + self._target_open_ratio = 1.0 + self._display_open_ratio = 1.0 + self._last_rendered_ratio = None + self._anim_timer = None self._perf = { "last_log": time.perf_counter(), "status_count": 0, "gl_updates": 0, - "cache_hits": 0, - "cache_misses": 0, "mesh_total": 0.0, "mesh_max": 0.0, "set_total": 0.0, @@ -1321,6 +1354,10 @@ class GripperStage(QFrame): self.setMinimumSize(240, 300) self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self._init_gl_view() + if QTimer is not None: + self._anim_timer = QTimer(self) + self._anim_timer.setInterval(self.GL_ANIM_INTERVAL_MS) + self._anim_timer.timeout.connect(self._tick_gl_animation) def _init_gl_view(self): layout = QVBoxLayout(self) @@ -1433,36 +1470,65 @@ class GripperStage(QFrame): colors[:, 3] = 1.0 return colors - def _combined_mesh_data(self, open_bin): - key = ("combined", int(open_bin)) - cached = self._vertex_cache.get(key) - if cached is not None: - self._perf["cache_hits"] += 1 - vertexes, face_colors = cached - return vertexes, self._combined_faces, face_colors - - self._perf["cache_misses"] += 1 - open_ratio = int(open_bin) / float(self.MODEL_OPEN_BINS) - vertex_parts = [] + def _build_local_parts(self): + """Store link-local vertices once; joint pose is applied each frame.""" + self._local_parts = [] face_parts = [] color_parts = [] offset = 0 + # Shade once at the open pose; lighting stays stable while joints move. + open_joint_positions = self._joint_positions(1.0) + open_tf_cache = {} + urdf = self._load_urdf_model() + identity = np.eye(4, dtype=np.float64) for name, tris in self._mesh_dict.items(): - transformed = self._transform_mesh(name, tris, open_ratio) - vertexes = np.asarray(transformed.reshape(-1, 3), dtype=np.float32) - faces = np.arange(vertexes.shape[0], dtype=np.uint32).reshape(-1, 3) + offset - vertex_parts.append(vertexes) + local = np.asarray(tris.reshape(-1, 3), dtype=np.float32) + link_name = None if urdf is None else self._mesh_link_map().get(name) + visual_tf = identity + if urdf is not None and link_name is not None: + visual = urdf["links"][link_name] + visual_tf = self._matrix_from_xyz_rpy( + visual["origin_xyz"], visual["origin_rpy"] + ) + self._local_parts.append((link_name, local, visual_tf)) + faces = np.arange(local.shape[0], dtype=np.uint32).reshape(-1, 3) + offset face_parts.append(faces) - color_parts.append(self._face_colors(name, transformed)) - offset += vertexes.shape[0] - if not vertex_parts: - return None, None, None - combined_vertexes = np.vstack(vertex_parts) - combined_colors = np.vstack(color_parts) - if self._combined_faces is None: - self._combined_faces = np.vstack(face_parts) - self._vertex_cache[key] = (combined_vertexes, combined_colors) - return combined_vertexes, self._combined_faces, combined_colors + if urdf is not None and link_name is not None: + link_tf = self._link_transform(link_name, open_joint_positions, open_tf_cache) + world = self._apply_tf(local, link_tf @ visual_tf).reshape((-1, 3, 3)) + color_parts.append(self._face_colors(name, world)) + else: + color_parts.append(self._face_colors(name, tris)) + offset += local.shape[0] + if not face_parts: + self._combined_faces = None + self._combined_colors = None + return False + self._combined_faces = np.vstack(face_parts) + self._combined_colors = np.vstack(color_parts) + return True + + @staticmethod + def _apply_tf(verts, tf): + """Apply 4x4 transform to (N, 3) row vectors.""" + matrix = np.asarray(tf, dtype=np.float32) + rotated = verts @ matrix[:3, :3].T + rotated += matrix[:3, 3] + return rotated + + def _mesh_vertexes_at_ratio(self, open_ratio): + if not self._local_parts: + return None + joint_positions = self._joint_positions(open_ratio) + tf_cache = {} + parts = [] + for link_name, local, visual_tf in self._local_parts: + if link_name is None: + parts.append(local) + continue + link_tf = self._link_transform(link_name, joint_positions, tf_cache) + parts.append(self._apply_tf(local, link_tf @ visual_tf)) + return np.vstack(parts) def _build_gl_model(self): meshes = self._load_meshes() @@ -1470,15 +1536,16 @@ class GripperStage(QFrame): return False self._mesh_dict = dict(meshes) self._mesh_face_counts = {name: len(tris) for name, tris in meshes} - self._prewarm_mesh_cache() + if not self._build_local_parts(): + return False - vertexes, faces, face_colors = self._combined_mesh_data(self.MODEL_OPEN_BINS) - if vertexes is None or faces is None: + vertexes = self._mesh_vertexes_at_ratio(self._display_open_ratio) + if vertexes is None or self._combined_faces is None: return False self._gl_item = gl.GLMeshItem( vertexes=vertexes, - faces=faces, - faceColors=face_colors, + faces=self._combined_faces, + faceColors=self._combined_colors, color=(0.72, 0.75, 0.78, 1.0), smooth=False, computeNormals=False, @@ -1488,84 +1555,86 @@ class GripperStage(QFrame): ) self._gl_item.setGLOptions("opaque") self.gl_view.addItem(self._gl_item) - self._update_gl_model(force=True) + self._last_rendered_ratio = self._display_open_ratio return True - def _prewarm_mesh_cache(self): - if self._mesh_cache_prewarmed: + def _set_target_open_ratio(self, open_ratio, *, snap=False): + ratio = max(0.0, min(1.0, float(open_ratio))) + self._target_open_ratio = ratio + if snap: + self._display_open_ratio = ratio + self._render_gl_at_ratio(ratio, force=True) + if self._anim_timer is not None and self._anim_timer.isActive(): + self._anim_timer.stop() return - start = time.perf_counter() - for open_bin in range(self.MODEL_OPEN_BINS + 1): - self._combined_mesh_data(open_bin) - self._mesh_cache_prewarmed = True - elapsed_ms = (time.perf_counter() - start) * 1000.0 - triangle_count = sum(len(tris) for tris in self._mesh_dict.values()) - print( - "[perf] ui_3d prewarm " - f"bins={self.MODEL_OPEN_BINS + 1} " - f"triangles={triangle_count} " - f"elapsed={elapsed_ms:.1f}ms" - ) + if abs(ratio - self._display_open_ratio) < self.GL_RATIO_EPS: + self._display_open_ratio = ratio + if self._anim_timer is not None and self._anim_timer.isActive(): + self._anim_timer.stop() + return + if self._anim_timer is not None and not self._anim_timer.isActive(): + self._anim_timer.start() - def _update_gl_model(self, force=False): + def _tick_gl_animation(self): + target = self._target_open_ratio + display = self._display_open_ratio + delta = target - display + if abs(delta) <= self.GL_RATIO_EPS: + self._display_open_ratio = target + self._render_gl_at_ratio(target) + if self._anim_timer is not None: + self._anim_timer.stop() + return + display += delta * self.GL_SMOOTH_ALPHA + if abs(target - display) <= self.GL_RATIO_EPS: + display = target + self._display_open_ratio = display + self._render_gl_at_ratio(display) + + def _render_gl_at_ratio(self, open_ratio, force=False): if not self._gl_ready and not force: return if self._gl_item is None: return - open_ratio = self._position_open_ratio() - open_bin = self._open_ratio_bin(open_ratio) - if self._last_gl_open_bin == open_bin and not force: + ratio = max(0.0, min(1.0, float(open_ratio))) + if ( + not force + and self._last_rendered_ratio is not None + and abs(ratio - self._last_rendered_ratio) < self.GL_RATIO_EPS + ): return - self._last_gl_open_bin = open_bin self._perf["gl_updates"] += 1 t0 = time.perf_counter() - vertexes, faces, face_colors = self._combined_mesh_data(open_bin) + vertexes = self._mesh_vertexes_at_ratio(ratio) mesh_dt = time.perf_counter() - t0 self._perf["mesh_total"] += mesh_dt self._perf["mesh_max"] = max(self._perf["mesh_max"], mesh_dt) - if vertexes is None or faces is None: + if vertexes is None or self._combined_faces is None: return t1 = time.perf_counter() - self._gl_item.setMeshData(vertexes=vertexes, faces=faces, faceColors=face_colors) + self._gl_item.setMeshData( + vertexes=vertexes, + faces=self._combined_faces, + faceColors=self._combined_colors, + ) set_dt = time.perf_counter() - t1 self._perf["set_total"] += set_dt self._perf["set_max"] = max(self._perf["set_max"], set_dt) + self._last_rendered_ratio = ratio - @staticmethod - def _open_ratio_bin(open_ratio): - return int( - round( - max(0.0, min(1.0, float(open_ratio))) - * float(GripperStage.MODEL_OPEN_BINS) - ) - ) - - def _cached_vertexes(self, name, open_bin): - key = (name, int(open_bin)) - cached = self._vertex_cache.get(key) - if cached is not None: - self._perf["cache_hits"] += 1 - return cached - self._perf["cache_misses"] += 1 - tris = self._mesh_dict.get(name) - if tris is None: - return None - open_ratio = int(open_bin) / float(self.MODEL_OPEN_BINS) - transformed = self._transform_mesh(name, tris, open_ratio) - vertexes, _ = self._mesh_to_vertex_faces(transformed) - vertexes = np.asarray(vertexes, dtype=np.float32) - self._vertex_cache[key] = vertexes - return vertexes + def _update_gl_model(self, force=False): + open_ratio = self._position_open_ratio() + self._set_target_open_ratio(open_ratio, snap=force) def _position_open_ratio(self): if self.gripper_position is None: - return 1.0 + return self._display_open_ratio if self._gl_ready else 1.0 try: open_pos = float(self.open_pos) close_pos = float(self.close_pos) position = float(self.gripper_position) except Exception: - return 1.0 + return self._display_open_ratio if self._gl_ready else 1.0 span = close_pos - open_pos if abs(span) < 1e-6: return 1.0 @@ -2039,17 +2108,15 @@ class GripperStage(QFrame): now = time.perf_counter() if now - self._perf["last_log"] < 1.0: return - mesh_ops = self._perf["cache_hits"] + self._perf["cache_misses"] set_ops = max(1, self._perf["gl_updates"]) - mesh_avg = self._perf["mesh_total"] / max(1, mesh_ops) * 1000.0 + mesh_avg = self._perf["mesh_total"] / set_ops * 1000.0 set_avg = self._perf["set_total"] / set_ops * 1000.0 print( "[perf] ui_3d " f"status_hz={self._perf['status_count']} " f"gl_updates={self._perf['gl_updates']} " - f"bin={self._last_gl_open_bin} " - f"cache_hit={self._perf['cache_hits']} " - f"cache_miss={self._perf['cache_misses']} " + f"ratio={self._display_open_ratio:.3f} " + f"target={self._target_open_ratio:.3f} " f"mesh_avg={mesh_avg:.2f}ms " f"mesh_max={self._perf['mesh_max'] * 1000.0:.2f}ms " f"set_avg={set_avg:.2f}ms " @@ -2060,8 +2127,6 @@ class GripperStage(QFrame): "last_log": now, "status_count": 0, "gl_updates": 0, - "cache_hits": 0, - "cache_misses": 0, "mesh_total": 0.0, "mesh_max": 0.0, "set_total": 0.0, @@ -2312,9 +2377,11 @@ class CameraDiscoveryDialog(QDialog): def scan(): try: + # Prefer lightweight PnP listing; do not open cameras in the dialog worker. + cameras = list_camera_ids() self._scan_result = ( scan_id, - list_camera_ids(), + cameras, None, ) except Exception as exc: @@ -2334,6 +2401,7 @@ class CameraDiscoveryDialog(QDialog): self.refresh_button.setEnabled(True) current_sources = self.viewer.camera_sources() found_sources = set() + default_left, default_right = default_sensor_camera_sources(cameras) for side, combo in self.camera_combos.items(): combo.clear() for camera in cameras: @@ -2342,15 +2410,21 @@ class CameraDiscoveryDialog(QDialog): current = current_sources.get(side, "") if current and current not in found_sources: combo.addItem(f"当前配置 {current}（可能正在使用）", current) + preferred = default_left if side == "left" else default_right current_index = combo.findData(current) + if current_index < 0: + current_index = combo.findData(preferred) if current_index >= 0: combo.setCurrentIndex(current_index) if error: self.status_label.setText(f"搜索失败：{error}") elif cameras: - self.status_label.setText(f"已列出 {len(cameras)} 个摄像头 ID（未占用设备）") + ids = ", ".join(camera.source for camera in cameras) + self.status_label.setText( + f"已筛选 SunplusIT USB Camera，可用 ID: {ids}（默认左0 / 右1）" + ) else: - self.status_label.setText("未发现空闲摄像头；正在使用的当前配置仍可保留") + self.status_label.setText("未发现 SunplusIT USB Camera 传感器") has_both = all(combo.count() > 0 for combo in self.camera_combos.values()) for combo in self.camera_combos.values(): combo.setEnabled(combo.count() > 0) @@ -2421,6 +2495,7 @@ class VisualSettingsDialog(QDialog): "baseline_seconds", "filter_alpha", "shear_filter_alpha", + "shear_control_axes", "dry_run", "open_at_end", "trigger_force_update", @@ -2476,6 +2551,11 @@ class VisualSettingsDialog(QDialog): "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", @@ -2530,6 +2610,7 @@ class VisualSettingsDialog(QDialog): "baseline_seconds": "基准时间", "filter_alpha": "法向滤波", "shear_filter_alpha": "切向滤波", + "shear_control_axes": "切向控制轴 (xy/x/y)", "dry_run": "干运行", "open_at_end": "结束时张开", "trigger_force_update": "触发力更新", @@ -2546,7 +2627,7 @@ class VisualSettingsDialog(QDialog): "force_ramp_step": "加力步进", "force_ramp_interval": "加力间隔", "grip_settle_seconds": "夹持稳定时间", - "grip_behavior_mode": "夹持行为模式", + "grip_behavior_mode": "开合行为预设", "adaptive_grip_enabled": "启用自适应夹持", "adaptive_shear_start_force": "自适应起始切向力", "adaptive_shear_full_force": "自适应满量程切向力", @@ -2576,6 +2657,11 @@ class VisualSettingsDialog(QDialog): "release_require_slope_settle": "释放前要求法向斜率趋零（夹持稳定）", "release_slope_hold_seconds": "斜率趋零后还需保持多久才算夹稳 (s)", "release_settle_normal_only": "夹稳判定仅看法向（忽略切向）", + "release_shear_oscillation_open": "切向振荡时不待夹稳也开合", + "release_shear_oscillation_window_seconds": "切向振荡检测窗口 (s)", + "release_shear_oscillation_min_flips": "切向振荡最少换向次数", + "release_shear_oscillation_amplitude": "切向振荡峰峰值门槛 (N)", + "release_shear_oscillation_confirm_samples": "切向振荡确认帧数", "release_side_shear_enabled": "允许切向左右差侧触张开", "release_side_normal_enabled": "允许法向左右差侧触张开", "release_shear_onset_enabled": "允许切向通道触发普通释放", @@ -2634,7 +2720,7 @@ class VisualSettingsDialog(QDialog): except Exception: self.config_data = {} if "grip_behavior_mode" not in self.config_data: - self.config_data["grip_behavior_mode"] = "stable" + self.config_data["grip_behavior_mode"] = "sensitive" def _build_ui(self): outer = QVBoxLayout(self) @@ -2823,14 +2909,16 @@ class VisualSettingsDialog(QDialog): try: current = normalize_grip_behavior_mode(value) except Exception: - current = "stable" + current = "sensitive" 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 @@ -3057,7 +3145,11 @@ class VisualSettingsDialog(QDialog): def _begin_drag(self, global_pos): self._drag_start_global = global_pos - self._drag_start_pos = self.pos() + # Top-level tool windows use screen coordinates; embedded widgets use parent coords. + if self.isWindow(): + self._drag_start_pos = self.frameGeometry().topLeft() + else: + self._drag_start_pos = self.pos() self.setCursor(Qt.SizeAllCursor) def _continue_drag(self, global_pos): @@ -3065,6 +3157,18 @@ class VisualSettingsDialog(QDialog): return delta = global_pos - self._drag_start_global target = self._drag_start_pos + delta + if self.isWindow(): + host = None + if self.viewer is not None: + host = self.viewer.centralWidget() or self.viewer + if host is not None: + origin = host.mapToGlobal(QPoint(0, 0)) + area = QRect(origin, host.size()) + target.setX(max(area.left(), min(target.x(), area.right() - self.width() + 1))) + target.setY(max(area.top(), min(target.y(), area.bottom() - self.height() + 1))) + self.move(target) + self._user_moved = True + return parent = self.parentWidget() if parent is not None: target.setX(max(0, min(target.x(), parent.width() - self.width()))) @@ -3119,6 +3223,9 @@ class GripperDemo02Viewer(QMainWindow): self._build_ui() self._configure_plot() + # Never block window creation on USB/PnP/OpenCV probes. + QTimer.singleShot(0, lambda: self.search_main_serial_ports(silent=True)) + QTimer.singleShot(50, lambda: self.auto_assign_sensor_cameras(silent=True)) self.timer = QTimer(self) self.timer.timeout.connect(self._refresh) @@ -3290,6 +3397,7 @@ class GripperDemo02Viewer(QMainWindow): "port": ("--port",), "slave_id": ("--slave-id",), }[config_key] + source = str(source) for index, arg in enumerate(args): if arg in option_names and index + 1 < len(args): args[index + 1] = source @@ -3300,13 +3408,79 @@ class GripperDemo02Viewer(QMainWindow): args[index] = option + "=" + source self.argv = args return + args.extend([option_names[0], source]) + self.argv = args - def apply_camera_sources(self, left_source, right_source): + def _write_serial_device_config(self, port, slave_id=None): + """Persist gripper serial settings. Returns True when the file changed.""" + port = str(port or "").strip() + if not port: + return False + if slave_id is None: + _, slave_id = self._load_serial_device_config() + slave_id = max(1, min(247, int(slave_id))) + 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: + self._update_config_argv("port", port) + self._update_config_argv("slave_id", str(slave_id)) + return False + data["port"] = port + data["slave_id"] = slave_id + config_path.parent.mkdir(parents=True, exist_ok=True) + 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: + try: + if temp_path.exists(): + temp_path.unlink() + except OSError: + pass + raise + 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) + return True + + def _sync_runtime_serial_port(self, *, persist_live=True): + """Align combo / argv / json before the actuator opens a port.""" + if not hasattr(self, "main_port_combo"): + return "" + port = self._combo_source_value(self.main_port_combo) + if " [" in port: + port = port.split(" [", 1)[0].strip() + port = str(port or "").strip() + if not port: + return "" + if persist_live: + try: + self._write_serial_device_config(port) + except Exception as exc: + print(f"serial config sync failed: {exc}") + self._update_config_argv("port", port) + else: + self._update_config_argv("port", port) + return port + + def apply_camera_sources(self, left_source, right_source, *, silent=False): if left_source == right_source: - QMessageBox.warning(self, "摄像头冲突", "左右两侧需要选择不同的摄像头。") + if not silent: + QMessageBox.warning(self, "摄像头冲突", "左右两侧需要选择不同的摄像头。") return False sources = self.camera_sources() if left_source == sources.get("left") and right_source == sources.get("right"): + self._update_config_argv("video1", left_source) + self._update_config_argv("video2", right_source) return True config_path = self._control_config_path() @@ -3329,7 +3503,10 @@ class GripperDemo02Viewer(QMainWindow): temp_path.unlink() except OSError: pass - QMessageBox.warning(self, "保存失败", f"无法保存摄像头配置:\n{exc}") + if not silent: + QMessageBox.warning(self, "保存失败", f"无法保存摄像头配置:\n{exc}") + else: + print(f"camera config sync failed: {exc}") return False self._update_config_argv("video1", left_source) @@ -3338,14 +3515,48 @@ class GripperDemo02Viewer(QMainWindow): for config_key, source in (("video1", left_source), ("video2", right_source)): self.visual_settings_dialog.config_data[config_key] = source self.visual_settings_dialog._set_control_value(config_key, source) - QMessageBox.information( - self, - "双摄像头已切换", - f"左侧摄像头: {left_source}\n右侧摄像头: {right_source}\n控制程序将自动重启。", - ) - self.restart_control() + if not silent: + QMessageBox.information( + self, + "双摄像头已切换", + f"左侧摄像头: {left_source}\n右侧摄像头: {right_source}\n控制程序将自动重启。", + ) + self.restart_control() + elif self.control_thread is not None and self.control_thread.is_alive(): + self.restart_control() return True + def auto_assign_sensor_cameras(self, silent=True): + """Filter SunplusIT USB Camera sensors and default to OpenCV IDs 0/1. + + Uses the lightweight PnP ID list (no OpenCV open) so startup stays responsive. + """ + try: + cameras = list_camera_ids() + except Exception as exc: + if not silent: + QMessageBox.warning(self, "相机搜索失败", str(exc)) + else: + print(f"sensor camera search failed: {exc}") + return False + if len(cameras) < 2: + if not silent: + QMessageBox.information( + self, + "未找到传感器相机", + "未找到足够的 SunplusIT USB Camera（需要左右各一台）。", + ) + return False + left, right = default_sensor_camera_sources(cameras) + if not left or not right: + return False + ids = ", ".join( + f"{camera.source}:{camera.friendly_name or 'USB Camera'}/{camera.manufacturer or 'SunplusIT'}" + for camera in cameras + ) + print(f"sensor cameras available: {ids}; default left={left} right={right}") + return self.apply_camera_sources(left, right, silent=silent) + def _load_serial_device_config(self): port = "" slave_id = 1 @@ -3376,10 +3587,26 @@ class GripperDemo02Viewer(QMainWindow): return str(selected) return combo.currentText().strip() - def search_main_serial_ports(self): + def search_main_serial_ports(self, checked=False, *, silent=False, show_popup=None): + """Enumerate serial ports into the main combo. + + Manual search opens the dropdown. Startup uses silent=True (no dialogs / + popups). Prefer the configured port when it is still present; otherwise + fall back to the first discovered USB serial device. + """ + del checked # QPushButton.clicked may pass a bool + if show_popup is None: + show_popup = not silent + if not hasattr(self, "main_port_combo"): + return current = self._combo_source_value(self.main_port_combo) + # Editable combo may leave display text like "COMx [desc]". + if " [" in current: + current = current.split(" [", 1)[0].strip() + current = str(current or "").strip() if serial_list_ports is None: - QMessageBox.warning(self, "搜索失败", "PySerial 串口枚举模块未安装。") + if not silent: + QMessageBox.warning(self, "搜索失败", "PySerial 串口枚举模块未安装。") return try: ports = sorted( @@ -3392,66 +3619,80 @@ class GripperDemo02Viewer(QMainWindow): ), ) except Exception as exc: - QMessageBox.warning(self, "搜索失败", f"无法枚举串口:\n{exc}") + if not silent: + 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: + self.main_port_combo.blockSignals(True) + try: + 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.upper() == str(current).upper(): + selected_index = self.main_port_combo.count() - 1 + if current and selected_index < 0: + # Keep stale config visible, but do not prefer it over live ports. + self.main_port_combo.addItem(f"{current} [当前配置]", current) + if selected_index < 0 and ports: + selected_index = 0 + elif selected_index < 0 and 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 selected_index >= 0: + self.main_port_combo.setCurrentIndex(selected_index) + finally: + self.main_port_combo.blockSignals(False) + if not ports: - QMessageBox.information(self, "搜索串口", "未发现可用串口，已保留当前配置。") + if current: + self.main_port_combo.setToolTip(f"未发现新串口，保留 {current}") + if not silent: + QMessageBox.information(self, "搜索串口", "未发现可用串口，已保留当前配置。") return - self.main_port_combo.setToolTip(f"找到 {len(ports)} 个串口") - QTimer.singleShot(50, self.main_port_combo.showPopup) + selected = self._combo_source_value(self.main_port_combo) + if " [" in selected: + selected = selected.split(" [", 1)[0].strip() + live_devices = {str(port.device).upper() for port in ports} + self.main_port_combo.setToolTip(f"找到 {len(ports)} 个串口，当前 {selected}") + + # Silent startup must also rewrite gripper.json / argv; otherwise the + # actuator still opens the stale configured port (e.g. missing COM10). + if selected and selected.upper() in live_devices: + saved_port, saved_slave = self._load_serial_device_config() + if selected.upper() != str(saved_port).upper(): + try: + changed = self._write_serial_device_config(selected, saved_slave) + except Exception as exc: + if not silent: + QMessageBox.warning(self, "保存失败", f"无法写入串口配置:\n{exc}") + else: + print(f"auto serial persist failed: {exc}") + changed = False + if changed and silent and self.control_thread is not None and self.control_thread.is_alive(): + self.restart_control() + + if show_popup: + QTimer.singleShot(50, self.main_port_combo.showPopup) def apply_main_device_config(self): port = self._combo_source_value(self.main_port_combo) + if " [" in port: + port = port.split(" [", 1)[0].strip() 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) + changed = self._write_serial_device_config(port, slave_id) except Exception as exc: - try: - if temp_path.exists(): - temp_path.unlink() - except OSError: - pass QMessageBox.warning(self, "保存失败", f"无法保存夹爪设备配置:\n{exc}") return False + if not changed: + return True - 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, "夹爪设备已更新", @@ -3628,27 +3869,6 @@ class GripperDemo02Viewer(QMainWindow): 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) @@ -3671,20 +3891,18 @@ class GripperDemo02Viewer(QMainWindow): 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, 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, 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) - gripper_layout.addWidget(self.manual_open_button, 2, 4) - gripper_layout.addWidget(self.manual_close_button, 3, 4) - for col in range(4): + gripper_layout.addWidget(self.manual_mode_button, 0, 1) + gripper_layout.addWidget(self.smart_mode_button, 0, 2) + gripper_layout.addWidget(self.speed_control, 1, 0, 1, 2) + gripper_layout.addWidget(self.record_button, 1, 2) + gripper_layout.addWidget(self.force_control, 2, 0, 1, 3) + gripper_layout.addWidget(self.position_control, 3, 0, 1, 3) + gripper_layout.addWidget(self.manual_hold_button, 1, 3) + gripper_layout.addWidget(self.manual_open_button, 2, 3) + gripper_layout.addWidget(self.manual_close_button, 3, 3) + for col in range(3): gripper_layout.setColumnStretch(col, 1) - gripper_layout.setColumnStretch(4, 0) + gripper_layout.setColumnStretch(3, 0) gripper_layout.setRowStretch(0, 0) gripper_layout.setRowStretch(1, 0) gripper_layout.setRowStretch(2, 0) @@ -3830,9 +4048,10 @@ class GripperDemo02Viewer(QMainWindow): plot.getAxis("bottom").setStyle(showValues=row == 2) plot.getAxis("bottom").setHeight(30 if row == 2 else 10) plot.getAxis("bottom").setTickSpacing(major=1.0, minor=1.0) - plot.setYRange(PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N, padding=0) + y_min, y_max = self._plot_default_y_range(key) + plot.setYRange(y_min, y_max, padding=0) plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False) - self.plot_y_ranges[key] = (PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N) + self.plot_y_ranges[key] = (y_min, y_max) self.plots.append(plot) self.plot_by_key[key] = plot self.plot_title_templates[key] = title @@ -3850,46 +4069,18 @@ class GripperDemo02Viewer(QMainWindow): self.curves[key] = curve @staticmethod - def _plot_axis_step(value): - value = abs(float(value)) - if value <= 2.0: - return 0.5 - if value <= 5.0: - return 1.0 - return 2.0 + def _plot_default_y_range(key=None): + if key in NORMAL_FORCE_PLOT_KEYS: + return PLOT_Y_NORMAL_MIN_N, PLOT_Y_NORMAL_MAX_N + return PLOT_Y_FIXED_MIN_N, PLOT_Y_FIXED_MAX_N def _reset_plot_y_ranges(self): if not hasattr(self, "plot_y_ranges"): return for key, plot in getattr(self, "plot_by_key", {}).items(): - self.plot_y_ranges[key] = (PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N) - plot.setYRange(PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N, padding=0) - - def _update_plot_y_range(self, key, values): - plot = self.plot_by_key.get(key) - if plot is None or values.size == 0: - return - finite = values[np.isfinite(values)] - if finite.size == 0: - return - current_min, current_max = self.plot_y_ranges.get( - key, - (PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N), - ) - data_min = float(np.min(finite)) - data_max = float(np.max(finite)) - next_min = current_min - next_max = current_max - if data_max > current_max: - step = self._plot_axis_step(data_max) - next_max = math.ceil((data_max + step * 0.12) / step) * step - if next_min != PLOT_Y_DEFAULT_MIN_N: - next_min = PLOT_Y_DEFAULT_MIN_N - if next_min != current_min or next_max != current_max: - if next_max - next_min < 0.5: - next_max = next_min + 0.5 - self.plot_y_ranges[key] = (next_min, next_max) - plot.setYRange(next_min, next_max, padding=0) + y_min, y_max = self._plot_default_y_range(key) + self.plot_y_ranges[key] = (y_min, y_max) + plot.setYRange(y_min, y_max, padding=0) def _set_button_state(self, state): labels = { @@ -3932,10 +4123,6 @@ 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 @@ -3970,69 +4157,6 @@ 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) @@ -4076,7 +4200,6 @@ class GripperDemo02Viewer(QMainWindow): 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}") @@ -4107,12 +4230,19 @@ class GripperDemo02Viewer(QMainWindow): dialog = self.visual_settings_dialog dialog.hide() dialog._user_moved = False - dialog.setParent(self.centralWidget()) - dialog.setWindowFlags(Qt.Widget) + # Must be a real top-level window under fullscreen. Parenting as Qt.Widget + # makes QComboBox popups appear behind the fullscreen surface / unusable. + dialog.setParent(self) + dialog.setWindowFlags( + Qt.Tool + | Qt.FramelessWindowHint + | Qt.WindowStaysOnTopHint + ) dialog.setWindowIcon(_load_company_icon(64)) self._position_embedded_settings() dialog.show() dialog.raise_() + dialog.activateWindow() def _show_visual_settings_window(self): dialog = self.visual_settings_dialog @@ -4134,23 +4264,26 @@ class GripperDemo02Viewer(QMainWindow): def _position_embedded_settings(self): dialog = self.visual_settings_dialog - parent = self.centralWidget() - if dialog is None or parent is None: - return - if getattr(dialog, "_user_moved", False): - dialog.move( - max(0, min(dialog.x(), parent.width() - dialog.width())), - max(0, min(dialog.y(), parent.height() - dialog.height())), - ) + host = self.centralWidget() or self + if dialog is None or host is None: return margin = 24 - available_w = max(320, parent.width() - margin * 2) - available_h = max(260, parent.height() - margin * 2) - width = min(980, max(760, int(parent.width() * 0.72)), available_w) - height = min(680, max(560, int(parent.height() * 0.78)), available_h) - x = max(margin, (parent.width() - width) // 2) - y = max(margin, (parent.height() - height) // 2) - dialog.setGeometry(x, y, width, height) + available_w = max(320, host.width() - margin * 2) + available_h = max(260, host.height() - margin * 2) + width = min(980, max(760, int(host.width() * 0.72)), available_w) + height = min(680, max(560, int(host.height() * 0.78)), available_h) + if getattr(dialog, "_user_moved", False): + origin = host.mapToGlobal(QPoint(0, 0)) + area = QRect(origin, host.size()) + geom = dialog.frameGeometry() + x = max(area.left(), min(geom.x(), area.right() - geom.width() + 1)) + y = max(area.top(), min(geom.y(), area.bottom() - geom.height() + 1)) + dialog.setGeometry(x, y, geom.width(), geom.height()) + return + x = max(margin, (host.width() - width) // 2) + y = max(margin, (host.height() - height) // 2) + top_left = host.mapToGlobal(QPoint(x, y)) + dialog.setGeometry(top_left.x(), top_left.y(), width, height) def toggle_fullscreen(self): if self.isFullScreen(): @@ -4240,6 +4373,10 @@ class GripperDemo02Viewer(QMainWindow): def start_control(self, from_restart=False): if self.control_thread is not None and self.control_thread.is_alive(): return + # Re-scan serial before connect. Camera IDs stay at default 0/1 unless + # the user changed them; avoid opening cameras here (actuator will). + self.search_main_serial_ports(silent=True, show_popup=False) + self._sync_runtime_serial_port(persist_live=True) self.force_history.clear() self._reset_plot_y_ranges() self.start_time = time.time() @@ -4254,8 +4391,6 @@ 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( @@ -4312,8 +4447,12 @@ class GripperDemo02Viewer(QMainWindow): action = status.get("action", "--") unit = status.get("normal_unit", "N") shear_unit = status.get("shear_unit", "N") - self.status_chip.setText(f"state={label_with_cn(state, STATE_TEXT)}") - self.action_label.setText(f"action={label_with_cn(action, ACTION_TEXT)}") + self.status_chip.setText( + shorten_status_text(f"state={label_with_cn(state, STATE_TEXT)}", 28) + ) + self.action_label.setText( + shorten_status_text(f"action={label_with_cn(action, ACTION_TEXT)}", 28) + ) self.manual_hold_button.setText( "释放" if bool(status.get("manual_hold_active")) else "保持" ) @@ -4391,10 +4530,10 @@ class GripperDemo02Viewer(QMainWindow): elapsed, plot_force_value(status.get("left_normal", 0.0) or 0.0), plot_force_value(status.get("right_normal", 0.0) or 0.0), - plot_force_value(status.get("left_shear_x", 0.0) or 0.0), - plot_force_value(status.get("left_shear_y", 0.0) or 0.0), - plot_force_value(status.get("right_shear_x", 0.0) or 0.0), - plot_force_value(status.get("right_shear_y", 0.0) or 0.0), + plot_force_value(status.get("left_shear_x", 0.0) or 0.0, signed=True), + plot_force_value(status.get("left_shear_y", 0.0) or 0.0, signed=True), + plot_force_value(status.get("right_shear_x", 0.0) or 0.0, signed=True), + plot_force_value(status.get("right_shear_y", 0.0) or 0.0, signed=True), ) ) if len(self.force_history) > PLOT_POINTS: @@ -4415,12 +4554,14 @@ class GripperDemo02Viewer(QMainWindow): "right_shear_y", ] for idx, key in enumerate(keys, start=1): - values = smooth_plot_values(np.maximum(0.0, data[:, idx])) + series = data[:, idx] + if key not in SIGNED_FORCE_PLOT_KEYS: + series = np.maximum(0.0, series) + values = smooth_plot_values(series) self.curves[key].setData(x, values) baseline = self.baseline_curves.get(key) if baseline is not None: baseline.setData(x, np.zeros_like(values)) - self._update_plot_y_range(key, values) if len(x) >= 2: x_max = max(6.0, float(np.ceil(x[-1]))) x_min = max(0.0, x_max - 6.0) @@ -4459,7 +4600,7 @@ class GripperDemo02Viewer(QMainWindow): if ( self.visual_settings_dialog is not None and self.visual_settings_dialog.isVisible() - and self.visual_settings_dialog.parentWidget() is self.centralWidget() + and self.isFullScreen() ): self._position_embedded_settings()