Compare commits
6 Commits
dev
..
d948883e93
| Author | SHA1 | Date | |
|---|---|---|---|
 cheng
|
d948883e93 | ||
|
cheng
|
8be8a80cff | ||
|
cheng
|
6393be9518 | ||
|
cheng
|
c064cdb191 | ||
|
cheng
|
858e020c77 | ||
|
cheng
|
daa0ec8aec |
+5
-5
@@ -20,13 +20,15 @@ ENV/
|
||||
.python-version
|
||||
pip-wheel-metadata/
|
||||
|
||||
# Python packaging and PyInstaller output
|
||||
# Local working / generated directories
|
||||
build/
|
||||
debug/
|
||||
logs/
|
||||
tests/
|
||||
dist/
|
||||
wheelhouse/
|
||||
*.egg-info/
|
||||
.eggs/
|
||||
build/_pyinstaller/
|
||||
build/*.spec
|
||||
*.manifest
|
||||
|
||||
# Test, coverage, and profiling output
|
||||
@@ -36,12 +38,10 @@ coverage.xml
|
||||
htmlcov/
|
||||
prof/
|
||||
*.prof
|
||||
debug/
|
||||
|
||||
# Runtime logs and machine-local state
|
||||
*.log
|
||||
log/
|
||||
logs/
|
||||
debug.txt
|
||||
config/gui_layout.json
|
||||
config/gui_layout.json.tmp
|
||||
|
||||
@@ -11,14 +11,19 @@
|
||||
5. 闭合过程中检测到物体后，切到低力并保持当前位置。
|
||||
6. 像 01 一样进入 `gripping`，先慢慢加力到基础 `HOLD_FORCE`。
|
||||
7. 进入 `hold_check` 后，根据切向力估计物体负载；切向力越大，目标夹紧力越高，并按小步进继续补力。
|
||||
8. 自适应补力停止并稳定 `RELEASE_ARM_DELAY_SECONDS` 后，重新记录释放参考力，才开始检测人取物松开。
|
||||
9. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
|
||||
8. 接触起立即检测新的侧触；侧触释放不等待固定夹稳计时。普通法向变化另有短保护，无需等爬力完成。
|
||||
9. 新外力偏离既有曲线且持续，经新帧确认后张开到 `OPEN_POS`；原曲线变平、斜率变号和自然回落继续跟随，不靠累计位移释放。
|
||||
10. 张开后进入 `open_recover`，保持张开并等待恢复条件。
|
||||
11. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，继续休息至少 `REARM_SECONDS`。
|
||||
12. 休息结束后才回到 `open_wait` 并重新武装；休息期间不管有没有物体扫过，都不会闭合。
|
||||
|
||||
## 运行
|
||||
|
||||
GUI 的完整模块说明、操作流程、参数表和故障排查见
|
||||
[GUI 使用说明书](docs/GUI_USER_MANUAL.zh-CN.md)。
|
||||
正式产品使用说明书（HTML，含界面截图，可打印/导出 PDF）见
|
||||
[产品使用说明书](docs/PRODUCT_MANUAL.zh-CN.html)。
|
||||
|
||||
从项目根目录运行：
|
||||
|
||||
```powershell
|
||||
@@ -86,6 +91,17 @@ python build\build_exe.py --profile lite --clean-dist
|
||||
python build\build_exe.py --profile full --clean-dist
|
||||
```
|
||||
|
||||
需要包含源码和 Python 3.11 虚拟环境的 Windows x64 包时，在 `py311`
|
||||
环境运行：
|
||||
|
||||
```powershell
|
||||
conda activate py311
|
||||
python build\build_venv_bundle.py
|
||||
```
|
||||
|
||||
该包使用精简运行环境，不包含 Torch/CUDA、CuPy 和 Open3D。输出位于
|
||||
`release\GripperDemo02-windows-x64-py311-venv-YYYYMMDD.zip`。
|
||||
|
||||
轻量配置要求传感器配置中的 `tracker.algorithm` 为 `dis`；打包脚本会在
|
||||
构建前检查并阻止错误的 RAFT 轻量包。
|
||||
|
||||
@@ -137,14 +153,14 @@ python -m gripper.visualizer --windowed
|
||||
|
||||
- `trigger_normal_force`: 单侧法向力触发阈值。
|
||||
- `trigger_shear_force`: 单侧切向力触发阈值。
|
||||
- `trigger_clear_stable_seconds`: 张开恢复后，触发力必须清掉并稳定多久，才允许下一次扫过触发。
|
||||
- `trigger_clear_stable_seconds`: 旧配置兼容字段；当前稳定基线增量触发不使用它。
|
||||
- `grip_normal_force`: 夹住时左右接触阈值。
|
||||
- `grip_requires_both`: 是否必须左右都接触才算夹住。
|
||||
- `grip_start_force`: 检测到物体后先切到的低力。
|
||||
- `force_ramp_step`: 慢慢加力时每次增加多少 force_pct。
|
||||
- `force_ramp_interval`: 慢慢加力时两次加力的间隔。
|
||||
- `hold_force`: 慢慢加力到这个值后进入 `hold_check`。
|
||||
- `grip_settle_seconds`: 夹住后等待稳定多久再记录参考力。
|
||||
- `grip_settle_seconds`: `gripping` 进入 `hold_check` 的最早时间，不控制释放保护。
|
||||
- `adaptive_grip_enabled`: 是否启用按切向力估计负载并自适应补夹紧力。
|
||||
- `adaptive_shear_start_force`: 切向力低于这个值时，认为是轻物体，只使用最小目标力。
|
||||
- `adaptive_shear_full_force`: 切向力达到这个值时，认为负载较大，目标力加到最大目标力。
|
||||
@@ -152,9 +168,19 @@ python -m gripper.visualizer --windowed
|
||||
- `adaptive_force_max`: 自适应夹持的最大目标力；重物夹不住优先加这个，但不要超过 `force_max`。
|
||||
- `adaptive_force_step`: 自适应补力时每次增加多少 `force_pct`。
|
||||
- `adaptive_force_interval`: 自适应补力两次之间的间隔，越大越柔和。
|
||||
- `release_arm_delay_seconds`: 自适应补力停止后，等待多久再启用“人取物松开”检测。
|
||||
- `release_normal_change`: 夹住后法向力变化超过它就松开。
|
||||
- `release_shear_change`: 夹住后切向力变化超过它就松开。
|
||||
- `release_arm_delay_seconds`: 自运动实际命令执行完成后的短保护，默认 0.12s，不是旧式总释放延时。
|
||||
- `release_min_hold_seconds`: 普通法向接触保护，默认 0.28s；即时侧触路径不等待它。
|
||||
- `release_settle_seconds`: 脉冲拒绝/断流后的短恢复保护，默认 0.06s；不再要求全局持续安静。
|
||||
- `release_normal_rate` / `release_shear_rate`: 突变释放的速率门槛，默认 1.6 N/s。
|
||||
- `release_min_change`: 普通突变/轻触的最小净变化，默认 0.04 N。
|
||||
- `release_side_touch_change`: 即时侧触的左右力差变化，默认 0.06 N。
|
||||
- `release_touch_window_seconds`: 新外力起始证据有效期，默认 0.2s；不是等待时间。
|
||||
- `release_confirm_samples`: 突变确认新帧数，默认 2，包含首帧；平台偏移也能确认。
|
||||
- `release_slow_rate`: 慢变日志分类速率，默认 0.4 N/s；参考跟随由曲线延续决定，不门控就绪。
|
||||
- `release_normal_change` / `release_shear_change`: 中速外力累积的弱路径阈值，默认 0.28 N。
|
||||
- `release_slow_confirm_samples`: 弱路径确认新帧数，默认 5。
|
||||
- `release_confirm_window_seconds`: 释放候选最长有效期，默认 0.2s。
|
||||
- `release_sample_timeout_seconds`: 采样断流后重新等待平稳的门槛，默认 0.2s。
|
||||
- `release_open_speed`: 检测到松开后，张开到 `open_pos` 使用的速度。
|
||||
- `release_open_accel`: release 张开的加速度。
|
||||
- `release_open_decel`: release 张开的减速度。
|
||||
@@ -175,7 +201,7 @@ python -m gripper.visualizer --windowed
|
||||
- `maxN`: 左右法向力最大值，单位 N。
|
||||
- `shear`: 左右切向力最大值，单位 N。
|
||||
- `trigger`: 是否满足单侧触发条件。
|
||||
- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 且是新的一次触发上升沿才会闭合。
|
||||
- `armed`: 是否已经获得稳定通道基线；当前值相对该基线增长到触发阈值才会闭合。
|
||||
- `grip_contact`: 是否满足夹住接触条件。
|
||||
- `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。
|
||||
- `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。
|
||||
@@ -183,7 +209,8 @@ python -m gripper.visualizer --windowed
|
||||
- `hold_pos`: 夹住后保持的位置。
|
||||
- `force_pct`: 当前夹爪目标力百分比。
|
||||
- `target`: 当前根据切向力计算出来的自适应目标力。
|
||||
- `release_armed`: 是否已经启用释放检测；`0` 表示还在补力/等待稳定，`1` 表示法向或切向变化会触发张开。
|
||||
- `release_armed`: 普通监视或候选确认；独立即时侧触是否运行还需看 `side_watch`。
|
||||
- `side_watch`: 即时侧触监视，普通法向保护期间也可以为 `1`。
|
||||
- `action`: 当前动作。
|
||||
|
||||
## 可视化展示
|
||||
@@ -194,7 +221,7 @@ python -m gripper.visualizer --windowed
|
||||
- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。
|
||||
- 当前 `force_pct`、自适应目标力、速度 `speed_pct`、`hold_pos`。
|
||||
- 顶部有 `重启` 按钮，会先停止当前控制线程，释放相机与夹爪后再重新启动。
|
||||
- 当前 `release_armed`，用于判断系统是在补夹紧力，还是已经开始检测人取物松开。
|
||||
- 当前 `release_armed` 及 `release_side_watch`，分别表示普通释放与即时侧触检测。
|
||||
- 折线图带图例，默认固定 y 轴为 `0~3 N`，范围在 `visualizer.py` 顶部的 `PLOT_FORCE_Y_MIN_N / PLOT_FORCE_Y_MAX_N` 调整。
|
||||
- 左右传感器 FPS、接触标志、光流均值/最大值。
|
||||
- 中间 3D 夹爪使用 `crt_ctag2f90d_gripper_visualization` 里的完整 STL/URDF 实体面片模型；启动时会预缓存 41 个开合档位，运行中只按实际位置切换缓存，避免拖慢控制延迟。
|
||||
@@ -204,7 +231,7 @@ python -m gripper.visualizer --windowed
|
||||
- `open_wait`: 夹爪张开，等待任意单侧触发。
|
||||
- `closing`: 单侧触发后正在闭合，等待检测到物体。
|
||||
- `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。
|
||||
- `hold_check`: 已夹住；先根据切向力自适应补夹紧力，补力稳定后再监测法向/切向变化；变化超过阈值会直接张开。
|
||||
- `hold_check`: 已达到基础保持力；每 tick 先监视释放，再决定是否补力；候选确认优先于补力。
|
||||
- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。
|
||||
|
||||
## 常见动作说明
|
||||
@@ -212,14 +239,35 @@ python -m gripper.visualizer --windowed
|
||||
- `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。
|
||||
- `adaptive-grip-ramp`: 切向力显示负载较大，正在按小步进增加夹紧力。
|
||||
- `adaptive-grip-wait`: 已经需要更高目标力，但还没到下一次加力时间。
|
||||
- `release-arm-delay`: 补力刚停止，正在等待力稳定，暂时不触发松开。
|
||||
- `release-reference-armed`: 已重新记录释放参考力，后续法向/切向变化会触发张开。
|
||||
- `release-grip-settle`: 普通法向接触保护，独立即时侧触仍可检测。
|
||||
- `release-self-settle`: 普通法向自运动保护，独立即时切向侧触仍可检测。
|
||||
- `release-ready`: 夹稳通过，开始监视释放。
|
||||
- `release-sudden`: 突变外力候选及确认。
|
||||
- `release-touch`: 新的曲线起始变化及轻推偏移确认，不要求大瞬时速率。
|
||||
- `release-side_touch`: 即时侧触确认，不等固定夹稳时间。
|
||||
- `release-slow_internal`: 缓慢变化跟随参考。
|
||||
- `release-slow_external`: 新起始变化后达到弱路径幅度，经较长确认。
|
||||
- `release-transient-rejected`: 候选回弹或方向改变，丢弃此次证据。
|
||||
- `release-settling-turn` / `curve=settling_turn`: 平稳拐点，继续夹持。
|
||||
- `release-sample-gap`: 传感器断流，丢弃旧证据，等待新帧后恢复监视。
|
||||
- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。
|
||||
- `open-ready-position`: 夹爪已经回到初始点位，并且休息时间结束，重新进入等待下一次扫过触发；此时会直接武装，但残余触发保持不变不会自动闭合。
|
||||
- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。
|
||||
- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。
|
||||
- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。
|
||||
- `open-wait-trigger-held`: 已经武装，但当前触发不是新上升沿，等待力先降下去再下一次扫过。
|
||||
- `open-ready-position`: 夹爪已经回到初始点位，并且休息时间结束，重新进入等待下一次扫过触发；此时重新采集稳定基线，残余力保持不变不会自动闭合。
|
||||
- `open-wait-armed`: 已锁定稳定通道基线，下一次增量侧触可以闭合。
|
||||
- `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。
|
||||
- `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定和休息时间结束。
|
||||
- `open-ready`: 力已经清空并稳定，并且休息时间结束，重新进入等待下一次扫过触发。
|
||||
|
||||
## 释放状态设计与验证
|
||||
|
||||
[释放控制说明](docs/RELEASE_CONTROL.zh-CN.md) 包含独立 FSM、参数语义、曲线判据、
|
||||
回放命令及实机验收步骤。每次自运动稳定后会保留至少约 100ms 的观察窗口；基础爬力
|
||||
和自适应补力间隔因此可能比配置更长。侧触路径从接触就开始运行，普通法向与两侧共同变化仍有短保护。
|
||||
|
||||
```powershell
|
||||
python -m unittest discover -s tests -v
|
||||
```
|
||||
|
||||
软件回放验证稳定后外力至释放决定不超过 200ms；相机、串口和机械延迟需实机测量。
|
||||
|
||||
所有释放路径都要求新的曲线起始变化。闭合前的趋势会带入接触阶段，避免把原曲线的拐点当新触碰。
|
||||
每次自动释放后保存 `logs/release-last.json`，包含此前最多 120 帧控制快照与判据，用于复核实机误开。
|
||||
|
||||
@@ -1,252 +0,0 @@
|
||||
import argparse
|
||||
import importlib.util
|
||||
import json
|
||||
import shutil
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
BUILD_DIR = Path(__file__).resolve().parent
|
||||
PROJECT_ROOT = BUILD_DIR.parent
|
||||
VISUALIZATION_DIR = PROJECT_ROOT / "crt_ctag2f90d_gripper_visualization"
|
||||
LOGO_FILES = (
|
||||
PROJECT_ROOT / "gripper" / "logo.png",
|
||||
PROJECT_ROOT / "gripper" / "logo_topbar.png",
|
||||
)
|
||||
LITE_EXCLUDES = (
|
||||
# RAFT and its CUDA runtime. The default application uses OpenCV DIS.
|
||||
"torch",
|
||||
"torchvision",
|
||||
"torchaudio",
|
||||
"torchgen",
|
||||
# Optional Orisys GPU Poisson backends.
|
||||
"cupy",
|
||||
"cupyx",
|
||||
"cupy_backends",
|
||||
"pyopencl",
|
||||
"pyvkfft",
|
||||
# Optional Orisys mesh viewer and its notebook/web dependencies.
|
||||
"open3d",
|
||||
"dash",
|
||||
"flask",
|
||||
"plotly",
|
||||
"IPython",
|
||||
"jedi",
|
||||
"parso",
|
||||
"nbformat",
|
||||
"notebook",
|
||||
# Transitive data-science packages not used by this GUI.
|
||||
"matplotlib",
|
||||
"pandas",
|
||||
"sklearn",
|
||||
)
|
||||
|
||||
|
||||
def module_exists(name):
|
||||
return importlib.util.find_spec(name) is not None
|
||||
|
||||
|
||||
def require_module(name, install_hint):
|
||||
if module_exists(name):
|
||||
return
|
||||
raise SystemExit(f"Missing module '{name}'. Install it first: {install_hint}")
|
||||
|
||||
|
||||
def copy_runtime_dir(source, app_dir, name):
|
||||
target = app_dir / name
|
||||
if target.exists():
|
||||
shutil.rmtree(target)
|
||||
shutil.copytree(
|
||||
source,
|
||||
target,
|
||||
ignore=shutil.ignore_patterns(
|
||||
"__pycache__",
|
||||
"*.pyc",
|
||||
"gui_layout.json",
|
||||
"gui_layout.json.tmp",
|
||||
),
|
||||
)
|
||||
return target
|
||||
|
||||
|
||||
def directory_size(path):
|
||||
return sum(item.stat().st_size for item in path.rglob("*") if item.is_file())
|
||||
|
||||
|
||||
def print_size_report(app_dir, budget_mb):
|
||||
total_bytes = directory_size(app_dir)
|
||||
total_mb = total_bytes / (1024 * 1024)
|
||||
print(f"Package size: {total_mb:.1f} MiB ({total_bytes / 1_000_000:.1f} MB)")
|
||||
if total_mb > budget_mb:
|
||||
print(f"Warning: package exceeds the {budget_mb:.0f} MiB target.")
|
||||
|
||||
largest = sorted(
|
||||
(item for item in app_dir.rglob("*") if item.is_file()),
|
||||
key=lambda item: item.stat().st_size,
|
||||
reverse=True,
|
||||
)[:10]
|
||||
print("Largest packaged files:")
|
||||
for item in largest:
|
||||
print(f" {item.stat().st_size / (1024 * 1024):8.1f} MiB {item.relative_to(app_dir)}")
|
||||
|
||||
|
||||
def validate_lite_sensor_configs():
|
||||
control_path = PROJECT_ROOT / "config" / "gripper.json"
|
||||
try:
|
||||
control_config = json.loads(control_path.read_text(encoding="utf-8"))
|
||||
except (OSError, ValueError) as exc:
|
||||
raise SystemExit(f"Cannot validate lite build config '{control_path}': {exc}") from exc
|
||||
|
||||
config_names = {
|
||||
control_config.get("sensor_config1", "./config/ddjx01.json"),
|
||||
control_config.get("sensor_config2", "./config/ddjx01.json"),
|
||||
}
|
||||
for config_name in config_names:
|
||||
config_path = Path(config_name)
|
||||
if not config_path.is_absolute():
|
||||
config_path = PROJECT_ROOT / config_path
|
||||
try:
|
||||
sensor_config = json.loads(config_path.read_text(encoding="utf-8"))
|
||||
except (OSError, ValueError) as exc:
|
||||
raise SystemExit(f"Cannot validate lite sensor config '{config_path}': {exc}") from exc
|
||||
algorithm = str(sensor_config.get("tracker", {}).get("algorithm", "dis")).lower()
|
||||
if algorithm == "raft":
|
||||
raise SystemExit(
|
||||
f"Lite profile cannot package RAFT configured in '{config_path}'. "
|
||||
"Set tracker.algorithm to 'dis' or build with --profile full."
|
||||
)
|
||||
|
||||
|
||||
def build(app_name, clean_dist, profile, size_budget_mb):
|
||||
require_module("PyInstaller", "pip install pyinstaller")
|
||||
require_module("numba", "pip install numba")
|
||||
if profile == "lite":
|
||||
validate_lite_sensor_configs()
|
||||
|
||||
dist_dir = PROJECT_ROOT / "dist"
|
||||
work_dir = BUILD_DIR / "_pyinstaller"
|
||||
launcher = BUILD_DIR / "run_visualizer.py"
|
||||
|
||||
if clean_dist:
|
||||
app_dist = dist_dir / app_name
|
||||
if app_dist.exists():
|
||||
shutil.rmtree(app_dist)
|
||||
|
||||
args = [
|
||||
sys.executable,
|
||||
"-m",
|
||||
"PyInstaller",
|
||||
"--noconfirm",
|
||||
"--clean",
|
||||
"--onedir",
|
||||
"--windowed",
|
||||
"--name",
|
||||
app_name,
|
||||
"--distpath",
|
||||
str(dist_dir),
|
||||
"--workpath",
|
||||
str(work_dir),
|
||||
"--specpath",
|
||||
str(BUILD_DIR),
|
||||
"--paths",
|
||||
str(PROJECT_ROOT),
|
||||
"--paths",
|
||||
str(PROJECT_ROOT / "sdk"),
|
||||
"--hidden-import",
|
||||
"changingtek_p_rtu_Servo",
|
||||
"--hidden-import",
|
||||
"minimalmodbus",
|
||||
"--hidden-import",
|
||||
"serial",
|
||||
"--hidden-import",
|
||||
"cv2",
|
||||
"--hidden-import",
|
||||
"numpy",
|
||||
"--hidden-import",
|
||||
"pyqtgraph.opengl",
|
||||
]
|
||||
|
||||
if profile == "lite":
|
||||
for module_name in LITE_EXCLUDES:
|
||||
args.extend(["--exclude-module", module_name])
|
||||
|
||||
if not VISUALIZATION_DIR.exists():
|
||||
print(f"Warning: visualization assets were not found: {VISUALIZATION_DIR}")
|
||||
|
||||
for logo_file in LOGO_FILES:
|
||||
if not logo_file.exists():
|
||||
print(f"Warning: logo asset was not found: {logo_file}")
|
||||
|
||||
if module_exists("OpenGL"):
|
||||
args.extend(["--collect-all", "OpenGL"])
|
||||
else:
|
||||
print(
|
||||
"Warning: module 'OpenGL' was not found; install PyOpenGL for the solid 3D model."
|
||||
)
|
||||
|
||||
if module_exists("orisys"):
|
||||
args.extend(["--collect-submodules", "orisys"])
|
||||
else:
|
||||
print("Warning: module 'orisys' was not found; camera runtime may fail unless it is installed.")
|
||||
|
||||
args.append(str(launcher))
|
||||
|
||||
subprocess.run(args, cwd=PROJECT_ROOT, check=True)
|
||||
|
||||
executable_name = f"{app_name}.exe" if sys.platform == "win32" else app_name
|
||||
exe_path = dist_dir / app_name / executable_name
|
||||
if not exe_path.exists():
|
||||
raise SystemExit(f"Build finished but exe was not found: {exe_path}")
|
||||
|
||||
app_dir = exe_path.parent
|
||||
config_dir = copy_runtime_dir(PROJECT_ROOT / "config", app_dir, "config")
|
||||
sdk_dir = copy_runtime_dir(PROJECT_ROOT / "sdk", app_dir, "sdk")
|
||||
visualization_dir = None
|
||||
if VISUALIZATION_DIR.exists():
|
||||
visualization_dir = copy_runtime_dir(
|
||||
VISUALIZATION_DIR,
|
||||
app_dir,
|
||||
"crt_ctag2f90d_gripper_visualization",
|
||||
)
|
||||
runtime_logo_dir = app_dir / "gripper"
|
||||
runtime_logo_dir.mkdir(exist_ok=True)
|
||||
copied_logos = []
|
||||
for logo_file in LOGO_FILES:
|
||||
if logo_file.exists():
|
||||
copied_logos.append(shutil.copy2(logo_file, runtime_logo_dir / logo_file.name))
|
||||
|
||||
print()
|
||||
print(f"Build complete: {exe_path}")
|
||||
print(f"Runtime config copied to: {config_dir}")
|
||||
print(f"Runtime sdk copied to: {sdk_dir}")
|
||||
if visualization_dir is not None:
|
||||
print(f"Runtime 3D model copied to: {visualization_dir}")
|
||||
for logo_path in copied_logos:
|
||||
print(f"Runtime logo copied to: {logo_path}")
|
||||
print(f"Build profile: {profile}")
|
||||
print_size_report(app_dir, size_budget_mb)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--name", default="GripperDemo02", help="Output exe/app folder name.")
|
||||
parser.add_argument("--clean-dist", action="store_true", help="Delete dist/<name> before building.")
|
||||
parser.add_argument(
|
||||
"--profile",
|
||||
choices=("lite", "full"),
|
||||
default="lite",
|
||||
help="lite excludes RAFT/CUDA/CuPy/Open3D; full bundles installed optional backends.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--size-budget-mb",
|
||||
type=float,
|
||||
default=500.0,
|
||||
help="Print a warning when the onedir package exceeds this many MiB.",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
build(args.name, args.clean_dist, args.profile, args.size_budget_mb)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,19 +0,0 @@
|
||||
import multiprocessing as mp
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
if getattr(sys, "frozen", False):
|
||||
os.chdir(Path(sys.executable).resolve().parent)
|
||||
|
||||
from gripper.common.runtime_logging import configure_frozen_logging
|
||||
|
||||
configure_frozen_logging()
|
||||
|
||||
from gripper.visualizer import main
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
mp.freeze_support()
|
||||
raise SystemExit(main(sys.argv[1:]))
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 2.3 MiB After Width: | Height: | Size: 2.0 MiB |
@@ -0,0 +1,44 @@
|
||||
{
|
||||
"name": "prototype",
|
||||
"version": "1.0",
|
||||
"camera": {
|
||||
"resolution_raw": [1920, 1080],
|
||||
"fps": 30,
|
||||
"units": 15,
|
||||
"units_used": 9,
|
||||
"resolution_stitch": [365, 365],
|
||||
"stitch_res_list": [365,263,123,63]
|
||||
},
|
||||
"stitcher": {
|
||||
"min_area_frac": 0.01,
|
||||
"max_area_frac": 0.05,
|
||||
"max_aspect_ratio": 50,
|
||||
"index": [1,2,3,6,7,8,11,12,13],
|
||||
"margin": 10,
|
||||
"nominal_ov_pos": 100,
|
||||
"n": [3,3],
|
||||
"row_tol": 0.6,
|
||||
"box_size": [250, 250]
|
||||
},
|
||||
"tracker": {
|
||||
"adaptive": true,
|
||||
"lower_threshold": 12.0,
|
||||
"upper_threshold": 18.0,
|
||||
"loop_closure_interval": 1,
|
||||
"algorithm": "dis",
|
||||
"dis_preset": "medium",
|
||||
"dis_mode": 3,
|
||||
"algorithm_list": ["raft", "dis"],
|
||||
"hhd_downsample": 3
|
||||
},
|
||||
"contact": {
|
||||
"min_contour_area": 3000,
|
||||
"flow_threshold":5,
|
||||
"div_threshold":0.05,
|
||||
"expansion_threshold": 0.01,
|
||||
"kernel_size": [5,5],
|
||||
"denoise_sigma": 12
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+70
-34
@@ -1,8 +1,8 @@
|
||||
{
|
||||
"video1": "0",
|
||||
"video2": "1",
|
||||
"sensor_config1": "./config/ddjx01.json",
|
||||
"sensor_config2": "./config/ddjx01.json",
|
||||
"sensor_config1": "./config/ddjx01_q6a.json",
|
||||
"sensor_config2": "./config/ddjx01_q6a.json",
|
||||
"sensor_fps": 30.0,
|
||||
"motion_threshold": 0.0,
|
||||
"sensor_cpu": false,
|
||||
@@ -12,50 +12,80 @@
|
||||
"serial_timeout": 0.2,
|
||||
"open_pos": 0,
|
||||
"close_pos": 9000,
|
||||
"speed": 50,
|
||||
"accel": 80,
|
||||
"decel": 80,
|
||||
"release_open_speed": 50,
|
||||
"release_open_accel": 120,
|
||||
"release_open_decel": 120,
|
||||
"open_force": 20,
|
||||
"close_force": 20,
|
||||
"grip_start_force": 20,
|
||||
"hold_force": 20,
|
||||
"initial_force": 20,
|
||||
"speed": 30,
|
||||
"accel": 70,
|
||||
"decel": 70,
|
||||
"release_open_speed": 55,
|
||||
"release_open_accel": 70,
|
||||
"release_open_decel": 70,
|
||||
"open_force": 25,
|
||||
"close_force": 70,
|
||||
"grip_start_force": 70,
|
||||
"hold_force": 70,
|
||||
"initial_force": 25,
|
||||
"force_min": 20,
|
||||
"force_max": 80,
|
||||
"force_max": 100,
|
||||
"control_hz": 30.0,
|
||||
"baseline_seconds": 1.0,
|
||||
"filter_alpha": 0.55,
|
||||
"shear_filter_alpha": 0.65,
|
||||
"trigger_normal_force": 0.1,
|
||||
"trigger_shear_force": 0.1,
|
||||
"trigger_clear_stable_seconds": 0.2,
|
||||
"open_wait_stable_samples": 12,
|
||||
"open_wait_stable_range_n": 0.03,
|
||||
"filter_alpha": 0.42,
|
||||
"shear_filter_alpha": 0.52,
|
||||
"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,
|
||||
"open_wait_stable_trend_n": 0.015,
|
||||
"grip_normal_force": 0.2,
|
||||
"grip_requires_both": true,
|
||||
"close_timeout_seconds": 5.0,
|
||||
"close_timeout_seconds": 3.5,
|
||||
"closing_command_interval": 0.2,
|
||||
"force_ramp_step": 5,
|
||||
"force_ramp_interval": 0.02,
|
||||
"grip_settle_seconds": 0.05,
|
||||
"force_ramp_interval": 0.03,
|
||||
"grip_settle_seconds": 0.2,
|
||||
"grip_behavior_mode": "stable",
|
||||
"adaptive_grip_enabled": true,
|
||||
"adaptive_shear_start_force": 0.1,
|
||||
"adaptive_shear_full_force": 0.6,
|
||||
"adaptive_force_min": 15,
|
||||
"adaptive_force_max": 80,
|
||||
"adaptive_force_step": 1,
|
||||
"adaptive_force_interval": 0.05,
|
||||
"release_arm_delay_seconds": 0.8,
|
||||
"release_normal_change": 0.2,
|
||||
"release_shear_change": 0.2,
|
||||
"rearm_seconds": 0.5,
|
||||
"adaptive_force_min": 70,
|
||||
"adaptive_force_max": 70,
|
||||
"adaptive_force_step": 5,
|
||||
"adaptive_force_interval": 0.03,
|
||||
"release_arm_delay_seconds": 0.55,
|
||||
"release_min_hold_seconds": 2.0,
|
||||
"release_settle_seconds": 0.22,
|
||||
"release_min_change": 0.12,
|
||||
"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": false,
|
||||
"release_slope_hold_seconds": 0.9,
|
||||
"release_settle_normal_only": true,
|
||||
"release_shear_oscillation_open": false,
|
||||
"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": false,
|
||||
"release_shear_onset_enabled": true,
|
||||
"release_normal_onset_enabled": false,
|
||||
"rearm_seconds": 0.9,
|
||||
"recover_normal_force": 0.02,
|
||||
"recover_shear_force": 0.02,
|
||||
"recover_stable_seconds": 0.4,
|
||||
"recover_stable_seconds": 0.9,
|
||||
"open_position_recover_enabled": true,
|
||||
"open_position_tolerance": 100,
|
||||
"open_position_stable_seconds": 0.1,
|
||||
@@ -109,5 +139,11 @@
|
||||
"force_n": 2.06
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"release_guard_fast_paths": true,
|
||||
"grip_contact_hold_position": false,
|
||||
"release_shear_axes": "y",
|
||||
"release_shear_displacement": 0.6,
|
||||
"release_shear_displacement_samples": 6,
|
||||
"release_object_lost_open": true
|
||||
}
|
||||
|
||||
@@ -0,0 +1,775 @@
|
||||
# Orisys 夹爪 GUI 使用说明书
|
||||
|
||||
> **正式产品说明书（HTML，含截图）：** [产品使用说明书](PRODUCT_MANUAL.zh-CN.html)
|
||||
|
||||
> 文档版本：V0.1（快速初版）
|
||||
> 更新日期：2026-07-17
|
||||
> 适用对象：现场演示人员、设备调试人员、参数维护人员
|
||||
> 对应程序：Orisys 夹爪当前主分支 GUI
|
||||
|
||||
## 目录
|
||||
|
||||
1. [文档说明](#1-文档说明)
|
||||
2. [使用前检查](#2-使用前检查)
|
||||
3. [快速启动](#3-快速启动)
|
||||
4. [主界面总览](#4-主界面总览)
|
||||
5. [顶部状态与程序控制栏](#5-顶部状态与程序控制栏)
|
||||
6. [夹爪 3D 模型模块](#6-夹爪-3d-模型模块)
|
||||
7. [左右光流窗口](#7-左右光流窗口)
|
||||
8. [曲线图模块](#8-曲线图模块)
|
||||
9. [夹爪控制模块](#9-夹爪控制模块)
|
||||
10. [传感器控制模块](#10-传感器控制模块)
|
||||
11. [智能夹持标准流程](#11-智能夹持标准流程)
|
||||
12. [状态与动作说明](#12-状态与动作说明)
|
||||
13. [设置模块](#13-设置模块)
|
||||
14. [参数分组说明](#14-参数分组说明)
|
||||
15. [布局管理](#15-布局管理)
|
||||
16. [日志说明](#16-日志说明)
|
||||
17. [常见故障排查](#17-常见故障排查)
|
||||
18. [快捷键和退出](#18-快捷键和退出)
|
||||
19. [现场操作检查表](#19-现场操作检查表)
|
||||
|
||||
## 1. 文档说明
|
||||
|
||||
本说明书介绍 Orisys 夹爪图形界面的启动、监控、智能夹持、手动控制、参数设置、布局管理和故障排查。
|
||||
|
||||
系统由以下部分组成：
|
||||
|
||||
- 左、右两个视觉触觉传感器及摄像头。
|
||||
- 通过 Modbus RTU 串口控制的夹爪。
|
||||
- 触觉力计算和自动夹持控制程序。
|
||||
- PyQt5 GUI、夹爪 3D 模型、光流画面和六路力曲线。
|
||||
|
||||
数据和控制关系如下：
|
||||
|
||||
```text
|
||||
左/右摄像头 -> Orisys 触觉计算 -> 法向力/切向力/光流画面
|
||||
|
|
||||
v
|
||||
智能夹持状态机
|
||||
|
|
||||
v
|
||||
串口 <-------------------------- 夹爪控制器
|
||||
|
|
||||
v
|
||||
GUI 状态、3D 模型、曲线
|
||||
```
|
||||
|
||||
## 2. 使用前检查
|
||||
|
||||
### 2.1 硬件检查
|
||||
|
||||
1. 确认夹爪电源、控制器和机械连接正常。
|
||||
2. 确认左、右触觉传感器摄像头均已连接。
|
||||
3. 确认 USB 串口已经识别，Windows 下记下 `COM` 端口号。
|
||||
4. 清理夹爪开合范围内的物体，首次调试先使用较低速度和力度。
|
||||
5. 启动和重新标定基准时，让两侧触觉传感器保持空载、静止。
|
||||
|
||||
### 2.2 关键文件
|
||||
|
||||
打包版程序目录中的关键内容如下，运行时应保留完整目录结构：
|
||||
|
||||
```text
|
||||
GripperDemo02/
|
||||
├─ GripperDemo02.exe
|
||||
├─ _internal/
|
||||
├─ config/
|
||||
│ ├─ gripper.json
|
||||
│ ├─ ddjx01_q6a.json
|
||||
│ ├─ compound_eye.png
|
||||
│ └─ gui_layout.json # 保存布局后生成
|
||||
├─ sdk/
|
||||
├─ crt_ctag2f90d_gripper_visualization/
|
||||
└─ logs/ # EXE 首次运行后生成
|
||||
```
|
||||
|
||||
主要配置文件：
|
||||
|
||||
- `config/gripper.json`：摄像头、串口、位置、速度、力度和状态机参数。
|
||||
- `config/ddjx01_q6a.json`：触觉传感器和光流算法参数。
|
||||
- `config/gui_layout.json`：窗口、浮动面板、3D 视角和画面方向。
|
||||
|
||||
## 3. 快速启动
|
||||
|
||||
### 3.1 Windows 打包版
|
||||
|
||||
1. 解压整个发布包，不要只取出 EXE。
|
||||
2. 打开 `config/gripper.json`。
|
||||
3. 将 `video1`、`video2` 设置为左右摄像头编号，默认是 `"0"`、`"1"`。
|
||||
4. 将 `port` 设置为实际串口，例如 `"COM5"`。
|
||||
5. 双击 `GripperDemo02.exe`。
|
||||
6. GUI 显示后会自动启动控制程序，并默认进入智能模式。
|
||||
7. 启动阶段夹爪先张开，随后进行传感器基准标定。此时保持两侧传感器空载。
|
||||
8. 确认左右传感器状态点有数据、FPS 正常，并等待顶部显示 `open-wait-armed（已就绪）`。
|
||||
|
||||
程序窗口出现后通常不需要再次点击顶部“启动”图标。只有控制程序已经停止并回到空闲状态时，才使用该图标重新启动。
|
||||
|
||||
### 3.2 源码运行
|
||||
|
||||
Windows：
|
||||
|
||||
```powershell
|
||||
conda activate py311
|
||||
python -m gripper.visualizer
|
||||
```
|
||||
|
||||
窗口模式：
|
||||
|
||||
```powershell
|
||||
python -m gripper.visualizer --windowed
|
||||
```
|
||||
|
||||
只跳过真实夹爪串口动作、保留传感器和 GUI：
|
||||
|
||||
```powershell
|
||||
python -m gripper.visualizer --windowed --dry-run
|
||||
```
|
||||
|
||||
Linux：
|
||||
|
||||
```bash
|
||||
source .venv/bin/activate
|
||||
python -m gripper.visualizer --windowed
|
||||
```
|
||||
|
||||
Linux 下串口通常为 `/dev/ttyUSB0`，摄像头通常对应 `/dev/video0`、`/dev/video1`。Windows、Linux x86_64 和 Linux ARM64 使用各自平台构建的程序，Windows EXE 不跨平台运行。
|
||||
|
||||
## 4. 主界面总览
|
||||
|
||||
主界面分为以下模块：
|
||||
|
||||
| 模块 | 主要用途 |
|
||||
| --- | --- |
|
||||
| 顶部状态与程序控制栏 | 查看状态，打开设置，启动、停止或重启控制程序，管理窗口 |
|
||||
| 夹爪 3D 模型 | 查看夹爪实时开合位置、力比例和控制状态 |
|
||||
| 左右光流窗口 | 查看两个传感器的光流箭头图或幅值图 |
|
||||
| 曲线图区域 | 查看左右法向力、切向力 X/Y 的最近趋势 |
|
||||
| 夹爪控制区域 | 切换智能/手动模式，设置速度、最大力和位置，执行手动动作 |
|
||||
| 传感器控制区域 | 查看左右传感器状态、FPS、力值和夹爪运行参数，重启单侧传感器 |
|
||||
|
||||
光流窗口、曲线图、夹爪控制和传感器控制都是 3D 工作区上的浮动面板，可以移动和调整大小。
|
||||
|
||||
## 5. 顶部状态与程序控制栏
|
||||
|
||||
### 5.1 状态显示
|
||||
|
||||
顶部左侧显示两个字段：
|
||||
|
||||
- `state`：控制状态，例如等待触发、闭合中、夹持检测。
|
||||
- `action`：当前正在执行的细分动作，例如等待稳定、检测到物体、释放张开。
|
||||
|
||||
界面同时保留英文状态码和中文说明，便于和日志内容对应。
|
||||
|
||||
### 5.2 顶部图标
|
||||
|
||||
从设置图标开始，常用图标功能如下：
|
||||
|
||||
| 图标 | 功能 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| 设置 | 打开设置 | 调整画面、布局和 `gripper.json` 参数 |
|
||||
| 启动 | 启动控制程序 | 空闲状态可用；重新创建传感器、串口和控制器 |
|
||||
| 停止 | 停止控制程序 | 运行状态可用；停止过程需要等待设备释放 |
|
||||
| 重启 | 重启控制程序 | 先停止并释放旧控制器，再重新连接和标定 |
|
||||
| 最小化 | 最小化主窗口 | 不停止控制程序 |
|
||||
| 全屏 | 全屏/窗口切换 | 只改变显示方式 |
|
||||
| 关闭 | 退出程序 | 停止控制线程并关闭主窗口 |
|
||||
|
||||
鼠标悬停在图标上可查看提示。启动、停止、重启过程中，相关按钮会自动锁定，避免重复操作。
|
||||
|
||||
注意：顶部“停止”是软件控制流程的停止按钮，不是独立的硬件急停装置。
|
||||
|
||||
## 6. 夹爪 3D 模型模块
|
||||
|
||||
### 6.1 显示内容
|
||||
|
||||
3D 模型根据实时夹爪位置切换开合姿态。区域内还显示：
|
||||
|
||||
- 当前控制状态。
|
||||
- 左、右法向力。
|
||||
- 当前夹爪位置。
|
||||
- 当前力比例 `Force`。
|
||||
- 自适应目标力比例 `Target`。
|
||||
|
||||
### 6.2 视角操作
|
||||
|
||||
- 鼠标左键拖动：旋转观察视角。
|
||||
- 鼠标中键拖动：平移观察中心。
|
||||
- 鼠标滚轮：缩放模型。
|
||||
|
||||
保存当前布局时，3D 相机中心、距离、俯仰角、方位角和视场角会一并保存。
|
||||
|
||||
### 6.3 3D 模型异常
|
||||
|
||||
若该区域显示依赖或模型错误提示，依次检查：
|
||||
|
||||
1. `PyOpenGL` 和 `pyqtgraph.opengl` 是否存在。
|
||||
2. 显卡驱动和 OpenGL 是否正常。
|
||||
3. `crt_ctag2f90d_gripper_visualization` 目录及 STL/URDF 文件是否完整。
|
||||
4. EXE 是否仍位于完整打包目录内。
|
||||
|
||||
3D 模型异常不会改变配置文件，但会影响夹爪姿态的可视化观察。
|
||||
|
||||
## 7. 左右光流窗口
|
||||
|
||||
### 7.1 光流与幅值
|
||||
|
||||
左右窗口分别对应两个触觉传感器。点击窗口右上角的模式按钮，可在以下画面间切换：
|
||||
|
||||
- `光流`：显示光流方向箭头，用于观察触觉表面的位移方向。
|
||||
- `幅值`：显示光流变化强度，用于观察形变集中区域。
|
||||
|
||||
按钮文字表示当前显示模式。幅值画面暂时无数据时，窗口会回退显示光流画面。
|
||||
|
||||
### 7.2 移动和缩放
|
||||
|
||||
- 拖动窗口标题栏或画面区域可移动窗口。
|
||||
- 将鼠标移到窗口右下角约 18 像素区域，拖动可改变大小。
|
||||
- 图像始终保持原始宽高比，不会为了填满窗口而拉伸变形。
|
||||
- 窗口会被限制在 3D 工作区内，防止完全移出可视区域。
|
||||
|
||||
### 7.3 旋转和镜像
|
||||
|
||||
进入“设置 -> 画面”，左右画面可分别执行：
|
||||
|
||||
- `旋转`：每次顺时针旋转 90 度。
|
||||
- `水平`：切换水平镜像。
|
||||
- `垂直`：切换垂直镜像。
|
||||
- `重置`：恢复 0 度并关闭水平、垂直镜像。
|
||||
|
||||
画面变化立即生效。需要下次启动继续保留时，再进入“设置 -> 布局”执行“保存当前布局”。
|
||||
|
||||
## 8. 曲线图模块
|
||||
|
||||
### 8.1 曲线布局
|
||||
|
||||
曲线采用 3 行 x 2 列排列：
|
||||
|
||||
| 行 | 左列 | 右列 |
|
||||
| --- | --- | --- |
|
||||
| 第 1 行 | 左法向力 | 右法向力 |
|
||||
| 第 2 行 | 左切向力 X | 右切向力 X |
|
||||
| 第 3 行 | 左切向力 Y | 右切向力 Y |
|
||||
|
||||
横轴为时间，单位秒；纵轴为力，单位 N。
|
||||
|
||||
### 8.2 曲线显示规则
|
||||
|
||||
- 显示最近约 6 秒数据，最多保留 180 个 GUI 刷新点。
|
||||
- 默认 Y 轴范围为 `0~1 N`。
|
||||
- 数据超过当前范围后，Y 轴自动向上扩展。
|
||||
- 启动或重新启动控制程序时，曲线历史和 Y 轴范围重置。
|
||||
- 曲线经过平滑处理，适合观察趋势。
|
||||
- GUI 将小于 `0.05 N` 的显示值归零，负值也按 0 绘制。
|
||||
- 曲线区禁用了单图鼠标缩放和右键菜单，避免六张图的比例被分别改乱。
|
||||
|
||||
移动曲线面板时拖动“曲线图区域”标题，调整面板大小时拖动面板右下角。
|
||||
|
||||
## 9. 夹爪控制模块
|
||||
|
||||
### 9.1 智能模式
|
||||
|
||||
程序启动后默认选择智能模式。智能模式由触觉信号自动完成触发闭合、接触检测、逐步加力、自适应夹持和取物释放。
|
||||
|
||||
智能模式下，速度、最大力、位置滑条和手动动作按钮被锁定，避免手动命令干扰自动状态机。
|
||||
|
||||
### 9.2 手动模式
|
||||
|
||||
控制程序处于运行状态时，点击“手动模式”后可操作：
|
||||
|
||||
- 速度。
|
||||
- 最大力。
|
||||
- 夹爪位置。
|
||||
- 保持/释放。
|
||||
- 夹爪张开。
|
||||
- 夹爪闭合。
|
||||
|
||||
控制程序停止时，手动控件保持禁用。
|
||||
|
||||
### 9.3 速度
|
||||
|
||||
- 范围：`0~100%`。
|
||||
- 左右箭头步进：`5%`。
|
||||
- 拖动或点击调整后，本次运行的控制器速度会更新。
|
||||
- 运行时调整不会自动写回 `gripper.json`。
|
||||
|
||||
### 9.4 最大力
|
||||
|
||||
- 范围：`0~100%`。
|
||||
- 左右箭头步进：`5%`。
|
||||
- 调整滑条本身不会立即驱动夹爪。
|
||||
- 下一次执行张开、闭合、保持或指定位置动作时，该值随命令发送。
|
||||
- 实际执行值仍受 `force_min` 和 `force_max` 限制。
|
||||
|
||||
### 9.5 夹爪位置
|
||||
|
||||
- 可调范围由 `open_pos` 与 `close_pos` 的较小值和较大值确定。
|
||||
- 左右箭头步进：`100`。
|
||||
- 拖动滑条时只更新目标显示，释放滑条后发送位置命令。
|
||||
- 点击左右箭头后立即发送新的位置命令。
|
||||
- GUI 收到位置到达反馈后恢复跟随实际位置；长时间无到达反馈时约 3 秒后恢复跟随。
|
||||
|
||||
### 9.6 手动动作按钮
|
||||
|
||||
| 按钮 | 行为 |
|
||||
| --- | --- |
|
||||
| 夹爪张开 | 以当前速度、最大力移动到 `open_pos` |
|
||||
| 夹爪闭合 | 以当前速度、最大力移动到 `close_pos` |
|
||||
| 保持 | 读取当前位置，以当前最大力保持该位置；按钮文字变为“释放” |
|
||||
| 释放 | 取消手动保持状态，回到手动待机 |
|
||||
|
||||
“释放”不等同于发送张开命令。需要物理张开夹爪时，点击“夹爪张开”。
|
||||
|
||||
## 10. 传感器控制模块
|
||||
|
||||
### 10.1 传感器状态
|
||||
|
||||
左右传感器分别显示状态点和 FPS：
|
||||
|
||||
- 绿色：当前状态包中存在该侧传感器样本。
|
||||
- 红色：当前没有收到该侧传感器样本。
|
||||
- `--`：尚未获得有效状态或 FPS。
|
||||
|
||||
FPS 文本约每秒更新一次。短时间红点可能来自相机启动、重启或临时取帧中断，应结合日志判断。
|
||||
|
||||
### 10.2 单侧重启
|
||||
|
||||
左右传感器各有一个重启图标，仅在控制程序运行时启用。点击后会停止该侧旧采集进程、清理队列并重新拉起。
|
||||
|
||||
单侧重启不会重新执行整个控制程序的力基准标定。相机恢复后若力值零点明显漂移，应使用顶部“重启控制程序”，并在标定阶段保持传感器空载。
|
||||
|
||||
### 10.3 数据卡
|
||||
|
||||
| 数据 | 含义 |
|
||||
| --- | --- |
|
||||
| 左/右法向力 | 垂直触觉表面的力，单位 N |
|
||||
| 左/右切向力 | 触觉表面平面内的合力，单位 N |
|
||||
| 夹爪当前力比例 | 当前控制状态使用的力百分比 |
|
||||
| 夹爪目标力比例 | 自适应夹持计算出的目标力百分比 |
|
||||
| 速度 | 当前控制器速度百分比 |
|
||||
|
||||
## 11. 智能夹持标准流程
|
||||
|
||||
1. 启动控制程序后，夹爪先移动到张开位置。
|
||||
2. 系统在 `baseline_seconds` 内标定传感器基准，此时保持两侧空载。
|
||||
3. 状态进入 `open_wait（等待触发）`，先采集稳定样本。
|
||||
4. 顶部动作显示 `open-wait-armed（已就绪）` 后，用物体从任意一侧轻扫触觉传感器。
|
||||
5. 法向力或切向力变化达到阈值后，动作显示 `side-trigger-close`，夹爪开始闭合。
|
||||
6. 将物体移动到夹爪中间。默认配置要求左右两侧都达到接触阈值。
|
||||
7. 检测到物体后，夹爪先用较低力度保持当前位置，再逐步加到基础保持力。
|
||||
8. 系统根据切向力估计负载，按小步进提高目标夹紧力。
|
||||
9. 自适应加力稳定后，系统重新记录释放参考值并进入释放检测。
|
||||
10. 取走或移动物体造成法向力、切向力变化达到释放阈值时，夹爪张开。
|
||||
11. 状态进入 `open_recover（张开恢复）`，等待位置到位或残余力清除。
|
||||
12. 满足恢复和休息时间后重新进入等待触发。
|
||||
|
||||
若闭合超过 `close_timeout_seconds` 仍未检测到物体，系统会执行超时张开。
|
||||
|
||||
## 12. 状态与动作说明
|
||||
|
||||
### 12.1 state 状态
|
||||
|
||||
| state | 中文显示 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `open_wait` | 等待触发 | 建立稳定基准或等待新的侧边触发 |
|
||||
| `closing` | 闭合中 | 已触发闭合，正在寻找物体 |
|
||||
| `gripping` | 夹持加力 | 已检测到物体，正在逐步增加夹持力 |
|
||||
| `hold_check` | 夹持检测 | 自适应加力、保持及释放检测 |
|
||||
| `open_recover` | 张开恢复 | 已张开，等待位置或力信号恢复 |
|
||||
| `manual_close` | 手动闭合 | 手动闭合状态 |
|
||||
| `manual_hold` | 手动保持 | 手动保持当前位置 |
|
||||
| `manual_position` | 手动到位 | 手动移动到指定位置 |
|
||||
| `error` | 错误 | 控制线程发生异常，结合日志排查 |
|
||||
|
||||
### 12.2 常见 action 动作
|
||||
|
||||
| action | 含义 |
|
||||
| --- | --- |
|
||||
| `waiting-for-sensor-samples` | 等待左右传感器数据 |
|
||||
| `open-wait-stabilizing` | 正在建立稳定触发基准 |
|
||||
| `open-wait-armed` | 触发检测已经就绪 |
|
||||
| `side-trigger-close` | 侧边触发后开始闭合 |
|
||||
| `closing-to-grip` | 闭合寻找物体 |
|
||||
| `closing-continue` | 按间隔继续发送闭合命令 |
|
||||
| `object-detected-low-force` | 已检测到物体，切换到低力保持 |
|
||||
| `close-timeout-open` | 闭合超时，执行张开 |
|
||||
| `grip-ramp-up` | 正在逐步增加基础夹持力 |
|
||||
| `grip-hold` | 基础夹持力保持 |
|
||||
| `hold-reference-armed` | 已记录夹持参考值 |
|
||||
| `adaptive-grip-ramp` | 正在自适应增加夹持力 |
|
||||
| `adaptive-grip-wait` | 等待下一次自适应加力 |
|
||||
| `release-grip-settle` | 普通法向接触保护，即时侧触仍可检测 |
|
||||
| `release-self-settle` | 普通法向自运动保护，即时切向侧触仍可检测 |
|
||||
| `release-ready` | 释放检测已就绪 |
|
||||
| `release-sudden` | 突变外力新帧确认 |
|
||||
| `release-touch` | 新的曲线起始变化及轻推偏移确认 |
|
||||
| `release-side_touch` | 接触后即可识别的即时侧触确认 |
|
||||
| `release-slow_internal` | 慢变跟随参考 |
|
||||
| `release-slow_external` | 新起始变化后达到弱路径幅度，经较长确认 |
|
||||
| `release-transient-rejected` | 脉冲回弹，丢弃候选 |
|
||||
| `release-settling-turn` | 平稳曲线拐点或候选偏移回落，继续夹持 |
|
||||
| `release-sample-gap` | 采样断流，丢弃旧证据，等待新帧后恢复监视 |
|
||||
| `hold-check` | 正在监测保持状态 |
|
||||
| `force-change-release-open` | 检测到取物力变化并执行张开 |
|
||||
| `open-recover-position` | 正在等待张开位置恢复 |
|
||||
| `open-recover-position-stabilizing` | 张开位置已接近目标，等待稳定 |
|
||||
| `open-recover-wait-force-clear` | 等待残余触觉力清除 |
|
||||
| `open-recover-stabilizing` | 恢复条件已满足，等待稳定时间 |
|
||||
| `open-ready` / `open-ready-position` | 恢复完成，准备重新等待触发 |
|
||||
| `manual-*` | 当前执行的手动动作 |
|
||||
| `left-sensor-restart` / `right-sensor-restart` | 对应传感器正在重启 |
|
||||
| `*-sensor-restart-failed` | 对应传感器重启失败 |
|
||||
|
||||
## 13. 设置模块
|
||||
|
||||
点击顶部设置图标打开设置。窗口模式下，设置作为独立窗口显示；全屏模式下，设置面板嵌入主界面并可拖动。
|
||||
|
||||
### 13.1 画面页
|
||||
|
||||
分别调整左、右光流画面的旋转和镜像。修改立即生效，保存布局后跨次启动保留。
|
||||
|
||||
### 13.2 布局页
|
||||
|
||||
#### 保存当前布局
|
||||
|
||||
保存以下内容到 `config/gui_layout.json`：
|
||||
|
||||
- 主窗口普通状态的位置和大小。
|
||||
- 保存时的全屏或最大化状态。
|
||||
- 左右光流、曲线、夹爪控制、传感器控制面板的位置和大小。
|
||||
- 3D 相机中心、距离、俯仰角、方位角和视场角。
|
||||
- 左右画面的旋转和镜像。
|
||||
|
||||
#### 应用已保存布局
|
||||
|
||||
在当前会话中恢复：
|
||||
|
||||
- 浮动面板的位置和大小。
|
||||
- 3D 相机视角。
|
||||
- 左右画面的旋转和镜像。
|
||||
|
||||
该按钮不会改变当前主窗口大小、位置或全屏状态。完整窗口状态会在下次启动时恢复；使用 `--windowed` 启动时会保持窗口模式。
|
||||
|
||||
保存布局不包含曲线历史、当前光流/幅值选择、智能/手动模式、滑条临时值和控制运行状态。
|
||||
|
||||
### 13.3 参数页
|
||||
|
||||
设置界面按功能显示当前 `gripper.json` 中已有的参数：
|
||||
|
||||
- 相机 / 传感器。
|
||||
- 串口 / 设备。
|
||||
- 位置 / 运动。
|
||||
- 力度。
|
||||
- 控制 / 滤波。
|
||||
- 触发 / 夹持。
|
||||
- 自适应 / 释放。
|
||||
- 恢复。
|
||||
- 标定。
|
||||
- 其他未归类字段。
|
||||
|
||||
控件规则：
|
||||
|
||||
- 布尔值使用复选框。
|
||||
- 字典、数组使用 JSON 文本编辑框。
|
||||
- 数字和字符串使用单行输入框。
|
||||
|
||||
底部按钮：
|
||||
|
||||
- `重新载入`：重新读取磁盘配置，覆盖当前未保存编辑。
|
||||
- `保存到 gripper.json`：检查数据类型并写入配置文件。
|
||||
- `关闭`：关闭设置窗口。
|
||||
|
||||
保存配置后，点击顶部“重启控制程序”让全部参数重新加载。保存参数与保存布局是两个独立操作。
|
||||
|
||||
## 14. 参数分组说明
|
||||
|
||||
以下默认值来自当前 `config/gripper.json`。百分比参数通常使用 `0~100` 整数，力学阈值单位通常为 N，时间单位为秒。
|
||||
|
||||
### 14.1 相机 / 传感器
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `video1` | `"0"` | 左摄像头编号或设备路径 |
|
||||
| `video2` | `"1"` | 右摄像头编号或设备路径 |
|
||||
| `sensor_config1` | `./config/ddjx01_q6a.json` | 左触觉传感器配置 |
|
||||
| `sensor_config2` | `./config/ddjx01_q6a.json` | 右触觉传感器配置 |
|
||||
| `sensor_fps` | `30.0` | 传感器目标采集频率 |
|
||||
| `motion_threshold` | `0.0` | Orisys 形变/运动检测阈值 |
|
||||
| `sensor_cpu` | `false` | 是否要求传感器 SDK 使用 CPU 模式 |
|
||||
|
||||
### 14.2 串口 / 设备
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `port` | `COM5` | 夹爪串口号；Linux 示例 `/dev/ttyUSB0` |
|
||||
| `slave_id` | `1` | Modbus 从站 ID |
|
||||
| `baudrate` | `115200` | 串口波特率 |
|
||||
| `serial_timeout` | `0.2` | 单次串口操作超时时间 |
|
||||
|
||||
### 14.3 位置 / 运动
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `open_pos` | `0` | 完全张开目标位置 |
|
||||
| `close_pos` | `9000` | 完全闭合目标位置 |
|
||||
| `speed` | `50` | 默认运动速度百分比 |
|
||||
| `accel` | `80` | 默认加速度 |
|
||||
| `decel` | `80` | 默认减速度 |
|
||||
| `release_open_speed` | `50` | 检测到释放时的张开速度 |
|
||||
| `release_open_accel` | `120` | 释放张开的加速度 |
|
||||
| `release_open_decel` | `120` | 释放张开的减速度 |
|
||||
|
||||
### 14.4 力度
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `open_force` | `20` | 张开动作使用的力比例 |
|
||||
| `close_force` | `20` | 普通闭合动作使用的力比例 |
|
||||
| `grip_start_force` | `20` | 检测到物体后先使用的低力 |
|
||||
| `hold_force` | `20` | 基础保持目标力 |
|
||||
| `initial_force` | `20` | 配置展示字段；当前运行值由 `open_force` 生成 |
|
||||
| `force_min` | `20` | 所有夹爪力命令的下限 |
|
||||
| `force_max` | `80` | 所有夹爪力命令的上限 |
|
||||
|
||||
### 14.5 控制 / 滤波
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `control_hz` | `30.0` | 状态机目标控制频率 |
|
||||
| `baseline_seconds` | `1.0` | 启动时空载基准标定时间 |
|
||||
| `filter_alpha` | `0.55` | 法向力指数滤波权重；越大响应越快、平滑越弱 |
|
||||
| `shear_filter_alpha` | `0.65` | 切向力指数滤波权重 |
|
||||
| `dry_run` | `false` | 跳过真实夹爪串口动作，仅记录动作日志 |
|
||||
| `open_at_end` | `true` | 控制循环结束时尝试发送张开命令 |
|
||||
| `trigger_force_update` | `false` | 更新目标力后是否额外触发一次运动命令 |
|
||||
|
||||
### 14.6 触发 / 夹持
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `trigger_normal_force` | `0.1` | 相对稳定基准的法向触发阈值 |
|
||||
| `trigger_shear_force` | `0.1` | 相对稳定基准的切向触发阈值 |
|
||||
| `trigger_clear_stable_seconds` | `0.2` | 兼容配置字段；当前重新就绪主要由稳定样本和恢复参数控制 |
|
||||
| `open_wait_stable_samples` | `12` | 等待触发前所需稳定样本数 |
|
||||
| `open_wait_stable_range_n` | `0.03` | 稳定窗口允许的力值范围 |
|
||||
| `open_wait_stable_trend_n` | `0.015` | 稳定窗口允许的首尾趋势变化 |
|
||||
| `grip_normal_force` | `0.2` | 判断触碰到物体的法向力阈值 |
|
||||
| `grip_requires_both` | `true` | 是否要求左右两侧同时接触 |
|
||||
| `close_timeout_seconds` | `5.0` | 闭合寻找物体的最长时间 |
|
||||
| `closing_command_interval` | `0.2` | 闭合过程中重发命令的间隔 |
|
||||
| `force_ramp_step` | `5` | 基础夹持每次增加的力比例 |
|
||||
| `force_ramp_interval` | `0.02` | 基础夹持两次加力间隔 |
|
||||
| `grip_settle_seconds` | `0.05` | 到达基础保持力后的稳定时间 |
|
||||
|
||||
### 14.7 自适应 / 释放
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `adaptive_grip_enabled` | `true` | 启用按切向负载自适应加力 |
|
||||
| `adaptive_shear_start_force` | `0.1` | 开始提高目标力的切向力 |
|
||||
| `adaptive_shear_full_force` | `0.6` | 映射到最大目标力的切向力 |
|
||||
| `adaptive_force_min` | `15` | 自适应目标力下限 |
|
||||
| `adaptive_force_max` | `80` | 自适应目标力上限 |
|
||||
| `adaptive_force_step` | `1` | 自适应每次增加的力比例 |
|
||||
| `adaptive_force_interval` | `0.05` | 自适应两次加力间隔 |
|
||||
| `release_arm_delay_seconds` | `0.12` | 实际自运动命令执行完成后的短保护（秒），不是总释放延时 |
|
||||
| `release_min_hold_seconds` | `0.28` | 普通法向接触保护（秒），即时侧触不等待 |
|
||||
| `release_settle_seconds` | `0.06` | 脉冲/断流后的短恢复保护（秒） |
|
||||
| `release_normal_rate` / `release_shear_rate` | `1.6` | 突变速率门槛（N/s） |
|
||||
| `release_min_change` | `0.04` | 普通突变/轻触最小净变化（N） |
|
||||
| `release_side_touch_change` | `0.06` | 即时侧触的左右力差变化（N） |
|
||||
| `release_touch_window_seconds` | `0.2` | 新外力起始证据有效期（秒），非等待时间 |
|
||||
| `release_confirm_samples` | `2` | 突变确认新帧数，运行时至少 2 帧 |
|
||||
| `release_slow_rate` | `0.4` | 慢变日志分类速率（N/s），曲线跟随与就绪不由它门控 |
|
||||
| `release_normal_change` / `release_shear_change` | `0.28` | 中速外力累积的弱路径幅度（N） |
|
||||
| `release_slow_confirm_samples` | `5` | 弱路径确认新帧数 |
|
||||
| `release_confirm_window_seconds` | `0.2` | 确认有效窗口（秒） |
|
||||
| `release_sample_timeout_seconds` | `0.2` | 采样断流保护门槛（秒） |
|
||||
|
||||
每次自运动稳定后保留至少约 100ms 观察时间，候选确认优先于补力，因此实际加力
|
||||
间隔可能长于配置。`gripping` 期间也可以释放，无需等全部爬力完成。缓存帧不重复
|
||||
参与确认。详细曲线策略、保护窗口的取舍及实机验收见 [释放控制说明](RELEASE_CONTROL.zh-CN.md)。
|
||||
|
||||
### 14.8 恢复
|
||||
|
||||
| 参数 | 默认值 | 作用 |
|
||||
| --- | ---: | --- |
|
||||
| `rearm_seconds` | `0.5` | 张开后重新允许触发前的最短休息时间 |
|
||||
| `recover_normal_force` | `0.02` | 力恢复路径允许的最大法向残余力 |
|
||||
| `recover_shear_force` | `0.02` | 力恢复路径允许的最大切向残余力 |
|
||||
| `recover_stable_seconds` | `0.4` | 残余力满足阈值后的稳定时间 |
|
||||
| `open_position_recover_enabled` | `true` | 启用按张开位置判断恢复 |
|
||||
| `open_position_tolerance` | `100` | 实际位置接近 `open_pos` 的允许误差 |
|
||||
| `open_position_stable_seconds` | `0.1` | 张开位置到位后的稳定时间 |
|
||||
| `open_position_check_interval` | `0.1` | 读取张开位置的时间间隔 |
|
||||
| `hold_fallback_pos` | `null` | 读取不到当前位置时使用的保持位置；`null` 表示不指定 |
|
||||
|
||||
### 14.9 标定
|
||||
|
||||
`normal_force_calibration` 和 `shear_force_calibration` 是 JSON 对象，用于把传感器原始量映射到 N。常用字段：
|
||||
|
||||
- `enabled`：是否启用该标定。
|
||||
- `method`：当前使用分段线性映射 `piecewise_linear`。
|
||||
- `extrapolate`：是否允许超出标定点后继续外推。
|
||||
- `clamp_output_min`：输出下限。
|
||||
- `points`：原始值与真实力值的标定点列表。
|
||||
- `signed`：切向分量是否保留方向符号。
|
||||
- `input`：切向标定输入形式，当前为 `components`。
|
||||
|
||||
标定点会直接影响界面力值、触发阈值、接触判断和释放检测。修改前应备份 `gripper.json`，并使用测量数据核对单位和方向。
|
||||
|
||||
## 15. 布局管理
|
||||
|
||||
### 15.1 推荐保存步骤
|
||||
|
||||
1. 调整主窗口到需要的窗口或全屏状态。
|
||||
2. 移动并缩放左右光流窗口。
|
||||
3. 移动并缩放曲线、夹爪控制、传感器控制面板。
|
||||
4. 调整 3D 模型视角。
|
||||
5. 在“设置 -> 画面”调整左右画面方向。
|
||||
6. 在“设置 -> 布局”点击“保存当前布局”。
|
||||
|
||||
浮动面板使用相对于 3D 工作区的比例保存。窗口分辨率变化后，程序按比例恢复并把面板限制在可视区域内。
|
||||
|
||||
### 15.2 恢复默认布局
|
||||
|
||||
关闭程序后，将 `config/gui_layout.json` 备份并删除，再启动 GUI，即可使用程序默认布局。布局文件损坏、版本不匹配或不存在时，程序也会使用默认布局。
|
||||
|
||||
## 16. 日志说明
|
||||
|
||||
### 16.1 打包版日志
|
||||
|
||||
EXE 日志位于程序同级 `logs` 目录：
|
||||
|
||||
```text
|
||||
logs/
|
||||
├─ latest.txt
|
||||
└─ YYYYMMDD_HHMMSS_PID/
|
||||
├─ main.log
|
||||
├─ worker_PID.log
|
||||
└─ worker_PID.log
|
||||
```
|
||||
|
||||
- `latest.txt`：最近一次运行的会话目录名。
|
||||
- `main.log`：GUI、控制器、串口、性能信息和异常堆栈。
|
||||
- `worker_*.log`：左右传感器进程日志，也包含 OpenCV 原生警告。
|
||||
- 程序自动保留最近 10 次运行会话。
|
||||
|
||||
### 16.2 源码运行日志
|
||||
|
||||
源码方式运行时，日志主要输出在启动程序的终端窗口，不自动使用 EXE 的会话日志重定向。
|
||||
|
||||
### 16.3 推荐查看顺序
|
||||
|
||||
1. 打开 `logs/latest.txt`。
|
||||
2. 进入对应会话目录。
|
||||
3. 先看 `main.log` 最后的异常和 `state=error` 原因。
|
||||
4. 相机或光流问题再看两个 `worker_*.log`。
|
||||
5. 将 `main.log` 和 worker 日志一起保留，便于复现启动顺序和设备状态。
|
||||
|
||||
## 17. 常见故障排查
|
||||
|
||||
### 17.1 左右传感器没有画面或 FPS 为 `--`
|
||||
|
||||
检查顺序：
|
||||
|
||||
1. 检查摄像头 USB 连接和系统相机权限。
|
||||
2. 检查 `video1`、`video2` 是否对应实际编号，并确认没有填成同一个相机。
|
||||
3. 关闭其他正在占用摄像头的软件。
|
||||
4. 点击对应传感器的单侧重启图标。
|
||||
5. 仍未恢复时点击顶部“重启控制程序”。
|
||||
6. 查看对应 `worker_*.log`。
|
||||
|
||||
Windows 日志连续出现 `CvCapture_MSMF::grabFrame ... can't grab frame`，通常表示相机已经被打开，但系统未能继续取到视频帧。重点检查相机占用、USB 连接、带宽和设备编号。
|
||||
|
||||
### 17.2 串口连接失败
|
||||
|
||||
检查：
|
||||
|
||||
- `port` 是否为当前实际端口。
|
||||
- 夹爪电源是否开启。
|
||||
- `slave_id`、`baudrate` 是否与设备一致。
|
||||
- CH341 等串口驱动是否安装。
|
||||
- 串口是否被其他软件占用。
|
||||
- Linux 用户是否有 `dialout` 权限。
|
||||
|
||||
在 `main.log` 搜索 `serial`、`Modbus`、`motor is not connected` 和异常堆栈。
|
||||
|
||||
### 17.3 GUI 打开后立即进入 error
|
||||
|
||||
1. 查看顶部 `action` 中的异常摘要。
|
||||
2. 打开最近会话的 `main.log`。
|
||||
3. 检查两个传感器进程是否都成功输出样本。
|
||||
4. 检查串口配置和 Orisys SDK。
|
||||
5. 使用 `--dry-run` 区分夹爪串口问题和传感器问题。
|
||||
|
||||
### 17.4 力值漂移或空载不归零
|
||||
|
||||
1. 移开两侧传感器上的全部负载。
|
||||
2. 点击顶部“重启控制程序”。
|
||||
3. 在 `baseline_seconds` 标定阶段保持设备静止。
|
||||
4. 检查光照、摄像头固定和触觉表面是否发生变化。
|
||||
5. 核对标定 JSON 和传感器配置。
|
||||
|
||||
### 17.5 FPS 较低或 GUI 卡顿
|
||||
|
||||
1. 检查 `config/ddjx01_q6a.json` 中光流算法是否为 `dis`。
|
||||
2. 检查是否错误使用了完整 CUDA/RAFT 打包环境。
|
||||
3. 比较 `sensor_cpu=true` 与 `false` 的实际性能。
|
||||
4. 适当降低 `sensor_fps` 或传感器分辨率。
|
||||
5. 更新显卡驱动并检查 3D OpenGL 是否正常。
|
||||
6. 查看 `main.log` 中 `[perf]` 性能记录和 worker 日志。
|
||||
|
||||
### 17.6 保存参数后没有生效
|
||||
|
||||
1. 确认点击的是“保存到 gripper.json”。
|
||||
2. 确认保存提示显示成功。
|
||||
3. 关闭设置窗口。
|
||||
4. 点击顶部“重启控制程序”。
|
||||
5. 检查设置标题显示的配置文件路径是否是当前程序正在使用的文件。
|
||||
|
||||
### 17.7 布局异常或面板位置不合适
|
||||
|
||||
- 当前窗口内恢复：设置 -> 布局 -> 应用已保存布局。
|
||||
- 恢复默认布局：关闭程序，备份并删除 `config/gui_layout.json`，重新启动。
|
||||
- 更换显示器后，程序会尽量将主窗口和面板限制在可见区域，但不同 DPI 下不保证像素级一致。
|
||||
|
||||
## 18. 快捷键和退出
|
||||
|
||||
| 快捷键 | 功能 |
|
||||
| --- | --- |
|
||||
| `Esc` | 在全屏与普通窗口之间切换 |
|
||||
| `Q` | 退出 GUI |
|
||||
|
||||
正常退出会请求停止控制器，并等待控制线程结束。`open_at_end=true` 时，控制器结束流程会尝试让夹爪回到张开位置；设备断电、串口异常或控制器尚未成功连接时，以现场设备实际状态为准。
|
||||
|
||||
## 19. 现场操作检查表
|
||||
|
||||
### 启动前
|
||||
|
||||
- [ ] 左右摄像头已经识别。
|
||||
- [ ] 夹爪电源和串口已经识别。
|
||||
- [ ] `video1`、`video2`、`port` 配置正确。
|
||||
- [ ] 夹爪开合区域已经清理。
|
||||
- [ ] 两侧传感器表面无预加载。
|
||||
|
||||
### 启动后
|
||||
|
||||
- [ ] 左右传感器有 FPS，状态点正常。
|
||||
- [ ] 3D 模型和夹爪实际位置基本一致。
|
||||
- [ ] 顶部没有 `state=error`。
|
||||
- [ ] 智能模式等待到 `open-wait-armed` 后再触发。
|
||||
- [ ] 首次手动测试使用较低速度和最大力。
|
||||
|
||||
### 参数或布局修改后
|
||||
|
||||
- [ ] 参数已保存到 `gripper.json`。
|
||||
- [ ] 已重启控制程序加载新参数。
|
||||
- [ ] 布局已单独执行“保存当前布局”。
|
||||
- [ ] 最近一次运行日志可以正常生成和定位。
|
||||
|
||||
---
|
||||
|
||||
本版本为快速初版，后续版本可补充带编号的主界面截图、设备接线图、参数调试示例和现场验收记录表。
|
||||
|
||||
释放检测先识别相对原有曲线的新起始变化；自然减速、变平和拐点不因累计位移够大就开爪。
|
||||
实际自动释放前的逐帧数据保存在 `logs/release-last.json`，源码和打包启动均适用。
|
||||
@@ -0,0 +1,920 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<meta name="description" content="Orisys 夹爪产品使用说明书">
|
||||
<title>Orisys 夹爪产品使用说明书</title>
|
||||
<link rel="stylesheet" href="assets/manual.css">
|
||||
</head>
|
||||
<body>
|
||||
|
||||
<div class="manual-shell">
|
||||
|
||||
<nav class="manual-nav" aria-label="目录导航">
|
||||
<div class="manual-nav__brand">
|
||||
<h2>Orisys 夹爪</h2>
|
||||
<p>产品使用说明书<br>文档编号 GD02-UM-001</p>
|
||||
</div>
|
||||
<p class="nav-group">概述</p>
|
||||
<ul>
|
||||
<li><a href="#cover">封面</a></li>
|
||||
<li><a href="#revision">版本履历</a></li>
|
||||
<li><a href="#toc">目录</a></li>
|
||||
<li><a href="#sec-1">1. 引言</a></li>
|
||||
<li><a href="#sec-2">2. 安全须知</a></li>
|
||||
<li><a href="#sec-3">3. 产品说明</a></li>
|
||||
</ul>
|
||||
<p class="nav-group">安装与界面</p>
|
||||
<ul>
|
||||
<li><a href="#sec-4">4. 安装与配置</a></li>
|
||||
<li><a href="#sec-5">5. 启动与停止</a></li>
|
||||
<li><a href="#sec-6">6. 主界面说明</a></li>
|
||||
<li><a href="#sec-7">7. 功能模块</a></li>
|
||||
</ul>
|
||||
<p class="nav-group">操作</p>
|
||||
<ul>
|
||||
<li><a href="#sec-8">8. 智能夹持流程</a></li>
|
||||
<li><a href="#sec-9">9. 手动控制</a></li>
|
||||
<li><a href="#sec-10">10. 参数与设置</a></li>
|
||||
</ul>
|
||||
<p class="nav-group">维护</p>
|
||||
<ul>
|
||||
<li><a href="#sec-11">11. 状态与日志</a></li>
|
||||
<li><a href="#sec-12">12. 故障排除</a></li>
|
||||
<li><a href="#sec-13">13. 维护与检查</a></li>
|
||||
<li><a href="#sec-a">附录</a></li>
|
||||
</ul>
|
||||
</nav>
|
||||
|
||||
<main class="manual-main">
|
||||
<article class="manual-content">
|
||||
|
||||
<!-- 封面 -->
|
||||
<header class="cover" id="cover">
|
||||
<p class="cover__doc-type">用户手册 · User Manual</p>
|
||||
<h1 class="cover__title">Orisys 夹爪</h1>
|
||||
<p class="cover__subtitle">图形化监控与智能夹持控制软件</p>
|
||||
<table class="cover__meta">
|
||||
<tr><th>文档编号</th><td>GD02-UM-001</td></tr>
|
||||
<tr><th>文档版本</th><td>V1.0</td></tr>
|
||||
<tr><th>发布日期</th><td>2026-07-21</td></tr>
|
||||
<tr><th>适用软件</th><td>Orisys 夹爪 GUI（Release 20260717 及同系列版本）</td></tr>
|
||||
<tr><th>适用平台</th><td>Windows x64 / Linux x86_64</td></tr>
|
||||
<tr><th>文档密级</th><td>随产品交付资料</td></tr>
|
||||
</table>
|
||||
<p class="cover__footer">
|
||||
本文档为 Orisys 夹爪软件之正式使用说明。未经书面许可，不得擅自复制、修改或向第三方传播。<br>
|
||||
软件界面、参数默认值及功能以实际交付版本为准。
|
||||
</p>
|
||||
</header>
|
||||
|
||||
<!-- 版本履历 -->
|
||||
<section class="doc-section" id="revision">
|
||||
<div class="section-head">
|
||||
<span class="section-num">DOCUMENT CONTROL</span>
|
||||
<h2>版本履历</h2>
|
||||
</div>
|
||||
<table class="data-table revision-table">
|
||||
<caption>表 0-1 文档修订记录</caption>
|
||||
<thead>
|
||||
<tr>
|
||||
<th>版本</th>
|
||||
<th>日期</th>
|
||||
<th>修订人</th>
|
||||
<th>修订说明</th>
|
||||
</tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr>
|
||||
<td>V1.0</td>
|
||||
<td>2026-07-21</td>
|
||||
<td>—</td>
|
||||
<td>首版发布。含完整界面说明、操作流程、参数设置、故障排除及现场检查表。</td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<table class="data-table">
|
||||
<caption>表 0-2 适用读者</caption>
|
||||
<thead>
|
||||
<tr><th>读者角色</th><th>建议阅读章节</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>现场演示人员</td><td>第 2、5、6、8 章及附录 A</td></tr>
|
||||
<tr><td>设备调试人员</td><td>第 4、7、9、10、11、12 章</td></tr>
|
||||
<tr><td>参数维护人员</td><td>第 10、11、12 章及附录 B</td></tr>
|
||||
<tr><td>技术支持人员</td><td>全文及《GUI 详细技术说明》</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<div class="callout callout--note">
|
||||
<span class="callout__label">说明</span>
|
||||
<p>本手册描述软件操作与配置方法，不替代夹爪、传感器硬件厂商提供的机械安装与安全规范。硬件安装、电气接线须按对应硬件说明书执行。</p>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 目录 -->
|
||||
<nav class="toc" id="toc" aria-label="章节目录">
|
||||
<h2>目 录</h2>
|
||||
<ol>
|
||||
<li><a href="#sec-1">引言</a></li>
|
||||
<li><a href="#sec-2">安全须知</a></li>
|
||||
<li><a href="#sec-3">产品说明</a></li>
|
||||
<li><a href="#sec-4">安装与配置</a></li>
|
||||
<li><a href="#sec-5">启动与停止</a></li>
|
||||
<li><a href="#sec-6">主界面说明</a></li>
|
||||
<li><a href="#sec-7">功能模块操作</a></li>
|
||||
<li><a href="#sec-8">智能夹持标准流程</a></li>
|
||||
<li><a href="#sec-9">手动控制操作</a></li>
|
||||
<li><a href="#sec-10">参数配置与设置</a></li>
|
||||
<li><a href="#sec-11">运行状态与日志</a></li>
|
||||
<li><a href="#sec-12">故障排除</a></li>
|
||||
<li><a href="#sec-13">维护与定期检查</a></li>
|
||||
<li><a href="#sec-a">附录</a></li>
|
||||
</ol>
|
||||
</nav>
|
||||
|
||||
<!-- 第1章 -->
|
||||
<section class="doc-section" id="sec-1">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 1</span>
|
||||
<h2>引言</h2>
|
||||
</div>
|
||||
|
||||
<h3>1.1 编写目的</h3>
|
||||
<p>本手册用于指导用户正确安装、配置、操作和维护 Orisys 夹爪软件（以下简称「本软件」）。用户应在使用本软件前通读第 2 章安全须知，并按本手册规定步骤进行操作。</p>
|
||||
|
||||
<h3>1.2 产品简介</h3>
|
||||
<p>Orisys 夹爪是一套集成视觉触觉感知、智能夹持控制与图形化监控的演示系统。系统通过左右两个视觉触觉传感器采集法向力与切向力信息，经 Modbus RTU 串口驱动夹爪执行动作，并在 PyQt5 图形界面中同步显示 3D 模型、光流画面、力曲线及运行状态。</p>
|
||||
<p>本软件主要实现以下功能：</p>
|
||||
<ul>
|
||||
<li>单侧触觉触发后自动闭合、接触检测、分步加力、自适应夹持及取物释放；</li>
|
||||
<li>实时显示夹爪三维姿态、左右传感器光流/幅值图像及六路力值曲线；</li>
|
||||
<li>提供智能模式与手动模式，支持速度、力度、位置等参数调节；</li>
|
||||
<li>支持运行参数在线编辑、工作区布局保存及运行日志记录。</li>
|
||||
</ul>
|
||||
|
||||
<h3>1.3 术语与缩略语</h3>
|
||||
<table class="data-table data-table--compact">
|
||||
<caption>表 1-1 术语定义</caption>
|
||||
<thead>
|
||||
<tr><th>术语</th><th>含义</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>法向力</td><td>垂直于触觉敏感表面的力分量，单位 N</td></tr>
|
||||
<tr><td>切向力</td><td>沿触觉敏感表面平面内的合力，单位 N</td></tr>
|
||||
<tr><td>光流</td><td>基于图像序列估计的表面位移矢量场，用于可视化触觉形变</td></tr>
|
||||
<tr><td>智能模式</td><td>由状态机根据触觉信号自动控制夹爪，用户不可干预运动参数</td></tr>
|
||||
<tr><td>手动模式</td><td>用户通过界面直接下发张开、闭合、保持及位置命令</td></tr>
|
||||
<tr><td>state</td><td>控制状态机当前所处状态（如等待触发、闭合中、夹持检测等）</td></tr>
|
||||
<tr><td>action</td><td>当前控制周期内正在执行的具体动作描述</td></tr>
|
||||
<tr><td>dry-run</td><td>跳过真实串口动作、仅记录控制逻辑的调试模式</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<h3>1.4 引用文件</h3>
|
||||
<table class="data-table data-table--compact">
|
||||
<caption>表 1-2 相关文档</caption>
|
||||
<thead>
|
||||
<tr><th>文档名称</th><th>说明</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td><code>config/gripper.json</code></td><td>主运行配置文件</td></tr>
|
||||
<tr><td><code>config/ddjx01_q6a.json</code></td><td>触觉传感器及光流算法配置</td></tr>
|
||||
<tr><td><code>docs/GUI_USER_MANUAL.zh-CN.md</code></td><td>GUI 详细技术说明（完整参数表及深度故障排查）</td></tr>
|
||||
<tr><td><code>README.md</code></td><td>开发与打包说明</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
</section>
|
||||
|
||||
<!-- 第2章 -->
|
||||
<section class="doc-section" id="sec-2">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 2</span>
|
||||
<h2>安全须知</h2>
|
||||
</div>
|
||||
|
||||
<div class="callout callout--danger">
|
||||
<span class="callout__label">警告</span>
|
||||
<p>夹爪为运动部件，调试及演示过程中可能造成挤压伤害。操作前必须确认开合范围内无人员肢体及无关物体。首次运行须使用较低速度与力度。</p>
|
||||
</div>
|
||||
|
||||
<div class="callout callout--caution">
|
||||
<span class="callout__label">注意</span>
|
||||
<p>本软件顶部「停止」按钮仅停止软件控制流程，<strong>不是</strong>独立的硬件急停装置。发生异常时应立即切断夹爪电源或执行硬件急停（如设备具备）。</p>
|
||||
</div>
|
||||
|
||||
<h3>2.1 一般安全要求</h3>
|
||||
<ol>
|
||||
<li>仅经培训人员方可操作本系统；</li>
|
||||
<li>运行前检查夹爪安装牢固、电源及通信线缆连接可靠；</li>
|
||||
<li>传感器标定及系统重启标定阶段，须保持两侧触觉传感器空载且静止；</li>
|
||||
<li>修改力度、速度及触发阈值前，应备份 <code>config/gripper.json</code>；</li>
|
||||
<li>程序异常退出后，以夹爪实际物理状态为准，必要时手动断电复位。</li>
|
||||
</ol>
|
||||
|
||||
<h3>2.2 电气与设备安全</h3>
|
||||
<ul>
|
||||
<li>串口通信期间不得热插拔通信线缆（除非硬件支持）；</li>
|
||||
<li>USB 摄像头及串口设备应连接至稳定供电的 USB 端口，避免共用易过载的 USB Hub；</li>
|
||||
<li>Linux 环境下运行用户须具备 <code>video</code>、<code>dialout</code> 设备组权限。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<!-- 第3章 -->
|
||||
<section class="doc-section" id="sec-3">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 3</span>
|
||||
<h2>产品说明</h2>
|
||||
</div>
|
||||
|
||||
<h3>3.1 系统组成</h3>
|
||||
<p>本系统由感知层、软件处理层及执行与呈现层构成。左、右触觉传感器采集视频帧并计算触觉力；智能夹持状态机根据力信号经 Modbus RTU 串口驱动夹爪；GUI 同步显示 3D 姿态、光流图像、力曲线及运行状态。</p>
|
||||
<figure class="figure figure--diagram">
|
||||
<img src="assets/diagrams/system-architecture.svg" alt="Orisys 夹爪 系统组成与数据流示意图" width="1104" style="padding:1.25rem 1.5rem 0.75rem;background:#fff;">
|
||||
<figcaption class="figure__caption"><strong>图 3-1</strong> 系统组成与数据流示意图。实线为主要控制/数据流；虚线为位置反馈。</figcaption>
|
||||
</figure>
|
||||
|
||||
<h3>3.2 配置清单</h3>
|
||||
<table class="data-table">
|
||||
<caption>表 3-1 硬件配置参考清单</caption>
|
||||
<thead>
|
||||
<tr><th>序号</th><th>名称</th><th>数量</th><th>备注</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>1</td><td>智能夹爪及驱动控制器</td><td>1 套</td><td>Modbus RTU，USB 转串口连接</td></tr>
|
||||
<tr><td>2</td><td>视觉触觉传感器组件</td><td>2 套</td><td>左、右各一，含独立 USB 摄像头</td></tr>
|
||||
<tr><td>3</td><td>通信及电源线缆</td><td>若干</td><td>按现场安装要求配置</td></tr>
|
||||
<tr><td>4</td><td>运行计算机</td><td>1 台</td><td>算力版</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<h3>3.3 软件目录结构</h3>
|
||||
<p>发布包解压后须保持完整目录结构。禁止仅复制可执行文件至其他路径单独运行。</p>
|
||||
<pre class="code-block">GripperDemo02/
|
||||
├── GripperDemo02.exe # 主程序（Linux 无 .exe 后缀）
|
||||
├── _internal/ # 运行时依赖库（禁止删除）
|
||||
├── config/
|
||||
│ ├── gripper.json # 主配置文件
|
||||
│ ├── ddjx01_q6a.json # 传感器配置文件
|
||||
│ ├── compound_eye.png # 传感器资源
|
||||
│ └── gui_layout.json # 界面布局（保存后生成）
|
||||
├── sdk/ # 夹爪通信 SDK
|
||||
├── crt_ctag2f90d_gripper_visualization/ # 3D 模型资源
|
||||
└── logs/ # 运行日志（首次运行后自动生成）</pre>
|
||||
</section>
|
||||
|
||||
<!-- 第4章 -->
|
||||
<section class="doc-section" id="sec-4">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 4</span>
|
||||
<h2>安装与配置</h2>
|
||||
</div>
|
||||
|
||||
<h3>4.1 安装前检查</h3>
|
||||
<table class="data-table">
|
||||
<caption>表 4-1 安装前检查项目</caption>
|
||||
<thead>
|
||||
<tr><th>检查项</th><th>检查方法</th><th>合格标准</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>夹爪机械安装</td><td>目视及手动检查</td><td>安装牢固，开合路径无干涉</td></tr>
|
||||
<tr><td>夹爪电源</td><td>上电指示</td><td>控制器正常上电</td></tr>
|
||||
<tr><td>串口识别</td><td>设备管理器 / <code>dmesg</code></td><td>端口已识别（如 COM5、/dev/ttyUSB0）</td></tr>
|
||||
<tr><td>摄像头识别</td><td>系统相机应用或设备列表</td><td>两路摄像头均可独立打开</td></tr>
|
||||
<tr><td>工作区域</td><td>目视</td><td>夹爪开合范围内无杂物</td></tr>
|
||||
<tr><td>传感器状态</td><td>目视</td><td>触觉敏感面无预加载、无遮挡</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<h3>4.2 基本配置</h3>
|
||||
<p>使用文本编辑器打开 <code>config/gripper.json</code>，至少须正确配置以下参数后方可运行。</p>
|
||||
<table class="data-table">
|
||||
<caption>表 4-2 必配参数</caption>
|
||||
<thead>
|
||||
<tr><th>参数名</th><th>说明</th><th>Windows 示例</th><th>Linux 示例</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td><code>video1</code></td><td>左摄像头设备编号或路径</td><td><code>"0"</code></td><td><code>"0"</code> 或 <code>"/dev/video0"</code></td></tr>
|
||||
<tr><td><code>video2</code></td><td>右摄像头设备编号或路径</td><td><code>"1"</code></td><td><code>"1"</code> 或 <code>"/dev/video1"</code></td></tr>
|
||||
<tr><td><code>port</code></td><td>夹爪串口</td><td><code>"COM5"</code></td><td><code>"/dev/ttyUSB0"</code></td></tr>
|
||||
<tr><td><code>baudrate</code></td><td>串口波特率</td><td colspan="2"><code>115200</code>（须与设备一致）</td></tr>
|
||||
<tr><td><code>slave_id</code></td><td>Modbus 从站地址</td><td colspan="2"><code>1</code>（须与设备一致）</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<div class="callout callout--note">
|
||||
<span class="callout__label">说明</span>
|
||||
<p><code>video1</code> 与 <code>video2</code> 必须对应不同的物理摄像头。配置错误将导致单侧无数据或数据重复。</p>
|
||||
</div>
|
||||
|
||||
<h3>4.3 系统要求</h3>
|
||||
<p>本系统默认随<strong>算力版</strong>主机一并交付，出厂已完成操作系统、驱动及软件环境配置，用户开箱连接夹爪与传感器即可使用。若用户自备运行计算机，须满足表 4-3 所列环境要求，并自行完成摄像头、串口及依赖库配置。</p>
|
||||
<table class="data-table data-table--compact">
|
||||
<caption>表 4-3 自备计算机运行环境（可选）</caption>
|
||||
<thead>
|
||||
<tr><th>项目</th><th>要求</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>部署方式</td><td>算力版（默认交付）或用户自备计算机</td></tr>
|
||||
<tr><td>操作系统</td><td>Windows 10/11 x64，或 Ubuntu 20.04+ x86_64</td></tr>
|
||||
<tr><td>显示</td><td>分辨率 ≥ 1920×1080，支持 OpenGL 3.0+</td></tr>
|
||||
<tr><td>内存</td><td>≥ 8 GB</td></tr>
|
||||
<tr><td>USB</td><td>独立端口连接两路摄像头及串口设备</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
</section>
|
||||
|
||||
<!-- 第5章 -->
|
||||
<section class="doc-section" id="sec-5">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 5</span>
|
||||
<h2>启动与停止</h2>
|
||||
</div>
|
||||
|
||||
<h3>5.1 正常启动程序</h3>
|
||||
<ol>
|
||||
<li>确认第 4 章检查项目均合格；</li>
|
||||
<li>双击运行 <code>GripperDemo02.exe</code>（Linux：<code>./GripperDemo02</code>）；</li>
|
||||
<li>程序启动后自动创建控制线程，夹爪执行张开动作；</li>
|
||||
<li>系统在 <code>baseline_seconds</code>（默认 1.0 s）内完成传感器空载基准标定，此期间保持两侧传感器无接触；</li>
|
||||
<li>待顶部状态栏显示 <code>state=open_wait（等待触发）</code>，且 <code>action=open-wait-armed（已就绪）</code>；</li>
|
||||
<li>确认左右传感器状态指示为绿色，FPS 显示正常数值（非 <code>--</code>）。</li>
|
||||
</ol>
|
||||
|
||||
<div class="callout callout--note">
|
||||
<span class="callout__label">说明</span>
|
||||
<p>正常启动后无需再次点击顶部「启动」图标。仅当控制程序处于空闲（已停止）状态时，方使用该按钮重新建立设备连接。</p>
|
||||
</div>
|
||||
|
||||
<h3>5.2 正常停止与退出</h3>
|
||||
<table class="data-table">
|
||||
<caption>表 5-1 停止方式说明</caption>
|
||||
<thead>
|
||||
<tr><th>操作</th><th>作用范围</th><th>说明</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>顶部「停止」</td><td>控制线程</td><td>停止状态机及传感器采集，释放串口与摄像头资源</td></tr>
|
||||
<tr><td>顶部「关闭」或快捷键 Q</td><td>整个程序</td><td>先请求停止控制线程，等待结束后关闭窗口</td></tr>
|
||||
<tr><td>顶部「重启」</td><td>控制线程</td><td>等效于先停止再启动，重新标定传感器基准</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<p>当配置项 <code>open_at_end=true</code>（默认）时，控制流程结束时会尝试发送夹爪张开命令。若设备已断电或通信异常，以夹爪实际状态为准。</p>
|
||||
|
||||
<h3>5.3 源码方式运行（调试人员）</h3>
|
||||
<pre class="code-block"># Windows
|
||||
conda activate py311
|
||||
python -m gripper.visualizer --windowed
|
||||
|
||||
# 跳过真实串口（传感器与 GUI 仍运行）
|
||||
python -m gripper.visualizer --windowed --dry-run</pre>
|
||||
</section>
|
||||
|
||||
<!-- 第6章 -->
|
||||
<section class="doc-section" id="sec-6">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 6</span>
|
||||
<h2>主界面说明</h2>
|
||||
</div>
|
||||
|
||||
<p>本软件主界面由顶部状态控制栏、中央 3D 工作区及若干浮动功能面板组成。浮动面板可拖动、调整大小；布局可通过设置模块保存并在下次启动时恢复。</p>
|
||||
|
||||
<figure class="figure">
|
||||
<div class="figure-annotate figure--overview">
|
||||
<img src="images/manual/01-main-overview.png" alt="Orisys 夹爪 主界面" width="1600" height="960">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">A</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">B</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">C</span><span class="anno-label anno-label--c">左光流</span>
|
||||
<div class="anno-box anno-box--c-right"></div><span class="anno-tag anno-tag--c-right">C</span><span class="anno-label anno-label--c-right">右光流</span>
|
||||
<div class="anno-box anno-box--d"></div><span class="anno-tag anno-tag--d">D</span>
|
||||
<div class="anno-box anno-box--e"></div><span class="anno-tag anno-tag--e">E</span>
|
||||
<div class="anno-box anno-box--f"></div><span class="anno-tag anno-tag--f">F</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 6-1</strong> 主界面总览。彩色框标注各功能区域，对应表 6-1 编号。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">A</span> 顶部状态与控制栏</li>
|
||||
<li><span class="legend-tag legend-tag--b">B</span> 夹爪 3D 模型工作区</li>
|
||||
<li><span class="legend-tag legend-tag--c">C</span> 左/右光流窗口</li>
|
||||
<li><span class="legend-tag legend-tag--d">D</span> 六路力曲线图</li>
|
||||
<li><span class="legend-tag legend-tag--e">E</span> 夹爪控制区域</li>
|
||||
<li><span class="legend-tag legend-tag--f">F</span> 传感器控制区域</li>
|
||||
</ul>
|
||||
</figure>
|
||||
|
||||
<table class="data-table">
|
||||
<caption>表 6-1 主界面区域功能说明</caption>
|
||||
<thead>
|
||||
<tr><th>区域编号</th><th>名称</th><th>功能说明</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>A</td><td>顶部状态与控制栏</td><td>显示 <code>state</code>、<code>action</code>；提供设置、启停、重启及窗口管理</td></tr>
|
||||
<tr><td>B</td><td>夹爪 3D 模型工作区</td><td>显示夹爪实时开合姿态、当前状态及力/位置摘要</td></tr>
|
||||
<tr><td>C</td><td>左/右光流窗口</td><td>分别显示左、右传感器光流或幅值图像</td></tr>
|
||||
<tr><td>D</td><td>曲线图区域</td><td>六路力值（法向力、切向力 X/Y）实时趋势</td></tr>
|
||||
<tr><td>E</td><td>夹爪控制区域</td><td>智能/手动模式切换；速度、力度、位置及动作命令</td></tr>
|
||||
<tr><td>F</td><td>传感器控制区域</td><td>传感器在线状态、FPS、力值读数及单侧重启</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<figure class="figure figure--borderless">
|
||||
<div class="figure-annotate figure--topbar">
|
||||
<img src="images/manual/07-top-bar.png" alt="顶部状态与控制栏" width="1200" height="80">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">1</span><span class="anno-label anno-label--a">Logo</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">2</span><span class="anno-label anno-label--b">state / action</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">3</span><span class="anno-label anno-label--c">设置</span>
|
||||
<div class="anno-box anno-box--d"></div><span class="anno-tag anno-tag--d">4</span>
|
||||
<div class="anno-box anno-box--e"></div><span class="anno-tag anno-tag--e">5</span>
|
||||
<div class="anno-box anno-box--f"></div><span class="anno-tag anno-tag--f">6</span>
|
||||
<div class="anno-box anno-box--g"></div><span class="anno-tag anno-tag--g">7</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 6-2</strong> 顶部状态与控制栏局部放大标注。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">1</span> 品牌 Logo</li>
|
||||
<li><span class="legend-tag legend-tag--b">2</span> 运行状态（<code>state</code> / <code>action</code>）</li>
|
||||
<li><span class="legend-tag legend-tag--c">3</span> 设置</li>
|
||||
<li><span class="legend-tag legend-tag--d">4</span> 启动控制</li>
|
||||
<li><span class="legend-tag legend-tag--e">5</span> 停止控制</li>
|
||||
<li><span class="legend-tag legend-tag--f">6</span> 重启控制程序</li>
|
||||
<li><span class="legend-tag legend-tag--g">7</span> 最小化 / 全屏 / 关闭</li>
|
||||
</ul>
|
||||
</figure>
|
||||
</section>
|
||||
|
||||
<!-- 第7章 -->
|
||||
<section class="doc-section" id="sec-7">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 7</span>
|
||||
<h2>功能模块操作</h2>
|
||||
</div>
|
||||
|
||||
<h3>7.1 夹爪 3D 模型</h3>
|
||||
<p>3D 模型区域根据实时夹爪位置在预缓存的开合档位间切换，显示当前机械姿态。区域下方叠加显示左/右法向力、夹爪位置、当前力百分比及自适应目标力百分比。</p>
|
||||
<table class="data-table data-table--compact">
|
||||
<caption>表 7-1 3D 视图操作</caption>
|
||||
<thead>
|
||||
<tr><th>输入</th><th>操作</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>鼠标左键拖动</td><td>旋转观察视角</td></tr>
|
||||
<tr><td>鼠标中键拖动</td><td>平移观察中心</td></tr>
|
||||
<tr><td>鼠标滚轮</td><td>缩放</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<figure class="figure">
|
||||
<div class="figure-annotate figure--hold">
|
||||
<img src="images/manual/12-hold-check-state.png" alt="夹持检测状态界面" width="1600" height="960">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">1</span><span class="anno-label anno-label--a">state=hold_check</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">2</span><span class="anno-label anno-label--b">3D 开合姿态</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">3</span><span class="anno-label anno-label--c">力/位置摘要</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 7-1</strong> 夹持检测（<code>hold_check</code>）状态示例：标注顶部状态行、3D 模型区及底部实时摘要。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">1</span> 状态栏显示 <code>hold_check</code> / 当前动作</li>
|
||||
<li><span class="legend-tag legend-tag--b">2</span> 3D 模型反映当前开合档位</li>
|
||||
<li><span class="legend-tag legend-tag--c">3</span> 左/右法向力、位置、力比例摘要</li>
|
||||
</ul>
|
||||
</figure>
|
||||
|
||||
<h3>7.2 光流窗口</h3>
|
||||
<p>左、右光流窗口为独立浮动面板，分别对应两路触觉传感器。用户可通过右上角模式按钮在「光流」与「幅值」显示模式间切换。</p>
|
||||
<figure class="figure">
|
||||
<div class="figure-annotate figure--flow">
|
||||
<img src="images/manual/02-flow-windows.png" alt="左右光流窗口" width="1600" height="960">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">1</span><span class="anno-label anno-label--a">左侧传感器</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">2</span><span class="anno-label anno-label--b">右侧传感器</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">3</span><span class="anno-label anno-label--c">光流/幅值切换</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 7-2</strong> 光流窗口特写。标注左右面板及模式切换按钮位置。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">1</span> 左传感器光流/幅值画面</li>
|
||||
<li><span class="legend-tag legend-tag--b">2</span> 右传感器光流/幅值画面</li>
|
||||
<li><span class="legend-tag legend-tag--c">3</span> 「光流 / 幅值」模式切换按钮</li>
|
||||
</ul>
|
||||
</figure>
|
||||
<table class="data-table data-table--compact">
|
||||
<caption>表 7-2 光流窗口操作</caption>
|
||||
<thead>
|
||||
<tr><th>操作</th><th>方法</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>切换显示模式</td><td>点击窗口右上角「光流 / 幅值」按钮</td></tr>
|
||||
<tr><td>移动窗口</td><td>拖动标题栏或图像区域</td></tr>
|
||||
<tr><td>调整大小</td><td>拖动窗口右下角约 18 px 区域</td></tr>
|
||||
<tr><td>旋转 / 镜像</td><td>设置 → 画面 → 对应侧按钮（见 10.1 节）</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<h3>7.3 力曲线图</h3>
|
||||
<p>曲线区域以 3 行 × 2 列布局显示六路力值，横轴为时间（秒），纵轴为力（牛顿）。默认显示最近约 6 s 数据；显示值经平滑处理，绝对值低于 0.05 N 时归零显示。</p>
|
||||
<figure class="figure">
|
||||
<div class="figure-annotate figure--plots">
|
||||
<img src="images/manual/03-force-plots.png" alt="六路力曲线图" width="1600" height="960">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">D</span><span class="anno-label anno-label--a">曲线图区域</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">1</span><span class="anno-label anno-label--b">法向力</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">2</span>
|
||||
<div class="anno-box anno-box--d"></div><span class="anno-tag anno-tag--d">3</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 7-3</strong> 力曲线区域特写。三行分别对应法向力、切向力 X、切向力 Y。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">D</span> 曲线图浮动面板整体</li>
|
||||
<li><span class="legend-tag legend-tag--b">1</span> 第 1 行：左/右法向力</li>
|
||||
<li><span class="legend-tag legend-tag--c">2</span> 第 2 行：左/右切向力 X</li>
|
||||
<li><span class="legend-tag legend-tag--d">3</span> 第 3 行：左/右切向力 Y</li>
|
||||
</ul>
|
||||
</figure>
|
||||
|
||||
<h3>7.4 传感器控制区域</h3>
|
||||
<figure class="figure">
|
||||
<div class="figure-annotate figure--sensor">
|
||||
<img src="images/manual/06-sensor-control.png" alt="传感器控制区域" width="1600" height="960">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">F</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">1</span><span class="anno-label anno-label--b">在线状态</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">2</span>
|
||||
<div class="anno-box anno-box--d"></div><span class="anno-tag anno-tag--d">3</span><span class="anno-label anno-label--d">力值 / FPS</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 7-4</strong> 传感器控制区域特写。标注在线指示、FPS 及力值读数位置。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">F</span> 传感器控制浮动面板</li>
|
||||
<li><span class="legend-tag legend-tag--b">1</span> 绿/红状态点（是否收到样本）</li>
|
||||
<li><span class="legend-tag legend-tag--c">2</span> 左/右 FPS 显示</li>
|
||||
<li><span class="legend-tag legend-tag--d">3</span> 法向/切向力读数及重启按钮</li>
|
||||
</ul>
|
||||
</figure>
|
||||
<table class="data-table data-table--compact">
|
||||
<caption>表 7-3 传感器区域显示项</caption>
|
||||
<thead>
|
||||
<tr><th>显示项</th><th>说明</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>左/右法向力、切向力</td><td>经标定后的实时力值，单位 N</td></tr>
|
||||
<tr><td>夹爪当前力比例 / 目标力比例</td><td>控制器输出力百分比及自适应目标力百分比</td></tr>
|
||||
<tr><td>速度</td><td>当前控制器速度百分比</td></tr>
|
||||
<tr><td>重启图标</td><td>重启单侧传感器采集进程（不触发全程序基准重标定）</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<div class="callout callout--caution">
|
||||
<span class="callout__label">注意</span>
|
||||
<p>单侧重启仅恢复该侧相机采集。若力值零点出现明显漂移，应执行顶部「重启控制程序」，并在标定阶段保持传感器空载。</p>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- 第8章 -->
|
||||
<section class="doc-section" id="sec-8">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 8</span>
|
||||
<h2>智能夹持标准流程</h2>
|
||||
</div>
|
||||
|
||||
<p>智能模式为本软件默认工作模式。该模式下，夹爪动作由触觉信号驱动的状态机自动控制，用户不应在演示过程中切换至手动模式或干预运动参数。</p>
|
||||
|
||||
<figure class="figure figure--diagram">
|
||||
<img src="assets/diagrams/grip-state-flow.svg" alt="智能夹持状态流转图" width="1260" style="padding:1.25rem 1.5rem 0.75rem;background:#fff;">
|
||||
<figcaption class="figure__caption"><strong>图 8-1</strong> 智能夹持状态流转图。蓝色回环：恢复后重新就绪；红色虚线：闭合超时路径。</figcaption>
|
||||
</figure>
|
||||
|
||||
<h3>8.1 标准演示步骤</h3>
|
||||
<table class="data-table">
|
||||
<caption>表 8-1 现场演示操作步骤</caption>
|
||||
<thead>
|
||||
<tr><th>步骤</th><th>操作</th><th>预期现象</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>1</td><td>按 5.1 节完成启动，等待就绪</td><td><code>action=open-wait-armed（已就绪）</code></td></tr>
|
||||
<tr><td>2</td><td>用物体从任意一侧轻扫触觉传感器</td><td>力值上升；<code>action=side-trigger-close</code></td></tr>
|
||||
<tr><td>3</td><td>夹爪开始闭合，将物体移至钳口中央</td><td><code>state=closing</code> → <code>gripping</code></td></tr>
|
||||
<tr><td>4</td><td>系统检测接触后低力保持，再分步加力</td><td>3D 模型闭合；力比例逐步上升</td></tr>
|
||||
<tr><td>5</td><td>自适应补力完成后进入释放检测</td><td><code>state=hold_check</code></td></tr>
|
||||
<tr><td>6</td><td>取走或移动物体</td><td>法向/切向力变化超阈；夹爪张开</td></tr>
|
||||
<tr><td>7</td><td>等待恢复完成</td><td>返回 <code>open_wait</code>，可进行下一次演示</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<h3>8.2 运行约束</h3>
|
||||
<ul>
|
||||
<li>默认配置要求左右两侧均达到接触阈值（<code>grip_requires_both=true</code>）方判定夹持成功；</li>
|
||||
<li>闭合超时（默认 5 s，参数 <code>close_timeout_seconds</code>）未检测到物体时，系统自动张开；</li>
|
||||
<li>释放后须满足位置/力恢复条件并经过最短休息时间（<code>rearm_seconds</code>，默认 0.5 s），方可再次触发；</li>
|
||||
<li>休息期间即便传感器检测到扫过，也不会触发闭合。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<!-- 第9章 -->
|
||||
<section class="doc-section" id="sec-9">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 9</span>
|
||||
<h2>手动控制操作</h2>
|
||||
</div>
|
||||
|
||||
<p>控制程序处于运行状态时，用户可点击「手动模式」切换至手动控制。手动模式下，用户可直接设定速度、最大力及目标位置，并执行张开、闭合、保持等命令。</p>
|
||||
|
||||
<figure class="figure">
|
||||
<div class="figure-annotate figure--grip-smart">
|
||||
<img src="images/manual/04-gripper-control-smart.png" alt="智能模式夹爪控制面板" width="1600" height="960">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">E</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">1</span><span class="anno-label anno-label--b">智能模式（锁定）</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">2</span><span class="anno-label anno-label--c">滑条禁用</span>
|
||||
<div class="anno-box anno-box--d"></div><span class="anno-tag anno-tag--d">3</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 9-1</strong> 智能模式：模式切换、滑条与动作按钮均为灰色锁定状态。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">E</span> 夹爪控制面板</li>
|
||||
<li><span class="legend-tag legend-tag--b">1</span> 「智能模式」高亮，「手动模式」不可切换干预</li>
|
||||
<li><span class="legend-tag legend-tag--c">2</span> 速度 / 最大力 / 位置滑条禁用</li>
|
||||
<li><span class="legend-tag legend-tag--d">3</span> 张开 / 闭合 / 保持按钮禁用</li>
|
||||
</ul>
|
||||
</figure>
|
||||
|
||||
<figure class="figure">
|
||||
<div class="figure-annotate figure--grip-manual">
|
||||
<img src="images/manual/05-gripper-control-manual.png" alt="手动模式夹爪控制面板" width="1600" height="960">
|
||||
<div class="anno-box anno-box--a"></div><span class="anno-tag anno-tag--a">E</span>
|
||||
<div class="anno-box anno-box--b"></div><span class="anno-tag anno-tag--b">1</span><span class="anno-label anno-label--b">手动模式</span>
|
||||
<div class="anno-box anno-box--c"></div><span class="anno-tag anno-tag--c">2</span><span class="anno-label anno-label--c">动作按钮可用</span>
|
||||
</div>
|
||||
<figcaption class="figure__caption"><strong>图 9-2</strong> 手动模式：滑条与动作按钮均已启用，可直接下发命令。</figcaption>
|
||||
<ul class="figure__legend">
|
||||
<li><span class="legend-tag legend-tag--a">E</span> 夹爪控制面板</li>
|
||||
<li><span class="legend-tag legend-tag--b">1</span> 「手动模式」激活，可调整速度/力度/位置</li>
|
||||
<li><span class="legend-tag legend-tag--c">2</span> 保持、张开、闭合按钮可点击</li>
|
||||
</ul>
|
||||
</figure>
|
||||
|
||||
<table class="data-table">
|
||||
<caption>表 9-1 手动控制参数</caption>
|
||||
<thead>
|
||||
<tr><th>参数</th><th>范围</th><th>步进</th><th>生效方式</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>速度</td><td>0～100%</td><td>5%</td><td>调整后立即更新控制器速度</td></tr>
|
||||
<tr><td>最大力</td><td>0～100%</td><td>5%</td><td>随下一次动作命令发送；受 <code>force_min</code>、<code>force_max</code> 约束</td></tr>
|
||||
<tr><td>夹爪位置</td><td><code>open_pos</code>～<code>close_pos</code></td><td>100</td><td>释放滑条或点击步进按钮后发送位置命令</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<table class="data-table">
|
||||
<caption>表 9-2 手动动作按钮</caption>
|
||||
<thead>
|
||||
<tr><th>按钮</th><th>功能</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>夹爪张开</td><td>以当前速度、最大力运动至 <code>open_pos</code></td></tr>
|
||||
<tr><td>夹爪闭合</td><td>以当前速度、最大力运动至 <code>close_pos</code></td></tr>
|
||||
<tr><td>保持</td><td>读取当前位置并以当前最大力保持；按钮文字变为「释放」</td></tr>
|
||||
<tr><td>释放</td><td>取消手动保持状态，返回手动待机；<strong>不</strong>自动执行张开动作</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
</section>
|
||||
|
||||
<!-- 第10章 -->
|
||||
<section class="doc-section" id="sec-10">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 10</span>
|
||||
<h2>参数配置与设置</h2>
|
||||
</div>
|
||||
|
||||
<p>点击顶部设置图标打开设置窗口。设置窗口分为「画面」「布局」及若干参数分组页，参数分组内容直接映射 <code>config/gripper.json</code> 文件字段。</p>
|
||||
|
||||
<figure class="figure">
|
||||
<img src="images/manual/08-settings-visual.png" alt="设置窗口-画面页" width="980" height="680">
|
||||
<figcaption class="figure__caption"><strong>图 10-1</strong> 设置 — 画面页。可分别对左、右光流画面执行旋转（90° 步进）、水平镜像、垂直镜像及重置。</figcaption>
|
||||
</figure>
|
||||
|
||||
<figure class="figure">
|
||||
<img src="images/manual/09-settings-layout.png" alt="设置窗口-布局页" width="980" height="680">
|
||||
<figcaption class="figure__caption"><strong>图 10-2</strong> 设置 — 布局页。可将当前窗口尺寸、浮动面板位置、3D 视角及画面方向保存至 <code>config/gui_layout.json</code>。</figcaption>
|
||||
</figure>
|
||||
|
||||
<figure class="figure">
|
||||
<img src="images/manual/11-settings-serial.png" alt="设置窗口-串口参数" width="980" height="680">
|
||||
<figcaption class="figure__caption"><strong>图 10-3</strong> 设置 — 串口 / 设备页：配置 <code>port</code>、<code>baudrate</code>、<code>slave_id</code> 等通信参数。</figcaption>
|
||||
</figure>
|
||||
|
||||
<figure class="figure">
|
||||
<img src="images/manual/10-settings-trigger.png" alt="设置窗口-触发夹持参数" width="980" height="680">
|
||||
<figcaption class="figure__caption"><strong>图 10-4</strong> 设置 — 触发 / 夹持页：触发阈值、接触判定、闭合超时及分步加力参数。</figcaption>
|
||||
</figure>
|
||||
|
||||
<h3>10.1 参数保存程序</h3>
|
||||
<p>在设置窗口中修改参数后，须按以下顺序操作，否则运行中的控制程序不会加载新值：</p>
|
||||
<ol>
|
||||
<li>点击「保存到 gripper.json」，确认保存成功提示；</li>
|
||||
<li>关闭设置窗口；</li>
|
||||
<li>点击顶部「重启控制程序」。</li>
|
||||
</ol>
|
||||
|
||||
<div class="callout callout--caution">
|
||||
<span class="callout__label">注意</span>
|
||||
<p>「保存当前布局」与「保存到 gripper.json」为相互独立操作。布局文件 <code>gui_layout.json</code> 不包含运行参数；参数文件不包含曲线历史及当前运行状态。</p>
|
||||
</div>
|
||||
|
||||
<h3>10.2 常用参数速查</h3>
|
||||
<table class="data-table">
|
||||
<caption>表 10-1 常用运行参数</caption>
|
||||
<thead>
|
||||
<tr><th>参数</th><th>默认值</th><th>说明</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td><code>trigger_normal_force</code></td><td>0.1 N</td><td>单侧法向力触发阈值</td></tr>
|
||||
<tr><td><code>trigger_shear_force</code></td><td>0.1 N</td><td>单侧切向力触发阈值</td></tr>
|
||||
<tr><td><code>grip_normal_force</code></td><td>0.2 N</td><td>判定接触的法向力阈值</td></tr>
|
||||
<tr><td><code>grip_requires_both</code></td><td>true</td><td>是否要求双侧同时接触</td></tr>
|
||||
<tr><td><code>hold_force</code></td><td>20%</td><td>基础保持目标力百分比</td></tr>
|
||||
<tr><td><code>adaptive_force_max</code></td><td>80%</td><td>自适应夹持力上限</td></tr>
|
||||
<tr><td><code>release_normal_change</code></td><td>0.2 N</td><td>释放检测法向力变化阈值</td></tr>
|
||||
<tr><td><code>rearm_seconds</code></td><td>0.5 s</td><td>张开后再次允许触发的最短休息时间</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
<p>完整参数分组及默认值见《GUI 详细技术说明》（<code>docs/GUI_USER_MANUAL.zh-CN.md</code>）第 14 章。</p>
|
||||
</section>
|
||||
|
||||
<!-- 第11章 -->
|
||||
<section class="doc-section" id="sec-11">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 11</span>
|
||||
<h2>运行状态与日志</h2>
|
||||
</div>
|
||||
|
||||
<h3>11.1 状态码说明</h3>
|
||||
<table class="data-table">
|
||||
<caption>表 11-1 主要 state 状态</caption>
|
||||
<thead>
|
||||
<tr><th>state</th><th>界面显示</th><th>含义</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td><code>open_wait</code></td><td>等待触发</td><td>夹爪张开，等待触觉触发</td></tr>
|
||||
<tr><td><code>closing</code></td><td>闭合中</td><td>已触发，正在闭合并寻找物体</td></tr>
|
||||
<tr><td><code>gripping</code></td><td>夹持加力</td><td>已检测到物体，正在增加夹持力</td></tr>
|
||||
<tr><td><code>hold_check</code></td><td>夹持检测</td><td>自适应补力、保持及释放检测</td></tr>
|
||||
<tr><td><code>open_recover</code></td><td>张开恢复</td><td>已张开，等待位置或力信号恢复</td></tr>
|
||||
<tr><td><code>error</code></td><td>错误</td><td>控制线程异常，须查看日志</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<h3>11.2 日志文件</h3>
|
||||
<p>打包版程序运行日志保存在程序目录下 <code>logs/</code> 文件夹。程序自动保留最近 10 次运行会话。</p>
|
||||
<pre class="code-block">logs/
|
||||
├── latest.txt # 最近一次会话目录名
|
||||
└── YYYYMMDD_HHMMSS_PID/
|
||||
├── main.log # GUI、控制器、串口及异常信息
|
||||
└── worker_*.log # 左、右传感器进程日志</pre>
|
||||
<p>建议故障排查顺序：读取 <code>latest.txt</code> → 打开对应会话目录 → 检查 <code>main.log</code> 末尾异常 → 必要时对照 <code>worker_*.log</code>。</p>
|
||||
|
||||
<h3>11.3 快捷键</h3>
|
||||
<table class="data-table data-table--compact">
|
||||
<caption>表 11-2 键盘快捷键</caption>
|
||||
<thead>
|
||||
<tr><th>快捷键</th><th>功能</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr><td>Esc</td><td>全屏与窗口模式切换</td></tr>
|
||||
<tr><td>Q</td><td>退出程序</td></tr>
|
||||
</tbody>
|
||||
</table>
|
||||
</section>
|
||||
|
||||
<!-- 第12章 -->
|
||||
<section class="doc-section" id="sec-12">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 12</span>
|
||||
<h2>故障排除</h2>
|
||||
</div>
|
||||
|
||||
<table class="data-table">
|
||||
<caption>表 12-1 常见故障及处理</caption>
|
||||
<thead>
|
||||
<tr><th>序号</th><th>故障现象</th><th>可能原因</th><th>处理措施</th></tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr>
|
||||
<td>1</td>
|
||||
<td>传感器无画面，FPS 显示 <code>--</code></td>
|
||||
<td>摄像头未连接；设备编号错误；被其他程序占用</td>
|
||||
<td>检查 USB 连接及 <code>video1</code>/<code>video2</code>；关闭占用程序；执行单侧或全部重启</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>2</td>
|
||||
<td>串口连接失败</td>
|
||||
<td>端口号错误；驱动未安装；端口被占用；从站地址/波特率不匹配</td>
|
||||
<td>核对 <code>port</code>、<code>baudrate</code>、<code>slave_id</code>；检查 <code>main.log</code> 中 Modbus 相关报错</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>3</td>
|
||||
<td>启动后 <code>state=error</code></td>
|
||||
<td>传感器或串口初始化失败；配置文件格式错误</td>
|
||||
<td>查看 <code>main.log</code> 异常栈；可用 <code>--dry-run</code> 区分串口与传感器故障</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>4</td>
|
||||
<td>空载力值不归零 / 漂移</td>
|
||||
<td>标定阶段有负载；光照或摄像头松动；标定参数不当</td>
|
||||
<td>移开负载后重启控制程序；标定阶段保持静止；检查标定 JSON</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>5</td>
|
||||
<td>修改参数未生效</td>
|
||||
<td>未保存文件或未重启控制程序</td>
|
||||
<td>确认保存成功并执行「重启控制程序」</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>6</td>
|
||||
<td>3D 模型无法显示</td>
|
||||
<td>OpenGL 不可用；模型资源缺失</td>
|
||||
<td>更新显卡驱动；确认 <code>crt_ctag2f90d_gripper_visualization</code> 目录完整</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>7</td>
|
||||
<td>界面卡顿、FPS 低</td>
|
||||
<td>光流算法配置不当；分辨率或帧率过高</td>
|
||||
<td>确认 <code>ddjx01_q6a.json</code> 中算法为 <code>dis</code>；适当降低 <code>sensor_fps</code></td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</table>
|
||||
</section>
|
||||
|
||||
<!-- 第13章 -->
|
||||
<section class="doc-section" id="sec-13">
|
||||
<div class="section-head">
|
||||
<span class="section-num">CHAPTER 13</span>
|
||||
<h2>维护与定期检查</h2>
|
||||
</div>
|
||||
|
||||
<h3>13.1 日常维护</h3>
|
||||
<ul>
|
||||
<li>保持触觉敏感面清洁，避免油污及硬质颗粒；</li>
|
||||
<li>定期检查 USB 及串口线缆有无松动或外皮破损；</li>
|
||||
<li>长期停用后重新启用前，执行一次完整重启并完成空载标定；</li>
|
||||
<li>重要参数变更前备份 <code>config/gripper.json</code> 及 <code>config/gui_layout.json</code>。</li>
|
||||
</ul>
|
||||
|
||||
<h3>13.2 标定参数维护</h3>
|
||||
<p><code>normal_force_calibration</code> 与 <code>shear_force_calibration</code> 用于将传感器原始量映射至牛顿单位，直接影响触发、接触及释放判断。修改标定点须有实测数据支持，修改后须重新验证全流程。</p>
|
||||
</section>
|
||||
|
||||
<!-- 附录 -->
|
||||
<section class="doc-section" id="sec-a">
|
||||
<div class="section-head">
|
||||
<span class="section-num">APPENDIX</span>
|
||||
<h2>附录</h2>
|
||||
</div>
|
||||
|
||||
<h3 id="appendix-a">附录 A 现场操作检查表</h3>
|
||||
<h4>启动前检查</h4>
|
||||
<ul class="checklist">
|
||||
<li>左右摄像头已被操作系统识别</li>
|
||||
<li>夹爪电源及串口通信正常</li>
|
||||
<li><code>video1</code>、<code>video2</code>、<code>port</code> 配置正确</li>
|
||||
<li>夹爪开合范围内无人员及杂物</li>
|
||||
<li>两侧触觉传感器无预加载</li>
|
||||
</ul>
|
||||
<h4>启动后确认</h4>
|
||||
<ul class="checklist">
|
||||
<li>左右传感器 FPS 正常，状态点为绿色</li>
|
||||
<li>3D 模型姿态与夹爪实际位置一致</li>
|
||||
<li>顶部状态无 <code>error</code></li>
|
||||
<li>已等待 <code>open-wait-armed（已就绪）</code> 后再进行演示</li>
|
||||
</ul>
|
||||
|
||||
<h3 id="appendix-b">附录 B 插图维护说明</h3>
|
||||
<p>本手册插图由软件界面自动截取生成。软件界面变更后，可在项目根目录执行以下命令重新生成插图：</p>
|
||||
<pre class="code-block">conda activate py311
|
||||
python build/capture_gui_screenshots.py
|
||||
python build/generate_manual_diagrams.py</pre>
|
||||
<p>截图输出路径：<code>docs/images/manual/</code>；矢量示意图输出路径：<code>docs/assets/diagrams/</code>。</p>
|
||||
|
||||
<hr style="margin: 2.5rem 0; border: none; border-top: 1px solid var(--border);">
|
||||
<p style="font-size: 0.8125rem; color: var(--ink-light); text-align: center;">
|
||||
— 文档结束 · GD02-UM-001 V1.0 —<br>
|
||||
Orisys 夹爪产品使用说明书
|
||||
</p>
|
||||
</section>
|
||||
|
||||
</article>
|
||||
</main>
|
||||
</div>
|
||||
|
||||
<button type="button" class="toolbar-hint" onclick="window.print()" title="打印或导出 PDF">打印 / 导出 PDF</button>
|
||||
|
||||
<script>
|
||||
(function () {
|
||||
var links = document.querySelectorAll('.manual-nav a[href^="#"]');
|
||||
var sections = [];
|
||||
links.forEach(function (a) {
|
||||
var id = a.getAttribute('href').slice(1);
|
||||
var el = document.getElementById(id);
|
||||
if (el) sections.push({ link: a, el: el });
|
||||
});
|
||||
function onScroll() {
|
||||
var scroll = window.scrollY + 80;
|
||||
var current = sections[0];
|
||||
sections.forEach(function (s) {
|
||||
if (s.el.offsetTop <= scroll) current = s;
|
||||
});
|
||||
links.forEach(function (a) { a.classList.remove('active'); });
|
||||
if (current) current.link.classList.add('active');
|
||||
}
|
||||
window.addEventListener('scroll', onScroll, { passive: true });
|
||||
onScroll();
|
||||
})();
|
||||
</script>
|
||||
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,29 @@
|
||||
# Orisys 夹爪产品使用说明书
|
||||
|
||||
> **正式版文档已迁移为 HTML 格式。**
|
||||
|
||||
请打开以下文件查阅完整产品使用说明书（含封面、版本履历、规范图号表号及全部界面截图）：
|
||||
|
||||
**[`PRODUCT_MANUAL.zh-CN.html`](PRODUCT_MANUAL.zh-CN.html)**
|
||||
|
||||
## 使用方式
|
||||
|
||||
1. 在文件管理器中双击 `docs/PRODUCT_MANUAL.zh-CN.html`，使用浏览器打开；
|
||||
2. 点击右下角「打印 / 导出 PDF」或使用浏览器打印功能（Ctrl+P）导出 PDF；
|
||||
3. 左侧导航栏可快速跳转章节。
|
||||
|
||||
## 相关文档
|
||||
|
||||
| 文档 | 说明 |
|
||||
| --- | --- |
|
||||
| [PRODUCT_MANUAL.zh-CN.html](PRODUCT_MANUAL.zh-CN.html) | 正式产品使用说明书（推荐） |
|
||||
| [GUI_USER_MANUAL.zh-CN.md](GUI_USER_MANUAL.zh-CN.md) | 详细技术说明与完整参数表 |
|
||||
| [assets/manual.css](assets/manual.css) | 说明书样式表 |
|
||||
| [images/manual/](images/manual/) | 界面截图资源 |
|
||||
|
||||
## 更新截图
|
||||
|
||||
```powershell
|
||||
conda activate py311
|
||||
python build/capture_gui_screenshots.py
|
||||
```
|
||||
@@ -0,0 +1,184 @@
|
||||
# 释放控制与回放验证
|
||||
|
||||
## 行为模式（灵敏 / 较稳定 / 十分稳定）
|
||||
|
||||
设置里可切换三套预设（`grip_behavior_mode`），切换后改写**夹持力**与**释放/开合**相关参数；
|
||||
闭合触发（`trigger_*`、等待再武装稳定门槛）一律固定为灵敏档，闭合速度各档相同。
|
||||
|
||||
| 模式 | 夹持力 | 释放 |
|
||||
| --- | --- | --- |
|
||||
| `sensitive`（灵敏） | 起步 25%，按切向负载自适应 33~80% | 跟手；振荡/侧触在夹紧期间也生效，法向也可张开 |
|
||||
| `stable`（较稳定） | 闭合指令即 70%，接触后不锁位置（`grip_contact_hold_position=false`），继续以 70% 朝闭合方向顶住，不再加力 | 接触后 2 s 保护期内不触发任何张开（`release_min_hold_seconds=2.0`，不再额外等法向夹稳）；张开只看 Y 切向拟合偏移：保护期结束时记下两侧带符号 Y 的拟合值作基准，之后任一侧拟合值偏离基准 ≥ 0.6 N 连续 6 帧即张开（`release_shear_displacement=0.6`，不取绝对值、回头不取消，基准只在接近时缓慢跟随漂移）；法向、切向振荡和侧触/突变/触碰路径都不参与。夹持中夹爪走到闭合极限（物体已脱出）立即张开（`release_object_lost_open=true`，不受保护期限制） |
|
||||
| `firm`（十分稳定） | 同较稳定档的恒力方式，但闭合指令即 100% | 与较稳定档相同的张开方式，保护期 2.5 s（`release_min_hold_seconds=2.5`），Y 拟合偏移阈值 0.8 N（`release_shear_displacement=0.8`）；同样支持物体脱出张开 |
|
||||
|
||||
满力需要 `force_max` ≥ 100。
|
||||
|
||||
预设见 `gripper/config/behavior_profiles.py`。旧配置里的 `rapid` /「迅速开合」映射为 `sensitive`。
|
||||
|
||||
## 会话录制（优化张开用）
|
||||
|
||||
主界面「录制数据」可开关。开启后每个控制 tick 追加写入
|
||||
`logs/sessions/session-YYYYMMDD-HHMMSS.jsonl`：
|
||||
|
||||
- 滤波后力 `feedback`、原始左右样本 `raw_left` / `raw_right`
|
||||
- 状态机 `state` / `action`、行为模式、力指令
|
||||
- 释放判定 `release`（phase/reason/confirm/rates…）
|
||||
- 执行器 `motion_pending` / `motion_completed_at`
|
||||
|
||||
文件为 JSONL：首行 `session_header`（含当时 release 相关配置），末行 `session_footer`。
|
||||
停止控制或再点按钮会结束录制。另有短窗快照 `logs/release-last.json`（仅最近一次释放前后）。
|
||||
|
||||
---
|
||||
|
||||
## 平稳拐点继续跟随，新外力才释放
|
||||
|
||||
`ReleaseWatch` 位于 `gripper/control/release_watch.py`，消费力数据和执行器状态，返回
|
||||
释放决定及是否允许补力；`controller.py` 通过 `ActuatorWorker` 发令。外围主循环保留原结构。
|
||||
|
||||
当前释放路径共用 `force_curve.py` 的曲线判断。旧的“短窗口净变化够大、两帧仍有偏移”
|
||||
会把缓升变平或自然回落当成侧触；现在仅有幅度、速率或左右力差都不足以释放，必须
|
||||
先出现相对已有曲线的新起始变化，再检查幅度和持续性。
|
||||
|
||||
具体处理：
|
||||
|
||||
1. 闭合阶段保留最近几帧，接触时继承各通道的趋势，不把接触帧当作静止起点。
|
||||
2. 每个通道保留可配置长度的历史（`release_curve_history`，stable 约 30 点）。
|
||||
**起始检测**仍只用最近约 6 点做局部二次拟合；**夹稳斜率**用更长窗口（`release_slope_window_seconds`）
|
||||
做一次拟合，得到 `fit_slope`（N/s）。夹住加力过程表现为该斜率逐渐变小并趋近于 0。
|
||||
3. 自然减速、变平、相近速度的斜率变号是曲线延续；它们不因累计位移达到门槛而启动释放候选。
|
||||
4. 新的偏离方向须与实际变化方向一致，并超过小幅噪声门槛（当前为
|
||||
`min(release_min_change, release_side_touch_change)` 的 20%）。真正的起始变化
|
||||
会被短暂保留，允许轻推在后续几帧达到幅度门槛。拟合残差及近期残差中位数用于估计
|
||||
噪声，预测误差容许范围随外推距离扩大；新偏离还必须超过该范围，不能把噪声外推成外力。
|
||||
5. 候选必须同时具有实际测得的位移和相对原曲线的偏移，避免曲线外推把小噪声放大成
|
||||
虚假的大变化。确认时扣除原曲线的继续变化，因此拐点附近新的触碰仍可检测。
|
||||
6. 尚未达到幅度的小变化一旦变平，或偏移落回预测误差范围，就丢弃起始证据，不能续接
|
||||
后续自然变化。已达幅度的候选若明显回落则取消，记录 `settling-turn`；回落判定容许
|
||||
当前噪声范围内的波动，避免真实触碰后的轻微抖动被误拒绝。
|
||||
同一套起始判断覆盖 `side_touch`、`touch`、`sudden` 和弱路径，避免其他分支绕过。
|
||||
7. **`release_require_slope_settle`（默认开）**：仅看法向通道长窗 `|fit_slope|`
|
||||
（`release_settle_normal_only`）低于 `release_slope_settle_rate`，并连续保持
|
||||
`release_slope_hold_seconds`，**首次**达到后闩锁为已夹稳。之后自适应加力或主动
|
||||
张开使斜率再升高，**不会**撤销释放武装。
|
||||
8. **通道开关**：默认法向与切向均允许 onset 与侧触；可用
|
||||
`release_*_onset_enabled` / `release_side_*_enabled` 单独关闭。夹稳判定仍可只看法向
|
||||
（`release_settle_normal_only`）。
|
||||
9. **切向振荡旁路（默认开）**：短窗内切向反复换向且峰峰值够大时，走
|
||||
`shear_oscillation`，**不要求**已夹稳闩锁，可在 `force-settling` 期间直接确认开合。
|
||||
|
||||
没有增加固定夹稳等待，也没有恢复“四通道同时安静”的全局条件。以下是幅度与确认路径：
|
||||
|
||||
| 路径 | 检测 | 确认 |
|
||||
| --- | --- | --- |
|
||||
| 即时侧触 `side_touch` | 继承接触前左右力差曲线，检测偏离该曲线的新侧触 | 默认两次左右均更新的传感器帧；不等 0.28s 夹稳计时 |
|
||||
| 普通轻触 `touch` | 出现新起始变化后，实际位移和相对原曲线的偏移均达到 0.04N | 默认两帧；不要求瞬时速率达到 1.6N/s |
|
||||
| 突变 `sudden` / 弱路径 `slow_external` | 新起始变化叠加大速率 / 大幅偏移 | 默认两帧 / 五帧 |
|
||||
| 切向振荡 `shear_oscillation` | 切向短窗内多次换向 + 峰峰值 | 默认两帧；绕过斜率夹稳门控 |
|
||||
|
||||
即时侧触使用左减右的法向/切向力差的变化，抵消两侧共同压紧或负载变化。命令排队、
|
||||
执行及短响应尾部内，法向差分仍受保护，切向差分继续检测。普通共同法向卸载仍经过
|
||||
接触与自运动保护，不能把它与即时切向侧触混为一谈。
|
||||
|
||||
差分投票要求左右都提供新帧，避免相机异步更新制造虚假左右差。观察窗口不是必须
|
||||
等待的时间：出现新起始变化并达到幅度后立即确认。确认要求同一通道、同方向的
|
||||
新偏移持续存在；平稳拐点、单帧脉冲、回弹、换边不能拼票。
|
||||
|
||||
差分不能完美区分真实触碰与持续的单侧机械扰动。实机需特别检查单侧摩擦明显的物体；
|
||||
两帧确认和共同受力抵消并不等于所有机械扰动都能被识别。
|
||||
|
||||
## 状态与补力
|
||||
|
||||
| 状态 | 行为 |
|
||||
| --- | --- |
|
||||
| `disarmed` | 没有夹持周期，不检测释放 |
|
||||
| `grip_settle` | 普通法向接触保护，独立即时侧触路径已运行 |
|
||||
| `self_settle` | 普通法向自运动/瞬态恢复保护，即时切向侧触继续运行 |
|
||||
| `watching` | 突变、轻触和弱路径均可检测；慢漂移跟随参考 |
|
||||
| `confirming` | 候选确认优先，暂停补力 |
|
||||
| `released` | 一次性输出释放效果，进入张开恢复 |
|
||||
|
||||
保护计时结束且获得左右新帧，即恢复普通监视；不再要求四通道同时安静。脉冲拒绝或
|
||||
断流后的小恢复窗同样有界，不会因噪声反复延长。即时侧触确认独立于这些普通保护状态。
|
||||
|
||||
`gripping` 和 `hold_check` 每 tick 先检测释放，再考虑补力。恢复普通监视后至少保留
|
||||
`max(0.1s, release_confirm_samples/control_hz)` 观察时间再补力。轻推初期也暂缓补力，
|
||||
防止新命令覆盖取物信号；低速蠕变不会永久禁止补力。配置的补力间隔仍是最短间隔。
|
||||
|
||||
## 参数
|
||||
|
||||
| 参数 | 默认及当前 JSON | 语义 |
|
||||
| --- | ---: | --- |
|
||||
| `release_side_touch_change` | 0.06 N | 即时侧触的左右力差变化门槛 |
|
||||
| `release_touch_window_seconds` | 0.2s | 新外力起始证据有效期；不是额外等待 |
|
||||
| `release_min_change` | 0.04 N | 普通突变/轻触路径的最小净变化 |
|
||||
| `release_confirm_samples` | 2 | 突变/轻触/侧触确认帧数，含首帧；至少两帧 |
|
||||
| `release_min_hold_seconds` | 0.28s | 普通法向接触保护；不阻塞即时侧触 |
|
||||
| `release_arm_delay_seconds` | 0.12s | 实际命令执行完成后的普通法向保护；即时切向侧触仍运行 |
|
||||
| `release_settle_seconds` | 0.06s | 脉冲拒绝/断流后的短恢复保护；不再表示全局安静时间 |
|
||||
| `release_normal_rate` / `release_shear_rate` | 1.6 N/s | 突变路径门槛；轻触不必跨过它 |
|
||||
| `release_slow_rate` | 0.4 N/s | 慢变日志分类阈值；参考跟随由曲线延续决定，不门控就绪 |
|
||||
| `release_normal_change` / `release_shear_change` | 0.28 N | 新外力起始之后的大幅变化弱路径门槛，旧曲线累计变化不能绕过起始判断 |
|
||||
| `release_slow_confirm_samples` | 5 | 弱路径确认帧数，不少于突变路径 |
|
||||
| `release_confirm_window_seconds` | 0.2s | 候选最长确认时间；超时丢弃 |
|
||||
| `release_sample_timeout_seconds` | 0.2s | 任一侧断流或时间戳间隔过大时，丢弃已有证据 |
|
||||
| `release_curve_history` | 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` | 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 | 是否允许法向左右差侧触 |
|
||||
| `release_shear_onset_enabled` | true | 是否允许切向触发普通释放路径 |
|
||||
| `release_normal_onset_enabled` | true | 是否允许法向触发普通释放路径 |
|
||||
|
||||
N/N/s 适用于当前标定配置。缓存帧不重复滤波或投票，提高控制频率不能代替真实传感器帧。
|
||||
`grip_settle_seconds` 只控制外围状态进入 `hold_check` 的最早时间。
|
||||
`trigger_clear_stable_seconds` 是未使用的兼容字段，开爪等待仍采用稳定基线增量。
|
||||
|
||||
## 日志
|
||||
|
||||
- `release-side_touch`：接触/自运动保护期间检测到侧触，正在确认。
|
||||
- `release-touch`：检测到新的轻推起始及持续偏移，不需要很大的瞬时速率。
|
||||
- `release-settling-turn` / `curve=settling_turn`：平稳拐点或候选回落，继续夹持。
|
||||
- `curve=settling_flatten`：原曲线自然减速、变平。
|
||||
- `force-settling` / `curve=force_settling`：长历史斜率尚未趋零，暂不侧触、不进 watching。
|
||||
- `channel` / `innovation`：候选通道及起始时偏离曲线预测的量。
|
||||
- `release-sudden` / `release-slow_external`：突变或弱路径确认。
|
||||
- `release-grip-settle` / `release-self-settle`：普通法向保护；`side_watch=1` 表示即时侧触仍运行。
|
||||
- `release-slow_internal`：慢变跟随参考，不累积无限漂移。
|
||||
- `release-transient-rejected` / `release-confirm-expired`：脉冲回弹或确认过期，未释放。
|
||||
- `release-sample-gap` / `release-waiting-sample`：断流保护或等待新帧。
|
||||
|
||||
`release_armed` 表示普通监视或已有侧触候选；不能单凭其为 0 判断侧触关闭。
|
||||
新增 `side_watch` 日志和 `release_side_watch` UI 状态单独表示即时侧触。
|
||||
状态/原因变化立即记录，重复日志约 0.2s 节流。
|
||||
|
||||
## 验证
|
||||
|
||||
```powershell
|
||||
conda activate py311
|
||||
python -m unittest discover -s tests -v
|
||||
```
|
||||
|
||||
回放先复现了旧版在没有突变的余弦缓升、变平、回落中误开的问题，再验证修正后不释放。
|
||||
覆盖 20/30Hz、单侧差分与双侧曲线、接触前已有上升趋势、大速率自然曲线、弱路径绕过、
|
||||
有界随机噪声，以及在上升、峰顶、回落阶段叠加一次真实新触碰。额外覆盖 60 组带噪声的
|
||||
平滑曲线和 60 组在带噪声曲线上叠加新触碰的回放，防止外推放大噪声或确认误拒绝。
|
||||
原有即时侧触和轻推
|
||||
响应回归保留，但软件回放不能证明实机已不误开。
|
||||
|
||||
每次实际自动释放后，会将释放前最多 120 个控制 tick 的逐帧力值、左右传感器时间戳、
|
||||
状态、起始判据及参数保存到 `logs/release-last.json`（源码启动也有效，覆盖上一次记录）。
|
||||
写文件在张开命令提交后进行，诊断不能阻塞该次发令。该文件用于复核真实拐点和误开原因，
|
||||
无需根据界面里被节流的几行日志猜测。
|
||||
|
||||
这些时间只覆盖软件决定，不包含相机处理、串口排队和机械动作。脉冲、回弹、慢漂移、
|
||||
断流、手动命令、闭合超时和当前 JSON 参数回放仍参与回归。实机应检查接触后立即侧触、
|
||||
持续噪声下轻推、长时间夹持误开，以及不同单侧摩擦条件下自运动是否被误识别为侧触。
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 60 KiB |
@@ -0,0 +1,81 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 920 324" role="img">
|
||||
<defs>
|
||||
<linearGradient id="gIdle" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#dbeafe"/><stop offset="100%" stop-color="#bfdbfe"/>
|
||||
</linearGradient>
|
||||
<linearGradient id="gAct" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#fef3c7"/><stop offset="100%" stop-color="#fde68a"/>
|
||||
</linearGradient>
|
||||
<linearGradient id="gHold" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#fce7f3"/><stop offset="100%" stop-color="#fbcfe8"/>
|
||||
</linearGradient>
|
||||
<linearGradient id="gRec" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#d1fae5"/><stop offset="100%" stop-color="#a7f3d0"/>
|
||||
</linearGradient>
|
||||
<marker id="arr" markerWidth="8" markerHeight="8" refX="7" refY="4" orient="auto">
|
||||
<path d="M0,0 L8,4 L0,8 Z" fill="#475569"/>
|
||||
</marker>
|
||||
<filter id="sh" x="-6%" y="-8%" width="112%" height="120%">
|
||||
<feDropShadow dx="0" dy="2" stdDeviation="2.5" flood-color="#0f172a" flood-opacity="0.10"/>
|
||||
</filter>
|
||||
<style>
|
||||
.cn{font:700 14px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#0f172a}
|
||||
.en{font:600 11px Consolas,monospace;fill:#475569}
|
||||
.sub{font:400 11px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#64748b}
|
||||
.ed{font:500 11px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#334155}
|
||||
.nt{font:400 11px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#64748b}
|
||||
</style>
|
||||
</defs>
|
||||
|
||||
<g filter="url(#sh)"><rect x="24" y="40" width="148" height="72" rx="10" fill="url(#gIdle)" stroke="#2563eb" stroke-width="1.5"/>
|
||||
<text x="98" y="66" text-anchor="middle" class="cn">等待触发</text>
|
||||
<text x="98" y="84" text-anchor="middle" class="en">open_wait</text>
|
||||
<text x="98" y="100" text-anchor="middle" class="sub">已就绪 armed</text></g>
|
||||
|
||||
<path d="M172 76 H208" stroke="#64748b" stroke-width="1.5" marker-end="url(#arr)"/>
|
||||
<text x="190" y="68" text-anchor="middle" class="ed">侧边触发</text>
|
||||
|
||||
<g filter="url(#sh)"><rect x="208" y="40" width="148" height="72" rx="10" fill="url(#gAct)" stroke="#d97706" stroke-width="1.5"/>
|
||||
<text x="282" y="66" text-anchor="middle" class="cn">闭合中</text>
|
||||
<text x="282" y="84" text-anchor="middle" class="en">closing</text>
|
||||
<text x="282" y="100" text-anchor="middle" class="sub">寻找物体</text></g>
|
||||
|
||||
<path d="M356 76 H392" stroke="#64748b" stroke-width="1.5" marker-end="url(#arr)"/>
|
||||
<text x="374" y="68" text-anchor="middle" class="ed">双侧接触</text>
|
||||
|
||||
<g filter="url(#sh)"><rect x="392" y="40" width="148" height="72" rx="10" fill="url(#gAct)" stroke="#d97706" stroke-width="1.5"/>
|
||||
<text x="466" y="66" text-anchor="middle" class="cn">夹持加力</text>
|
||||
<text x="466" y="84" text-anchor="middle" class="en">gripping</text>
|
||||
<text x="466" y="100" text-anchor="middle" class="sub">低力 → 基础力</text></g>
|
||||
|
||||
<path d="M540 76 H576" stroke="#64748b" stroke-width="1.5" marker-end="url(#arr)"/>
|
||||
<text x="558" y="68" text-anchor="middle" class="ed">到达保持力</text>
|
||||
|
||||
<g filter="url(#sh)"><rect x="576" y="40" width="148" height="72" rx="10" fill="url(#gHold)" stroke="#db2777" stroke-width="1.5"/>
|
||||
<text x="650" y="66" text-anchor="middle" class="cn">夹持检测</text>
|
||||
<text x="650" y="84" text-anchor="middle" class="en">hold_check</text>
|
||||
<text x="650" y="100" text-anchor="middle" class="sub">自适应补力 / 释放检测</text></g>
|
||||
|
||||
<path d="M724 76 H760" stroke="#64748b" stroke-width="1.5" marker-end="url(#arr)"/>
|
||||
<text x="742" y="68" text-anchor="middle" class="ed">取物释放</text>
|
||||
|
||||
<g filter="url(#sh)"><rect x="760" y="40" width="136" height="72" rx="10" fill="url(#gRec)" stroke="#059669" stroke-width="1.5"/>
|
||||
<text x="828" y="66" text-anchor="middle" class="cn">张开恢复</text>
|
||||
<text x="828" y="84" text-anchor="middle" class="en">open_recover</text>
|
||||
<text x="828" y="100" text-anchor="middle" class="sub">位置 / 力恢复</text></g>
|
||||
|
||||
<path d="M828 112 V204 H98 V112" fill="none" stroke="#2563eb" stroke-width="1.5" marker-end="url(#arr)"/>
|
||||
<rect x="340" y="188" width="240" height="24" rx="4" fill="#eff6ff" stroke="#bfdbfe"/>
|
||||
<text x="460" y="204" text-anchor="middle" class="ed" fill="#1e40af">恢复完成 + 休息时间（rearm）→ 再次就绪</text>
|
||||
|
||||
<path d="M282 112 V152 H828" fill="none" stroke="#dc2626" stroke-width="1.2" stroke-dasharray="5 4"/>
|
||||
<rect x="200" y="128" width="164" height="22" rx="4" fill="#fef2f2" stroke="#fecaca"/>
|
||||
<text x="282" y="143" text-anchor="middle" class="ed" fill="#b91c1c">闭合超时 close_timeout → 张开</text>
|
||||
|
||||
<rect x="24" y="232" width="872" height="76" rx="8" fill="#f8fafc" stroke="#e2e8f0"/>
|
||||
<text x="40" y="254" class="cn" font-size="12">阶段说明</text>
|
||||
<text x="40" y="274" class="nt">① 等待触发：夹爪张开，单侧力超阈且为新上升沿时进入闭合。</text>
|
||||
<text x="40" y="294" class="nt">② 闭合～夹持：物体接触后低力保持，分步加力至 hold_force，并按切向负载自适应补力。</text>
|
||||
<text x="40" y="314" class="nt">③ 释放～恢复：力变化超阈则张开；满足恢复条件并经过 rearm_seconds 后，方可再次触发。</text>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 5.2 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 152 KiB |
@@ -0,0 +1,114 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 880 520" role="img">
|
||||
<defs>
|
||||
<linearGradient id="sensorGrad" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#dbeafe"/><stop offset="100%" stop-color="#bfdbfe"/>
|
||||
</linearGradient>
|
||||
<linearGradient id="procGrad" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#e0e7ff"/><stop offset="100%" stop-color="#c7d2fe"/>
|
||||
</linearGradient>
|
||||
<linearGradient id="ctrlGrad" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#fef3c7"/><stop offset="100%" stop-color="#fde68a"/>
|
||||
</linearGradient>
|
||||
<linearGradient id="guiGrad" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#d1fae5"/><stop offset="100%" stop-color="#a7f3d0"/>
|
||||
</linearGradient>
|
||||
<linearGradient id="hwGrad" x1="0%" y1="0%" x2="0%" y2="100%">
|
||||
<stop offset="0%" stop-color="#f1f5f9"/><stop offset="100%" stop-color="#e2e8f0"/>
|
||||
</linearGradient>
|
||||
<marker id="arrow" markerWidth="8" markerHeight="8" refX="7" refY="4" orient="auto">
|
||||
<path d="M0,0 L8,4 L0,8 Z" fill="#475569"/>
|
||||
</marker>
|
||||
<filter id="shadow" x="-4%" y="-4%" width="108%" height="112%">
|
||||
<feDropShadow dx="0" dy="2" stdDeviation="3" flood-color="#0f172a" flood-opacity="0.10"/>
|
||||
</filter>
|
||||
<style>
|
||||
.t1{font:700 15px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#0f172a}
|
||||
.t2{font:400 12px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#475569}
|
||||
.tl{font:700 11px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#64748b;letter-spacing:.08em}
|
||||
.te{font:500 11px "Microsoft YaHei","Noto Sans SC",sans-serif;fill:#334155}
|
||||
.tp{font:600 10px Consolas,monospace;fill:#1e40af}
|
||||
</style>
|
||||
</defs>
|
||||
|
||||
<rect x="24" y="52" width="832" height="108" rx="8" fill="#f8fafc" stroke="#e2e8f0"/>
|
||||
<text x="40" y="72" class="tl">感知层</text>
|
||||
<rect x="24" y="176" width="832" height="148" rx="8" fill="#fafafa" stroke="#e2e8f0"/>
|
||||
<text x="40" y="196" class="tl">软件处理层</text>
|
||||
<rect x="24" y="340" width="832" height="108" rx="8" fill="#f8fafc" stroke="#e2e8f0"/>
|
||||
<text x="40" y="360" class="tl">执行与呈现层</text>
|
||||
|
||||
<g filter="url(#shadow)">
|
||||
<rect x="56" y="88" width="168" height="56" rx="8" fill="url(#sensorGrad)" stroke="#3b82f6" stroke-width="1.5"/>
|
||||
<text x="140" y="112" text-anchor="middle" class="t1">左触觉传感器</text>
|
||||
<text x="140" y="130" text-anchor="middle" class="t2">USB 摄像头 + 敏感面</text>
|
||||
</g>
|
||||
<g filter="url(#shadow)">
|
||||
<rect x="656" y="88" width="168" height="56" rx="8" fill="url(#sensorGrad)" stroke="#3b82f6" stroke-width="1.5"/>
|
||||
<text x="740" y="112" text-anchor="middle" class="t1">右触觉传感器</text>
|
||||
<text x="740" y="130" text-anchor="middle" class="t2">USB 摄像头 + 敏感面</text>
|
||||
</g>
|
||||
|
||||
<path d="M224 116 H340" fill="none" stroke="#64748b" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<path d="M656 116 H540" fill="none" stroke="#64748b" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<text x="440" y="108" text-anchor="middle" class="te">视频帧采集</text>
|
||||
|
||||
<g filter="url(#shadow)">
|
||||
<rect x="300" y="212" width="280" height="76" rx="10" fill="url(#procGrad)" stroke="#6366f1" stroke-width="1.5"/>
|
||||
<text x="440" y="240" text-anchor="middle" class="t1">触觉力计算 / 光流处理</text>
|
||||
<text x="440" y="260" text-anchor="middle" class="t2">法向力 · 切向力 · 光流/幅值图像</text>
|
||||
<text x="440" y="276" text-anchor="middle" class="t2">Orisys SDK · OpenCV DIS</text>
|
||||
</g>
|
||||
|
||||
<path d="M140 144 V188 H300" fill="none" stroke="#64748b" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<path d="M740 144 V188 H580" fill="none" stroke="#64748b" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
|
||||
<g filter="url(#shadow)">
|
||||
<rect x="56" y="220" width="200" height="60" rx="8" fill="url(#ctrlGrad)" stroke="#d97706" stroke-width="1.5"/>
|
||||
<text x="156" y="246" text-anchor="middle" class="t1">智能夹持状态机</text>
|
||||
<text x="156" y="264" text-anchor="middle" class="t2">触发 · 闭合 · 加力 · 释放</text>
|
||||
</g>
|
||||
|
||||
<path d="M300 250 H256" fill="none" stroke="#64748b" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<text x="278" y="242" text-anchor="middle" class="te">力信号</text>
|
||||
|
||||
<g filter="url(#shadow)">
|
||||
<rect x="624" y="220" width="200" height="60" rx="8" fill="url(#hwGrad)" stroke="#64748b" stroke-width="1.5"/>
|
||||
<text x="724" y="246" text-anchor="middle" class="t1">夹爪控制器</text>
|
||||
<text x="724" y="264" text-anchor="middle" class="t2">Modbus RTU 从站</text>
|
||||
</g>
|
||||
|
||||
<path d="M256 250 H420" fill="none" stroke="#64748b" stroke-width="1.5"/>
|
||||
<path d="M420 250 H624" fill="none" stroke="#64748b" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<text x="440" y="242" text-anchor="middle" class="tp">Modbus RTU / 串口</text>
|
||||
|
||||
<g filter="url(#shadow)">
|
||||
<rect x="624" y="368" width="200" height="56" rx="8" fill="url(#hwGrad)" stroke="#475569" stroke-width="1.5"/>
|
||||
<text x="724" y="392" text-anchor="middle" class="t1">智能夹爪（执行机构）</text>
|
||||
<text x="724" y="410" text-anchor="middle" class="t2">位置 · 力度 · 开/合/保持</text>
|
||||
</g>
|
||||
<path d="M724 280 V368" fill="none" stroke="#475569" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<text x="738" y="328" class="te">驱动</text>
|
||||
|
||||
<g filter="url(#shadow)">
|
||||
<rect x="240" y="356" width="280" height="80" rx="10" fill="url(#guiGrad)" stroke="#059669" stroke-width="1.5"/>
|
||||
<text x="380" y="384" text-anchor="middle" class="t1">GUI 监控界面</text>
|
||||
<text x="380" y="404" text-anchor="middle" class="t2">3D 模型 · 光流画面 · 力曲线 · 状态</text>
|
||||
<text x="380" y="422" text-anchor="middle" class="t2">PyQt5 · PyOpenGL · pyqtgraph</text>
|
||||
</g>
|
||||
|
||||
<path d="M156 280 V320 H380 V356" fill="none" stroke="#059669" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<text x="168" y="312" class="te">状态 / 动作</text>
|
||||
<path d="M440 288 V356" fill="none" stroke="#059669" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<text x="452" y="328" class="te">图像 / 力值</text>
|
||||
|
||||
<path d="M624 396 H520 V250 H256" fill="none" stroke="#94a3b8" stroke-width="1.2" stroke-dasharray="5 4" marker-end="url(#arrow)"/>
|
||||
<text x="572" y="388" text-anchor="middle" class="te">位置反馈</text>
|
||||
|
||||
<g transform="translate(56,468)">
|
||||
<line x1="0" y1="8" x2="28" y2="8" stroke="#64748b" stroke-width="1.5" marker-end="url(#arrow)"/>
|
||||
<text x="36" y="12" class="t2">数据 / 控制流</text>
|
||||
<line x1="140" y1="8" x2="168" y2="8" stroke="#94a3b8" stroke-width="1.2" stroke-dasharray="5 4" marker-end="url(#arrow)"/>
|
||||
<text x="176" y="12" class="t2">反馈</text>
|
||||
</g>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 6.6 KiB |
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,38 @@
|
||||
\documentclass[tikz,border=8pt]{standalone}
|
||||
\usepackage{ctex}
|
||||
\usepackage{tikz}
|
||||
\usetikzlibrary{positioning,arrows.meta,fit,backgrounds}
|
||||
|
||||
\tikzset{
|
||||
st/.style={draw, rounded corners=2pt, align=center, font=\small,
|
||||
minimum width=2.55cm, minimum height=1.15cm, inner sep=2pt},
|
||||
idle/.style={st, fill=blue!8, draw=blue!55},
|
||||
act/.style={st, fill=orange!10, draw=orange!60!black},
|
||||
hold/.style={st, fill=magenta!6, draw=magenta!55},
|
||||
rec/.style={st, fill=teal!8, draw=teal!60!black},
|
||||
arr/.style={-{Stealth[length=2.2mm]}, semithick, draw=gray!70},
|
||||
darr/.style={-{Stealth[length=2.2mm]}, semithick, draw=red!65, dashed},
|
||||
loop/.style={-{Stealth[length=2.2mm]}, semithick, draw=blue!60},
|
||||
ed/.style={font=\scriptsize, fill=white, inner sep=1pt, text=gray!25!black},
|
||||
}
|
||||
|
||||
\begin{document}
|
||||
\begin{tikzpicture}[node distance=7mm]
|
||||
|
||||
\node[idle] (w) {等待触发\\\scriptsize open\_wait};
|
||||
\node[act, right=of w] (c) {闭合中\\\scriptsize closing};
|
||||
\node[act, right=of c] (g) {夹持加力\\\scriptsize gripping};
|
||||
\node[hold, right=of g] (h) {夹持检测\\\scriptsize hold\_check};
|
||||
\node[rec, right=of h] (o) {张开恢复\\\scriptsize open\_recover};
|
||||
|
||||
\draw[arr] (w) -- node[ed, above] {侧边触发} (c);
|
||||
\draw[arr] (c) -- node[ed, above] {接触} (g);
|
||||
\draw[arr] (g) -- node[ed, above] {到保持力} (h);
|
||||
\draw[arr] (h) -- node[ed, above] {取物} (o);
|
||||
|
||||
\draw[loop] (o.south) -- ++(0,-0.75) -| node[ed, pos=0.25, below] {恢复 + rearm} (w.south);
|
||||
|
||||
\draw[darr] (c.south) -- ++(0,-0.55) -| node[ed, pos=0.12, below] {超时} (o.south);
|
||||
|
||||
\end{tikzpicture}
|
||||
\end{document}
|
||||
@@ -0,0 +1,49 @@
|
||||
\documentclass[tikz,border=8pt]{standalone}
|
||||
\usepackage{ctex}
|
||||
\usepackage{tikz}
|
||||
\usetikzlibrary{positioning,fit,arrows.meta,calc,backgrounds}
|
||||
|
||||
\tikzset{
|
||||
base/.style={draw, rounded corners=2pt, align=center, font=\small},
|
||||
sensor/.style={base, fill=blue!8, draw=blue!55, minimum width=3.0cm, minimum height=1.05cm},
|
||||
process/.style={base, fill=violet!8, draw=violet!55, minimum width=4.2cm, minimum height=1.15cm},
|
||||
control/.style={base, fill=orange!10, draw=orange!60!black, minimum width=3.0cm, minimum height=1.0cm},
|
||||
device/.style={base, fill=gray!10, draw=gray!60, minimum width=3.0cm, minimum height=1.0cm},
|
||||
gui/.style={base, fill=teal!8, draw=teal!60!black, minimum width=4.2cm, minimum height=1.05cm},
|
||||
layer/.style={draw=gray!35, rounded corners=3pt, inner sep=10pt},
|
||||
arr/.style={-{Stealth[length=2.2mm]}, semithick, draw=gray!70},
|
||||
darr/.style={-{Stealth[length=2.2mm]}, semithick, draw=gray!55, dashed},
|
||||
lbl/.style={font=\scriptsize, fill=white, inner sep=1pt, text=gray!30!black},
|
||||
}
|
||||
|
||||
\begin{document}
|
||||
\begin{tikzpicture}[node distance=8mm and 10mm]
|
||||
|
||||
\node[sensor] (L) {左触觉传感器\\{\scriptsize USB 摄像头}};
|
||||
\node[sensor, right=5.8cm of L] (R) {右触觉传感器\\{\scriptsize USB 摄像头}};
|
||||
|
||||
\node[process, below=1.35cm of $(L)!0.5!(R)$] (P) {触觉力计算 / 光流\\{\scriptsize 法向力 · 切向力 · 图像}};
|
||||
|
||||
\node[control, below left=1.1cm and 0.2cm of P] (SM) {智能夹持\\状态机};
|
||||
\node[device, below right=1.1cm and 0.2cm of P] (GC) {夹爪控制器\\{\scriptsize Modbus RTU}};
|
||||
|
||||
\node[device, below=1.0cm of GC] (G) {智能夹爪};
|
||||
\node[gui, below=1.0cm of P] (UI) {GUI 监控界面\\{\scriptsize 3D · 光流 · 曲线 · 状态}};
|
||||
|
||||
\begin{scope}[on background layer]
|
||||
\node[layer, fit=(L)(R), label={[font=\scriptsize\bfseries]above left:感知层}] {};
|
||||
\node[layer, fit=(P)(SM)(GC), label={[font=\scriptsize\bfseries]above left:软件处理层}] {};
|
||||
\node[layer, fit=(G)(UI), label={[font=\scriptsize\bfseries]above left:执行与呈现层}] {};
|
||||
\end{scope}
|
||||
|
||||
\draw[arr] (L) -- node[lbl, above] {视频帧} (P);
|
||||
\draw[arr] (R) -- node[lbl, above] {视频帧} (P);
|
||||
\draw[arr] (P) -- node[lbl, left] {力信号} (SM);
|
||||
\draw[arr] (SM) -- node[lbl, above] {串口命令} (GC);
|
||||
\draw[arr] (GC) -- node[lbl, right] {驱动} (G);
|
||||
\draw[arr] (P) -- node[lbl, right] {图像/力值} (UI);
|
||||
\draw[arr] (SM) -- node[lbl, left] {状态} (UI);
|
||||
\draw[darr] (G.west) -| node[lbl, near start, below] {位置反馈} (SM.south);
|
||||
|
||||
\end{tikzpicture}
|
||||
\end{document}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 218 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 218 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 218 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 218 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 219 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 218 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 9.1 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 31 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 25 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 54 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 30 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 218 KiB |
@@ -0,0 +1,303 @@
|
||||
"""Grip behavior presets: sensitive / stable / firm (grip force + release)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
|
||||
|
||||
# sensitive = 灵敏：跟手释放，自适应夹持力
|
||||
# stable = 较稳定：接触即 70% 恒力夹持，2 s 保护期，Y 拟合偏移 0.6 N 张开
|
||||
# firm = 十分稳定：接触即 100% 恒力夹持，2.5 s 保护期，Y 拟合偏移 0.8 N 张开
|
||||
GRIP_BEHAVIOR_MODES = ("sensitive", "stable", "firm")
|
||||
|
||||
GRIP_BEHAVIOR_LABELS = {
|
||||
"sensitive": "灵敏",
|
||||
"stable": "较稳定",
|
||||
"firm": "十分稳定",
|
||||
}
|
||||
|
||||
# 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",
|
||||
)
|
||||
|
||||
# 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 = (
|
||||
"speed",
|
||||
"grip_contact_hold_position",
|
||||
"close_force",
|
||||
"grip_start_force",
|
||||
"hold_force",
|
||||
"force_ramp_step",
|
||||
"force_ramp_interval",
|
||||
"adaptive_force_min",
|
||||
"adaptive_force_max",
|
||||
"adaptive_force_step",
|
||||
"adaptive_force_interval",
|
||||
"release_guard_fast_paths",
|
||||
"release_shear_axes",
|
||||
"release_shear_displacement",
|
||||
"release_shear_displacement_samples",
|
||||
"release_object_lost_open",
|
||||
"release_open_speed",
|
||||
"release_open_accel",
|
||||
"release_open_decel",
|
||||
"release_arm_delay_seconds",
|
||||
"release_min_hold_seconds",
|
||||
"release_settle_seconds",
|
||||
"release_min_change",
|
||||
"release_touch_window_seconds",
|
||||
"release_side_touch_change",
|
||||
"release_confirm_window_seconds",
|
||||
"release_sample_timeout_seconds",
|
||||
"release_confirm_samples",
|
||||
"release_slow_confirm_samples",
|
||||
"release_normal_rate",
|
||||
"release_shear_rate",
|
||||
"release_slow_rate",
|
||||
"release_normal_change",
|
||||
"release_shear_change",
|
||||
"release_curve_history",
|
||||
"release_slope_window_seconds",
|
||||
"release_slope_settle_rate",
|
||||
"release_slope_min_points",
|
||||
"release_require_slope_settle",
|
||||
"release_slope_hold_seconds",
|
||||
"release_settle_normal_only",
|
||||
"release_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",
|
||||
"release_normal_onset_enabled",
|
||||
"rearm_seconds",
|
||||
"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,
|
||||
}
|
||||
|
||||
# Adaptive clamp: start light, climb with payload shear.
|
||||
_ADAPTIVE_FORCE = {
|
||||
"grip_contact_hold_position": True,
|
||||
"close_force": 33,
|
||||
"grip_start_force": 25,
|
||||
"hold_force": 33,
|
||||
"force_ramp_step": 1,
|
||||
"force_ramp_interval": 0.1,
|
||||
"adaptive_force_min": 33,
|
||||
"adaptive_force_max": 80,
|
||||
"adaptive_force_step": 5,
|
||||
"adaptive_force_interval": 0.08,
|
||||
}
|
||||
|
||||
def _constant_force(pct):
|
||||
"""Close at *pct* and keep that close command squeezing after contact
|
||||
(no position hold, no later ramp). Close speed stays the global speed."""
|
||||
return {
|
||||
"grip_contact_hold_position": False,
|
||||
"close_force": pct,
|
||||
"grip_start_force": pct,
|
||||
"hold_force": pct,
|
||||
"force_ramp_step": 5,
|
||||
"force_ramp_interval": 0.03,
|
||||
"adaptive_force_min": pct,
|
||||
"adaptive_force_max": pct,
|
||||
"adaptive_force_step": 5,
|
||||
"adaptive_force_interval": 0.03,
|
||||
}
|
||||
|
||||
_SENSITIVE_PROFILE = {
|
||||
**_SENSITIVE_TRIGGER,
|
||||
**_ADAPTIVE_FORCE,
|
||||
"speed": 55,
|
||||
"release_guard_fast_paths": False,
|
||||
"release_shear_axes": "",
|
||||
"release_shear_displacement": 0.0,
|
||||
"release_shear_displacement_samples": 6,
|
||||
"release_object_lost_open": False,
|
||||
"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 = {
|
||||
**_constant_force(70),
|
||||
"speed": 30,
|
||||
"release_guard_fast_paths": True,
|
||||
# Release looks at Y shear only, regardless of the global control axes.
|
||||
"release_shear_axes": "y",
|
||||
# Full clamp swings Y by several N and flips sign, so onset confirmation
|
||||
# keeps cancelling; open on sustained fitted Y displacement instead.
|
||||
"release_shear_displacement": 0.6,
|
||||
"release_shear_displacement_samples": 6,
|
||||
# The close command keeps pushing: reaching close_pos means the object is gone.
|
||||
"release_object_lost_open": True,
|
||||
"release_open_speed": 55,
|
||||
"release_open_accel": 70,
|
||||
"release_open_decel": 70,
|
||||
"release_arm_delay_seconds": 0.55,
|
||||
# Protection: no release of any kind within 2s of contact.
|
||||
"release_min_hold_seconds": 2.0,
|
||||
"release_settle_seconds": 0.22,
|
||||
"release_min_change": 0.12,
|
||||
"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,
|
||||
# Full constant clamp: the protection window is the only arming gate.
|
||||
"release_require_slope_settle": False,
|
||||
"release_slope_hold_seconds": 0.9,
|
||||
"release_settle_normal_only": True,
|
||||
# Open by fitted Y shear displacement only, no oscillation path.
|
||||
"release_shear_oscillation_open": False,
|
||||
"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,
|
||||
# Normal only judges the clamp plateau; opening is by Y shear only.
|
||||
"release_side_shear_enabled": True,
|
||||
"release_side_normal_enabled": False,
|
||||
"release_shear_onset_enabled": True,
|
||||
"release_normal_onset_enabled": False,
|
||||
"rearm_seconds": 0.9,
|
||||
"recover_stable_seconds": 0.9,
|
||||
}
|
||||
|
||||
# Same release scheme as stable, with a harder clamp and larger margins.
|
||||
_FIRM_PROFILE = {
|
||||
**_STABLE_PROFILE,
|
||||
**_constant_force(100),
|
||||
"release_min_hold_seconds": 2.5,
|
||||
"release_shear_displacement": 0.8,
|
||||
}
|
||||
|
||||
GRIP_BEHAVIOR_PROFILES = {
|
||||
"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": "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",
|
||||
}
|
||||
if text in aliases:
|
||||
return aliases[text]
|
||||
if text not in GRIP_BEHAVIOR_PROFILES:
|
||||
return "sensitive"
|
||||
return text
|
||||
|
||||
|
||||
def _merged_profile(mode: str) -> dict:
|
||||
"""Release/open params from *mode*, trigger params always from sensitive."""
|
||||
mode = normalize_grip_behavior_mode(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 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 = _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):
|
||||
setattr(target, key, copy.deepcopy(value))
|
||||
return mode
|
||||
+134
-5
@@ -13,6 +13,10 @@ from .rotation import (
|
||||
DEFAULT_ROTATION2,
|
||||
normalize_rotation_config,
|
||||
)
|
||||
from .behavior_profiles import (
|
||||
apply_grip_behavior_profile,
|
||||
normalize_grip_behavior_mode,
|
||||
)
|
||||
|
||||
|
||||
DEFAULT_CONTROL_CONFIG = str(
|
||||
@@ -32,8 +36,8 @@ class Demo02Config:
|
||||
video2: str = "1"
|
||||
rotation1: dict | str = field(default_factory=lambda: copy.deepcopy(DEFAULT_ROTATION1))
|
||||
rotation2: dict | str = field(default_factory=lambda: copy.deepcopy(DEFAULT_ROTATION2))
|
||||
sensor_config1: str = "./config/ddjx01.json"
|
||||
sensor_config2: str = "./config/ddjx01.json"
|
||||
sensor_config1: str = "./config/ddjx01_q6a.json"
|
||||
sensor_config2: str = "./config/ddjx01_q6a.json"
|
||||
sensor_fps: float = 30.0
|
||||
motion_threshold: float = 0.0
|
||||
sensor_cpu: bool = False
|
||||
@@ -64,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
|
||||
@@ -78,6 +84,10 @@ class Demo02Config:
|
||||
force_ramp_step: int = 1
|
||||
force_ramp_interval: float = 0.1
|
||||
grip_settle_seconds: float = 0.1
|
||||
# On contact: True = hold at the contact position; False = keep the close
|
||||
# command squeezing toward close_pos at full commanded force.
|
||||
grip_contact_hold_position: bool = True
|
||||
grip_behavior_mode: str = "sensitive"
|
||||
adaptive_grip_enabled: bool = True
|
||||
adaptive_shear_start_force: float = 0.10
|
||||
adaptive_shear_full_force: float = 0.60
|
||||
@@ -85,9 +95,47 @@ class Demo02Config:
|
||||
adaptive_force_max: int = 45
|
||||
adaptive_force_step: int = 1
|
||||
adaptive_force_interval: float = 0.15
|
||||
release_arm_delay_seconds: float = 0.8
|
||||
release_normal_change: float = 0.03
|
||||
release_shear_change: float = 0.03
|
||||
release_arm_delay_seconds: float = 0.12
|
||||
release_min_hold_seconds: float = 0.28
|
||||
release_settle_seconds: float = 0.06
|
||||
release_min_change: float = 0.04
|
||||
release_touch_window_seconds: float = 0.2
|
||||
release_side_touch_change: float = 0.06
|
||||
release_confirm_window_seconds: float = 0.2
|
||||
release_sample_timeout_seconds: float = 0.2
|
||||
release_confirm_samples: int = 2
|
||||
release_slow_confirm_samples: int = 5
|
||||
release_normal_rate: float = 1.6
|
||||
release_shear_rate: float = 1.6
|
||||
release_slow_rate: float = 0.4
|
||||
release_normal_change: float = 0.28
|
||||
release_shear_change: float = 0.28
|
||||
release_curve_history: int = 6
|
||||
release_slope_window_seconds: float = 0.8
|
||||
release_slope_settle_rate: float = 0.2
|
||||
release_slope_min_points: int = 8
|
||||
release_require_slope_settle: bool = False
|
||||
release_slope_hold_seconds: float = 0.0
|
||||
release_settle_normal_only: bool = True
|
||||
release_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
|
||||
# Block oscillation/side-touch release during contact hold and servo motion.
|
||||
release_guard_fast_paths: bool = False
|
||||
# Shear axes for release only: "" follows shear_control_axes; "y" | "x" | "xy".
|
||||
release_shear_axes: str = ""
|
||||
# >0: open when curve-fitted signed Y shear leaves its post-protection
|
||||
# baseline by this much (N) for N consecutive samples; replaces onset paths.
|
||||
release_shear_displacement: float = 0.0
|
||||
release_shear_displacement_samples: int = 6
|
||||
# Open when the gripper travels to close_pos while holding (object gone).
|
||||
release_object_lost_open: bool = False
|
||||
release_side_shear_enabled: bool = True
|
||||
release_side_normal_enabled: bool = True
|
||||
release_shear_onset_enabled: bool = True
|
||||
release_normal_onset_enabled: bool = True
|
||||
rearm_seconds: float = 0.5
|
||||
recover_normal_force: float = 0.02
|
||||
recover_shear_force: float = 0.02
|
||||
@@ -156,6 +204,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))
|
||||
@@ -171,6 +225,17 @@ class Demo02Config:
|
||||
add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step))
|
||||
add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval))
|
||||
add("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds))
|
||||
add("--grip-contact-hold-position", dest="grip_contact_hold_position", action="store_true",
|
||||
default=get("grip_contact_hold_position", cls.grip_contact_hold_position))
|
||||
add("--no-grip-contact-hold-position", dest="grip_contact_hold_position", action="store_false")
|
||||
add(
|
||||
"--grip-behavior-mode",
|
||||
type=str,
|
||||
choices=["sensitive", "stable", "firm"],
|
||||
default=normalize_grip_behavior_mode(
|
||||
get("grip_behavior_mode", cls.grip_behavior_mode)
|
||||
),
|
||||
)
|
||||
add("--enable-adaptive-grip", dest="adaptive_grip_enabled", action="store_true", default=get("adaptive_grip_enabled", cls.adaptive_grip_enabled))
|
||||
add("--disable-adaptive-grip", dest="adaptive_grip_enabled", action="store_false")
|
||||
add("--adaptive-shear-start-force", type=float, default=get("adaptive_shear_start_force", cls.adaptive_shear_start_force))
|
||||
@@ -180,8 +245,66 @@ class Demo02Config:
|
||||
add("--adaptive-force-step", type=int, default=get("adaptive_force_step", cls.adaptive_force_step))
|
||||
add("--adaptive-force-interval", type=float, default=get("adaptive_force_interval", cls.adaptive_force_interval))
|
||||
add("--release-arm-delay-seconds", type=float, default=get("release_arm_delay_seconds", cls.release_arm_delay_seconds))
|
||||
add("--release-min-hold-seconds", type=float, default=get("release_min_hold_seconds", cls.release_min_hold_seconds))
|
||||
add("--release-settle-seconds", type=float, default=get("release_settle_seconds", cls.release_settle_seconds))
|
||||
add("--release-min-change", type=float, default=get("release_min_change", cls.release_min_change))
|
||||
add("--release-touch-window-seconds", type=float, default=get("release_touch_window_seconds", cls.release_touch_window_seconds))
|
||||
add("--release-side-touch-change", type=float, default=get("release_side_touch_change", cls.release_side_touch_change))
|
||||
add("--release-confirm-window-seconds", type=float, default=get("release_confirm_window_seconds", cls.release_confirm_window_seconds))
|
||||
add("--release-sample-timeout-seconds", type=float, default=get("release_sample_timeout_seconds", cls.release_sample_timeout_seconds))
|
||||
add("--release-confirm-samples", type=int, default=get("release_confirm_samples", cls.release_confirm_samples))
|
||||
add("--release-slow-confirm-samples", type=int, default=get("release_slow_confirm_samples", cls.release_slow_confirm_samples))
|
||||
add("--release-normal-rate", type=float, default=get("release_normal_rate", cls.release_normal_rate))
|
||||
add("--release-shear-rate", type=float, default=get("release_shear_rate", cls.release_shear_rate))
|
||||
add("--release-slow-rate", type=float, default=get("release_slow_rate", cls.release_slow_rate))
|
||||
add("--release-normal-change", type=float, default=get("release_normal_change", cls.release_normal_change))
|
||||
add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
|
||||
add("--release-curve-history", type=int, default=get("release_curve_history", cls.release_curve_history))
|
||||
add("--release-slope-window-seconds", type=float, default=get("release_slope_window_seconds", cls.release_slope_window_seconds))
|
||||
add("--release-slope-settle-rate", type=float, default=get("release_slope_settle_rate", cls.release_slope_settle_rate))
|
||||
add("--release-slope-min-points", type=int, default=get("release_slope_min_points", cls.release_slope_min_points))
|
||||
add("--release-require-slope-settle", dest="release_require_slope_settle", action="store_true",
|
||||
default=get("release_require_slope_settle", cls.release_require_slope_settle))
|
||||
add("--no-release-require-slope-settle", dest="release_require_slope_settle", action="store_false")
|
||||
add("--release-slope-hold-seconds", type=float, default=get("release_slope_hold_seconds", cls.release_slope_hold_seconds))
|
||||
add("--release-settle-normal-only", dest="release_settle_normal_only", action="store_true",
|
||||
default=get("release_settle_normal_only", cls.release_settle_normal_only))
|
||||
add("--release-settle-all-channels", dest="release_settle_normal_only", action="store_false")
|
||||
add("--release-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-guard-fast-paths", dest="release_guard_fast_paths", action="store_true",
|
||||
default=get("release_guard_fast_paths", cls.release_guard_fast_paths))
|
||||
add("--no-release-guard-fast-paths", dest="release_guard_fast_paths", action="store_false")
|
||||
add("--release-shear-axes", type=str, choices=["", "xy", "x", "y"],
|
||||
default=str(get("release_shear_axes", cls.release_shear_axes) or "").strip().lower())
|
||||
add("--release-shear-displacement", type=float,
|
||||
default=get("release_shear_displacement", cls.release_shear_displacement))
|
||||
add("--release-shear-displacement-samples", type=int,
|
||||
default=get("release_shear_displacement_samples", cls.release_shear_displacement_samples))
|
||||
add("--release-object-lost-open", dest="release_object_lost_open", action="store_true",
|
||||
default=get("release_object_lost_open", cls.release_object_lost_open))
|
||||
add("--no-release-object-lost-open", dest="release_object_lost_open", action="store_false")
|
||||
add("--release-side-shear-enabled", dest="release_side_shear_enabled", action="store_true",
|
||||
default=get("release_side_shear_enabled", cls.release_side_shear_enabled))
|
||||
add("--no-release-side-shear", dest="release_side_shear_enabled", action="store_false")
|
||||
add("--release-side-normal-enabled", dest="release_side_normal_enabled", action="store_true",
|
||||
default=get("release_side_normal_enabled", cls.release_side_normal_enabled))
|
||||
add("--no-release-side-normal", dest="release_side_normal_enabled", action="store_false")
|
||||
add("--release-shear-onset-enabled", dest="release_shear_onset_enabled", action="store_true",
|
||||
default=get("release_shear_onset_enabled", cls.release_shear_onset_enabled))
|
||||
add("--no-release-shear-onset", dest="release_shear_onset_enabled", action="store_false")
|
||||
add("--release-normal-onset-enabled", dest="release_normal_onset_enabled", action="store_true",
|
||||
default=get("release_normal_onset_enabled", cls.release_normal_onset_enabled))
|
||||
add("--no-release-normal-onset", dest="release_normal_onset_enabled", action="store_false")
|
||||
add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds))
|
||||
add("--recover-normal-force", type=float, default=get("recover_normal_force", cls.recover_normal_force))
|
||||
add("--recover-shear-force", type=float, default=get("recover_shear_force", cls.recover_shear_force))
|
||||
@@ -208,4 +331,10 @@ class Demo02Config:
|
||||
values["shear_force_calibration"] = copy.deepcopy(
|
||||
config_data.get("shear_force_calibration", DEFAULT_SHEAR_FORCE_CALIBRATION)
|
||||
)
|
||||
values["grip_behavior_mode"] = normalize_grip_behavior_mode(
|
||||
values.get("grip_behavior_mode", cls.grip_behavior_mode)
|
||||
)
|
||||
return cls(**values)
|
||||
|
||||
def apply_grip_behavior_mode(self, mode: str) -> str:
|
||||
return apply_grip_behavior_profile(self, mode)
|
||||
|
||||
@@ -1,5 +1,11 @@
|
||||
from .controller import SideTriggerGripController
|
||||
from .feedback import TactileFeedbackProcessor
|
||||
from .actuator import ActuatorWorker, GripCommand
|
||||
|
||||
|
||||
__all__ = ["SideTriggerGripController", "TactileFeedbackProcessor"]
|
||||
__all__ = [
|
||||
"SideTriggerGripController",
|
||||
"TactileFeedbackProcessor",
|
||||
"ActuatorWorker",
|
||||
"GripCommand",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,376 @@
|
||||
"""Serial-owning actuator worker: command queue + position cache."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GripCommand:
|
||||
kind: str
|
||||
force_pct: Optional[int] = None
|
||||
position: Optional[int] = None
|
||||
label: str = ""
|
||||
speed_pct: Optional[int] = None
|
||||
accel: Optional[int] = None
|
||||
decel: Optional[int] = None
|
||||
|
||||
|
||||
_MOTION_KINDS = frozenset({"open", "close", "move", "hold"})
|
||||
|
||||
|
||||
class ActuatorWorker:
|
||||
"""Runs all GripperClient I/O on one thread; control loop only submits."""
|
||||
|
||||
def __init__(self, gripper, position_poll_hz: float = 30.0):
|
||||
self.gripper = gripper
|
||||
self.position_poll_hz = max(1.0, float(position_poll_hz))
|
||||
self._stop_event = threading.Event()
|
||||
self._started = threading.Event()
|
||||
self._start_error: Optional[BaseException] = None
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
|
||||
self._cmd_lock = threading.Lock()
|
||||
self._commands: deque[GripCommand] = deque()
|
||||
self._active_command: Optional[GripCommand] = None
|
||||
self._motion_completed_at: Optional[float] = None
|
||||
self._wake = threading.Event()
|
||||
self._busy = threading.Event()
|
||||
# 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
|
||||
self._hold_pos: Optional[int] = None
|
||||
self._last_error: Optional[str] = None
|
||||
self._cmd_count = 0
|
||||
self._cmd_total_s = 0.0
|
||||
self._cmd_max_s = 0.0
|
||||
self._pos_count = 0
|
||||
self._pos_total_s = 0.0
|
||||
self._pos_max_s = 0.0
|
||||
self._perf_last_log = 0.0
|
||||
|
||||
@property
|
||||
def position(self) -> Optional[int]:
|
||||
with self._state_lock:
|
||||
return self._position
|
||||
|
||||
@property
|
||||
def hold_pos(self) -> Optional[int]:
|
||||
with self._state_lock:
|
||||
return self._hold_pos
|
||||
|
||||
@property
|
||||
def last_error(self) -> Optional[str]:
|
||||
with self._state_lock:
|
||||
return self._last_error
|
||||
|
||||
@property
|
||||
def motion_status(self):
|
||||
"""Atomic queue/in-flight snapshot and actual I/O completion time."""
|
||||
with self._cmd_lock:
|
||||
return bool(self._commands) or self._active_command is not None, self._motion_completed_at
|
||||
|
||||
def start(self, timeout: float = 10.0) -> None:
|
||||
if self._thread is not None and self._thread.is_alive():
|
||||
return
|
||||
self._stop_event.clear()
|
||||
self._started.clear()
|
||||
self._start_error = None
|
||||
self._thread = threading.Thread(
|
||||
target=self._run,
|
||||
name="gripper-actuator",
|
||||
daemon=True,
|
||||
)
|
||||
self._thread.start()
|
||||
if not self._started.wait(timeout=timeout):
|
||||
raise TimeoutError("actuator worker failed to start in time")
|
||||
if self._start_error is not None:
|
||||
raise RuntimeError(f"actuator worker failed to connect: {self._start_error}")
|
||||
|
||||
def stop(self, timeout: float = 3.0) -> None:
|
||||
self._stop_event.set()
|
||||
self._wake.set()
|
||||
thread = self._thread
|
||||
if thread is not None:
|
||||
thread.join(timeout=timeout)
|
||||
self._thread = None
|
||||
|
||||
def submit(self, command: GripCommand) -> None:
|
||||
with self._cmd_lock:
|
||||
if command.kind in _MOTION_KINDS:
|
||||
self._commands = deque(
|
||||
item for item in self._commands
|
||||
if item.kind not in _MOTION_KINDS and item.kind != "set_force"
|
||||
)
|
||||
elif command.kind == "set_force":
|
||||
self._commands = deque(
|
||||
item for item in self._commands if item.kind != "set_force"
|
||||
)
|
||||
self._commands.append(command)
|
||||
self._wake.set()
|
||||
|
||||
def open(
|
||||
self,
|
||||
force_pct: int,
|
||||
label: str = "open",
|
||||
speed_pct=None,
|
||||
accel=None,
|
||||
decel=None,
|
||||
) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="open",
|
||||
force_pct=int(force_pct),
|
||||
label=label,
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
)
|
||||
)
|
||||
|
||||
def close(
|
||||
self,
|
||||
force_pct: int,
|
||||
label: str = "close",
|
||||
speed_pct=None,
|
||||
accel=None,
|
||||
decel=None,
|
||||
) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="close",
|
||||
force_pct=int(force_pct),
|
||||
label=label,
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
)
|
||||
)
|
||||
|
||||
def move(
|
||||
self,
|
||||
position: int,
|
||||
force_pct: int,
|
||||
label: str = "move",
|
||||
speed_pct=None,
|
||||
accel=None,
|
||||
decel=None,
|
||||
) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="move",
|
||||
position=int(position),
|
||||
force_pct=int(force_pct),
|
||||
label=label,
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
)
|
||||
)
|
||||
|
||||
def set_force(self, force_pct: int) -> None:
|
||||
self.submit(GripCommand(kind="set_force", force_pct=int(force_pct)))
|
||||
|
||||
def hold(self, force_pct: int, speed_pct=None, accel=None, decel=None) -> None:
|
||||
self.submit(
|
||||
GripCommand(
|
||||
kind="hold",
|
||||
force_pct=int(force_pct),
|
||||
speed_pct=speed_pct,
|
||||
accel=accel,
|
||||
decel=decel,
|
||||
label="clamp-hold",
|
||||
)
|
||||
)
|
||||
|
||||
def final_open_if_needed(self) -> None:
|
||||
self.submit(GripCommand(kind="final_open"))
|
||||
self.wait_idle(timeout=5.0)
|
||||
|
||||
def wait_idle(self, timeout: float = 5.0) -> bool:
|
||||
deadline = time.perf_counter() + timeout
|
||||
while time.perf_counter() < deadline:
|
||||
pending, _ = self.motion_status
|
||||
if not pending:
|
||||
return True
|
||||
time.sleep(0.01)
|
||||
return False
|
||||
|
||||
def _pop_command(self) -> Optional[GripCommand]:
|
||||
with self._cmd_lock:
|
||||
if not self._commands:
|
||||
return None
|
||||
self._active_command = self._commands.popleft()
|
||||
return self._active_command
|
||||
|
||||
def _set_error(self, message: str) -> None:
|
||||
with self._state_lock:
|
||||
self._last_error = message
|
||||
|
||||
def _update_position(self, position: Optional[int]) -> None:
|
||||
with self._state_lock:
|
||||
if position is not None:
|
||||
self._position = int(position)
|
||||
|
||||
def _run(self) -> None:
|
||||
try:
|
||||
self.gripper.connect()
|
||||
except BaseException as exc:
|
||||
self._start_error = exc
|
||||
self._started.set()
|
||||
return
|
||||
|
||||
self._started.set()
|
||||
next_poll = time.perf_counter()
|
||||
self._perf_last_log = time.perf_counter()
|
||||
|
||||
# Prime position cache once after connect.
|
||||
self._poll_position()
|
||||
|
||||
while not self._stop_event.is_set():
|
||||
cmd = self._pop_command()
|
||||
if cmd is not None:
|
||||
self._busy.set()
|
||||
try:
|
||||
self._execute(cmd)
|
||||
finally:
|
||||
with self._cmd_lock:
|
||||
self._motion_completed_at = time.perf_counter()
|
||||
self._active_command = None
|
||||
still_pending = bool(self._commands)
|
||||
if not still_pending:
|
||||
self._busy.clear()
|
||||
continue
|
||||
|
||||
self._busy.clear()
|
||||
now = time.perf_counter()
|
||||
if now >= next_poll:
|
||||
self._poll_position()
|
||||
next_poll = now + self._current_poll_interval(now)
|
||||
self._maybe_log_perf(now)
|
||||
continue
|
||||
|
||||
wait_s = min(0.02, max(0.0, next_poll - now))
|
||||
self._wake.wait(timeout=wait_s)
|
||||
self._wake.clear()
|
||||
|
||||
def _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:
|
||||
if cmd.kind == "open":
|
||||
self.gripper.open(
|
||||
cmd.force_pct,
|
||||
cmd.label or "open",
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "close":
|
||||
self.gripper.close(
|
||||
cmd.force_pct,
|
||||
cmd.label or "close",
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "move":
|
||||
self.gripper.move(
|
||||
cmd.position,
|
||||
cmd.force_pct,
|
||||
cmd.label or "move",
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "set_force":
|
||||
self.gripper.set_force(cmd.force_pct)
|
||||
elif cmd.kind == "hold":
|
||||
hold_pos = self.gripper.hold_current_position(
|
||||
cmd.force_pct,
|
||||
speed_pct=cmd.speed_pct,
|
||||
accel=cmd.accel,
|
||||
decel=cmd.decel,
|
||||
)
|
||||
with self._state_lock:
|
||||
self._hold_pos = hold_pos
|
||||
if hold_pos is not None:
|
||||
self._position = int(hold_pos)
|
||||
self._note_motion_command()
|
||||
elif cmd.kind == "final_open":
|
||||
self.gripper.final_open_if_needed()
|
||||
self._note_motion_command()
|
||||
else:
|
||||
self._set_error(f"unknown command kind: {cmd.kind}")
|
||||
except Exception as exc:
|
||||
self._set_error(f"{cmd.kind} failed: {exc}")
|
||||
print(f"[actuator] {cmd.kind} failed: {exc}")
|
||||
finally:
|
||||
dt = time.perf_counter() - t0
|
||||
with self._state_lock:
|
||||
self._cmd_count += 1
|
||||
self._cmd_total_s += dt
|
||||
self._cmd_max_s = max(self._cmd_max_s, dt)
|
||||
|
||||
def _poll_position(self) -> None:
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
position = self.gripper.read_position()
|
||||
self._update_position(position)
|
||||
except Exception as exc:
|
||||
self._set_error(f"read_position failed: {exc}")
|
||||
finally:
|
||||
dt = time.perf_counter() - t0
|
||||
with self._state_lock:
|
||||
self._pos_count += 1
|
||||
self._pos_total_s += dt
|
||||
self._pos_max_s = max(self._pos_max_s, dt)
|
||||
|
||||
def _maybe_log_perf(self, now: float) -> None:
|
||||
if now - self._perf_last_log < 1.0:
|
||||
return
|
||||
with self._state_lock:
|
||||
cmd_count = self._cmd_count
|
||||
cmd_total = self._cmd_total_s
|
||||
cmd_max = self._cmd_max_s
|
||||
pos_count = self._pos_count
|
||||
pos_total = self._pos_total_s
|
||||
pos_max = self._pos_max_s
|
||||
position = self._position
|
||||
self._cmd_count = 0
|
||||
self._cmd_total_s = 0.0
|
||||
self._cmd_max_s = 0.0
|
||||
self._pos_count = 0
|
||||
self._pos_total_s = 0.0
|
||||
self._pos_max_s = 0.0
|
||||
self._perf_last_log = now
|
||||
if cmd_count == 0 and pos_count == 0:
|
||||
return
|
||||
cmd_avg = (cmd_total / cmd_count * 1000.0) if cmd_count else 0.0
|
||||
pos_avg = (pos_total / pos_count * 1000.0) if pos_count else 0.0
|
||||
print(
|
||||
"[perf] actuator "
|
||||
f"cmd={cmd_count} avg={cmd_avg:.2f}ms max={cmd_max * 1000.0:.2f}ms "
|
||||
f"pos_read={pos_count} avg={pos_avg:.2f}ms max={pos_max * 1000.0:.2f}ms "
|
||||
f"pos={position}"
|
||||
)
|
||||
+437
-195
File diff suppressed because it is too large
Load Diff
+60
-16
@@ -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):
|
||||
@@ -39,6 +64,8 @@ class ForceFeedback:
|
||||
left_shear_y: float = 0.0
|
||||
right_shear_x: float = 0.0
|
||||
right_shear_y: float = 0.0
|
||||
left_timestamp: float | None = None
|
||||
right_timestamp: float | None = None
|
||||
|
||||
|
||||
class TactileFeedbackProcessor:
|
||||
@@ -50,6 +77,7 @@ class TactileFeedbackProcessor:
|
||||
self.reset_filters()
|
||||
|
||||
def reset_filters(self):
|
||||
self._sample_timestamps = {}
|
||||
self.left_normal_filter = ExponentialFilter(self.config.filter_alpha)
|
||||
self.right_normal_filter = ExponentialFilter(self.config.filter_alpha)
|
||||
self.left_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
|
||||
@@ -59,12 +87,24 @@ class TactileFeedbackProcessor:
|
||||
self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha)
|
||||
self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha)
|
||||
|
||||
def _filter_sample(self, key, value, timestamp):
|
||||
"""A cached camera frame must not advance the filter a second time."""
|
||||
filter_ = getattr(self, f"{key}_filter")
|
||||
if timestamp is None or self._sample_timestamps.get(key) != timestamp:
|
||||
self._sample_timestamps[key] = timestamp
|
||||
return filter_.update(value)
|
||||
return filter_.value
|
||||
|
||||
def _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
|
||||
@@ -122,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)
|
||||
@@ -129,27 +170,28 @@ 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_normal = self.left_normal_filter.update(left_force)
|
||||
right_normal = self.right_normal_filter.update(right_force)
|
||||
left_shear_filtered = self.left_shear_filter.update(left_shear)
|
||||
right_shear_filtered = self.right_shear_filter.update(right_shear)
|
||||
left_shear_x_filtered = self.left_shear_x_filter.update(left_shear_x)
|
||||
left_shear_y_filtered = self.left_shear_y_filter.update(left_shear_y)
|
||||
right_shear_x_filtered = self.right_shear_x_filter.update(right_shear_x)
|
||||
right_shear_y_filtered = self.right_shear_y_filter.update(right_shear_y)
|
||||
left_timestamp, right_timestamp = left.get("timestamp"), right.get("timestamp")
|
||||
left_normal = self._filter_sample("left_normal", left_force, left_timestamp)
|
||||
right_normal = self._filter_sample("right_normal", right_force, right_timestamp)
|
||||
left_shear_filtered = self._filter_sample("left_shear", left_shear, left_timestamp)
|
||||
right_shear_filtered = self._filter_sample("right_shear", right_shear, right_timestamp)
|
||||
left_shear_x_filtered = self._filter_sample("left_shear_x", left_shear_x, left_timestamp)
|
||||
left_shear_y_filtered = self._filter_sample("left_shear_y", left_shear_y, left_timestamp)
|
||||
right_shear_x_filtered = self._filter_sample("right_shear_x", right_shear_x, right_timestamp)
|
||||
right_shear_y_filtered = self._filter_sample("right_shear_y", right_shear_y, right_timestamp)
|
||||
|
||||
return ForceFeedback(
|
||||
left_normal=left_normal,
|
||||
@@ -165,4 +207,6 @@ class TactileFeedbackProcessor:
|
||||
left_shear_y=left_shear_y_filtered,
|
||||
right_shear_x=right_shear_x_filtered,
|
||||
right_shear_y=right_shear_y_filtered,
|
||||
left_timestamp=left_timestamp,
|
||||
right_timestamp=right_timestamp,
|
||||
)
|
||||
|
||||
@@ -0,0 +1,211 @@
|
||||
"""Recognize a new force onset, rather than accumulated settling movement."""
|
||||
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
from statistics import median
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _CurveModel:
|
||||
time: float
|
||||
scale: float
|
||||
coefficients: tuple
|
||||
covariance: tuple
|
||||
noise: float
|
||||
|
||||
def terms_at(self, stamp):
|
||||
x = (stamp - self.time) / self.scale
|
||||
return tuple(x ** i for i in range(len(self.coefficients)))
|
||||
|
||||
def predict(self, stamp):
|
||||
return sum(c * x for c, x in zip(self.coefficients, self.terms_at(stamp)))
|
||||
|
||||
def uncertainty(self, stamp):
|
||||
terms = self.terms_at(stamp)
|
||||
variance = sum(a * b * self.covariance[i][j]
|
||||
for i, a in enumerate(terms) for j, b in enumerate(terms))
|
||||
# Include the new sample's noise, and widen the corridor with forecast
|
||||
# distance. A tiny fitting error must not grow into release evidence.
|
||||
return 4.0 * self.noise * math.sqrt(1.0 + max(0.0, variance))
|
||||
|
||||
def change_uncertainty(self, before, after):
|
||||
terms = tuple(b - a for a, b in zip(self.terms_at(before), self.terms_at(after)))
|
||||
variance = sum(a * b * self.covariance[i][j]
|
||||
for i, a in enumerate(terms) for j, b in enumerate(terms))
|
||||
return 4.0 * self.noise * math.sqrt(2.0 + max(0.0, variance))
|
||||
|
||||
@property
|
||||
def slope(self):
|
||||
"""Physical dy/dt from the linear term of the scaled polynomial."""
|
||||
if len(self.coefficients) < 2 or self.scale <= 0:
|
||||
return 0.0
|
||||
return self.coefficients[1] / self.scale
|
||||
|
||||
|
||||
def _fit_curve(history, degree, noise_floor):
|
||||
"""Small least-squares fit with its prediction uncertainty (no I/O)."""
|
||||
stamp = history[-1][0]
|
||||
scale = max(stamp - history[0][0], 1e-6)
|
||||
rows = [tuple(((t - stamp) / scale) ** i for i in range(degree + 1))
|
||||
for t, _ in history]
|
||||
size = degree + 1
|
||||
matrix = [[sum(row[i] * row[j] for row in rows) for j in range(size)]
|
||||
+ [float(i == j) for j in range(size)] for i in range(size)]
|
||||
# Invert the at-most 3x3 normal matrix. Sensor times have already been
|
||||
# checked for duplicates/restarts before samples enter the history.
|
||||
for column in range(size):
|
||||
pivot = max(range(column, size), key=lambda row: abs(matrix[row][column]))
|
||||
matrix[column], matrix[pivot] = matrix[pivot], matrix[column]
|
||||
divisor = matrix[column][column]
|
||||
if abs(divisor) < 1e-12:
|
||||
return _fit_curve(history, degree - 1, noise_floor)
|
||||
matrix[column] = [value / divisor for value in matrix[column]]
|
||||
for row in range(size):
|
||||
if row != column:
|
||||
multiplier = matrix[row][column]
|
||||
matrix[row] = [a - multiplier * b for a, b in zip(matrix[row], matrix[column])]
|
||||
covariance = tuple(tuple(row[size:]) for row in matrix)
|
||||
target = [sum(row[i] * value for row, (_, value) in zip(rows, history)) for i in range(size)]
|
||||
coefficients = tuple(sum(a * b for a, b in zip(row, target)) for row in covariance)
|
||||
error = sum((value - sum(c * x for c, x in zip(coefficients, row))) ** 2
|
||||
for row, (_, value) in zip(rows, history))
|
||||
noise = max(noise_floor, math.sqrt(error / max(1, len(history) - size)))
|
||||
return _CurveModel(stamp, scale, coefficients, covariance, noise)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ForceOnset:
|
||||
time: float
|
||||
origin: float
|
||||
direction: float
|
||||
innovation: float
|
||||
model: _CurveModel
|
||||
|
||||
def baseline_at(self, stamp):
|
||||
return self.model.predict(stamp)
|
||||
|
||||
def significant_at(self, stamp, value):
|
||||
return abs(value - self.baseline_at(stamp)) > self.model.uncertainty(stamp)
|
||||
|
||||
|
||||
class ForceCurve:
|
||||
"""Local polynomial continuation plus signed slope/turn classification.
|
||||
|
||||
A short recent window still drives onset detection. A longer retained history
|
||||
estimates the settling slope (grip force ramp decaying toward zero).
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
history = max(6, int(getattr(config, "release_curve_history", 6) or 6))
|
||||
self.history = deque(maxlen=history)
|
||||
self._noise_history = deque(maxlen=6)
|
||||
self.onset = None
|
||||
self.rate = 0.0
|
||||
self.delta = 0.0
|
||||
self.innovation = 0.0
|
||||
self.motion = "curve_follow"
|
||||
self.fit_slope = 0.0
|
||||
|
||||
def discard_onset(self):
|
||||
self.onset = None
|
||||
|
||||
def _onset_history(self):
|
||||
"""Keep onset detection local even when the retained history is long."""
|
||||
onset_points = max(3, min(6, len(self.history)))
|
||||
if len(self.history) <= onset_points:
|
||||
return list(self.history)
|
||||
return list(self.history)[-onset_points:]
|
||||
|
||||
def window_slope(self, now=None, window_s=None):
|
||||
"""Linear least-squares slope over a longer settling window (N/s)."""
|
||||
if len(self.history) < 2:
|
||||
return 0.0
|
||||
window_s = float(
|
||||
getattr(self.config, "release_slope_window_seconds", 0.8)
|
||||
if window_s is None
|
||||
else window_s
|
||||
)
|
||||
now = self.history[-1][0] if now is None else now
|
||||
cutoff = now - max(0.05, window_s)
|
||||
points = [(t, v) for t, v in self.history if t >= cutoff]
|
||||
if len(points) < 2:
|
||||
points = list(self.history)
|
||||
floor = min(self.config.release_min_change, self.config.release_side_touch_change)
|
||||
model = _fit_curve(points, min(1, len(points) - 1), max(1e-6, floor * 0.025))
|
||||
return model.slope
|
||||
|
||||
def observe(self, stamp, value):
|
||||
if self.history:
|
||||
if stamp == self.history[-1][0]:
|
||||
return
|
||||
if stamp < self.history[-1][0]:
|
||||
self.history.clear()
|
||||
self._noise_history.clear()
|
||||
self.onset = None
|
||||
self.rate = self.delta = self.innovation = 0.0
|
||||
self.fit_slope = 0.0
|
||||
self.motion = "curve_follow"
|
||||
if not self.history:
|
||||
self.history.append((stamp, value))
|
||||
return
|
||||
previous_time, previous_value = self.history[-1]
|
||||
dt = stamp - previous_time
|
||||
self.delta = value - previous_value
|
||||
self.rate = self.delta / dt
|
||||
previous_rate = 0.0
|
||||
prior_rates = []
|
||||
if len(self.history) >= 2:
|
||||
older_time, older_value = self.history[-2]
|
||||
previous_rate = (previous_value - older_value) / (previous_time - older_time)
|
||||
prior_rates.append(previous_rate)
|
||||
if len(self.history) >= 3:
|
||||
oldest_time, oldest_value = self.history[-3]
|
||||
older_rate = (older_value - oldest_value) / (older_time - oldest_time)
|
||||
prior_rates.append(older_rate)
|
||||
floor = min(self.config.release_min_change, self.config.release_side_touch_change)
|
||||
noise_floor = floor * 0.025
|
||||
onset_history = self._onset_history()
|
||||
model = _fit_curve(onset_history, min(2, len(onset_history) - 1), noise_floor)
|
||||
self._noise_history.append(model.noise)
|
||||
model = replace(model, noise=max(model.noise, median(self._noise_history)))
|
||||
self.innovation = value - model.predict(stamp)
|
||||
# Append first so the settling slope includes the new sample.
|
||||
self.history.append((stamp, value))
|
||||
self.fit_slope = self.window_slope(stamp)
|
||||
# A sign reversal of comparable speed is a turn, not another impulse.
|
||||
turning = (previous_rate * self.rate < 0
|
||||
and abs(self.rate) <= 1.5 * max((abs(r) for r in prior_rates), default=0.0))
|
||||
decelerating = (previous_rate * self.rate >= 0
|
||||
and abs(self.rate) <= abs(previous_rate) + 1e-9)
|
||||
onset = self.onset
|
||||
if onset is not None:
|
||||
if self.delta * onset.direction < -floor * 0.1:
|
||||
self.onset = None
|
||||
turning = True
|
||||
elif not onset.significant_at(stamp, value):
|
||||
# Losing evidence before reaching the amplitude threshold is
|
||||
# not permission to accumulate later, unrelated curve travel.
|
||||
self.onset = None
|
||||
elif ((value - onset.origin) * onset.direction < floor
|
||||
and self.delta * onset.direction < floor * 0.1):
|
||||
# A sub-threshold onset which has already flattened is over.
|
||||
# Do not let the forecast alone grow a small bump into a later
|
||||
# candidate while the real signal resumes its natural curve.
|
||||
self.onset = None
|
||||
elif stamp - onset.time > self.config.release_touch_window_seconds:
|
||||
self.onset = None
|
||||
if turning:
|
||||
self.motion = "settling_turn"
|
||||
elif decelerating and abs(previous_rate) > 1e-9:
|
||||
self.motion = "settling_flatten"
|
||||
|
||||
if (self.onset is None and not turning and not decelerating
|
||||
and abs(self.innovation) + 1e-9 >= floor * 0.2
|
||||
and abs(self.innovation) > model.uncertainty(stamp)
|
||||
and self.innovation * self.delta > 0):
|
||||
self.onset = ForceOnset(stamp, previous_value,
|
||||
1.0 if self.innovation > 0 else -1.0, self.innovation,
|
||||
model)
|
||||
self.motion = "new_onset"
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Route release decisions to the FSM owned by the current grip behavior mode.
|
||||
|
||||
sensitive -> release_watch_sensitive (frozen copy of the tuned sensitive logic)
|
||||
stable / firm -> release_watch (constant clamp, fitted Y displacement)
|
||||
"""
|
||||
|
||||
from . import release_watch as held_release
|
||||
from . import release_watch_sensitive as sensitive_release
|
||||
|
||||
|
||||
class ModeReleaseWatch:
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self._sensitive = sensitive_release.ReleaseWatch(config)
|
||||
self._held = held_release.ReleaseWatch(config)
|
||||
|
||||
@property
|
||||
def active(self):
|
||||
mode = getattr(self.config, "grip_behavior_mode", "sensitive")
|
||||
return self._sensitive if mode == "sensitive" else self._held
|
||||
|
||||
def reset(self):
|
||||
self._sensitive.reset()
|
||||
self._held.reset()
|
||||
|
||||
def __getattr__(self, name):
|
||||
return getattr(self.active, name)
|
||||
@@ -0,0 +1,817 @@
|
||||
"""Release FSM. No clock, serial I/O or callbacks: step() returns a decision."""
|
||||
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
import math
|
||||
|
||||
from .feedback import combine_shear_components
|
||||
from .force_curve import ForceCurve, ForceOnset, _fit_curve
|
||||
|
||||
|
||||
CHANNELS = ("left_normal", "right_normal", "left_shear", "right_shear")
|
||||
_SHEAR_COMPONENTS = ("left_shear_x", "left_shear_y", "right_shear_x", "right_shear_y")
|
||||
|
||||
|
||||
def channel_values(config, feedback):
|
||||
"""Release channels; shear may use its own axes independent of the trigger."""
|
||||
values = {key: float(getattr(feedback, key)) for key in CHANNELS}
|
||||
axes = str(getattr(config, "release_shear_axes", "") or "").strip().lower()
|
||||
components = [float(getattr(feedback, key, 0.0) or 0.0) for key in _SHEAR_COMPONENTS]
|
||||
# Magnitude-mode calibration has no per-axis components; keep feedback shear.
|
||||
if axes and any(components):
|
||||
values["left_shear"] = combine_shear_components(components[0], components[1], axes)
|
||||
values["right_shear"] = combine_shear_components(components[2], components[3], axes)
|
||||
return values
|
||||
|
||||
|
||||
def signed_shear_y(feedback, key):
|
||||
"""Signed Y for *key* ("left_shear"/"right_shear"); magnitude if no components."""
|
||||
side = key.split("_", 1)[0]
|
||||
x = float(getattr(feedback, f"{side}_shear_x", 0.0) or 0.0)
|
||||
y = float(getattr(feedback, f"{side}_shear_y", 0.0) or 0.0)
|
||||
if x or y:
|
||||
return y
|
||||
return float(getattr(feedback, key))
|
||||
|
||||
|
||||
class _ShearDisplacement:
|
||||
"""Sustained signed-Y departure from the post-protection baseline.
|
||||
|
||||
Each side is smoothed by a local least-squares line (no abs fold, so a
|
||||
sign flip under full clamp is ordinary travel). A turn never cancels:
|
||||
only falling back under the threshold resets the consecutive count. The
|
||||
baseline follows slow drift while the fitted value stays near it.
|
||||
"""
|
||||
|
||||
KEYS = ("left_shear", "right_shear")
|
||||
FIT_POINTS = 6
|
||||
DRIFT_GAIN = 0.02
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.interrupt()
|
||||
self.baseline = {}
|
||||
|
||||
def interrupt(self):
|
||||
self.history = {key: deque(maxlen=self.FIT_POINTS) for key in self.KEYS}
|
||||
self.fitted = {}
|
||||
self.count = 0
|
||||
self.channel = ""
|
||||
self.deviation = 0.0
|
||||
|
||||
def observe(self, key, stamp, value):
|
||||
history = self.history[key]
|
||||
if history and stamp <= history[-1][0]:
|
||||
if stamp == history[-1][0]:
|
||||
return
|
||||
history.clear()
|
||||
history.append((stamp, value))
|
||||
if len(history) >= 3:
|
||||
noise_floor = max(1e-6, float(self.config.release_min_change) * 0.025)
|
||||
self.fitted[key] = _fit_curve(list(history), 1, noise_floor).predict(stamp)
|
||||
else:
|
||||
self.fitted[key] = value
|
||||
|
||||
def step(self, threshold):
|
||||
"""Update after fresh samples; returns True once the baseline is armed."""
|
||||
if len(self.fitted) < len(self.KEYS):
|
||||
return False
|
||||
if not self.baseline:
|
||||
self.baseline = dict(self.fitted)
|
||||
return False
|
||||
deviations = {key: self.fitted[key] - self.baseline[key] for key in self.KEYS}
|
||||
key = max(deviations, key=lambda k: abs(deviations[k]))
|
||||
self.deviation = deviations[key]
|
||||
if abs(self.deviation) + 1e-9 >= threshold:
|
||||
self.count += 1
|
||||
self.channel = key
|
||||
return True
|
||||
self.count = 0
|
||||
self.channel = ""
|
||||
for name, deviation in deviations.items():
|
||||
if abs(deviation) < 0.5 * threshold:
|
||||
self.baseline[name] += self.DRIFT_GAIN * deviation
|
||||
return True
|
||||
|
||||
|
||||
class ReleasePhase(str, Enum):
|
||||
DISARMED = "disarmed"
|
||||
GRIP_SETTLE = "grip_settle"
|
||||
SELF_SETTLE = "self_settle"
|
||||
WATCHING = "watching"
|
||||
CONFIRMING = "confirming"
|
||||
RELEASED = "released"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ReleaseDecision:
|
||||
phase: ReleasePhase
|
||||
reason: str
|
||||
release: bool = False
|
||||
allow_force_update: bool = False
|
||||
normal_change: float = 0.0
|
||||
shear_change: float = 0.0
|
||||
normal_rate: float = 0.0
|
||||
shear_rate: float = 0.0
|
||||
confirm_count: int = 0
|
||||
confirm_needed: int = 0
|
||||
channel: str = ""
|
||||
innovation: float = 0.0
|
||||
curve_motion: str = ""
|
||||
|
||||
@property
|
||||
def action(self):
|
||||
return (
|
||||
f"release-{self.reason} phase={self.phase.value} "
|
||||
f"confirm={self.confirm_count}/{self.confirm_needed} "
|
||||
f"rN={self.normal_rate:.2f} rS={self.shear_rate:.2f}"
|
||||
f" channel={self.channel or '-'} innovation={self.innovation:.4f} curve={self.curve_motion or '-'}"
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Candidate:
|
||||
kind: str
|
||||
channel: str
|
||||
origin: float
|
||||
direction: float
|
||||
peak: float
|
||||
started_at: float
|
||||
needed: int
|
||||
floor: float
|
||||
count: int = 1
|
||||
onset: ForceOnset | None = None
|
||||
last_displacement: float = 0.0
|
||||
last_sample_time: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Rebound:
|
||||
origin: float
|
||||
direction: float
|
||||
remaining: float
|
||||
|
||||
|
||||
def _returning_tail(tails, key, value, floor):
|
||||
"""Suppress only a rejected pulse returning towards its original baseline.
|
||||
|
||||
This is channel-local and ends on return or renewed movement. It does not
|
||||
wait for unrelated channels to become quiet or impose a new hold timer.
|
||||
"""
|
||||
tail = tails.get(key)
|
||||
if tail is None:
|
||||
return False
|
||||
remaining = (value - tail.origin) * tail.direction
|
||||
if abs(remaining) <= floor * 0.5:
|
||||
del tails[key]
|
||||
return True
|
||||
if remaining < 0 or remaining > tail.remaining + floor * 0.25:
|
||||
del tails[key]
|
||||
return False
|
||||
tail.remaining = min(tail.remaining, remaining)
|
||||
return True
|
||||
|
||||
|
||||
class _SideTouch:
|
||||
"""Observe a new lateral touch even during clamp protection.
|
||||
|
||||
Bilateral common-mode force is cancelled. A vote needs new frames from
|
||||
both sides so asynchronous camera updates cannot fabricate a difference.
|
||||
Normal force stays masked during an actual force command and its tail;
|
||||
differential shear remains available without a contact timer.
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.last_pair = None
|
||||
self.seen = set()
|
||||
self.candidate = None
|
||||
self.recover_until = 0.0
|
||||
self.tails = {}
|
||||
self.curves = {index: ForceCurve(self.config) for index in (1, 2)}
|
||||
|
||||
def seed(self, now, values):
|
||||
self.last_pair = (now, values["left_normal"] - values["right_normal"],
|
||||
values["left_shear"] - values["right_shear"])
|
||||
for index in (1, 2):
|
||||
if not self.curves[index].history:
|
||||
self.curves[index].observe(now, self.last_pair[index])
|
||||
|
||||
def prime(self, now, values):
|
||||
for index, field in ((1, "normal"), (2, "shear")):
|
||||
self.curves[index].observe(now, values[f"left_{field}"] - values[f"right_{field}"])
|
||||
self.curves[index].discard_onset()
|
||||
|
||||
def step(self, now, values, fresh, enabled, normal_masked, sample_time,
|
||||
onset_allowed=True):
|
||||
self.seen.update(fresh)
|
||||
if not all(key in self.seen for key in CHANNELS):
|
||||
return None
|
||||
self.seen.clear()
|
||||
sample = (sample_time, values["left_normal"] - values["right_normal"],
|
||||
values["left_shear"] - values["right_shear"])
|
||||
for index in (1, 2):
|
||||
self.curves[index].observe(sample_time, sample[index])
|
||||
if normal_masked:
|
||||
self.curves[1].discard_onset()
|
||||
if not enabled or now < self.recover_until:
|
||||
self.seed(sample_time, values)
|
||||
self.candidate = None
|
||||
for curve in self.curves.values():
|
||||
curve.discard_onset()
|
||||
return None
|
||||
self.last_pair = sample
|
||||
# Stable grip: keep learning the long ramp curve, but do not arm side
|
||||
# touch while the settling slope is still decaying toward zero.
|
||||
if not onset_allowed:
|
||||
self.candidate = None
|
||||
for curve in self.curves.values():
|
||||
curve.discard_onset()
|
||||
return None
|
||||
# Drop leftover candidates that the profile has disabled.
|
||||
if self.candidate is not None:
|
||||
if (self.candidate.channel == "side_shear"
|
||||
and not getattr(self.config, "release_side_shear_enabled", True)):
|
||||
self.candidate = None
|
||||
self.curves[2].discard_onset()
|
||||
elif (self.candidate.channel == "side_normal"
|
||||
and not getattr(self.config, "release_side_normal_enabled", True)):
|
||||
self.candidate = None
|
||||
self.curves[1].discard_onset()
|
||||
returning = {index for index in (1, 2) if _returning_tail(
|
||||
self.tails, index, sample[index], self.config.release_side_touch_change)}
|
||||
if returning:
|
||||
for index in returning:
|
||||
self.curves[index].discard_onset()
|
||||
candidate = self.candidate
|
||||
if candidate is not None:
|
||||
index = 1 if candidate.channel == "side_normal" else 2
|
||||
baseline = candidate.onset.baseline_at(sample_time)
|
||||
delta = (sample[index] - baseline) * candidate.direction
|
||||
return_margin = max(self.config.release_min_change * 0.1,
|
||||
candidate.onset.model.change_uncertainty(candidate.last_sample_time, sample_time))
|
||||
turning = delta < candidate.last_displacement - return_margin
|
||||
if (now - candidate.started_at > self.config.release_confirm_window_seconds
|
||||
or delta + 1e-9 < max(candidate.floor, 0.6 * candidate.peak)
|
||||
or not candidate.onset.significant_at(sample_time, sample[index])
|
||||
or (normal_masked and index == 1) or turning):
|
||||
self.candidate = None
|
||||
self.tails[index] = _Rebound(baseline, candidate.direction, delta)
|
||||
self.recover_until = now + self.config.release_settle_seconds
|
||||
self.seed(sample_time, values)
|
||||
self.curves[index].discard_onset()
|
||||
return "settling-turn" if turning else "rejected"
|
||||
candidate.peak = max(candidate.peak, delta)
|
||||
candidate.last_displacement = delta
|
||||
candidate.last_sample_time = sample_time
|
||||
candidate.count += 1
|
||||
else:
|
||||
shear_ok = getattr(self.config, "release_side_shear_enabled", True)
|
||||
normal_ok = getattr(self.config, "release_side_normal_enabled", True)
|
||||
channels = []
|
||||
if shear_ok:
|
||||
channels.append((2, "side_shear"))
|
||||
if normal_ok:
|
||||
channels.append((1, "side_normal"))
|
||||
for index, channel in channels:
|
||||
if index in returning or (normal_masked and index == 1):
|
||||
continue
|
||||
onset = self.curves[index].onset
|
||||
if onset is None:
|
||||
continue
|
||||
delta = sample[index] - onset.baseline_at(sample_time)
|
||||
if (abs(delta) + 1e-9 >= self.config.release_side_touch_change
|
||||
and (sample[index] - onset.origin) * onset.direction + 1e-9 >= self.config.release_side_touch_change
|
||||
and onset.significant_at(sample_time, sample[index])
|
||||
and delta * onset.direction > 0):
|
||||
candidate = _Candidate("side_touch", channel, onset.origin,
|
||||
1.0 if delta > 0 else -1.0, abs(delta), now,
|
||||
max(2, self.config.release_confirm_samples),
|
||||
self.config.release_side_touch_change,
|
||||
onset=onset, last_displacement=abs(delta), last_sample_time=sample_time)
|
||||
self.candidate = candidate
|
||||
break
|
||||
return candidate
|
||||
|
||||
|
||||
class ReleaseWatch:
|
||||
"""Immediate side touch + bounded clamp protection + persistent displacement.
|
||||
|
||||
A force command masks normal changes through execution and a short tail.
|
||||
Lateral shear remains observable; global quiet never gates release.
|
||||
New force commands require an observation window, so repeated adaptive
|
||||
commands cannot continuously re-arm a timer or erase release evidence.
|
||||
Sensor timestamps are optional for replay; production supplies both sides.
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self._side_touch = _SideTouch(config)
|
||||
self._shear_disp = _ShearDisplacement(config)
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.phase = ReleasePhase.DISARMED
|
||||
self._contact_at = None
|
||||
self._grip_settled = False
|
||||
self._slope_settle_since = None
|
||||
self._plateau_latched = False
|
||||
self._reference = {}
|
||||
self._previous = {}
|
||||
self._recovery_until = 0.0
|
||||
self._settle_seen = set()
|
||||
self._tails = {}
|
||||
self._watch_since = None
|
||||
self._candidate = None
|
||||
self._osc_count = 0
|
||||
self._osc_channel = ""
|
||||
self._last_fresh = {}
|
||||
self._rates = {}
|
||||
self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
|
||||
self._side_touch.reset()
|
||||
self._shear_disp.reset()
|
||||
|
||||
@property
|
||||
def armed(self):
|
||||
return (self.phase in {ReleasePhase.WATCHING, ReleasePhase.CONFIRMING}
|
||||
or self._side_touch.candidate is not None)
|
||||
|
||||
@property
|
||||
def side_watch_active(self):
|
||||
return (self.phase not in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}
|
||||
and self._side_touch.last_pair is not None and len(self._previous) == len(CHANNELS))
|
||||
|
||||
def contact(self, now, feedback=None, history=()):
|
||||
self.reset()
|
||||
self._contact_at = now
|
||||
self.phase = ReleasePhase.GRIP_SETTLE
|
||||
for frame_time, frame in history:
|
||||
values = channel_values(self.config, frame)
|
||||
for key, value in values.items():
|
||||
stamp = getattr(frame, f"{key.split('_', 1)[0]}_timestamp", None)
|
||||
self._curves[key].observe(frame_time if stamp is None else stamp, value)
|
||||
self._curves[key].discard_onset()
|
||||
self._side_touch.prime(self._pair_time(frame_time, frame), values)
|
||||
if feedback is not None:
|
||||
values = channel_values(self.config, feedback)
|
||||
self._side_touch.seed(self._pair_time(now, feedback), values)
|
||||
for key in CHANNELS:
|
||||
stamp = getattr(feedback, f"{key.split('_', 1)[0]}_timestamp", None)
|
||||
self._curves[key].observe(now if stamp is None else stamp, values[key])
|
||||
self._curves[key].discard_onset()
|
||||
|
||||
@staticmethod
|
||||
def _pair_time(now, feedback):
|
||||
stamps = (getattr(feedback, "left_timestamp", None), getattr(feedback, "right_timestamp", None))
|
||||
return now if None in stamps else max(stamps)
|
||||
|
||||
def suspend(self):
|
||||
"""Discard evidence during missing feedback, sensor restart or manual mode."""
|
||||
if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
|
||||
return
|
||||
self._protect("sample-gap", {})
|
||||
self._side_touch.reset()
|
||||
self._shear_disp.interrupt()
|
||||
self._reference.clear()
|
||||
self._previous.clear()
|
||||
self._rates.clear()
|
||||
self._last_fresh.clear()
|
||||
self._tails.clear()
|
||||
self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
|
||||
|
||||
def _normal_slopes_live_ok(self, now):
|
||||
"""Live check: normal long-window slopes are near zero long enough."""
|
||||
if not getattr(self.config, "release_require_slope_settle", False):
|
||||
return True
|
||||
threshold = max(0.0, float(getattr(self.config, "release_slope_settle_rate", 0.2) or 0.2))
|
||||
min_points = max(3, int(getattr(self.config, "release_slope_min_points", 8) or 8))
|
||||
hold = max(0.0, float(getattr(self.config, "release_slope_hold_seconds", 0.0) or 0.0))
|
||||
normal_only = getattr(self.config, "release_settle_normal_only", True)
|
||||
keys = CHANNELS[:2] if normal_only else CHANNELS
|
||||
for key in keys:
|
||||
curve = self._curves[key]
|
||||
if len(curve.history) < min_points or abs(curve.fit_slope) > threshold:
|
||||
self._slope_settle_since = None
|
||||
return False
|
||||
if self._slope_settle_since is None:
|
||||
self._slope_settle_since = now
|
||||
if hold > 0 and now - self._slope_settle_since < hold:
|
||||
return False
|
||||
return True
|
||||
|
||||
def _force_settled(self, now):
|
||||
"""Release-armed after the first relative normal plateau (latched).
|
||||
|
||||
The live slope near zero is only required to *enter* the armed state.
|
||||
Once latched, further adaptive 加力 or an intentional open push may make
|
||||
the slope non-zero again — that must not revoke release permission, or
|
||||
the open gesture is mistaken for "still settling".
|
||||
"""
|
||||
if not getattr(self.config, "release_require_slope_settle", False):
|
||||
self._plateau_latched = True
|
||||
return True
|
||||
if self._plateau_latched:
|
||||
return True
|
||||
if self._normal_slopes_live_ok(now):
|
||||
self._plateau_latched = True
|
||||
return True
|
||||
return False
|
||||
|
||||
def _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
|
||||
else ReleasePhase.GRIP_SETTLE
|
||||
)
|
||||
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():
|
||||
curve.discard_onset()
|
||||
return ReleaseDecision(self.phase, reason, **metrics)
|
||||
|
||||
def step(self, now, feedback, *, motion_pending=False, motion_completed_at=None):
|
||||
if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
|
||||
return ReleaseDecision(self.phase, self.phase.value)
|
||||
|
||||
cfg = self.config
|
||||
timeout = max(0.001, cfg.release_sample_timeout_seconds)
|
||||
values = channel_values(cfg, feedback)
|
||||
if not all(math.isfinite(value) for value in values.values()):
|
||||
self._previous.clear()
|
||||
self._rates.clear()
|
||||
self._side_touch.reset()
|
||||
self._shear_disp.interrupt()
|
||||
self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
|
||||
self._recovery_until = now + cfg.release_settle_seconds
|
||||
return self._protect("invalid-sample", {})
|
||||
|
||||
rates, fresh = {}, set()
|
||||
discontinuity = False
|
||||
for key, value in values.items():
|
||||
side = key.split("_", 1)[0]
|
||||
stamp = getattr(feedback, f"{side}_timestamp", None)
|
||||
stamp = now if stamp is None else stamp
|
||||
previous = self._previous.get(key)
|
||||
if previous is not None and stamp == previous[0]:
|
||||
continue
|
||||
fresh.add(key)
|
||||
self._last_fresh[key] = now
|
||||
if previous is not None:
|
||||
dt = stamp - previous[0]
|
||||
if 0 < dt <= timeout:
|
||||
rates[key] = (value - previous[1]) / dt
|
||||
else:
|
||||
discontinuity = True
|
||||
self._previous[key] = (stamp, value)
|
||||
self._rates.update(rates)
|
||||
|
||||
normal_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[:2]), default=0.0)
|
||||
shear_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[2:]), default=0.0)
|
||||
normal_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[:2])
|
||||
shear_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[2:])
|
||||
metrics = dict(normal_change=normal_change, shear_change=shear_change,
|
||||
normal_rate=normal_rate, shear_rate=shear_rate)
|
||||
stale = any(now - self._last_fresh.get(k, now) > timeout for k in CHANNELS)
|
||||
if stale or discontinuity:
|
||||
self._side_touch.reset()
|
||||
self._shear_disp.interrupt()
|
||||
self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
|
||||
self._recovery_until = now + cfg.release_settle_seconds
|
||||
return self._protect("sample-gap", values, **metrics)
|
||||
|
||||
for key in fresh:
|
||||
self._curves[key].observe(self._previous[key][0], values[key])
|
||||
motions = {curve.motion for curve in self._curves.values()}
|
||||
metrics["curve_motion"] = ("new_onset" if "new_onset" in motions else
|
||||
"settling_turn" if "settling_turn" in motions else
|
||||
"settling_flatten" if "settling_flatten" in motions else "curve_follow")
|
||||
|
||||
self_tail = (
|
||||
motion_completed_at is not None
|
||||
and now - motion_completed_at < max(0.0, cfg.release_arm_delay_seconds)
|
||||
)
|
||||
contact_guard = now - self._contact_at < max(0.0, cfg.release_min_hold_seconds)
|
||||
displacement = float(getattr(cfg, "release_shear_displacement", 0.0) or 0.0)
|
||||
if displacement > 0:
|
||||
return self._displacement_step(feedback, fresh, contact_guard, displacement, metrics)
|
||||
# Guarded modes: a full-force clamp shakes the shear channels, so the
|
||||
# fast paths (oscillation / side touch) wait for contact hold + servo idle.
|
||||
guarded = bool(getattr(cfg, "release_guard_fast_paths", False))
|
||||
fast_blocked = guarded and (motion_pending or self_tail or contact_guard)
|
||||
|
||||
# Shear oscillation = intentional open/close: bypass plateau / settle wait.
|
||||
if not fast_blocked:
|
||||
osc_decision = self._shear_oscillation_release(now, metrics)
|
||||
if osc_decision is not None:
|
||||
return osc_decision
|
||||
|
||||
protected = motion_pending or self_tail or contact_guard or self.phase in {
|
||||
ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE,
|
||||
}
|
||||
force_settled = self._force_settled(now)
|
||||
if not force_settled:
|
||||
metrics["curve_motion"] = "force_settling"
|
||||
if guarded:
|
||||
side_enabled = not fast_blocked and (
|
||||
self.phase == ReleasePhase.WATCHING
|
||||
or self._side_touch.candidate is not None
|
||||
)
|
||||
else:
|
||||
side_enabled = protected or self._side_touch.candidate is not None
|
||||
side = self._side_touch.step(now, values, fresh,
|
||||
enabled=side_enabled,
|
||||
normal_masked=motion_pending or self_tail or fast_blocked,
|
||||
sample_time=self._pair_time(now, feedback),
|
||||
onset_allowed=force_settled and not fast_blocked)
|
||||
if side in ("rejected", "settling-turn"):
|
||||
reason = "settling-turn" if side == "settling-turn" else "transient-rejected"
|
||||
return self._cancel(now, values, reason, metrics, reset_side=False)
|
||||
if side is not None:
|
||||
self._candidate = None
|
||||
released = side.count >= side.needed
|
||||
if released:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
return ReleaseDecision(ReleasePhase.RELEASED if released else ReleasePhase.CONFIRMING,
|
||||
"side_touch", release=released, confirm_count=side.count,
|
||||
confirm_needed=side.needed, channel=side.channel,
|
||||
innovation=side.onset.innovation,
|
||||
**dict(metrics, curve_motion="onset_confirm"))
|
||||
|
||||
slope_gate = getattr(cfg, "release_require_slope_settle", False)
|
||||
# Contact min-hold always clamps. Self-motion clamps until the first
|
||||
# normal plateau is latched (stable) — or always when slope gate is off.
|
||||
if contact_guard or ((motion_pending or self_tail) and (not slope_gate or not force_settled)):
|
||||
reason = ("force-settling" if slope_gate and not force_settled
|
||||
else "self-settle" if self._grip_settled else "grip-settle")
|
||||
return self._protect(reason, values, **metrics)
|
||||
if slope_gate and force_settled and (motion_pending or self_tail):
|
||||
# Adaptive 加力 after plateau: keep watching for intentional open.
|
||||
if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
|
||||
self._grip_settled = True
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
if self._watch_since is None:
|
||||
self._watch_since = now
|
||||
for curve in self._curves.values():
|
||||
curve.discard_onset()
|
||||
|
||||
if not fresh:
|
||||
return ReleaseDecision(self.phase, "waiting-sample", **metrics)
|
||||
|
||||
# Quiet is diagnostic only. No global "all four rates quiet" arming
|
||||
# gate: even small continuous camera noise can keep that gate shut.
|
||||
quiet = (
|
||||
len(self._rates) == len(CHANNELS)
|
||||
and max(abs(rate) for rate in self._rates.values()) <= cfg.release_slow_rate
|
||||
)
|
||||
if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
|
||||
self._settle_seen.update(fresh)
|
||||
self._reference.update(values)
|
||||
if now < self._recovery_until or len(self._settle_seen) < len(CHANNELS):
|
||||
reason = "self-settle" if self._grip_settled else "grip-settle"
|
||||
return ReleaseDecision(self.phase, reason, **metrics)
|
||||
# Stable mode waits until the long-history slope has decayed near
|
||||
# zero before ordinary watching / release paths arm.
|
||||
if not force_settled:
|
||||
return ReleaseDecision(self.phase, "force-settling", **metrics)
|
||||
self._grip_settled = True
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
self._watch_since = now
|
||||
for curve in self._curves.values():
|
||||
curve.discard_onset()
|
||||
return ReleaseDecision(self.phase, "ready", **metrics)
|
||||
returning = {key for key in fresh if _returning_tail(
|
||||
self._tails, key, values[key], cfg.release_min_change)}
|
||||
for key in returning:
|
||||
self._reference[key] = values[key]
|
||||
self._curves[key].discard_onset()
|
||||
|
||||
# Drop watching candidates on channel families the profile disabled.
|
||||
if (self._candidate is not None
|
||||
and self._candidate.channel.endswith("normal")
|
||||
and not getattr(cfg, "release_normal_onset_enabled", True)):
|
||||
self._candidate = None
|
||||
if self._grip_settled:
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
if (self._candidate is not None
|
||||
and self._candidate.channel.endswith("shear")
|
||||
and not getattr(cfg, "release_shear_onset_enabled", True)):
|
||||
self._candidate = None
|
||||
if self._grip_settled:
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
|
||||
candidate = self._candidate
|
||||
if candidate is not None:
|
||||
if now - candidate.started_at > cfg.release_confirm_window_seconds:
|
||||
return self._cancel(now, values, "confirm-expired", metrics)
|
||||
if candidate.channel not in fresh:
|
||||
return ReleaseDecision(self.phase, "waiting-sample", **metrics)
|
||||
stamp = self._previous[candidate.channel][0]
|
||||
displacement = (values[candidate.channel] - candidate.onset.baseline_at(stamp)) * candidate.direction
|
||||
return_margin = max(cfg.release_min_change * 0.1,
|
||||
candidate.onset.model.change_uncertainty(candidate.last_sample_time, stamp))
|
||||
if displacement < candidate.last_displacement - return_margin:
|
||||
return self._cancel(now, values, "settling-turn", metrics)
|
||||
# A spike returning to baseline, reversal, or channel swap is not a
|
||||
# second vote. A step that stays displaced is, even with dF/dt=0.
|
||||
if displacement + 1e-9 < max(candidate.floor, 0.6 * candidate.peak):
|
||||
return self._cancel(now, values, "transient-rejected", metrics)
|
||||
if not candidate.onset.significant_at(stamp, values[candidate.channel]):
|
||||
return self._cancel(now, values, "transient-rejected", metrics)
|
||||
candidate.peak = max(candidate.peak, displacement)
|
||||
candidate.last_displacement = displacement
|
||||
candidate.last_sample_time = stamp
|
||||
candidate.count += 1
|
||||
else:
|
||||
choices = []
|
||||
for key in fresh:
|
||||
if key in returning:
|
||||
continue
|
||||
is_normal = key.endswith("normal")
|
||||
if is_normal and not getattr(cfg, "release_normal_onset_enabled", True):
|
||||
continue
|
||||
if (not is_normal
|
||||
and not getattr(cfg, "release_shear_onset_enabled", True)):
|
||||
continue
|
||||
onset = self._curves[key].onset
|
||||
if onset is None or not onset.significant_at(self._previous[key][0], values[key]):
|
||||
continue
|
||||
rate_limit = cfg.release_normal_rate if is_normal else cfg.release_shear_rate
|
||||
weak_limit = cfg.release_normal_change if is_normal else cfg.release_shear_change
|
||||
delta = values[key] - onset.baseline_at(self._previous[key][0])
|
||||
rate = rates.get(key, 0.0)
|
||||
kind = None
|
||||
origin = onset.origin
|
||||
measured = (values[key] - origin) * onset.direction
|
||||
if (abs(rate) >= rate_limit and abs(delta) >= cfg.release_min_change
|
||||
and measured + 1e-9 >= cfg.release_min_change and rate * delta > 0):
|
||||
kind, needed, floor = "sudden", max(2, cfg.release_confirm_samples), cfg.release_min_change
|
||||
elif (abs(delta) + 1e-9 >= cfg.release_min_change
|
||||
and measured + 1e-9 >= cfg.release_min_change and delta * onset.direction > 0):
|
||||
kind, needed, floor = "touch", max(2, cfg.release_confirm_samples), cfg.release_min_change
|
||||
elif abs(delta) >= weak_limit and measured >= weak_limit:
|
||||
kind = "slow_external"
|
||||
needed = max(2, cfg.release_confirm_samples, cfg.release_slow_confirm_samples)
|
||||
floor = weak_limit
|
||||
if kind:
|
||||
choices.append((kind == "sudden", abs(delta), key, kind, needed, floor, delta, origin, onset))
|
||||
if choices:
|
||||
_, _, key, kind, needed, floor, delta, origin, onset = max(choices)
|
||||
candidate = _Candidate(kind, key, origin,
|
||||
1.0 if delta > 0 else -1.0, abs(delta), now, needed, floor,
|
||||
onset=onset, last_displacement=abs(delta),
|
||||
last_sample_time=self._previous[key][0])
|
||||
self._candidate = candidate
|
||||
self.phase = ReleasePhase.CONFIRMING
|
||||
|
||||
if candidate is not None:
|
||||
released = candidate.count >= candidate.needed
|
||||
if released:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
return ReleaseDecision(self.phase, candidate.kind, release=released,
|
||||
confirm_count=candidate.count,
|
||||
confirm_needed=candidate.needed, channel=candidate.channel,
|
||||
innovation=candidate.onset.innovation,
|
||||
**dict(metrics, curve_motion="onset_confirm"))
|
||||
|
||||
# Follow curve continuation, including large smooth turns. Displacement
|
||||
# without a new onset must never become a weak-path release later.
|
||||
for key in fresh:
|
||||
if self._curves[key].onset is None:
|
||||
self._reference[key] = values[key]
|
||||
observation = max(0.1, max(2, cfg.release_confirm_samples) / max(1.0, cfg.control_hz))
|
||||
# Ordinary noise must not prevent clamp force from reaching its target.
|
||||
# Meaningful coherent movement reserves the next frame for release.
|
||||
touch_pending = any(
|
||||
self._curves[key].onset is not None
|
||||
and abs(values[key] - self._curves[key].onset.baseline_at(self._previous[key][0])) >= cfg.release_min_change * 0.5
|
||||
for key in fresh
|
||||
)
|
||||
allow_force = (not touch_pending and self._watch_since is not None
|
||||
and now - self._watch_since >= observation)
|
||||
reason = "settling-turn" if metrics["curve_motion"] == "settling_turn" else "slow_internal" if quiet else "watch"
|
||||
return ReleaseDecision(self.phase, reason,
|
||||
allow_force_update=allow_force, **metrics)
|
||||
|
||||
def _displacement_step(self, feedback, fresh, contact_guard, threshold, metrics):
|
||||
"""Sole release path when displacement is enabled (onset paths off)."""
|
||||
disp = self._shear_disp
|
||||
for key in disp.KEYS:
|
||||
if key in fresh:
|
||||
disp.observe(key, self._previous[key][0], signed_shear_y(feedback, key))
|
||||
metrics = dict(metrics, curve_motion="shear_displacement")
|
||||
if contact_guard:
|
||||
disp.baseline.clear()
|
||||
disp.count = 0
|
||||
self.phase = ReleasePhase.GRIP_SETTLE
|
||||
return ReleaseDecision(self.phase, "grip-settle", **metrics)
|
||||
if not any(key in fresh for key in disp.KEYS):
|
||||
return ReleaseDecision(self.phase, "waiting-sample", allow_force_update=True, **metrics)
|
||||
if not disp.step(threshold):
|
||||
self._grip_settled = True
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
return ReleaseDecision(self.phase, "ready", allow_force_update=True, **metrics)
|
||||
needed = max(1, int(getattr(self.config, "release_shear_displacement_samples", 6) or 6))
|
||||
metrics["shear_change"] = abs(disp.deviation)
|
||||
if disp.count >= needed:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
else:
|
||||
self.phase = ReleasePhase.CONFIRMING if disp.count else ReleasePhase.WATCHING
|
||||
return ReleaseDecision(
|
||||
self.phase, "shear_displacement" if disp.count else "watch",
|
||||
release=self.phase == ReleasePhase.RELEASED,
|
||||
allow_force_update=not disp.count,
|
||||
confirm_count=disp.count, confirm_needed=needed, channel=disp.channel,
|
||||
**metrics,
|
||||
)
|
||||
|
||||
def _cancel(self, now, values, reason, metrics, *, reset_side=True):
|
||||
# Wait for the rejected pulse's tail; otherwise its falling edge can
|
||||
# become a fresh negative-direction candidate and release on rebound.
|
||||
self._recovery_until = now + self.config.release_settle_seconds
|
||||
if self._candidate is not None:
|
||||
candidate = self._candidate
|
||||
baseline = candidate.onset.baseline_at(self._previous[candidate.channel][0])
|
||||
remaining = (values[candidate.channel] - baseline) * candidate.direction
|
||||
self._tails[candidate.channel] = _Rebound(baseline, candidate.direction, remaining)
|
||||
if reset_side:
|
||||
self._side_touch.reset()
|
||||
self._side_touch.recover_until = self._recovery_until
|
||||
return self._protect(reason, values, **metrics)
|
||||
@@ -0,0 +1,675 @@
|
||||
"""Release FSM. No clock, serial I/O or callbacks: step() returns a decision."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
import math
|
||||
|
||||
from .force_curve import ForceCurve, ForceOnset
|
||||
|
||||
|
||||
CHANNELS = ("left_normal", "right_normal", "left_shear", "right_shear")
|
||||
|
||||
|
||||
class ReleasePhase(str, Enum):
|
||||
DISARMED = "disarmed"
|
||||
GRIP_SETTLE = "grip_settle"
|
||||
SELF_SETTLE = "self_settle"
|
||||
WATCHING = "watching"
|
||||
CONFIRMING = "confirming"
|
||||
RELEASED = "released"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ReleaseDecision:
|
||||
phase: ReleasePhase
|
||||
reason: str
|
||||
release: bool = False
|
||||
allow_force_update: bool = False
|
||||
normal_change: float = 0.0
|
||||
shear_change: float = 0.0
|
||||
normal_rate: float = 0.0
|
||||
shear_rate: float = 0.0
|
||||
confirm_count: int = 0
|
||||
confirm_needed: int = 0
|
||||
channel: str = ""
|
||||
innovation: float = 0.0
|
||||
curve_motion: str = ""
|
||||
|
||||
@property
|
||||
def action(self):
|
||||
return (
|
||||
f"release-{self.reason} phase={self.phase.value} "
|
||||
f"confirm={self.confirm_count}/{self.confirm_needed} "
|
||||
f"rN={self.normal_rate:.2f} rS={self.shear_rate:.2f}"
|
||||
f" channel={self.channel or '-'} innovation={self.innovation:.4f} curve={self.curve_motion or '-'}"
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Candidate:
|
||||
kind: str
|
||||
channel: str
|
||||
origin: float
|
||||
direction: float
|
||||
peak: float
|
||||
started_at: float
|
||||
needed: int
|
||||
floor: float
|
||||
count: int = 1
|
||||
onset: ForceOnset | None = None
|
||||
last_displacement: float = 0.0
|
||||
last_sample_time: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Rebound:
|
||||
origin: float
|
||||
direction: float
|
||||
remaining: float
|
||||
|
||||
|
||||
def _returning_tail(tails, key, value, floor):
|
||||
"""Suppress only a rejected pulse returning towards its original baseline.
|
||||
|
||||
This is channel-local and ends on return or renewed movement. It does not
|
||||
wait for unrelated channels to become quiet or impose a new hold timer.
|
||||
"""
|
||||
tail = tails.get(key)
|
||||
if tail is None:
|
||||
return False
|
||||
remaining = (value - tail.origin) * tail.direction
|
||||
if abs(remaining) <= floor * 0.5:
|
||||
del tails[key]
|
||||
return True
|
||||
if remaining < 0 or remaining > tail.remaining + floor * 0.25:
|
||||
del tails[key]
|
||||
return False
|
||||
tail.remaining = min(tail.remaining, remaining)
|
||||
return True
|
||||
|
||||
|
||||
class _SideTouch:
|
||||
"""Observe a new lateral touch even during clamp protection.
|
||||
|
||||
Bilateral common-mode force is cancelled. A vote needs new frames from
|
||||
both sides so asynchronous camera updates cannot fabricate a difference.
|
||||
Normal force stays masked during an actual force command and its tail;
|
||||
differential shear remains available without a contact timer.
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.last_pair = None
|
||||
self.seen = set()
|
||||
self.candidate = None
|
||||
self.recover_until = 0.0
|
||||
self.tails = {}
|
||||
self.curves = {index: ForceCurve(self.config) for index in (1, 2)}
|
||||
|
||||
def seed(self, now, values):
|
||||
self.last_pair = (now, values["left_normal"] - values["right_normal"],
|
||||
values["left_shear"] - values["right_shear"])
|
||||
for index in (1, 2):
|
||||
if not self.curves[index].history:
|
||||
self.curves[index].observe(now, self.last_pair[index])
|
||||
|
||||
def prime(self, now, values):
|
||||
for index, field in ((1, "normal"), (2, "shear")):
|
||||
self.curves[index].observe(now, values[f"left_{field}"] - values[f"right_{field}"])
|
||||
self.curves[index].discard_onset()
|
||||
|
||||
def step(self, now, values, fresh, enabled, normal_masked, sample_time,
|
||||
onset_allowed=True):
|
||||
self.seen.update(fresh)
|
||||
if not all(key in self.seen for key in CHANNELS):
|
||||
return None
|
||||
self.seen.clear()
|
||||
sample = (sample_time, values["left_normal"] - values["right_normal"],
|
||||
values["left_shear"] - values["right_shear"])
|
||||
for index in (1, 2):
|
||||
self.curves[index].observe(sample_time, sample[index])
|
||||
if normal_masked:
|
||||
self.curves[1].discard_onset()
|
||||
if not enabled or now < self.recover_until:
|
||||
self.seed(sample_time, values)
|
||||
self.candidate = None
|
||||
for curve in self.curves.values():
|
||||
curve.discard_onset()
|
||||
return None
|
||||
self.last_pair = sample
|
||||
# Stable grip: keep learning the long ramp curve, but do not arm side
|
||||
# touch while the settling slope is still decaying toward zero.
|
||||
if not onset_allowed:
|
||||
self.candidate = None
|
||||
for curve in self.curves.values():
|
||||
curve.discard_onset()
|
||||
return None
|
||||
# Drop leftover candidates that the profile has disabled.
|
||||
if self.candidate is not None:
|
||||
if (self.candidate.channel == "side_shear"
|
||||
and not getattr(self.config, "release_side_shear_enabled", True)):
|
||||
self.candidate = None
|
||||
self.curves[2].discard_onset()
|
||||
elif (self.candidate.channel == "side_normal"
|
||||
and not getattr(self.config, "release_side_normal_enabled", True)):
|
||||
self.candidate = None
|
||||
self.curves[1].discard_onset()
|
||||
returning = {index for index in (1, 2) if _returning_tail(
|
||||
self.tails, index, sample[index], self.config.release_side_touch_change)}
|
||||
if returning:
|
||||
for index in returning:
|
||||
self.curves[index].discard_onset()
|
||||
candidate = self.candidate
|
||||
if candidate is not None:
|
||||
index = 1 if candidate.channel == "side_normal" else 2
|
||||
baseline = candidate.onset.baseline_at(sample_time)
|
||||
delta = (sample[index] - baseline) * candidate.direction
|
||||
return_margin = max(self.config.release_min_change * 0.1,
|
||||
candidate.onset.model.change_uncertainty(candidate.last_sample_time, sample_time))
|
||||
turning = delta < candidate.last_displacement - return_margin
|
||||
if (now - candidate.started_at > self.config.release_confirm_window_seconds
|
||||
or delta + 1e-9 < max(candidate.floor, 0.6 * candidate.peak)
|
||||
or not candidate.onset.significant_at(sample_time, sample[index])
|
||||
or (normal_masked and index == 1) or turning):
|
||||
self.candidate = None
|
||||
self.tails[index] = _Rebound(baseline, candidate.direction, delta)
|
||||
self.recover_until = now + self.config.release_settle_seconds
|
||||
self.seed(sample_time, values)
|
||||
self.curves[index].discard_onset()
|
||||
return "settling-turn" if turning else "rejected"
|
||||
candidate.peak = max(candidate.peak, delta)
|
||||
candidate.last_displacement = delta
|
||||
candidate.last_sample_time = sample_time
|
||||
candidate.count += 1
|
||||
else:
|
||||
shear_ok = getattr(self.config, "release_side_shear_enabled", True)
|
||||
normal_ok = getattr(self.config, "release_side_normal_enabled", True)
|
||||
channels = []
|
||||
if shear_ok:
|
||||
channels.append((2, "side_shear"))
|
||||
if normal_ok:
|
||||
channels.append((1, "side_normal"))
|
||||
for index, channel in channels:
|
||||
if index in returning or (normal_masked and index == 1):
|
||||
continue
|
||||
onset = self.curves[index].onset
|
||||
if onset is None:
|
||||
continue
|
||||
delta = sample[index] - onset.baseline_at(sample_time)
|
||||
if (abs(delta) + 1e-9 >= self.config.release_side_touch_change
|
||||
and (sample[index] - onset.origin) * onset.direction + 1e-9 >= self.config.release_side_touch_change
|
||||
and onset.significant_at(sample_time, sample[index])
|
||||
and delta * onset.direction > 0):
|
||||
candidate = _Candidate("side_touch", channel, onset.origin,
|
||||
1.0 if delta > 0 else -1.0, abs(delta), now,
|
||||
max(2, self.config.release_confirm_samples),
|
||||
self.config.release_side_touch_change,
|
||||
onset=onset, last_displacement=abs(delta), last_sample_time=sample_time)
|
||||
self.candidate = candidate
|
||||
break
|
||||
return candidate
|
||||
|
||||
|
||||
class ReleaseWatch:
|
||||
"""Immediate side touch + bounded clamp protection + persistent displacement.
|
||||
|
||||
A force command masks normal changes through execution and a short tail.
|
||||
Lateral shear remains observable; global quiet never gates release.
|
||||
New force commands require an observation window, so repeated adaptive
|
||||
commands cannot continuously re-arm a timer or erase release evidence.
|
||||
Sensor timestamps are optional for replay; production supplies both sides.
|
||||
"""
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self._side_touch = _SideTouch(config)
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.phase = ReleasePhase.DISARMED
|
||||
self._contact_at = None
|
||||
self._grip_settled = False
|
||||
self._slope_settle_since = None
|
||||
self._plateau_latched = False
|
||||
self._reference = {}
|
||||
self._previous = {}
|
||||
self._recovery_until = 0.0
|
||||
self._settle_seen = set()
|
||||
self._tails = {}
|
||||
self._watch_since = None
|
||||
self._candidate = None
|
||||
self._osc_count = 0
|
||||
self._osc_channel = ""
|
||||
self._last_fresh = {}
|
||||
self._rates = {}
|
||||
self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
|
||||
self._side_touch.reset()
|
||||
|
||||
@property
|
||||
def armed(self):
|
||||
return (self.phase in {ReleasePhase.WATCHING, ReleasePhase.CONFIRMING}
|
||||
or self._side_touch.candidate is not None)
|
||||
|
||||
@property
|
||||
def side_watch_active(self):
|
||||
return (self.phase not in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}
|
||||
and self._side_touch.last_pair is not None and len(self._previous) == len(CHANNELS))
|
||||
|
||||
def contact(self, now, feedback=None, history=()):
|
||||
self.reset()
|
||||
self._contact_at = now
|
||||
self.phase = ReleasePhase.GRIP_SETTLE
|
||||
for frame_time, frame in history:
|
||||
values = {key: float(getattr(frame, key)) for key in CHANNELS}
|
||||
for key, value in values.items():
|
||||
stamp = getattr(frame, f"{key.split('_', 1)[0]}_timestamp", None)
|
||||
self._curves[key].observe(frame_time if stamp is None else stamp, value)
|
||||
self._curves[key].discard_onset()
|
||||
self._side_touch.prime(self._pair_time(frame_time, frame), values)
|
||||
if feedback is not None:
|
||||
self._side_touch.seed(self._pair_time(now, feedback), {key: float(getattr(feedback, key)) for key in CHANNELS})
|
||||
for key in CHANNELS:
|
||||
stamp = getattr(feedback, f"{key.split('_', 1)[0]}_timestamp", None)
|
||||
self._curves[key].observe(now if stamp is None else stamp, float(getattr(feedback, key)))
|
||||
self._curves[key].discard_onset()
|
||||
|
||||
@staticmethod
|
||||
def _pair_time(now, feedback):
|
||||
stamps = (getattr(feedback, "left_timestamp", None), getattr(feedback, "right_timestamp", None))
|
||||
return now if None in stamps else max(stamps)
|
||||
|
||||
def suspend(self):
|
||||
"""Discard evidence during missing feedback, sensor restart or manual mode."""
|
||||
if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
|
||||
return
|
||||
self._protect("sample-gap", {})
|
||||
self._side_touch.reset()
|
||||
self._reference.clear()
|
||||
self._previous.clear()
|
||||
self._rates.clear()
|
||||
self._last_fresh.clear()
|
||||
self._tails.clear()
|
||||
self._curves = {key: ForceCurve(self.config) for key in CHANNELS}
|
||||
|
||||
def _normal_slopes_live_ok(self, now):
|
||||
"""Live check: normal long-window slopes are near zero long enough."""
|
||||
if not getattr(self.config, "release_require_slope_settle", False):
|
||||
return True
|
||||
threshold = max(0.0, float(getattr(self.config, "release_slope_settle_rate", 0.2) or 0.2))
|
||||
min_points = max(3, int(getattr(self.config, "release_slope_min_points", 8) or 8))
|
||||
hold = max(0.0, float(getattr(self.config, "release_slope_hold_seconds", 0.0) or 0.0))
|
||||
normal_only = getattr(self.config, "release_settle_normal_only", True)
|
||||
keys = CHANNELS[:2] if normal_only else CHANNELS
|
||||
for key in keys:
|
||||
curve = self._curves[key]
|
||||
if len(curve.history) < min_points or abs(curve.fit_slope) > threshold:
|
||||
self._slope_settle_since = None
|
||||
return False
|
||||
if self._slope_settle_since is None:
|
||||
self._slope_settle_since = now
|
||||
if hold > 0 and now - self._slope_settle_since < hold:
|
||||
return False
|
||||
return True
|
||||
|
||||
def _force_settled(self, now):
|
||||
"""Release-armed after the first relative normal plateau (latched).
|
||||
|
||||
The live slope near zero is only required to *enter* the armed state.
|
||||
Once latched, further adaptive 加力 or an intentional open push may make
|
||||
the slope non-zero again — that must not revoke release permission, or
|
||||
the open gesture is mistaken for "still settling".
|
||||
"""
|
||||
if not getattr(self.config, "release_require_slope_settle", False):
|
||||
self._plateau_latched = True
|
||||
return True
|
||||
if self._plateau_latched:
|
||||
return True
|
||||
if self._normal_slopes_live_ok(now):
|
||||
self._plateau_latched = True
|
||||
return True
|
||||
return False
|
||||
|
||||
def _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
|
||||
else ReleasePhase.GRIP_SETTLE
|
||||
)
|
||||
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():
|
||||
curve.discard_onset()
|
||||
return ReleaseDecision(self.phase, reason, **metrics)
|
||||
|
||||
def step(self, now, feedback, *, motion_pending=False, motion_completed_at=None):
|
||||
if self.phase in {ReleasePhase.DISARMED, ReleasePhase.RELEASED}:
|
||||
return ReleaseDecision(self.phase, self.phase.value)
|
||||
|
||||
cfg = self.config
|
||||
timeout = max(0.001, cfg.release_sample_timeout_seconds)
|
||||
values = {key: float(getattr(feedback, key)) for key in CHANNELS}
|
||||
if not all(math.isfinite(value) for value in values.values()):
|
||||
self._previous.clear()
|
||||
self._rates.clear()
|
||||
self._side_touch.reset()
|
||||
self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
|
||||
self._recovery_until = now + cfg.release_settle_seconds
|
||||
return self._protect("invalid-sample", {})
|
||||
|
||||
rates, fresh = {}, set()
|
||||
discontinuity = False
|
||||
for key, value in values.items():
|
||||
side = key.split("_", 1)[0]
|
||||
stamp = getattr(feedback, f"{side}_timestamp", None)
|
||||
stamp = now if stamp is None else stamp
|
||||
previous = self._previous.get(key)
|
||||
if previous is not None and stamp == previous[0]:
|
||||
continue
|
||||
fresh.add(key)
|
||||
self._last_fresh[key] = now
|
||||
if previous is not None:
|
||||
dt = stamp - previous[0]
|
||||
if 0 < dt <= timeout:
|
||||
rates[key] = (value - previous[1]) / dt
|
||||
else:
|
||||
discontinuity = True
|
||||
self._previous[key] = (stamp, value)
|
||||
self._rates.update(rates)
|
||||
|
||||
normal_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[:2]), default=0.0)
|
||||
shear_rate = max((abs(rates.get(k, 0.0)) for k in CHANNELS[2:]), default=0.0)
|
||||
normal_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[:2])
|
||||
shear_change = max(abs(values[k] - self._reference.get(k, values[k])) for k in CHANNELS[2:])
|
||||
metrics = dict(normal_change=normal_change, shear_change=shear_change,
|
||||
normal_rate=normal_rate, shear_rate=shear_rate)
|
||||
stale = any(now - self._last_fresh.get(k, now) > timeout for k in CHANNELS)
|
||||
if stale or discontinuity:
|
||||
self._side_touch.reset()
|
||||
self._curves = {key: ForceCurve(cfg) for key in CHANNELS}
|
||||
self._recovery_until = now + cfg.release_settle_seconds
|
||||
return self._protect("sample-gap", values, **metrics)
|
||||
|
||||
for key in fresh:
|
||||
self._curves[key].observe(self._previous[key][0], values[key])
|
||||
motions = {curve.motion for curve in self._curves.values()}
|
||||
metrics["curve_motion"] = ("new_onset" if "new_onset" in motions else
|
||||
"settling_turn" if "settling_turn" in motions else
|
||||
"settling_flatten" if "settling_flatten" in motions else "curve_follow")
|
||||
|
||||
# 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)
|
||||
)
|
||||
contact_guard = now - self._contact_at < max(0.0, cfg.release_min_hold_seconds)
|
||||
protected = motion_pending or self_tail or contact_guard or self.phase in {
|
||||
ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE,
|
||||
}
|
||||
force_settled = self._force_settled(now)
|
||||
if not force_settled:
|
||||
metrics["curve_motion"] = "force_settling"
|
||||
side = self._side_touch.step(now, values, fresh,
|
||||
enabled=protected or self._side_touch.candidate is not None,
|
||||
normal_masked=motion_pending or self_tail,
|
||||
sample_time=self._pair_time(now, feedback),
|
||||
onset_allowed=force_settled)
|
||||
if side in ("rejected", "settling-turn"):
|
||||
reason = "settling-turn" if side == "settling-turn" else "transient-rejected"
|
||||
return self._cancel(now, values, reason, metrics, reset_side=False)
|
||||
if side is not None:
|
||||
self._candidate = None
|
||||
released = side.count >= side.needed
|
||||
if released:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
return ReleaseDecision(ReleasePhase.RELEASED if released else ReleasePhase.CONFIRMING,
|
||||
"side_touch", release=released, confirm_count=side.count,
|
||||
confirm_needed=side.needed, channel=side.channel,
|
||||
innovation=side.onset.innovation,
|
||||
**dict(metrics, curve_motion="onset_confirm"))
|
||||
|
||||
slope_gate = getattr(cfg, "release_require_slope_settle", False)
|
||||
# Contact min-hold always clamps. Self-motion clamps until the first
|
||||
# normal plateau is latched (stable) — or always when slope gate is off.
|
||||
if contact_guard or ((motion_pending or self_tail) and (not slope_gate or not force_settled)):
|
||||
reason = ("force-settling" if slope_gate and not force_settled
|
||||
else "self-settle" if self._grip_settled else "grip-settle")
|
||||
return self._protect(reason, values, **metrics)
|
||||
if slope_gate and force_settled and (motion_pending or self_tail):
|
||||
# Adaptive 加力 after plateau: keep watching for intentional open.
|
||||
if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
|
||||
self._grip_settled = True
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
if self._watch_since is None:
|
||||
self._watch_since = now
|
||||
for curve in self._curves.values():
|
||||
curve.discard_onset()
|
||||
|
||||
if not fresh:
|
||||
return ReleaseDecision(self.phase, "waiting-sample", **metrics)
|
||||
|
||||
# Quiet is diagnostic only. No global "all four rates quiet" arming
|
||||
# gate: even small continuous camera noise can keep that gate shut.
|
||||
quiet = (
|
||||
len(self._rates) == len(CHANNELS)
|
||||
and max(abs(rate) for rate in self._rates.values()) <= cfg.release_slow_rate
|
||||
)
|
||||
if self.phase in {ReleasePhase.GRIP_SETTLE, ReleasePhase.SELF_SETTLE}:
|
||||
self._settle_seen.update(fresh)
|
||||
self._reference.update(values)
|
||||
if now < self._recovery_until or len(self._settle_seen) < len(CHANNELS):
|
||||
reason = "self-settle" if self._grip_settled else "grip-settle"
|
||||
return ReleaseDecision(self.phase, reason, **metrics)
|
||||
# Stable mode waits until the long-history slope has decayed near
|
||||
# zero before ordinary watching / release paths arm.
|
||||
if not force_settled:
|
||||
return ReleaseDecision(self.phase, "force-settling", **metrics)
|
||||
self._grip_settled = True
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
self._watch_since = now
|
||||
for curve in self._curves.values():
|
||||
curve.discard_onset()
|
||||
return ReleaseDecision(self.phase, "ready", **metrics)
|
||||
returning = {key for key in fresh if _returning_tail(
|
||||
self._tails, key, values[key], cfg.release_min_change)}
|
||||
for key in returning:
|
||||
self._reference[key] = values[key]
|
||||
self._curves[key].discard_onset()
|
||||
|
||||
# Drop watching candidates on channel families the profile disabled.
|
||||
if (self._candidate is not None
|
||||
and self._candidate.channel.endswith("normal")
|
||||
and not getattr(cfg, "release_normal_onset_enabled", True)):
|
||||
self._candidate = None
|
||||
if self._grip_settled:
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
if (self._candidate is not None
|
||||
and self._candidate.channel.endswith("shear")
|
||||
and not getattr(cfg, "release_shear_onset_enabled", True)):
|
||||
self._candidate = None
|
||||
if self._grip_settled:
|
||||
self.phase = ReleasePhase.WATCHING
|
||||
|
||||
candidate = self._candidate
|
||||
if candidate is not None:
|
||||
if now - candidate.started_at > cfg.release_confirm_window_seconds:
|
||||
return self._cancel(now, values, "confirm-expired", metrics)
|
||||
if candidate.channel not in fresh:
|
||||
return ReleaseDecision(self.phase, "waiting-sample", **metrics)
|
||||
stamp = self._previous[candidate.channel][0]
|
||||
displacement = (values[candidate.channel] - candidate.onset.baseline_at(stamp)) * candidate.direction
|
||||
return_margin = max(cfg.release_min_change * 0.1,
|
||||
candidate.onset.model.change_uncertainty(candidate.last_sample_time, stamp))
|
||||
if displacement < candidate.last_displacement - return_margin:
|
||||
return self._cancel(now, values, "settling-turn", metrics)
|
||||
# A spike returning to baseline, reversal, or channel swap is not a
|
||||
# second vote. A step that stays displaced is, even with dF/dt=0.
|
||||
if displacement + 1e-9 < max(candidate.floor, 0.6 * candidate.peak):
|
||||
return self._cancel(now, values, "transient-rejected", metrics)
|
||||
if not candidate.onset.significant_at(stamp, values[candidate.channel]):
|
||||
return self._cancel(now, values, "transient-rejected", metrics)
|
||||
candidate.peak = max(candidate.peak, displacement)
|
||||
candidate.last_displacement = displacement
|
||||
candidate.last_sample_time = stamp
|
||||
candidate.count += 1
|
||||
else:
|
||||
choices = []
|
||||
for key in fresh:
|
||||
if key in returning:
|
||||
continue
|
||||
is_normal = key.endswith("normal")
|
||||
if is_normal and not getattr(cfg, "release_normal_onset_enabled", True):
|
||||
continue
|
||||
if (not is_normal
|
||||
and not getattr(cfg, "release_shear_onset_enabled", True)):
|
||||
continue
|
||||
onset = self._curves[key].onset
|
||||
if onset is None or not onset.significant_at(self._previous[key][0], values[key]):
|
||||
continue
|
||||
rate_limit = cfg.release_normal_rate if is_normal else cfg.release_shear_rate
|
||||
weak_limit = cfg.release_normal_change if is_normal else cfg.release_shear_change
|
||||
delta = values[key] - onset.baseline_at(self._previous[key][0])
|
||||
rate = rates.get(key, 0.0)
|
||||
kind = None
|
||||
origin = onset.origin
|
||||
measured = (values[key] - origin) * onset.direction
|
||||
if (abs(rate) >= rate_limit and abs(delta) >= cfg.release_min_change
|
||||
and measured + 1e-9 >= cfg.release_min_change and rate * delta > 0):
|
||||
kind, needed, floor = "sudden", max(2, cfg.release_confirm_samples), cfg.release_min_change
|
||||
elif (abs(delta) + 1e-9 >= cfg.release_min_change
|
||||
and measured + 1e-9 >= cfg.release_min_change and delta * onset.direction > 0):
|
||||
kind, needed, floor = "touch", max(2, cfg.release_confirm_samples), cfg.release_min_change
|
||||
elif abs(delta) >= weak_limit and measured >= weak_limit:
|
||||
kind = "slow_external"
|
||||
needed = max(2, cfg.release_confirm_samples, cfg.release_slow_confirm_samples)
|
||||
floor = weak_limit
|
||||
if kind:
|
||||
choices.append((kind == "sudden", abs(delta), key, kind, needed, floor, delta, origin, onset))
|
||||
if choices:
|
||||
_, _, key, kind, needed, floor, delta, origin, onset = max(choices)
|
||||
candidate = _Candidate(kind, key, origin,
|
||||
1.0 if delta > 0 else -1.0, abs(delta), now, needed, floor,
|
||||
onset=onset, last_displacement=abs(delta),
|
||||
last_sample_time=self._previous[key][0])
|
||||
self._candidate = candidate
|
||||
self.phase = ReleasePhase.CONFIRMING
|
||||
|
||||
if candidate is not None:
|
||||
released = candidate.count >= candidate.needed
|
||||
if released:
|
||||
self.phase = ReleasePhase.RELEASED
|
||||
return ReleaseDecision(self.phase, candidate.kind, release=released,
|
||||
confirm_count=candidate.count,
|
||||
confirm_needed=candidate.needed, channel=candidate.channel,
|
||||
innovation=candidate.onset.innovation,
|
||||
**dict(metrics, curve_motion="onset_confirm"))
|
||||
|
||||
# Follow curve continuation, including large smooth turns. Displacement
|
||||
# without a new onset must never become a weak-path release later.
|
||||
for key in fresh:
|
||||
if self._curves[key].onset is None:
|
||||
self._reference[key] = values[key]
|
||||
observation = max(0.1, max(2, cfg.release_confirm_samples) / max(1.0, cfg.control_hz))
|
||||
# Ordinary noise must not prevent clamp force from reaching its target.
|
||||
# Meaningful coherent movement reserves the next frame for release.
|
||||
touch_pending = any(
|
||||
self._curves[key].onset is not None
|
||||
and abs(values[key] - self._curves[key].onset.baseline_at(self._previous[key][0])) >= cfg.release_min_change * 0.5
|
||||
for key in fresh
|
||||
)
|
||||
allow_force = (not touch_pending and self._watch_since is not None
|
||||
and now - self._watch_since >= observation)
|
||||
reason = "settling-turn" if metrics["curve_motion"] == "settling_turn" else "slow_internal" if quiet else "watch"
|
||||
return ReleaseDecision(self.phase, reason,
|
||||
allow_force_update=allow_force, **metrics)
|
||||
|
||||
def _cancel(self, now, values, reason, metrics, *, reset_side=True):
|
||||
# Wait for the rejected pulse's tail; otherwise its falling edge can
|
||||
# become a fresh negative-direction candidate and release on rebound.
|
||||
self._recovery_until = now + self.config.release_settle_seconds
|
||||
if self._candidate is not None:
|
||||
candidate = self._candidate
|
||||
baseline = candidate.onset.baseline_at(self._previous[candidate.channel][0])
|
||||
remaining = (values[candidate.channel] - baseline) * candidate.direction
|
||||
self._tails[candidate.channel] = _Rebound(baseline, candidate.direction, remaining)
|
||||
if reset_side:
|
||||
self._side_touch.reset()
|
||||
self._side_touch.recover_until = self._recovery_until
|
||||
return self._protect(reason, values, **metrics)
|
||||
@@ -0,0 +1,124 @@
|
||||
"""Session recorder for grip/release tuning.
|
||||
|
||||
Writes one JSON object per control tick (JSONL). Enable from UI or config; keep
|
||||
I/O off the hot path when disabled.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import asdict, is_dataclass
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def _jsonable(value):
|
||||
if value is None or isinstance(value, (bool, int, float, str)):
|
||||
return value
|
||||
if is_dataclass(value):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (list, tuple)):
|
||||
return [_jsonable(item) for item in value]
|
||||
if hasattr(value, "value") and not callable(value):
|
||||
# Enum-like
|
||||
try:
|
||||
return value.value
|
||||
except Exception:
|
||||
pass
|
||||
return str(value)
|
||||
|
||||
|
||||
class SessionRecorder:
|
||||
def __init__(self, root: Path):
|
||||
self.root = Path(root)
|
||||
self._lock = threading.Lock()
|
||||
self._enabled = False
|
||||
self._path = None
|
||||
self._file = None
|
||||
self._frames = 0
|
||||
self._started_at = None
|
||||
self._meta = {}
|
||||
|
||||
@property
|
||||
def enabled(self) -> bool:
|
||||
with self._lock:
|
||||
return self._enabled
|
||||
|
||||
@property
|
||||
def path(self):
|
||||
with self._lock:
|
||||
return self._path
|
||||
|
||||
@property
|
||||
def frames(self) -> int:
|
||||
with self._lock:
|
||||
return self._frames
|
||||
|
||||
def start(self, meta=None) -> Path:
|
||||
with self._lock:
|
||||
if self._enabled:
|
||||
return self._path
|
||||
self.root.mkdir(parents=True, exist_ok=True)
|
||||
stamp = datetime.now().strftime("%Y%m%d-%H%M%S")
|
||||
path = self.root / f"session-{stamp}.jsonl"
|
||||
handle = path.open("w", encoding="utf-8")
|
||||
header = {
|
||||
"type": "session_header",
|
||||
"started_at": time.time(),
|
||||
"started_at_iso": datetime.now().isoformat(timespec="seconds"),
|
||||
"meta": _jsonable(meta or {}),
|
||||
}
|
||||
handle.write(json.dumps(header, ensure_ascii=False) + "\n")
|
||||
handle.flush()
|
||||
self._file = handle
|
||||
self._path = path
|
||||
self._enabled = True
|
||||
self._frames = 0
|
||||
self._started_at = time.perf_counter()
|
||||
self._meta = dict(meta or {})
|
||||
print(f"[session-record] start {path}")
|
||||
return path
|
||||
|
||||
def stop(self) -> Path | None:
|
||||
with self._lock:
|
||||
if not self._enabled:
|
||||
return None
|
||||
path = self._path
|
||||
frames = self._frames
|
||||
started = self._started_at
|
||||
try:
|
||||
footer = {
|
||||
"type": "session_footer",
|
||||
"stopped_at": time.time(),
|
||||
"frames": frames,
|
||||
"duration_s": (
|
||||
None if started is None else time.perf_counter() - started
|
||||
),
|
||||
}
|
||||
if self._file is not None:
|
||||
self._file.write(json.dumps(footer, ensure_ascii=False) + "\n")
|
||||
self._file.flush()
|
||||
self._file.close()
|
||||
finally:
|
||||
self._file = None
|
||||
self._enabled = False
|
||||
self._path = None
|
||||
self._frames = 0
|
||||
self._started_at = None
|
||||
print(f"[session-record] stop {path} frames={frames}")
|
||||
return path
|
||||
|
||||
def record(self, frame: dict) -> None:
|
||||
with self._lock:
|
||||
if not self._enabled or self._file is None:
|
||||
return
|
||||
payload = {"type": "tick", **_jsonable(frame)}
|
||||
self._file.write(json.dumps(payload, ensure_ascii=False) + "\n")
|
||||
self._frames += 1
|
||||
# Flush periodically so a crash still keeps most of the capture.
|
||||
if self._frames % 30 == 0:
|
||||
self._file.flush()
|
||||
@@ -0,0 +1,371 @@
|
||||
"""Discover USB cameras without coupling discovery to the sensor runtime."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import contextlib
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import subprocess
|
||||
import sys
|
||||
from dataclasses import dataclass, replace
|
||||
|
||||
import cv2
|
||||
|
||||
|
||||
# Most USB camera stacks expose devices from 0 upward. Keeping this bounded
|
||||
# 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:
|
||||
index: int
|
||||
backend_name: str
|
||||
backend_api: int
|
||||
width: int
|
||||
height: int
|
||||
friendly_name: str = ""
|
||||
manufacturer: str = ""
|
||||
vid_pid: str = ""
|
||||
location: str = ""
|
||||
instance_id: str = ""
|
||||
|
||||
@property
|
||||
def source(self):
|
||||
if sys.platform.startswith("linux"):
|
||||
return f"/dev/video{self.index}"
|
||||
return str(self.index)
|
||||
|
||||
def label(self):
|
||||
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 ""
|
||||
where = f" {self.location}" if self.location else ""
|
||||
return f"{device} {meta}{resolution}{where} [{self.backend_name}]"
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def _suppress_opencv_noise():
|
||||
previous_level = None
|
||||
set_level = None
|
||||
try:
|
||||
logging = getattr(getattr(cv2, "utils", None), "logging", None)
|
||||
if logging is not None:
|
||||
set_level = getattr(logging, "setLogLevel", None)
|
||||
get_level = getattr(logging, "getLogLevel", None)
|
||||
silent = getattr(logging, "LOG_LEVEL_SILENT", 0)
|
||||
if set_level is not None:
|
||||
if get_level is not None:
|
||||
previous_level = get_level()
|
||||
set_level(silent)
|
||||
except Exception:
|
||||
previous_level = None
|
||||
|
||||
devnull = open(os.devnull, "w")
|
||||
try:
|
||||
with contextlib.redirect_stderr(devnull):
|
||||
try:
|
||||
stderr_fd = sys.stderr.fileno()
|
||||
saved_fd = os.dup(stderr_fd)
|
||||
except (AttributeError, OSError):
|
||||
yield
|
||||
else:
|
||||
try:
|
||||
os.dup2(devnull.fileno(), stderr_fd)
|
||||
yield
|
||||
finally:
|
||||
os.dup2(saved_fd, stderr_fd)
|
||||
os.close(saved_fd)
|
||||
finally:
|
||||
devnull.close()
|
||||
if set_level is not None and previous_level is not None:
|
||||
try:
|
||||
set_level(previous_level)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def _backend_candidates():
|
||||
candidates = []
|
||||
if sys.platform == "win32":
|
||||
# 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)]
|
||||
|
||||
|
||||
def camera_source_index(source):
|
||||
"""Return the numeric index for ``0`` or ``/dev/video0`` sources."""
|
||||
text = str(source or "").strip()
|
||||
if text.isdigit():
|
||||
return int(text)
|
||||
if text.startswith("/dev/video"):
|
||||
suffix = text[len("/dev/video"):]
|
||||
if suffix.isdigit():
|
||||
return int(suffix)
|
||||
return None
|
||||
|
||||
|
||||
def _candidate_indices(max_index):
|
||||
if sys.platform.startswith("linux"):
|
||||
indices = []
|
||||
for path in Path("/dev").glob("video*"):
|
||||
index = camera_source_index(str(path))
|
||||
if index is not None:
|
||||
indices.append(index)
|
||||
if indices:
|
||||
return sorted(set(indices))
|
||||
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, meta=None):
|
||||
capture = cv2.VideoCapture(index, backend_api)
|
||||
try:
|
||||
if not capture.isOpened():
|
||||
return None
|
||||
ok, frame = capture.read()
|
||||
width = int(capture.get(cv2.CAP_PROP_FRAME_WIDTH) or 0)
|
||||
height = int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT) or 0)
|
||||
if ok and frame is not None:
|
||||
frame_height, frame_width = frame.shape[:2]
|
||||
width = width or int(frame_width)
|
||||
height = height or int(frame_height)
|
||||
elif width <= 0 or height <= 0:
|
||||
return None
|
||||
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()
|
||||
|
||||
|
||||
def list_cameras(
|
||||
max_index=DEFAULT_MAX_CAMERA_INDEX,
|
||||
skip_indices=None,
|
||||
fallback_backends=False,
|
||||
):
|
||||
"""Probe camera indices and return one working backend for each index."""
|
||||
found = []
|
||||
seen_indices = set()
|
||||
for source in skip_indices or ():
|
||||
index = camera_source_index(source)
|
||||
if index is not None:
|
||||
seen_indices.add(index)
|
||||
backends = _backend_candidates()
|
||||
if not fallback_backends:
|
||||
backends = backends[:1]
|
||||
with _suppress_opencv_noise():
|
||||
# Probe one backend across all indices before falling back to another
|
||||
# backend. On Windows this avoids opening every empty index twice when
|
||||
# DirectShow is available.
|
||||
for backend_name, backend_api in backends:
|
||||
for index in _candidate_indices(max_index):
|
||||
if index in seen_indices:
|
||||
continue
|
||||
info = _try_open(index, backend_name, backend_api)
|
||||
if info is not None:
|
||||
found.append(info)
|
||||
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 "", ""
|
||||
@@ -272,8 +272,8 @@ class TwoSensorForceReader:
|
||||
self,
|
||||
video1=0,
|
||||
video2=1,
|
||||
config1="./config/ddjx01.json",
|
||||
config2="./config/ddjx01.json",
|
||||
config1="./config/ddjx01_q6a.json",
|
||||
config2="./config/ddjx01_q6a.json",
|
||||
rotation1=None,
|
||||
rotation2=None,
|
||||
target_fps=30,
|
||||
@@ -433,8 +433,8 @@ def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--video1", "-v1", type=str, default="0")
|
||||
parser.add_argument("--video2", "-v2", type=str, default="1")
|
||||
parser.add_argument("--config1", type=str, default="./config/ddjx01.json")
|
||||
parser.add_argument("--config2", type=str, default="./config/ddjx01.json")
|
||||
parser.add_argument("--config1", type=str, default="./config/ddjx01_q6a.json")
|
||||
parser.add_argument("--config2", type=str, default="./config/ddjx01_q6a.json")
|
||||
parser.add_argument("--rotation1", "-r1", type=str, default=None)
|
||||
parser.add_argument("--rotation2", "-r2", type=str, default=None)
|
||||
parser.add_argument("--target-fps", type=float, default=30)
|
||||
|
||||
+1152
-199
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -0,0 +1 @@
|
||||
5675c6f6400b3d9b3d27602c4639c307cb35d0357e658ec5c344aebdc7eba7c0 GripperDemo02-ubuntu22.04-x86_64-lite-20260922.tar.gz
|
||||
Binary file not shown.
@@ -0,0 +1 @@
|
||||
8307c8fe39a850275c09b7a4092584216b8ee1bf7891ec0da12737604e7905d0 GripperDemo02-ubuntu26.04-x86_64-lite-20260922.tar.gz
|
||||
Binary file not shown.
@@ -0,0 +1 @@
|
||||
5f66fb92f4f3f17d129106fd436aeb56d5019f3d36f9dd81ae8a99ce847d6a69 GripperDemo02-windows-x64-lite-20260922.zip
|
||||
Reference in New Issue
Block a user