diff --git a/config/compound_eye.png b/config/compound_eye.png index 65688d1..019065f 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/examples/__pycache__/gripper_demo_02.cpython-313.pyc b/examples/__pycache__/gripper_demo_02.cpython-313.pyc new file mode 100644 index 0000000..b530e02 Binary files /dev/null and b/examples/__pycache__/gripper_demo_02.cpython-313.pyc differ diff --git a/examples/__pycache__/gripper_demo_02_viewer.cpython-313.pyc b/examples/__pycache__/gripper_demo_02_viewer.cpython-313.pyc new file mode 100644 index 0000000..b505853 Binary files /dev/null and b/examples/__pycache__/gripper_demo_02_viewer.cpython-313.pyc differ diff --git a/examples/gripper_demo_02_viewer.py b/examples/gripper_demo_02_viewer.py new file mode 100644 index 0000000..a0d2c9f --- /dev/null +++ b/examples/gripper_demo_02_viewer.py @@ -0,0 +1,17 @@ +"""Compatibility entry point for gripper demo 02 PyQt viewer.""" + +import multiprocessing as mp +import sys +from pathlib import Path + + +REPO_ROOT = Path(__file__).resolve().parents[1] +if str(REPO_ROOT) not in sys.path: + sys.path.insert(0, str(REPO_ROOT)) + +from gripper_control_02.visualizer import main + + +if __name__ == "__main__": + mp.freeze_support() + raise SystemExit(main(sys.argv[1:])) diff --git a/gripper_control/__pycache__/__init__.cpython-313.pyc b/gripper_control/__pycache__/__init__.cpython-313.pyc new file mode 100644 index 0000000..966d1ba Binary files /dev/null and b/gripper_control/__pycache__/__init__.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/calibration.cpython-313.pyc b/gripper_control/__pycache__/calibration.cpython-313.pyc new file mode 100644 index 0000000..f52676e Binary files /dev/null and b/gripper_control/__pycache__/calibration.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/config.cpython-313.pyc b/gripper_control/__pycache__/config.cpython-313.pyc new file mode 100644 index 0000000..498e25a Binary files /dev/null and b/gripper_control/__pycache__/config.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/controller.cpython-313.pyc b/gripper_control/__pycache__/controller.cpython-313.pyc new file mode 100644 index 0000000..714557b Binary files /dev/null and b/gripper_control/__pycache__/controller.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/feedback.cpython-313.pyc b/gripper_control/__pycache__/feedback.cpython-313.pyc new file mode 100644 index 0000000..c70cde5 Binary files /dev/null and b/gripper_control/__pycache__/feedback.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/filters.cpython-313.pyc b/gripper_control/__pycache__/filters.cpython-313.pyc new file mode 100644 index 0000000..dc53bb1 Binary files /dev/null and b/gripper_control/__pycache__/filters.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/force_reader.cpython-311.pyc b/gripper_control/__pycache__/force_reader.cpython-311.pyc index 759d9f3..4a0cd7d 100644 Binary files a/gripper_control/__pycache__/force_reader.cpython-311.pyc and b/gripper_control/__pycache__/force_reader.cpython-311.pyc differ diff --git a/gripper_control/__pycache__/force_reader.cpython-313.pyc b/gripper_control/__pycache__/force_reader.cpython-313.pyc new file mode 100644 index 0000000..3b2d84c Binary files /dev/null and b/gripper_control/__pycache__/force_reader.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/hardware.cpython-311.pyc b/gripper_control/__pycache__/hardware.cpython-311.pyc index c021cad..c8f45b1 100644 Binary files a/gripper_control/__pycache__/hardware.cpython-311.pyc and b/gripper_control/__pycache__/hardware.cpython-311.pyc differ diff --git a/gripper_control/__pycache__/hardware.cpython-313.pyc b/gripper_control/__pycache__/hardware.cpython-313.pyc new file mode 100644 index 0000000..fdfff96 Binary files /dev/null and b/gripper_control/__pycache__/hardware.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/main.cpython-313.pyc b/gripper_control/__pycache__/main.cpython-313.pyc new file mode 100644 index 0000000..947d181 Binary files /dev/null and b/gripper_control/__pycache__/main.cpython-313.pyc differ diff --git a/gripper_control/__pycache__/paths.cpython-313.pyc b/gripper_control/__pycache__/paths.cpython-313.pyc new file mode 100644 index 0000000..51b1b0a Binary files /dev/null and b/gripper_control/__pycache__/paths.cpython-313.pyc differ diff --git a/gripper_control/config/__pycache__/gripper_timed_cycle_control.cpython-313.pyc b/gripper_control/config/__pycache__/gripper_timed_cycle_control.cpython-313.pyc new file mode 100644 index 0000000..5af8c7e Binary files /dev/null and b/gripper_control/config/__pycache__/gripper_timed_cycle_control.cpython-313.pyc differ diff --git a/gripper_control/force_reader.py b/gripper_control/force_reader.py index ddecd17..bcf7d20 100644 --- a/gripper_control/force_reader.py +++ b/gripper_control/force_reader.py @@ -7,6 +7,9 @@ import queue import time from multiprocessing import Process, Queue +import cv2 +import numpy as np + from .paths import ensure_project_paths ensure_project_paths() @@ -38,6 +41,69 @@ def _put_latest(result_queue, data): pass +def _draw_magnitude_map(flow, visual_size, threshold=1.5): + magnitude = np.sqrt(flow[:, :, 0] ** 2 + flow[:, :, 1] ** 2) + h, w = magnitude.shape + if h <= 0 or w <= 0: + return None + + downsample_factor = 10 + magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor] + magnitude_smoothed = cv2.resize( + magnitude_downsampled, + (w, h), + interpolation=cv2.INTER_CUBIC, + ) + + magnitude_thresholded = magnitude_smoothed.copy() + magnitude_thresholded[magnitude_smoothed < threshold] = 0 + + mag_min = float(magnitude_thresholded.min()) + mag_max = float(magnitude_thresholded.max()) + if mag_max > mag_min: + magnitude_norm = ( + (magnitude_thresholded - mag_min) / (mag_max - mag_min) * 255 + ).astype(np.uint8) + else: + magnitude_norm = np.zeros_like(magnitude_thresholded, dtype=np.uint8) + + magnitude_colored = cv2.applyColorMap(magnitude_norm, cv2.COLORMAP_JET) + return cv2.resize( + magnitude_colored, + (visual_size, visual_size), + interpolation=cv2.INTER_CUBIC, + ) + + +def _make_visuals(orisys_module, img, flow, visual_size): + if img is None or flow is None or np.asarray(flow).size == 0: + return None, None, 0.0, 0.0 + + if len(img.shape) == 2: + img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + else: + img_bgr = img + + h, w = img_bgr.shape[:2] + bg = np.empty((h, w, 3), dtype=img_bgr.dtype) + bg[:, :, 0] = 22 + bg[:, :, 1] = 16 + bg[:, :, 2] = 11 + + arrows = orisys_module.util.draw_arrows( + bg, + flow, + threshold=2, + grid_spacing=20, + arrow_scale=0.5, + below_threshold_color=(160, 160, 160), + ) + arrows = cv2.resize(arrows, (visual_size, visual_size), interpolation=cv2.INTER_CUBIC) + magnitude = _draw_magnitude_map(flow, visual_size) + flow_mag = np.sqrt(flow[:, :, 0] ** 2 + flow[:, :, 1] ** 2) + return arrows, magnitude, float(flow_mag.mean()), float(flow_mag.max()) + + def _sensor_force_worker( sensor_id, vid_src, @@ -50,6 +116,8 @@ def _sensor_force_worker( isstitch, backend, motion_threshold, + include_visuals, + visual_size, ): ensure_project_paths() import orisys @@ -79,25 +147,49 @@ def _sensor_force_worker( check_motion=True, threshold=motion_threshold, ) - fps, fnormal, fshearx, fsheary = sensor.read_info( - sensor.info.FPS, - sensor.info.FNORMAL, - sensor.info.FSHEARX, - sensor.info.FSHEARY, - ) + if include_visuals: + fps, fnormal, fshearx, fsheary, flow, img_view = sensor.read_info( + sensor.info.FPS, + sensor.info.FNORMAL, + sensor.info.FSHEARX, + sensor.info.FSHEARY, + sensor.info.VRAW, + sensor.info.IMG, + ) + flow_view, magnitude_view, flow_mean, flow_max = _make_visuals( + orisys, + img_view, + flow, + int(visual_size), + ) + else: + fps, fnormal, fshearx, fsheary = sensor.read_info( + sensor.info.FPS, + sensor.info.FNORMAL, + sensor.info.FSHEARX, + sensor.info.FSHEARY, + ) + flow_view = None + magnitude_view = None + flow_mean = 0.0 + flow_max = 0.0 - _put_latest( - result_queue, - { - "sensor_id": sensor_id, - "fnormal": float(fnormal), - "fshearx": float(fshearx), - "fsheary": float(fsheary), - "fps": float(fps), - "is_contact": bool(is_contact), - "timestamp": time.time(), - }, - ) + data = { + "sensor_id": sensor_id, + "fnormal": float(fnormal), + "fshearx": float(fshearx), + "fsheary": float(fsheary), + "fps": float(fps), + "is_contact": bool(is_contact), + "timestamp": time.time(), + "flow_mean": flow_mean, + "flow_max": flow_max, + } + if include_visuals: + data["flow_view"] = flow_view + data["magnitude_view"] = magnitude_view + + _put_latest(result_queue, data) if frame_interval > 0: sleep_time = frame_interval - (time.perf_counter() - t_start) @@ -136,6 +228,8 @@ class TwoSensorForceReader: backend="auto", motion_threshold=0, queue_size=5, + include_visuals=False, + visual_size=320, ): self.sensor_configs = [ { @@ -157,6 +251,8 @@ class TwoSensorForceReader: self.backend = backend self.motion_threshold = motion_threshold self.queue_size = queue_size + self.include_visuals = include_visuals + self.visual_size = visual_size self.result_queues = [] self.stop_events = [] @@ -186,6 +282,8 @@ class TwoSensorForceReader: self.isstitch, self.backend, self.motion_threshold, + self.include_visuals, + self.visual_size, ), ) process.start() diff --git a/gripper_control/hardware.py b/gripper_control/hardware.py index a74cc1d..d31bd47 100644 --- a/gripper_control/hardware.py +++ b/gripper_control/hardware.py @@ -22,27 +22,38 @@ class GripperClient: ) return self - def move(self, position, force_pct, label): + def move(self, position, force_pct, label, speed_pct=None, accel=None, decel=None): config = self.config + speed_pct = config.speed if speed_pct is None else speed_pct + accel = config.accel if accel is None else accel + decel = config.decel if decel is None else decel + if config.dry_run: print( f"[dry-run] {label}: temp_move " - f"position={position}, speed={config.speed}, force={force_pct}, " - f"accel={config.accel}, decel={config.decel}" + f"position={position}, speed={speed_pct}, force={force_pct}, " + f"accel={accel}, decel={decel}" ) return self.motor.temp_move( position_mm=int(position), - speed_pct=int(config.speed), + speed_pct=int(speed_pct), force_pct=int(force_pct), - accel=int(config.accel), - decel=int(config.decel), + accel=int(accel), + decel=int(decel), trigger=True, ) - def open(self, force_pct, label="open"): - self.move(self.config.open_pos, force_pct, label) + def open(self, force_pct, label="open", speed_pct=None, accel=None, decel=None): + self.move( + self.config.open_pos, + force_pct, + label, + speed_pct=speed_pct, + accel=accel, + decel=decel, + ) def close(self, force_pct, label="close"): self.move(self.config.close_pos, force_pct, label) @@ -99,4 +110,3 @@ class GripperClient: self.motor.trigger_motion() except Exception as exc: print(f"stop by speed=0 failed: {exc}") - diff --git a/gripper_control_02/README.md b/gripper_control_02/README.md index e13c4ea..b7eddb0 100644 --- a/gripper_control_02/README.md +++ b/gripper_control_02/README.md @@ -12,8 +12,9 @@ 6. 像 01 一样进入 `gripping`，慢慢加力到 `HOLD_FORCE`。 7. 进入 `hold_check` 后记录夹持参考力。 8. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。 -9. 张开后进入 `open_recover`，保持张开并等待传感器力清空。 -10. 法向和切向读数都低于恢复阈值并稳定后，才回到 `open_wait` 等待下一次单侧扫过触发。 +9. 张开后进入 `open_recover`，保持张开并等待恢复条件。 +10. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，回到 `open_wait`。 +11. 回到 `open_wait` 后，程序必须先看到触发力清掉并稳定，才会重新武装；下一次扫过才允许闭合。 ## 运行 @@ -30,6 +31,24 @@ python -m gripper_control_02.main python -m gripper_control_02.main --dry-run ``` +PyQt 可视化展示： + +```powershell +python -m gripper_control_02.visualizer +``` + +窗口模式运行： + +```powershell +python -m gripper_control_02.visualizer --windowed +``` + +兼容入口： + +```powershell +python examples\gripper_demo_02_viewer.py +``` + 默认配置文件： ```text @@ -40,6 +59,7 @@ gripper_control_02\config\gripper_demo_02.py - `TRIGGER_NORMAL_FORCE`: 单侧法向力触发阈值。 - `TRIGGER_SHEAR_FORCE`: 单侧切向力触发阈值。 +- `TRIGGER_CLEAR_STABLE_SECONDS`: 张开恢复后，触发力必须清掉并稳定多久，才允许下一次扫过触发。 - `GRIP_NORMAL_FORCE`: 夹住时左右接触阈值。 - `GRIP_REQUIRES_BOTH`: 是否必须左右都接触才算夹住。 - `GRIP_START_FORCE`: 检测到物体后先切到的低力。 @@ -49,10 +69,17 @@ gripper_control_02\config\gripper_demo_02.py - `GRIP_SETTLE_SECONDS`: 夹住后等待稳定多久再记录参考力。 - `RELEASE_NORMAL_CHANGE`: 夹住后法向力变化超过它就松开。 - `RELEASE_SHEAR_CHANGE`: 夹住后切向力变化超过它就松开。 +- `RELEASE_OPEN_SPEED`: 检测到松开后，张开到 `OPEN_POS` 使用的速度。 +- `RELEASE_OPEN_ACCEL`: release 张开的加速度。 +- `RELEASE_OPEN_DECEL`: release 张开的减速度。 - `REARM_SECONDS`: 张开后最短等待时间，防止刚松开马上又误触发。 - `RECOVER_NORMAL_FORCE`: 张开恢复时允许重新触发的最大法向残余力。 - `RECOVER_SHEAR_FORCE`: 张开恢复时允许重新触发的最大切向残余力。 - `RECOVER_STABLE_SECONDS`: 力清空后需要连续稳定多久，才重新等待触发。 +- `OPEN_POSITION_RECOVER_ENABLED`: 是否允许夹爪回到 `OPEN_POS` 后直接恢复等待。 +- `OPEN_POSITION_TOLERANCE`: 当前点位和 `OPEN_POS` 差值小于它，就认为已经回到初始点位。 +- `OPEN_POSITION_STABLE_SECONDS`: 回到初始点位后需要稳定多久，才重新等待触发。 +- `OPEN_POSITION_CHECK_INTERVAL`: `open_recover` 中读取夹爪当前位置的间隔。 ## 日志字段 @@ -60,6 +87,7 @@ gripper_control_02\config\gripper_demo_02.py - `maxN`: 左右法向力最大值，单位 N。 - `shear`: 左右切向力最大值，单位 N。 - `trigger`: 是否满足单侧触发条件。 +- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 才会闭合。 - `grip_contact`: 是否满足夹住接触条件。 - `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。 - `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。 @@ -68,17 +96,31 @@ gripper_control_02\config\gripper_demo_02.py - `force_pct`: 当前夹爪目标力百分比。 - `action`: 当前动作。 +## 可视化展示 + +- 左右两边的光流箭头图。 +- 左右两边的光流幅值图。 +- 左右法向力、最大切向力、左右差值。 +- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。 +- 当前 `force_pct`、速度 `speed_pct`、`hold_pos`。 +- 左右传感器 FPS、接触标志、光流均值/最大值。 + ## 状态说明 - `open_wait`: 夹爪张开，等待任意单侧触发。 - `closing`: 单侧触发后正在闭合，等待检测到物体。 - `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。 - `hold_check`: 已夹住并记录参考力，监测法向/切向变化；变化超过阈值会直接张开。 -- `open_recover`: 已经张开到初始位置，正在等待法向/切向残余力清空；这个状态不会闭合夹爪。 +- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。 ## 常见动作说明 - `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。 +- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。 +- `open-ready-position`: 夹爪已经回到初始点位，重新进入等待下一次扫过触发。 +- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。 +- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。 +- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。 - `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。 - `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定时间。 - `open-ready`: 力已经清空并稳定，重新进入等待下一次扫过触发。 diff --git a/gripper_control_02/__pycache__/__init__.cpython-313.pyc b/gripper_control_02/__pycache__/__init__.cpython-313.pyc new file mode 100644 index 0000000..32fb669 Binary files /dev/null and b/gripper_control_02/__pycache__/__init__.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/controller.cpython-311.pyc b/gripper_control_02/__pycache__/controller.cpython-311.pyc index 59b1d3e..e60fa35 100644 Binary files a/gripper_control_02/__pycache__/controller.cpython-311.pyc and b/gripper_control_02/__pycache__/controller.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/controller.cpython-313.pyc b/gripper_control_02/__pycache__/controller.cpython-313.pyc new file mode 100644 index 0000000..c055ba8 Binary files /dev/null and b/gripper_control_02/__pycache__/controller.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/main.cpython-311.pyc b/gripper_control_02/__pycache__/main.cpython-311.pyc index def1165..7c168d9 100644 Binary files a/gripper_control_02/__pycache__/main.cpython-311.pyc and b/gripper_control_02/__pycache__/main.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/main.cpython-313.pyc b/gripper_control_02/__pycache__/main.cpython-313.pyc new file mode 100644 index 0000000..a59f98c Binary files /dev/null and b/gripper_control_02/__pycache__/main.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/settings.cpython-311.pyc b/gripper_control_02/__pycache__/settings.cpython-311.pyc index c3efdf4..721453e 100644 Binary files a/gripper_control_02/__pycache__/settings.cpython-311.pyc and b/gripper_control_02/__pycache__/settings.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/settings.cpython-313.pyc b/gripper_control_02/__pycache__/settings.cpython-313.pyc new file mode 100644 index 0000000..db32e39 Binary files /dev/null and b/gripper_control_02/__pycache__/settings.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc new file mode 100644 index 0000000..c2ec4f7 Binary files /dev/null and b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-313.pyc b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc new file mode 100644 index 0000000..1e9b90c Binary files /dev/null and b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc differ diff --git a/gripper_control_02/config/__pycache__/__init__.cpython-313.pyc b/gripper_control_02/config/__pycache__/__init__.cpython-313.pyc new file mode 100644 index 0000000..3e2593d Binary files /dev/null and b/gripper_control_02/config/__pycache__/__init__.cpython-313.pyc differ diff --git a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc index 903d344..0127587 100644 Binary files a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc and b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc differ diff --git a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc new file mode 100644 index 0000000..750d858 Binary files /dev/null and b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc differ diff --git a/gripper_control_02/config/gripper_demo_02.py b/gripper_control_02/config/gripper_demo_02.py index e0756b8..d8dc062 100644 --- a/gripper_control_02/config/gripper_demo_02.py +++ b/gripper_control_02/config/gripper_demo_02.py @@ -43,6 +43,9 @@ CLOSE_POS = 9000 SPEED = 25 ACCEL = 80 DECEL = 80 +RELEASE_OPEN_SPEED = 50 # 检测到人取物/力变化后，张开使用的速度。 +RELEASE_OPEN_ACCEL = 120 # release 张开的加速度。 +RELEASE_OPEN_DECEL = 120 # release 张开的减速度。 # 夹爪力度百分比。 @@ -54,21 +57,22 @@ FORCE_MIN = 10 FORCE_MAX = 30 -# 控制循环频率。 -CONTROL_HZ = 20.0 +# 控制循环频率。越高检测释放越快。 +CONTROL_HZ = 30.0 # 启动时采集空载基线的时间。 BASELINE_SECONDS = 1.0 # 滤波系数，越大响应越快，越小越平滑。 -FILTER_ALPHA = 0.35 -SHEAR_FILTER_ALPHA = 0.45 +FILTER_ALPHA = 0.55 +SHEAR_FILTER_ALPHA = 0.65 # 等待触发：任意一侧超过这些阈值，就开始闭合。 -TRIGGER_NORMAL_FORCE = 0.04 # 单侧法向力触发阈值，单位 N。 -TRIGGER_SHEAR_FORCE = 0.04 # 单侧切向力触发阈值，单位 N。 +TRIGGER_NORMAL_FORCE = 0.1 # 单侧法向力触发阈值，单位 N。 +TRIGGER_SHEAR_FORCE = 0.1 # 单侧切向力触发阈值，单位 N。 +TRIGGER_CLEAR_STABLE_SECONDS = 0.2 # 张开恢复后，必须先看到触发力清掉并稳定这么久，才允许下一次扫过触发。 # 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。 @@ -79,12 +83,12 @@ CLOSE_TIMEOUT_SECONDS = 5.0 # 触发闭合后这么久还没夹住，就重新 # 检测到物体后慢慢加力，类似 01 的 gripping 状态。 FORCE_RAMP_STEP = 1 -FORCE_RAMP_INTERVAL = 0.2 -GRIP_SETTLE_SECONDS = 0.3 # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。 +FORCE_RAMP_INTERVAL = 0.1 +GRIP_SETTLE_SECONDS = 0.1 # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。 # 松开触发阈值。夹住后，法向或切向变化超过其中任意一个，就张开。 -RELEASE_NORMAL_CHANGE = 0.05 # 左/右法向力相对夹住参考值的变化阈值，单位 N。 -RELEASE_SHEAR_CHANGE = 0.05 # 切向力相对夹住参考值的变化阈值，单位 N。 +RELEASE_NORMAL_CHANGE = 0.2 # 左/右法向力相对夹住参考值的变化阈值，单位 N。 +RELEASE_SHEAR_CHANGE = 0.2 # 左/右切向力相对夹住参考值的变化阈值，单位 N。 # 松开后至少等待多久重新进入等待触发，避免刚张开时马上再次触发。 REARM_SECONDS = 0.5 @@ -95,6 +99,13 @@ RECOVER_NORMAL_FORCE = 0.02 # 张开恢复时允许重新触发的最大法向 RECOVER_SHEAR_FORCE = 0.02 # 张开恢复时允许重新触发的最大切向残余力，单位 N。 RECOVER_STABLE_SECONDS = 0.4 # 力清空后需要连续稳定这么久，才回到 open_wait。 +# 松开/超时张开后，如果夹爪已经回到 OPEN_POS 初始点位，也允许重新触发。 +# 这样传感器上还有一点残余力时，不会一直卡在 open_recover。 +OPEN_POSITION_RECOVER_ENABLED = True +OPEN_POSITION_TOLERANCE = 100 # 当前位置和 OPEN_POS 的差值小于它，就认为回到初始点位。 +OPEN_POSITION_STABLE_SECONDS = 0.1 # 回到初始点位后稳定这么久，才回到 open_wait。 +OPEN_POSITION_CHECK_INTERVAL = 0.1 # open_recover 状态下读取夹爪位置的间隔，单位秒。 + # 如果读取当前位置失败，保持当前位置时使用这个备用位置；None 表示不使用。 HOLD_FALLBACK_POS = None @@ -158,6 +169,9 @@ CONFIG = { "speed": SPEED, "accel": ACCEL, "decel": DECEL, + "release_open_speed": RELEASE_OPEN_SPEED, + "release_open_accel": RELEASE_OPEN_ACCEL, + "release_open_decel": RELEASE_OPEN_DECEL, "open_force": OPEN_FORCE, "close_force": CLOSE_FORCE, @@ -174,6 +188,7 @@ CONFIG = { "trigger_normal_force": TRIGGER_NORMAL_FORCE, "trigger_shear_force": TRIGGER_SHEAR_FORCE, + "trigger_clear_stable_seconds": TRIGGER_CLEAR_STABLE_SECONDS, "grip_normal_force": GRIP_NORMAL_FORCE, "grip_requires_both": GRIP_REQUIRES_BOTH, "close_timeout_seconds": CLOSE_TIMEOUT_SECONDS, @@ -186,6 +201,10 @@ CONFIG = { "recover_normal_force": RECOVER_NORMAL_FORCE, "recover_shear_force": RECOVER_SHEAR_FORCE, "recover_stable_seconds": RECOVER_STABLE_SECONDS, + "open_position_recover_enabled": OPEN_POSITION_RECOVER_ENABLED, + "open_position_tolerance": OPEN_POSITION_TOLERANCE, + "open_position_stable_seconds": OPEN_POSITION_STABLE_SECONDS, + "open_position_check_interval": OPEN_POSITION_CHECK_INTERVAL, "hold_fallback_pos": HOLD_FALLBACK_POS, "trigger_force_update": TRIGGER_FORCE_UPDATE, diff --git a/gripper_control_02/controller.py b/gripper_control_02/controller.py index 94bd156..ef5161e 100644 --- a/gripper_control_02/controller.py +++ b/gripper_control_02/controller.py @@ -1,18 +1,32 @@ import time +import threading from gripper_control.filters import clamp class SideTriggerGripController: - def __init__(self, config, reader, gripper, feedback_processor): + def __init__( + self, + config, + reader, + gripper, + feedback_processor, + status_callback=None, + ): self.config = config self.reader = reader self.gripper = gripper self.feedback_processor = feedback_processor self.normal_unit = feedback_processor.normal_converter.unit self.shear_unit = feedback_processor.shear_converter.unit + self.status_callback = status_callback + self._stop_event = threading.Event() + + def stop(self): + self._stop_event.set() def run(self): + self._stop_event.clear() self.reader.start() try: @@ -59,6 +73,20 @@ class SideTriggerGripController: f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, " f"release_shear_change={self.config.release_shear_change:.3f}{self.shear_unit}" ) + print( + "Release open: " + f"speed={self.config.release_open_speed}, " + f"accel={self.config.release_open_accel}, " + f"decel={self.config.release_open_decel}" + ) + print( + "Recover: " + f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, " + f"force_shear={self.config.recover_shear_force:.3f}{self.shear_unit}, " + f"open_pos={self.config.open_pos}, " + f"pos_enabled={int(self.config.open_position_recover_enabled)}, " + f"pos_tol={self.config.open_position_tolerance}" + ) def _run_event_monitor(self): config = self.config @@ -67,18 +95,35 @@ class SideTriggerGripController: state = "open_wait" state_start = time.perf_counter() last_force_ramp_time = state_start + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = False force_pct = int(clamp(config.open_force, config.force_min, config.force_max)) hold_pos = None reference = None - while True: + while not self._stop_event.is_set(): tick_start = time.perf_counter() now = time.perf_counter() feedback = self.feedback_processor.read(self.reader, timeout=0.3) if feedback is None: print(f"state={state} action=waiting-for-sensor-samples") + self._emit_status( + { + "timestamp": time.time(), + "state": state, + "action": "waiting-for-sensor-samples", + "force_pct": force_pct, + "speed_pct": config.speed, + "normal_unit": self.normal_unit, + "shear_unit": self.shear_unit, + } + ) time.sleep(0.05) continue @@ -89,7 +134,19 @@ class SideTriggerGripController: action = "wait" if state == "open_wait": - if trigger: + if not trigger: + if trigger_clear_since is None: + trigger_clear_since = now + action = "open-wait-trigger-clearing" + elif now - trigger_clear_since >= config.trigger_clear_stable_seconds: + trigger_armed = True + action = "open-wait-armed" + else: + action = "open-wait-trigger-clearing" + elif not trigger_armed: + trigger_clear_since = None + action = "open-wait-clear-trigger" + else: force_pct = int(clamp( config.close_force, config.force_min, @@ -99,12 +156,16 @@ class SideTriggerGripController: state = "closing" state_start = now last_force_ramp_time = now + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = False hold_pos = None reference = None action = "side-trigger-close" - else: - action = "open-wait-touch" elif state == "closing": if grip_contact: @@ -130,7 +191,11 @@ class SideTriggerGripController: self.feedback_processor.reset_filters() state = "open_recover" state_start = now + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False recover_quiet_since = None + recover_position_since = None hold_pos = None reference = None action = "close-timeout-open" @@ -173,11 +238,21 @@ class SideTriggerGripController: config.force_min, config.force_max, )) - self.gripper.open(force_pct, "force-change-release-open") + self.gripper.open( + force_pct, + "force-change-release-open", + speed_pct=config.release_open_speed, + accel=config.release_open_accel, + decel=config.release_open_decel, + ) self.feedback_processor.reset_filters() state = "open_recover" state_start = now + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False recover_quiet_since = None + recover_position_since = None hold_pos = None reference = None action = "force-change-release-open" @@ -185,7 +260,41 @@ class SideTriggerGripController: action = "hold-check" elif state == "open_recover": - if self._recover_quiet(feedback): + current_pos = last_open_position + position_ready = last_open_position_ready + if ( + config.open_position_recover_enabled + and now - last_open_position_check_time + >= config.open_position_check_interval + ): + last_open_position_check_time = now + current_pos = self.gripper.read_position() + position_ready = self._open_position_ready(current_pos) + last_open_position = current_pos + last_open_position_ready = position_ready + + if position_ready: + if recover_position_since is None: + recover_position_since = now + action = f"open-recover-position pos={current_pos}" + elif ( + now - recover_position_since + >= config.open_position_stable_seconds + and now - state_start >= config.rearm_seconds + ): + state = "open_wait" + state_start = now + recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = False + hold_pos = None + reference = None + action = f"open-ready-position pos={current_pos}" + else: + action = f"open-recover-position-stabilizing pos={current_pos}" + elif self._recover_quiet(feedback): + recover_position_since = None if recover_quiet_since is None: recover_quiet_since = now action = "open-recover-quiet" @@ -196,6 +305,9 @@ class SideTriggerGripController: state = "open_wait" state_start = now recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = False hold_pos = None reference = None action = "open-ready" @@ -203,6 +315,7 @@ class SideTriggerGripController: action = "open-recover-stabilizing" else: recover_quiet_since = None + recover_position_since = None action = "open-recover-wait-force-clear" self._log_tick( @@ -215,12 +328,29 @@ class SideTriggerGripController: hold_pos=hold_pos, force_pct=force_pct, action=action, + trigger_armed=trigger_armed, + ) + self._emit_status( + self._make_status( + feedback=feedback, + state=state, + trigger=trigger, + grip_contact=grip_contact, + normal_change=normal_change, + shear_change=shear_change, + hold_pos=hold_pos, + force_pct=force_pct, + action=action, + trigger_armed=trigger_armed, + ) ) sleep_time = interval - (time.perf_counter() - tick_start) if sleep_time > 0: time.sleep(sleep_time) + print("demo02 control loop stopped.") + def _side_triggered(self, feedback): return ( feedback.max_normal >= self.config.trigger_normal_force @@ -240,10 +370,20 @@ class SideTriggerGripController: and feedback.max_shear <= self.config.recover_shear_force ) + def _open_position_ready(self, current_pos): + if current_pos is None: + return False + return ( + abs(int(current_pos) - int(self.config.open_pos)) + <= int(self.config.open_position_tolerance) + ) + def _make_reference(self, feedback): return { "left_normal": feedback.left_normal, "right_normal": feedback.right_normal, + "left_shear": feedback.left_shear, + "right_shear": feedback.right_shear, "max_shear": feedback.max_shear, } @@ -255,6 +395,11 @@ class SideTriggerGripController: abs(feedback.right_normal - reference["right_normal"]), ) shear_change = abs(feedback.max_shear - reference["max_shear"]) + shear_change = max( + shear_change, + abs(feedback.left_shear - reference["left_shear"]), + abs(feedback.right_shear - reference["right_shear"]), + ) return normal_change, shear_change def _log_tick( @@ -268,15 +413,78 @@ class SideTriggerGripController: hold_pos, force_pct, action, + trigger_armed, ): print( f"L={feedback.left_normal:7.3f}{self.normal_unit} " f"R={feedback.right_normal:7.3f}{self.normal_unit} " f"maxN={feedback.max_normal:7.3f}{self.normal_unit} " f"shear={feedback.max_shear:7.3f}{self.shear_unit} " - f"trigger={int(trigger)} grip_contact={int(grip_contact)} " + f"trigger={int(trigger)} armed={int(trigger_armed)} " + f"grip_contact={int(grip_contact)} " f"dN={normal_change:7.3f}{self.normal_unit} " f"dShear={shear_change:7.3f}{self.shear_unit} " f"state={state} hold_pos={hold_pos} " f"force_pct={force_pct:3d} action={action}" ) + + def _make_status( + self, + feedback, + state, + trigger, + grip_contact, + normal_change, + shear_change, + hold_pos, + force_pct, + action, + trigger_armed, + ): + speed_pct = self.config.speed + if action == "force-change-release-open": + speed_pct = self.config.release_open_speed + + left_sample = None + right_sample = None + try: + if len(self.reader.latest) >= 2: + left_sample = self.reader.latest[0] + right_sample = self.reader.latest[1] + except Exception: + pass + + return { + "timestamp": time.time(), + "state": state, + "action": action, + "trigger": bool(trigger), + "trigger_armed": bool(trigger_armed), + "grip_contact": bool(grip_contact), + "left_normal": feedback.left_normal, + "right_normal": feedback.right_normal, + "min_normal": feedback.min_normal, + "max_normal": feedback.max_normal, + "normal_diff": feedback.normal_diff, + "left_shear": feedback.left_shear, + "right_shear": feedback.right_shear, + "max_shear": feedback.max_shear, + "shear_diff": feedback.shear_diff, + "normal_change": normal_change, + "shear_change": shear_change, + "hold_pos": hold_pos, + "force_pct": force_pct, + "speed_pct": speed_pct, + "normal_unit": self.normal_unit, + "shear_unit": self.shear_unit, + "left_sample": left_sample, + "right_sample": right_sample, + } + + def _emit_status(self, status): + if self.status_callback is None: + return + try: + self.status_callback(status) + except Exception as exc: + print(f"status callback failed: {exc}") diff --git a/gripper_control_02/main.py b/gripper_control_02/main.py index 8c4a5c3..030ee91 100644 --- a/gripper_control_02/main.py +++ b/gripper_control_02/main.py @@ -10,7 +10,12 @@ from .controller import SideTriggerGripController from .settings import Demo02Config -def build_controller(argv=None): +def build_controller( + argv=None, + status_callback=None, + include_visuals=False, + visual_size=320, +): ensure_project_paths() config = Demo02Config.from_args(argv) @@ -33,10 +38,18 @@ def build_controller(argv=None): target_fps=config.sensor_fps, cuda=not config.sensor_cpu, motion_threshold=config.motion_threshold, + include_visuals=include_visuals, + visual_size=visual_size, ) gripper = GripperClient(config) feedback = TactileFeedbackProcessor(config, normal_converter, shear_converter) - return SideTriggerGripController(config, reader, gripper, feedback) + return SideTriggerGripController( + config, + reader, + gripper, + feedback, + status_callback=status_callback, + ) def main(argv=None): diff --git a/gripper_control_02/settings.py b/gripper_control_02/settings.py index 649c6b0..515ae85 100644 --- a/gripper_control_02/settings.py +++ b/gripper_control_02/settings.py @@ -43,6 +43,9 @@ class Demo02Config: speed: int = 25 accel: int = 80 decel: int = 80 + release_open_speed: int = 50 + release_open_accel: int = 120 + release_open_decel: int = 120 open_force: int = 10 close_force: int = 10 @@ -52,25 +55,30 @@ class Demo02Config: force_min: int = 10 force_max: int = 30 - control_hz: float = 20.0 + control_hz: float = 30.0 baseline_seconds: float = 1.0 - filter_alpha: float = 0.35 - shear_filter_alpha: float = 0.45 + filter_alpha: float = 0.55 + shear_filter_alpha: float = 0.65 trigger_normal_force: float = 0.04 trigger_shear_force: float = 0.04 + trigger_clear_stable_seconds: float = 0.2 grip_normal_force: float = 0.05 grip_requires_both: bool = True close_timeout_seconds: float = 5.0 force_ramp_step: int = 1 - force_ramp_interval: float = 0.2 - grip_settle_seconds: float = 0.3 - release_normal_change: float = 0.05 - release_shear_change: float = 0.05 + force_ramp_interval: float = 0.1 + grip_settle_seconds: float = 0.1 + release_normal_change: float = 0.03 + release_shear_change: float = 0.03 rearm_seconds: float = 0.5 recover_normal_force: float = 0.02 recover_shear_force: float = 0.02 recover_stable_seconds: float = 0.4 + open_position_recover_enabled: bool = True + open_position_tolerance: int = 100 + open_position_stable_seconds: float = 0.1 + open_position_check_interval: float = 0.1 hold_fallback_pos: int | None = None trigger_force_update: bool = False @@ -116,6 +124,9 @@ class Demo02Config: add("--speed", type=int, default=get("speed", cls.speed)) add("--accel", type=int, default=get("accel", cls.accel)) add("--decel", type=int, default=get("decel", cls.decel)) + add("--release-open-speed", type=int, default=get("release_open_speed", cls.release_open_speed)) + add("--release-open-accel", type=int, default=get("release_open_accel", cls.release_open_accel)) + add("--release-open-decel", type=int, default=get("release_open_decel", cls.release_open_decel)) add("--open-force", type=int, default=get("open_force", cls.open_force)) add("--close-force", type=int, default=get("close_force", cls.close_force)) @@ -131,6 +142,7 @@ class Demo02Config: 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)) + add("--trigger-clear-stable-seconds", type=float, default=get("trigger_clear_stable_seconds", cls.trigger_clear_stable_seconds)) add("--grip-normal-force", type=float, default=get("grip_normal_force", cls.grip_normal_force)) add("--grip-requires-both", dest="grip_requires_both", action="store_true", default=get("grip_requires_both", cls.grip_requires_both)) add("--allow-single-side-grip", dest="grip_requires_both", action="store_false") @@ -144,6 +156,11 @@ class Demo02Config: 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)) add("--recover-stable-seconds", type=float, default=get("recover_stable_seconds", cls.recover_stable_seconds)) + add("--open-position-recover-enabled", action="store_true", default=get("open_position_recover_enabled", cls.open_position_recover_enabled)) + add("--disable-open-position-recover", dest="open_position_recover_enabled", action="store_false") + add("--open-position-tolerance", type=int, default=get("open_position_tolerance", cls.open_position_tolerance)) + add("--open-position-stable-seconds", type=float, default=get("open_position_stable_seconds", cls.open_position_stable_seconds)) + add("--open-position-check-interval", type=float, default=get("open_position_check_interval", cls.open_position_check_interval)) add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos)) add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update)) diff --git a/gripper_control_02/visualizer.py b/gripper_control_02/visualizer.py new file mode 100644 index 0000000..2b10530 --- /dev/null +++ b/gripper_control_02/visualizer.py @@ -0,0 +1,562 @@ +import argparse +import multiprocessing as mp +import sys +import threading +import time + +import cv2 +import numpy as np + + +def _fix_qt_plugin_path(): + try: + from PyQt5.QtCore import QLibraryInfo + + import os + + os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = QLibraryInfo.location( + QLibraryInfo.PluginsPath + ) + except Exception: + pass + + +_fix_qt_plugin_path() + +_QT_IMPORT_ERROR = None +try: + from PyQt5.QtCore import Qt, QTimer + from PyQt5.QtGui import QFont, QImage, QPixmap + from PyQt5.QtWidgets import ( + QApplication, + QFrame, + QGridLayout, + QHBoxLayout, + QLabel, + QMainWindow, + QPushButton, + QSizePolicy, + QVBoxLayout, + QWidget, + ) +except Exception as exc: + _QT_IMPORT_ERROR = exc + Qt = QTimer = QFont = QImage = QPixmap = QApplication = None + QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = None + QVBoxLayout = QWidget = None + QFrame = QMainWindow = object + +try: + import pyqtgraph as pg +except Exception as exc: + pg = None + _PYQTGRAPH_IMPORT_ERROR = exc +else: + _PYQTGRAPH_IMPORT_ERROR = None + +from .main import build_controller + + +PLOT_POINTS = 180 +CURVE_COLOR = "#00E5FF" + + +def cv2_to_pixmap(img_bgr): + if img_bgr is None: + return QPixmap() + if len(img_bgr.shape) == 2: + img_bgr = cv2.cvtColor(img_bgr, cv2.COLOR_GRAY2BGR) + rgb = cv2.cvtColor(np.ascontiguousarray(img_bgr), cv2.COLOR_BGR2RGB) + h, w, ch = rgb.shape + qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888).copy() + return QPixmap.fromImage(qimg) + + +def fmt(value, unit="", width=0, precision=3): + if value is None: + text = "--" + else: + text = f"{float(value):.{precision}f}" + if width: + text = text.rjust(width) + return f"{text}{unit}" + + +STYLE = """ +QMainWindow { + background-color: #000000; +} +QWidget { + background-color: #000000; + color: #9a9a9a; + font-family: "Microsoft YaHei", "SimHei"; +} +QLabel#Title { + color: #d7d7d7; + font-size: 18px; + font-weight: 600; +} +QLabel#Subtle { + color: #777777; + font-size: 12px; +} +QLabel#MetricLabel { + color: #888888; + font-size: 12px; +} +QLabel#MetricValue { + color: #f2f2f2; + font-family: "Cascadia Code", "Consolas"; + font-size: 22px; + font-weight: 600; +} +QLabel#SmallValue { + color: #d8d8d8; + font-family: "Cascadia Code", "Consolas"; + font-size: 15px; +} +QLabel#StatusChip { + background-color: #101010; + border: 1px solid #303030; + border-radius: 4px; + color: #00e5ff; + font-family: "Cascadia Code", "Consolas"; + font-size: 18px; + font-weight: 600; + padding: 8px 12px; +} +QLabel#ActionChip { + background-color: #0b0b0b; + border: 1px solid #262626; + border-radius: 4px; + color: #c8c8c8; + font-family: "Cascadia Code", "Consolas"; + font-size: 13px; + padding: 7px 10px; +} +QLabel#ImagePanel { + background-color: #0b0b0b; + border: 1px solid #262626; + border-radius: 4px; +} +QFrame#Panel { + background-color: #050505; + border: 1px solid #222222; + border-radius: 4px; +} +QPushButton { + background-color: #1a1a1a; + color: #a8a8a8; + border: 1px solid #333333; + border-radius: 4px; + padding: 7px 16px; + font-size: 13px; +} +QPushButton:hover { + background-color: #262626; + color: #d4d4d4; + border: 1px solid #555555; +} +QPushButton#StartButton { + background-color: #0d2818; + color: #67d79a; + border: 1px solid #1a4d30; +} +QPushButton#StopButton { + background-color: #2b1010; + color: #f07070; + border: 1px solid #4d1a1a; +} +""" + + +class MetricBox(QFrame): + def __init__(self, title, value="--", sub=""): + super().__init__() + self.setObjectName("Panel") + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 8, 10, 8) + layout.setSpacing(3) + self.title_label = QLabel(title) + self.title_label.setObjectName("MetricLabel") + self.value_label = QLabel(value) + self.value_label.setObjectName("MetricValue") + self.sub_label = QLabel(sub) + self.sub_label.setObjectName("Subtle") + layout.addWidget(self.title_label) + layout.addWidget(self.value_label) + layout.addWidget(self.sub_label) + + def set_value(self, value, sub=None): + self.value_label.setText(value) + if sub is not None: + self.sub_label.setText(sub) + + +class ImageBox(QFrame): + def __init__(self, title): + super().__init__() + self.setObjectName("Panel") + layout = QVBoxLayout(self) + layout.setContentsMargins(8, 8, 8, 8) + layout.setSpacing(6) + self.title_label = QLabel(title) + self.title_label.setObjectName("MetricLabel") + self.image_label = QLabel() + self.image_label.setObjectName("ImagePanel") + self.image_label.setAlignment(Qt.AlignCenter) + self.image_label.setMinimumSize(180, 180) + self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.image_label.setScaledContents(True) + layout.addWidget(self.title_label) + layout.addWidget(self.image_label, 1) + + def set_image(self, img): + self.image_label.setPixmap(cv2_to_pixmap(img)) + + +class GripperDemo02Viewer(QMainWindow): + def __init__(self, argv=None): + super().__init__() + if _QT_IMPORT_ERROR is not None: + raise RuntimeError(f"PyQt5 import failed: {_QT_IMPORT_ERROR}") + if pg is None: + raise RuntimeError(f"pyqtgraph import failed: {_PYQTGRAPH_IMPORT_ERROR}") + + self.setWindowTitle("Gripper Demo 02") + self.setStyleSheet(STYLE) + self.controller = None + self.control_thread = None + self.latest_status = None + self.status_lock = threading.Lock() + self.controller_finished = False + self.start_time = time.time() + self.force_history = [] + self.argv = argv + + self._build_ui() + self._configure_plot() + + self.timer = QTimer(self) + self.timer.timeout.connect(self._refresh) + self.timer.start(33) + + def _build_ui(self): + root = QWidget() + self.setCentralWidget(root) + outer = QVBoxLayout(root) + outer.setContentsMargins(8, 8, 8, 8) + outer.setSpacing(8) + + top = QHBoxLayout() + top.setSpacing(8) + title = QLabel("Gripper Demo 02") + title.setObjectName("Title") + self.status_chip = QLabel("state=--") + self.status_chip.setObjectName("StatusChip") + self.action_chip = QLabel("action=--") + self.action_chip.setObjectName("ActionChip") + self.start_button = QPushButton("启动") + self.start_button.setObjectName("StartButton") + self.start_button.clicked.connect(self.start_control) + self.stop_button = QPushButton("停止") + self.stop_button.setObjectName("StopButton") + self.stop_button.clicked.connect(self.stop_control) + self.stop_button.setEnabled(False) + top.addWidget(title) + top.addWidget(self.status_chip) + top.addWidget(self.action_chip, 1) + top.addWidget(self.start_button) + top.addWidget(self.stop_button) + outer.addLayout(top) + + main = QGridLayout() + main.setSpacing(8) + outer.addLayout(main, 1) + + self.left_flow = ImageBox("左侧光流") + self.left_mag = ImageBox("左侧幅值图") + self.right_flow = ImageBox("右侧光流") + self.right_mag = ImageBox("右侧幅值图") + main.addWidget(self.left_flow, 0, 0) + main.addWidget(self.left_mag, 0, 1) + main.addWidget(self.right_flow, 1, 0) + main.addWidget(self.right_mag, 1, 1) + + side = QVBoxLayout() + side.setSpacing(8) + main.addLayout(side, 0, 2, 2, 1) + + metrics_grid = QGridLayout() + metrics_grid.setSpacing(8) + side.addLayout(metrics_grid) + self.left_force = MetricBox("左法向力", "--") + self.right_force = MetricBox("右法向力", "--") + self.shear_force = MetricBox("最大切向力", "--") + self.force_diff = MetricBox("左右差值", "--") + self.force_pct_box = MetricBox("force_pct", "--") + self.speed_box = MetricBox("速度", "--") + self.trigger_box = MetricBox("触发/武装", "--") + self.contact_box = MetricBox("接触", "--") + boxes = [ + self.left_force, + self.right_force, + self.shear_force, + self.force_diff, + self.force_pct_box, + self.speed_box, + self.trigger_box, + self.contact_box, + ] + for idx, box in enumerate(boxes): + metrics_grid.addWidget(box, idx // 2, idx % 2) + + pg.setConfigOption("background", "#000000") + pg.setConfigOption("foreground", "#777777") + pg.setConfigOptions(antialias=True) + self.plot_widget = pg.GraphicsLayoutWidget() + self.plot_widget.setMinimumHeight(240) + side.addWidget(self.plot_widget, 1) + + self.info_box = QFrame() + self.info_box.setObjectName("Panel") + info_layout = QVBoxLayout(self.info_box) + info_layout.setContentsMargins(10, 8, 10, 8) + self.info_label = QLabel("等待启动") + self.info_label.setObjectName("SmallValue") + self.info_label.setWordWrap(True) + info_layout.addWidget(self.info_label) + side.addWidget(self.info_box) + + main.setColumnStretch(0, 2) + main.setColumnStretch(1, 2) + main.setColumnStretch(2, 3) + main.setRowStretch(0, 1) + main.setRowStretch(1, 1) + + def _configure_plot(self): + self.plot = self.plot_widget.addPlot() + self.plot.setTitle("左右法向力与最大切向力", color="#999999", size="13pt") + self.plot.titleLabel.setFont(QFont("SimHei", 13)) + self.plot.showAxis("top", False) + self.plot.showAxis("right", False) + self.plot.showGrid(x=False, y=True, alpha=0.12) + self.plot.setLabel("bottom", "时间", units="s") + self.plot.setLabel("left", "力", units="N") + self.left_curve = self.plot.plot(pen=pg.mkPen(CURVE_COLOR, width=2), name="L") + self.right_curve = self.plot.plot(pen=pg.mkPen("#ffb000", width=2), name="R") + self.shear_curve = self.plot.plot(pen=pg.mkPen("#7ddc8a", width=2), name="Shear") + + def start_control(self): + if self.control_thread is not None and self.control_thread.is_alive(): + return + self.force_history.clear() + self.start_time = time.time() + self.start_button.setEnabled(False) + self.stop_button.setEnabled(True) + self.controller_finished = False + self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器") + + self.controller = build_controller( + self.argv, + status_callback=self._on_status, + include_visuals=True, + visual_size=320, + ) + self.control_thread = threading.Thread( + target=self._run_controller, + daemon=True, + ) + self.control_thread.start() + + def stop_control(self): + if self.controller is not None: + self.controller.stop() + self.stop_button.setEnabled(False) + self.info_label.setText("停止中：正在释放相机与夹爪") + + def _run_controller(self): + try: + self.controller.run() + except Exception as exc: + self._on_status({"state": "error", "action": str(exc), "timestamp": time.time()}) + finally: + self.controller_finished = True + + def _on_status(self, status): + with self.status_lock: + self.latest_status = status + + def _refresh(self): + with self.status_lock: + status = self.latest_status + if not status: + self._update_thread_buttons() + return + self._update_thread_buttons() + + state = status.get("state", "--") + action = status.get("action", "--") + unit = status.get("normal_unit", "N") + shear_unit = status.get("shear_unit", "N") + self.status_chip.setText(f"state={state}") + self.action_chip.setText(f"action={action}") + + self.left_force.set_value( + fmt(status.get("left_normal"), unit), + f"raw FPS {self._sample_fps(status, 'left_sample')}", + ) + self.right_force.set_value( + fmt(status.get("right_normal"), unit), + f"raw FPS {self._sample_fps(status, 'right_sample')}", + ) + self.shear_force.set_value( + fmt(status.get("max_shear"), shear_unit), + f"dShear {fmt(status.get('shear_change'), shear_unit)}", + ) + self.force_diff.set_value( + fmt(status.get("normal_diff"), unit), + f"dN {fmt(status.get('normal_change'), unit)}", + ) + self.force_pct_box.set_value( + str(status.get("force_pct", "--")), + f"hold_pos={status.get('hold_pos')}", + ) + self.speed_box.set_value( + str(status.get("speed_pct", "--")), + "speed_pct", + ) + self.trigger_box.set_value( + f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}", + "trigger / armed", + ) + self.contact_box.set_value( + str(int(bool(status.get("grip_contact")))), + self._contact_text(status), + ) + + left_sample = status.get("left_sample") or {} + right_sample = status.get("right_sample") or {} + self.left_flow.set_image(left_sample.get("flow_view")) + self.left_mag.set_image(left_sample.get("magnitude_view")) + self.right_flow.set_image(right_sample.get("flow_view")) + self.right_mag.set_image(right_sample.get("magnitude_view")) + + elapsed = time.time() - self.start_time + self.force_history.append( + ( + elapsed, + float(status.get("left_normal", 0.0) or 0.0), + float(status.get("right_normal", 0.0) or 0.0), + float(status.get("max_shear", 0.0) or 0.0), + ) + ) + if len(self.force_history) > PLOT_POINTS: + self.force_history = self.force_history[-PLOT_POINTS:] + self._update_plot() + + self.info_label.setText( + " | ".join( + [ + f"min={fmt(status.get('min_normal'), unit)}", + f"max={fmt(status.get('max_normal'), unit)}", + f"L_shear={fmt(status.get('left_shear'), shear_unit)}", + f"R_shear={fmt(status.get('right_shear'), shear_unit)}", + f"flow L/R={self._flow_text(left_sample)} / {self._flow_text(right_sample)}", + ] + ) + ) + + def _update_plot(self): + if not self.force_history: + return + data = np.asarray(self.force_history, dtype=float) + x = data[:, 0] + self.left_curve.setData(x, data[:, 1]) + self.right_curve.setData(x, data[:, 2]) + self.shear_curve.setData(x, data[:, 3]) + if len(x) >= 2: + self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0) + + def _update_thread_buttons(self): + if not self.controller_finished: + return + self.controller_finished = False + self.start_button.setEnabled(True) + self.stop_button.setEnabled(False) + if self.controller is not None: + self.info_label.setText("已停止") + + def _sample_fps(self, status, key): + sample = status.get(key) or {} + fps = sample.get("fps") + if fps is None: + return "--" + return f"{fps:.1f}" + + def _contact_text(self, status): + left = status.get("left_sample") or {} + right = status.get("right_sample") or {} + return f"L={int(bool(left.get('is_contact')))} R={int(bool(right.get('is_contact')))}" + + def _flow_text(self, sample): + if not sample: + return "--" + return f"{sample.get('flow_mean', 0.0):.2f}/{sample.get('flow_max', 0.0):.2f}" + + def keyPressEvent(self, event): + if event.key() == Qt.Key_Q: + self.close() + return + if event.key() == Qt.Key_Escape: + if self.isFullScreen(): + self.showNormal() + else: + self.showFullScreen() + return + super().keyPressEvent(event) + + def closeEvent(self, event): + self.timer.stop() + self.stop_control() + if self.control_thread is not None: + self.control_thread.join(timeout=3.0) + event.accept() + + +def main(argv=None): + parser = argparse.ArgumentParser() + parser.add_argument("--windowed", action="store_true") + ui_args, controller_argv = parser.parse_known_args(argv) + + if _QT_IMPORT_ERROR is not None: + print( + "PyQt5 is required for the visualizer. " + "Install it in the active environment, for example: pip install PyQt5 pyqtgraph" + ) + print(f"Import error: {_QT_IMPORT_ERROR}") + return 1 + if pg is None: + print( + "pyqtgraph is required for the visualizer. " + "Install it in the active environment, for example: pip install pyqtgraph" + ) + print(f"Import error: {_PYQTGRAPH_IMPORT_ERROR}") + return 1 + + QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True) + QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True) + app = QApplication(sys.argv[:1]) + window = GripperDemo02Viewer(controller_argv) + if ui_args.windowed: + window.resize(1500, 900) + window.show() + else: + window.showFullScreen() + window.start_control() + return app.exec_() + + +if __name__ == "__main__": + mp.freeze_support() + raise SystemExit(main(sys.argv[1:])) diff --git a/readme.md b/readme.md index b97e9a8..ffcf1b7 100644 --- a/readme.md +++ b/readme.md @@ -13,4 +13,15 @@ conda activate py311 别再循环了 02不是循环的。整个流程是 1. 某个单侧有东西扫过时开始闭合夹住 2. 闭合的时候要是检测到有物体，就像01那样几个状态，夹住它，不同的是后续有法向力，切向力输入的时候就松开夹爪（不要卡顿），直接张开到最开始的位置。 -3. 这就是整套流程，后续单侧再检测到有东西扫过的时候再执行这一整个过程 \ No newline at end of file +3. 这就是整套流程，后续单侧再检测到有东西扫过的时候再执行这一整个过程 + +# 展示03 +现在把hold_check状态抽离出来，这里后续需要扩写，这里我夹住时 物体的力可能受到 + +# 可视化展示 qt界面 +目前夹住程序还是ok，现在需要可视化展示，用pyqt写， +1. 展示两边的光流 和 赋值图 +2. 展示两边作用力的大小 +3. 展示当前状态 state=open_wait这种 +4. force_pct与速度 展示 +5. 你看看还有其他什么可以展示的 \ No newline at end of file diff --git a/sdk/__pycache__/changingtek_p_rtu_Servo.cpython-313.pyc b/sdk/__pycache__/changingtek_p_rtu_Servo.cpython-313.pyc new file mode 100644 index 0000000..46f4dba Binary files /dev/null and b/sdk/__pycache__/changingtek_p_rtu_Servo.cpython-313.pyc differ