commit 15088f9810644aa7bad867a002a5d4d69b07002f Author: cheng Date: Fri Jul 3 09:44:59 2026 +0800 init diff --git a/config/compound_eye.png b/config/compound_eye.png new file mode 100644 index 0000000..b90a7c2 Binary files /dev/null and b/config/compound_eye.png differ diff --git a/config/ddjx01.json b/config/ddjx01.json new file mode 100644 index 0000000..21b5e96 --- /dev/null +++ b/config/ddjx01.json @@ -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 + } +} + + diff --git a/config/gripper.json b/config/gripper.json new file mode 100644 index 0000000..67315e9 --- /dev/null +++ b/config/gripper.json @@ -0,0 +1,113 @@ +{ + "video1": "0", + "video2": "1", + "sensor_config1": "./config/ddjx01.json", + "sensor_config2": "./config/ddjx01.json", + "sensor_fps": 30.0, + "motion_threshold": 0.0, + "sensor_cpu": false, + "port": "COM5", + "slave_id": 1, + "baudrate": 115200, + "serial_timeout": 0.2, + "open_pos": 0, + "close_pos": 9000, + "speed": 25, + "accel": 80, + "decel": 80, + "release_open_speed": 50, + "release_open_accel": 120, + "release_open_decel": 120, + "open_force": 15, + "close_force": 15, + "grip_start_force": 10, + "hold_force": 15, + "initial_force": 15, + "force_min": 10, + "force_max": 80, + "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, + "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": 1, + "force_ramp_interval": 0.1, + "grip_settle_seconds": 0.1, + "adaptive_grip_enabled": true, + "adaptive_shear_start_force": 0.1, + "adaptive_shear_full_force": 0.6, + "adaptive_force_min": 15, + "adaptive_force_max": 45, + "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, + "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_position_stable_seconds": 0.1, + "open_position_check_interval": 0.1, + "hold_fallback_pos": null, + "trigger_force_update": false, + "open_at_end": true, + "dry_run": false, + "normal_force_calibration": { + "enabled": true, + "method": "piecewise_linear", + "extrapolate": true, + "clamp_output_min": 0.0, + "points": [ + { + "fnormal": 1000.0, + "force_n": 0.0 + }, + { + "fnormal": 10000.0, + "force_n": 0.377 + }, + { + "fnormal": 30000.0, + "force_n": 1.377 + }, + { + "fnormal": 62000.0, + "force_n": 2.377 + } + ] + }, + "shear_force_calibration": { + "enabled": true, + "method": "piecewise_linear", + "input": "components", + "signed": true, + "extrapolate": true, + "clamp_output_min": 0.0, + "points": [ + { + "raw": 1000.0, + "force_n": 0.0 + }, + { + "raw": 36000.0, + "force_n": 1.19 + }, + { + "raw": 68000.0, + "force_n": 2.06 + } + ] + } +} diff --git a/gripper/README.md b/gripper/README.md new file mode 100644 index 0000000..32ad748 --- /dev/null +++ b/gripper/README.md @@ -0,0 +1,147 @@ +# Gripper + +夹爪02是新的展示程序，和原来的定时开合逻辑分开。它不是定时循环开合；程序只是常驻监听单侧触发，每次触发后执行一次“闭合夹住 -> 监测变化 -> 直接张开”的流程。 + +## 展示流程 + +1. 程序启动后，夹爪先张开。 +2. 程序等待任意一侧触觉传感器感受到法向力或切向力。 +3. 展示时，把物体从某个侧边扫一下，触发单侧力变化。 +4. 触发后夹爪开始闭合，你把物体移动到中间。 +5. 闭合过程中检测到物体后，切到低力并保持当前位置。 +6. 像 01 一样进入 `gripping`，先慢慢加力到基础 `HOLD_FORCE`。 +7. 进入 `hold_check` 后，根据切向力估计物体负载；切向力越大，目标夹紧力越高，并按小步进继续补力。 +8. 自适应补力停止并稳定 `RELEASE_ARM_DELAY_SECONDS` 后，重新记录释放参考力，才开始检测人取物松开。 +9. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。 +10. 张开后进入 `open_recover`，保持张开并等待恢复条件。 +11. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，继续休息至少 `REARM_SECONDS`。 +12. 休息结束后才回到 `open_wait` 并重新武装；休息期间不管有没有物体扫过，都不会闭合。 + +## 运行 + +从项目根目录运行： + +```powershell +conda activate py311 +python -m gripper.main +``` + +只看逻辑、不打开串口： + +```powershell +python -m gripper.main --dry-run +``` + +PyQt 可视化展示： + +```powershell +python -m gripper.visualizer +``` + +窗口模式运行： + +```powershell +python -m gripper.visualizer --windowed +``` + +兼容入口： + +```powershell +python -m gripper.visualizer +``` + +默认配置文件： + +```text +config\gripper_demo_02.json +``` + +## 关键配置 + +- `trigger_normal_force`: 单侧法向力触发阈值。 +- `trigger_shear_force`: 单侧切向力触发阈值。 +- `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`: 夹住后等待稳定多久再记录参考力。 +- `adaptive_grip_enabled`: 是否启用按切向力估计负载并自适应补夹紧力。 +- `adaptive_shear_start_force`: 切向力低于这个值时，认为是轻物体，只使用最小目标力。 +- `adaptive_shear_full_force`: 切向力达到这个值时，认为负载较大，目标力加到最大目标力。 +- `adaptive_force_min`: 自适应夹持的最小目标力。 +- `adaptive_force_max`: 自适应夹持的最大目标力；重物夹不住优先加这个，但不要超过 `force_max`。 +- `adaptive_force_step`: 自适应补力时每次增加多少 `force_pct`。 +- `adaptive_force_interval`: 自适应补力两次之间的间隔，越大越柔和。 +- `release_arm_delay_seconds`: 自适应补力停止后，等待多久再启用“人取物松开”检测。 +- `release_normal_change`: 夹住后法向力变化超过它就松开。 +- `release_shear_change`: 夹住后切向力变化超过它就松开。 +- `release_open_speed`: 检测到松开后，张开到 `open_pos` 使用的速度。 +- `release_open_accel`: release 张开的加速度。 +- `release_open_decel`: release 张开的减速度。 +- `rearm_seconds`: 夹爪回到 `open_pos` 或恢复条件成立后，至少休息多久才重新等待触发；休息期间扫过也不会闭合。 +- `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` 中读取夹爪当前位置的间隔。 +- `normal_force_calibration`: 法向力标定，配置在 `config\gripper_demo_02.json`。 +- `shear_force_calibration`: 切向力标定，配置在 `config\gripper_demo_02.json`。 + +## 日志字段 + +- `L/R`: 左右法向力，单位 N。 +- `maxN`: 左右法向力最大值，单位 N。 +- `shear`: 左右切向力最大值，单位 N。 +- `trigger`: 是否满足单侧触发条件。 +- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 且是新的一次触发上升沿才会闭合。 +- `grip_contact`: 是否满足夹住接触条件。 +- `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。 +- `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。 +- `state`: 当前状态。 +- `hold_pos`: 夹住后保持的位置。 +- `force_pct`: 当前夹爪目标力百分比。 +- `target`: 当前根据切向力计算出来的自适应目标力。 +- `release_armed`: 是否已经启用释放检测；`0` 表示还在补力/等待稳定，`1` 表示法向或切向变化会触发张开。 +- `action`: 当前动作。 + +## 可视化展示 + +- 左右两边的光流箭头图。 +- 左右两边的光流幅值图。 +- 左右法向力、左切向力、右切向力。 +- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。 +- 当前 `force_pct`、自适应目标力、速度 `speed_pct`、`hold_pos`。 +- 顶部有 `重启` 按钮，会先停止当前控制线程，释放相机与夹爪后再重新启动。 +- 当前 `release_armed`，用于判断系统是在补夹紧力，还是已经开始检测人取物松开。 +- 折线图带图例，默认固定 y 轴为 `0~3 N`，范围在 `visualizer.py` 顶部的 `PLOT_FORCE_Y_MIN_N / PLOT_FORCE_Y_MAX_N` 调整。 +- 左右传感器 FPS、接触标志、光流均值/最大值。 + +## 状态说明 + +- `open_wait`: 夹爪张开，等待任意单侧触发。 +- `closing`: 单侧触发后正在闭合，等待检测到物体。 +- `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。 +- `hold_check`: 已夹住；先根据切向力自适应补夹紧力，补力稳定后再监测法向/切向变化；变化超过阈值会直接张开。 +- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。 + +## 常见动作说明 + +- `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。 +- `adaptive-grip-ramp`: 切向力显示负载较大，正在按小步进增加夹紧力。 +- `adaptive-grip-wait`: 已经需要更高目标力，但还没到下一次加力时间。 +- `release-arm-delay`: 补力刚停止，正在等待力稳定，暂时不触发松开。 +- `release-reference-armed`: 已重新记录释放参考力，后续法向/切向变化会触发张开。 +- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。 +- `open-ready-position`: 夹爪已经回到初始点位，并且休息时间结束，重新进入等待下一次扫过触发；此时会直接武装，但残余触发保持不变不会自动闭合。 +- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。 +- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。 +- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。 +- `open-wait-trigger-held`: 已经武装，但当前触发不是新上升沿，等待力先降下去再下一次扫过。 +- `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。 +- `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定和休息时间结束。 +- `open-ready`: 力已经清空并稳定，并且休息时间结束，重新进入等待下一次扫过触发。 diff --git a/gripper/__init__.py b/gripper/__init__.py new file mode 100644 index 0000000..93a8c3e --- /dev/null +++ b/gripper/__init__.py @@ -0,0 +1,2 @@ +"""Demo 02 gripper control: side-touch trigger, grip, and instant release.""" + diff --git a/gripper/__pycache__/__init__.cpython-311.pyc b/gripper/__pycache__/__init__.cpython-311.pyc new file mode 100644 index 0000000..2651537 Binary files /dev/null and b/gripper/__pycache__/__init__.cpython-311.pyc differ diff --git a/gripper/__pycache__/calibration.cpython-311.pyc b/gripper/__pycache__/calibration.cpython-311.pyc new file mode 100644 index 0000000..5b53cad Binary files /dev/null and b/gripper/__pycache__/calibration.cpython-311.pyc differ diff --git a/gripper/__pycache__/config_loader.cpython-311.pyc b/gripper/__pycache__/config_loader.cpython-311.pyc new file mode 100644 index 0000000..73af8de Binary files /dev/null and b/gripper/__pycache__/config_loader.cpython-311.pyc differ diff --git a/gripper/__pycache__/controller.cpython-311.pyc b/gripper/__pycache__/controller.cpython-311.pyc new file mode 100644 index 0000000..f2cdaff Binary files /dev/null and b/gripper/__pycache__/controller.cpython-311.pyc differ diff --git a/gripper/__pycache__/feedback.cpython-311.pyc b/gripper/__pycache__/feedback.cpython-311.pyc new file mode 100644 index 0000000..ade02c2 Binary files /dev/null and b/gripper/__pycache__/feedback.cpython-311.pyc differ diff --git a/gripper/__pycache__/filters.cpython-311.pyc b/gripper/__pycache__/filters.cpython-311.pyc new file mode 100644 index 0000000..99200d7 Binary files /dev/null and b/gripper/__pycache__/filters.cpython-311.pyc differ diff --git a/gripper/__pycache__/force_reader.cpython-311.pyc b/gripper/__pycache__/force_reader.cpython-311.pyc new file mode 100644 index 0000000..7ea4431 Binary files /dev/null and b/gripper/__pycache__/force_reader.cpython-311.pyc differ diff --git a/gripper/__pycache__/hardware.cpython-311.pyc b/gripper/__pycache__/hardware.cpython-311.pyc new file mode 100644 index 0000000..97ea914 Binary files /dev/null and b/gripper/__pycache__/hardware.cpython-311.pyc differ diff --git a/gripper/__pycache__/main.cpython-311.pyc b/gripper/__pycache__/main.cpython-311.pyc new file mode 100644 index 0000000..4faba43 Binary files /dev/null and b/gripper/__pycache__/main.cpython-311.pyc differ diff --git a/gripper/__pycache__/paths.cpython-311.pyc b/gripper/__pycache__/paths.cpython-311.pyc new file mode 100644 index 0000000..c559ae8 Binary files /dev/null and b/gripper/__pycache__/paths.cpython-311.pyc differ diff --git a/gripper/__pycache__/rotation.cpython-311.pyc b/gripper/__pycache__/rotation.cpython-311.pyc new file mode 100644 index 0000000..4a4408c Binary files /dev/null and b/gripper/__pycache__/rotation.cpython-311.pyc differ diff --git a/gripper/__pycache__/settings.cpython-311.pyc b/gripper/__pycache__/settings.cpython-311.pyc new file mode 100644 index 0000000..97b5eba Binary files /dev/null and b/gripper/__pycache__/settings.cpython-311.pyc differ diff --git a/gripper/__pycache__/visualizer.cpython-311.pyc b/gripper/__pycache__/visualizer.cpython-311.pyc new file mode 100644 index 0000000..6b647aa Binary files /dev/null and b/gripper/__pycache__/visualizer.cpython-311.pyc differ diff --git a/gripper/calibration.py b/gripper/calibration.py new file mode 100644 index 0000000..5697b22 --- /dev/null +++ b/gripper/calibration.py @@ -0,0 +1,103 @@ +from dataclasses import dataclass + + +DEFAULT_NORMAL_FORCE_CALIBRATION = { + "enabled": False, + "points": [], +} + +DEFAULT_SHEAR_FORCE_CALIBRATION = { + "enabled": False, + "points": [], +} + + +def parse_calibration_point(point): + if isinstance(point, dict): + raw = point.get("fnormal", point.get("raw", point.get("x"))) + force_n = point.get("force_n", point.get("n", point.get("y"))) + return float(raw), float(force_n) + if isinstance(point, (list, tuple)) and len(point) >= 2: + return float(point[0]), float(point[1]) + raise ValueError(f"invalid calibration point: {point!r}") + + +@dataclass(frozen=True) +class ForceConverter: + points: tuple + unit: str = "N" + enabled: bool = True + extrapolate: bool = True + clamp_output_min: float | None = 0.0 + input_mode: str = "magnitude" + signed: bool = False + + @classmethod + def from_config(cls, calibration, config_name): + if calibration is None: + calibration = {"enabled": False} + if not calibration.get("enabled", True): + return cls(points=(), unit="raw", enabled=False) + + method = str(calibration.get("method", "piecewise_linear")).lower() + if method != "piecewise_linear": + raise ValueError(f"unsupported {config_name} method: {method}") + + points = tuple(sorted( + parse_calibration_point(point) + for point in calibration.get("points", []) + )) + if len(points) < 2: + raise ValueError(f"{config_name}.points must contain at least two points") + + return cls( + points=points, + enabled=True, + extrapolate=bool(calibration.get("extrapolate", True)), + clamp_output_min=calibration.get("clamp_output_min", 0.0), + input_mode=str(calibration.get("input", "magnitude")).lower(), + signed=bool(calibration.get("signed", False)), + ) + + def convert(self, raw): + if not self.enabled: + return float(raw) + + raw = float(raw) + points = self.points + if raw <= points[0][0]: + segment = (points[0], points[1]) + if not self.extrapolate: + return self._clamp(points[0][1]) + elif raw >= points[-1][0]: + segment = (points[-2], points[-1]) + if not self.extrapolate: + return self._clamp(points[-1][1]) + else: + segment = None + for left, right in zip(points, points[1:]): + if left[0] <= raw <= right[0]: + segment = (left, right) + break + if segment is None: + return self._clamp(points[-1][1]) + + (raw0, n0), (raw1, n1) = segment + if raw1 == raw0: + value = n0 + else: + ratio = (raw - raw0) / (raw1 - raw0) + value = n0 + ratio * (n1 - n0) + return self._clamp(value) + + def convert_component(self, raw): + if not self.signed: + return self.convert(raw) + raw = float(raw) + sign = -1.0 if raw < 0.0 else 1.0 + return sign * self.convert(abs(raw)) + + def _clamp(self, value): + if self.clamp_output_min is not None: + value = max(float(self.clamp_output_min), value) + return value diff --git a/gripper/config_loader.py b/gripper/config_loader.py new file mode 100644 index 0000000..7bb559b --- /dev/null +++ b/gripper/config_loader.py @@ -0,0 +1,21 @@ +import json + + +def load_control_config(path): + return load_json_config(path) + + +def load_json_config(path): + try: + with open(path, "r", encoding="utf-8") as f: + config = json.load(f) + except FileNotFoundError: + print(f"[Info] control config not found: {path}; using CLI/default values") + return {} + except Exception as exc: + print(f"[Error] failed to read control config {path}: {exc}; using CLI/default values") + return {} + if not isinstance(config, dict): + print(f"[Error] control config {path} must be a JSON object; using CLI/default values") + return {} + return config diff --git a/gripper/controller.py b/gripper/controller.py new file mode 100644 index 0000000..f7d07b5 --- /dev/null +++ b/gripper/controller.py @@ -0,0 +1,771 @@ +import time +import threading + +from .filters import clamp + + +class SideTriggerGripController: + 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() + self._manual_lock = threading.Lock() + self._manual_command = None + + def stop(self): + self._stop_event.set() + + def request_manual_command(self, command): + if command not in {"open", "close", "hold", "release"}: + raise ValueError(f"unsupported manual command: {command}") + with self._manual_lock: + self._manual_command = command + + def _pop_manual_command(self): + with self._manual_lock: + command = self._manual_command + self._manual_command = None + return command + + def run(self): + self._stop_event.clear() + self.reader.start() + + try: + self.gripper.connect() + open_force = int(clamp( + self.config.open_force, + self.config.force_min, + self.config.force_max, + )) + print("demo02 start: open gripper and wait for side touch") + self.gripper.open(open_force, "startup-open") + + self._print_thresholds() + print( + f"Calibrating baseline for {self.config.baseline_seconds:.2f}s. " + "Keep sensors unloaded." + ) + baseline = self.feedback_processor.calibrate_baseline(self.reader) + print( + f"baseline normal: L={baseline.left_normal:.4f}{self.normal_unit} " + f"R={baseline.right_normal:.4f}{self.normal_unit}; " + f"shear: L={baseline.left_shear:.4f}{self.shear_unit} " + f"R={baseline.right_shear:.4f}{self.shear_unit}" + ) + + self._run_event_monitor() + + except KeyboardInterrupt: + print("\nStopping demo02.") + finally: + self.gripper.final_open_if_needed() + self.reader.stop() + + def _print_thresholds(self): + print( + "Trigger thresholds: " + f"normal={self.config.trigger_normal_force:.3f}{self.normal_unit}, " + f"shear={self.config.trigger_shear_force:.3f}{self.shear_unit}" + ) + print( + "Grip/release thresholds: " + f"grip_normal={self.config.grip_normal_force:.3f}{self.normal_unit}, " + f"requires_both={int(self.config.grip_requires_both)}, " + f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, " + f"release_shear_change={self.config.release_shear_change:.3f}{self.shear_unit}" + ) + print( + "Adaptive grip: " + f"enabled={int(self.config.adaptive_grip_enabled)}, " + f"shear_start={self.config.adaptive_shear_start_force:.3f}{self.shear_unit}, " + f"shear_full={self.config.adaptive_shear_full_force:.3f}{self.shear_unit}, " + f"force={self.config.adaptive_force_min}-{self.config.adaptive_force_max}, " + f"step={self.config.adaptive_force_step}, " + f"interval={self.config.adaptive_force_interval:.2f}s, " + f"release_arm_delay={self.config.release_arm_delay_seconds:.2f}s" + ) + 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 + interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.05 + + 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 + last_trigger = False + open_wait_samples = [] + open_wait_stable_values = {} + force_pct = int(clamp(config.open_force, config.force_min, config.force_max)) + adaptive_target_force = int(clamp(config.hold_force, config.force_min, config.force_max)) + release_armed = False + hold_pos = None + reference = None + + def reset_open_wait_stability(): + nonlocal open_wait_samples, open_wait_stable_values + open_wait_samples = [] + open_wait_stable_values = {} + + while not self._stop_event.is_set(): + tick_start = time.perf_counter() + now = time.perf_counter() + manual_action = None + + manual_command = self._pop_manual_command() + if manual_command == "open": + force_pct = int(clamp( + config.open_force, + config.force_min, + config.force_max, + )) + self.gripper.open(force_pct, "manual-open") + 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 + trigger_clear_since = None + trigger_armed = False + reset_open_wait_stability() + release_armed = False + hold_pos = None + reference = None + manual_action = "manual-open" + elif manual_command == "close": + force_pct = int(clamp( + config.close_force, + config.force_min, + config.force_max, + )) + self.gripper.close(force_pct, "manual-close") + state = "manual_close" + 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 + reset_open_wait_stability() + release_armed = False + hold_pos = None + reference = None + manual_action = "manual-close" + elif manual_command == "hold": + force_pct = int(clamp( + config.hold_force, + config.force_min, + config.force_max, + )) + self.gripper.set_force(force_pct) + hold_pos = self.gripper.hold_current_position(force_pct) + state = "manual_hold" + state_start = now + last_force_ramp_time = now + trigger_clear_since = None + trigger_armed = False + reset_open_wait_stability() + release_armed = False + reference = None + manual_action = "manual-hold" + elif manual_command == "release": + if state == "manual_hold": + state = "open_wait" + state_start = now + trigger_clear_since = None + trigger_armed = False + reset_open_wait_stability() + release_armed = False + hold_pos = None + reference = None + manual_action = "manual-release" + + feedback = self.feedback_processor.read(self.reader, timeout=0.3) + if feedback is None: + action = manual_action or "waiting-for-sensor-samples" + print(f"state={state} action={action}") + self._emit_status( + { + "timestamp": time.time(), + "state": state, + "action": action, + "force_pct": force_pct, + "adaptive_target_force": adaptive_target_force, + "adaptive_grip_enabled": config.adaptive_grip_enabled, + "release_armed": release_armed, + "manual_hold_active": state == "manual_hold", + "speed_pct": config.speed, + "normal_unit": self.normal_unit, + "shear_unit": self.shear_unit, + } + ) + time.sleep(0.05) + continue + + trigger = self._side_triggered(feedback) + grip_contact = self._grip_contact(feedback) + normal_change = 0.0 + shear_change = 0.0 + action = manual_action or "wait" + + if manual_action is not None: + pass + elif state == "open_wait": + open_wait_sample = self._open_wait_sample(feedback) + self._append_open_wait_sample(open_wait_samples, open_wait_sample) + stable_values = self._open_wait_stable_values(open_wait_samples) + trigger = self._open_wait_delta_triggered( + open_wait_sample, + open_wait_stable_values, + ) + if not trigger: + open_wait_stable_values.update(stable_values) + + trigger_armed = bool(open_wait_stable_values) + if not trigger_armed: + action = "open-wait-stabilizing" + elif not trigger: + action = "open-wait-armed" + else: + force_pct = int(clamp( + config.close_force, + config.force_min, + config.force_max, + )) + self.gripper.close(force_pct, "side-trigger-close") + 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 + reset_open_wait_stability() + adaptive_target_force = self._adaptive_force_target(feedback) + release_armed = False + hold_pos = None + reference = None + action = "side-trigger-close" + + elif state == "closing": + if grip_contact: + force_pct = int(clamp( + config.grip_start_force, + config.force_min, + config.force_max, + )) + self.gripper.set_force(force_pct) + hold_pos = self.gripper.hold_current_position(force_pct) + state = "gripping" + state_start = now + last_force_ramp_time = now + adaptive_target_force = self._adaptive_force_target(feedback) + release_armed = False + reference = None + action = "object-detected-low-force" + elif now - state_start >= config.close_timeout_seconds: + force_pct = int(clamp( + config.open_force, + config.force_min, + config.force_max, + )) + self.gripper.open(force_pct, "close-timeout-open") + self.feedback_processor.reset_filters() + state = "open_recover" + state_start = now + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False + recover_quiet_since = None + recover_position_since = None + reset_open_wait_stability() + release_armed = False + hold_pos = None + reference = None + action = "close-timeout-open" + else: + if ( + config.closing_command_interval > 0 + and now - last_force_ramp_time >= config.closing_command_interval + ): + self.gripper.close(force_pct, "closing-continue") + last_force_ramp_time = now + action = "closing-continue" + else: + action = "closing-to-grip" + + elif state == "gripping": + if ( + now - last_force_ramp_time >= config.force_ramp_interval + and force_pct < config.hold_force + ): + force_pct = int(clamp( + force_pct + config.force_ramp_step, + config.force_min, + config.hold_force, + )) + self.gripper.set_force(force_pct) + last_force_ramp_time = now + action = "grip-ramp-up" + elif ( + force_pct >= config.hold_force + and now - state_start >= config.grip_settle_seconds + ): + reference = self._make_reference(feedback) + state = "hold_check" + state_start = now + adaptive_target_force = self._adaptive_force_target(feedback) + release_armed = False + action = "hold-reference-armed" + else: + action = "grip-hold" + + elif state == "hold_check": + adaptive_target_force = self._adaptive_force_target(feedback) + should_adapt_force = ( + config.adaptive_grip_enabled + and not release_armed + and force_pct < adaptive_target_force + ) + + if ( + should_adapt_force + and now - last_force_ramp_time >= config.adaptive_force_interval + ): + force_pct = int(clamp( + force_pct + config.adaptive_force_step, + config.force_min, + adaptive_target_force, + )) + self.gripper.set_force(force_pct) + last_force_ramp_time = now + state_start = now + reference = self._make_reference(feedback) + release_armed = False + action = "adaptive-grip-ramp" + elif should_adapt_force: + release_armed = False + action = "adaptive-grip-wait" + elif ( + not release_armed + and now - max(state_start, last_force_ramp_time) + < config.release_arm_delay_seconds + ): + action = "release-arm-delay" + elif not release_armed: + reference = self._make_reference(feedback) + release_armed = True + action = "release-reference-armed" + else: + normal_change, shear_change = self._release_changes(feedback, reference) + should_release = ( + normal_change >= config.release_normal_change + or shear_change >= config.release_shear_change + ) + if should_release: + force_pct = int(clamp( + config.open_force, + config.force_min, + config.force_max, + )) + self.gripper.open( + force_pct, + "force-change-release-open", + speed_pct=config.release_open_speed, + accel=config.release_open_accel, + decel=config.release_open_decel, + ) + self.feedback_processor.reset_filters() + state = "open_recover" + state_start = now + last_open_position_check_time = 0.0 + last_open_position = None + last_open_position_ready = False + recover_quiet_since = None + recover_position_since = None + reset_open_wait_stability() + release_armed = False + hold_pos = None + reference = None + action = "force-change-release-open" + else: + action = "hold-check" + + elif state == "open_recover": + 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 + >= max( + config.open_position_stable_seconds, + config.rearm_seconds, + ) + ): + state = "open_wait" + state_start = now + recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = True + reset_open_wait_stability() + last_trigger = trigger + release_armed = False + 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" + elif ( + now - recover_quiet_since + >= max(config.recover_stable_seconds, config.rearm_seconds) + ): + state = "open_wait" + state_start = now + recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = True + reset_open_wait_stability() + last_trigger = trigger + release_armed = False + hold_pos = None + reference = None + action = "open-ready" + else: + action = "open-recover-stabilizing" + else: + recover_quiet_since = None + recover_position_since = None + action = "open-recover-wait-force-clear" + + elif state == "manual_hold": + action = "manual-hold" + + elif state == "manual_close": + action = "manual-close" + + self._log_tick( + 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, + adaptive_target_force=adaptive_target_force, + release_armed=release_armed, + 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, + adaptive_target_force=adaptive_target_force, + release_armed=release_armed, + action=action, + trigger_armed=trigger_armed, + ) + ) + last_trigger = trigger + + 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 + or feedback.max_shear >= self.config.trigger_shear_force + ) + + def _open_wait_sample(self, feedback): + return { + "left_normal": feedback.left_normal, + "right_normal": feedback.right_normal, + "left_shear": feedback.left_shear, + "right_shear": feedback.right_shear, + } + + def _append_open_wait_sample(self, samples, sample): + samples.append(sample) + stable_samples = max(2, int(self.config.open_wait_stable_samples)) + if len(samples) > stable_samples: + del samples[0] + + def _open_wait_stable_values(self, samples): + stable_samples = max(2, int(self.config.open_wait_stable_samples)) + if len(samples) < stable_samples: + return {} + + stable = {} + first = samples[0] + last = samples[-1] + for key in first: + values = [sample[key] for sample in samples] + value_range = max(values) - min(values) + trend = abs(last[key] - first[key]) + if ( + value_range <= self.config.open_wait_stable_range_n + and trend <= self.config.open_wait_stable_trend_n + ): + stable[key] = sum(values) / len(values) + return stable + + def _open_wait_delta_triggered(self, sample, stable_values): + thresholds = { + "left_normal": self.config.trigger_normal_force, + "right_normal": self.config.trigger_normal_force, + "left_shear": self.config.trigger_shear_force, + "right_shear": self.config.trigger_shear_force, + } + for key, stable_value in stable_values.items(): + if sample[key] - stable_value >= thresholds[key]: + return True + return False + + def _grip_contact(self, feedback): + left_contact = feedback.left_normal >= self.config.grip_normal_force + right_contact = feedback.right_normal >= self.config.grip_normal_force + if self.config.grip_requires_both: + return left_contact and right_contact + return left_contact or right_contact + + def _recover_quiet(self, feedback): + return ( + feedback.max_normal <= self.config.recover_normal_force + 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 _adaptive_force_target(self, feedback): + config = self.config + if not config.adaptive_grip_enabled: + return int(clamp(config.hold_force, config.force_min, config.force_max)) + + force_min = int(clamp( + config.adaptive_force_min, + config.force_min, + config.force_max, + )) + force_max = int(clamp( + config.adaptive_force_max, + config.force_min, + config.force_max, + )) + if force_max < force_min: + force_max = force_min + + shear_start = max(0.0, float(config.adaptive_shear_start_force)) + shear_full = max(shear_start + 1e-6, float(config.adaptive_shear_full_force)) + shear = max(0.0, float(feedback.max_shear)) + if shear <= shear_start: + return force_min + if shear >= shear_full: + return force_max + + ratio = (shear - shear_start) / (shear_full - shear_start) + target = round(force_min + ratio * (force_max - force_min)) + return int(clamp(target, force_min, force_max)) + + def _make_reference(self, feedback): + return { + "left_normal": feedback.left_normal, + "right_normal": feedback.right_normal, + "left_shear": feedback.left_shear, + "right_shear": feedback.right_shear, + "max_shear": feedback.max_shear, + } + + def _release_changes(self, feedback, reference): + if reference is None: + return 0.0, 0.0 + normal_change = max( + abs(feedback.left_normal - reference["left_normal"]), + abs(feedback.right_normal - reference["right_normal"]), + ) + shear_change = abs(feedback.max_shear - reference["max_shear"]) + shear_change = max( + shear_change, + abs(feedback.left_shear - reference["left_shear"]), + abs(feedback.right_shear - reference["right_shear"]), + ) + return normal_change, shear_change + + def _log_tick( + self, + feedback, + state, + trigger, + grip_contact, + normal_change, + shear_change, + hold_pos, + force_pct, + adaptive_target_force, + release_armed, + 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)} 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} target={adaptive_target_force:3d} " + f"release_armed={int(release_armed)} action={action}" + ) + + def _make_status( + self, + feedback, + state, + trigger, + grip_contact, + normal_change, + shear_change, + hold_pos, + force_pct, + adaptive_target_force, + release_armed, + 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, + "left_shear_x": feedback.left_shear_x, + "left_shear_y": feedback.left_shear_y, + "right_shear_x": feedback.right_shear_x, + "right_shear_y": feedback.right_shear_y, + "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, + "adaptive_target_force": adaptive_target_force, + "adaptive_grip_enabled": self.config.adaptive_grip_enabled, + "release_armed": bool(release_armed), + "manual_hold_active": state == "manual_hold", + "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/feedback.py b/gripper/feedback.py new file mode 100644 index 0000000..b0ceffc --- /dev/null +++ b/gripper/feedback.py @@ -0,0 +1,168 @@ +from dataclasses import dataclass +import time + +from .filters import ExponentialFilter + + +def shear_magnitude(sample): + return (float(sample["fshearx"]) ** 2 + float(sample["fsheary"]) ** 2) ** 0.5 + + +def _component_shear_enabled(converter): + return getattr(converter, "input_mode", "magnitude") == "components" + + +@dataclass +class ForceBaseline: + left_normal: float = 0.0 + right_normal: float = 0.0 + left_shear: float = 0.0 + right_shear: float = 0.0 + left_shear_x: float = 0.0 + left_shear_y: float = 0.0 + right_shear_x: float = 0.0 + right_shear_y: float = 0.0 + + +@dataclass +class ForceFeedback: + left_normal: float + right_normal: float + min_normal: float + max_normal: float + normal_diff: float + left_shear: float + right_shear: float + max_shear: float + shear_diff: float + left_shear_x: float = 0.0 + left_shear_y: float = 0.0 + right_shear_x: float = 0.0 + right_shear_y: float = 0.0 + + +class TactileFeedbackProcessor: + def __init__(self, config, normal_converter, shear_converter): + self.config = config + self.normal_converter = normal_converter + self.shear_converter = shear_converter + self.baseline = ForceBaseline() + self.reset_filters() + + def reset_filters(self): + 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) + self.right_shear_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.left_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.left_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha) + self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha) + + def _convert_shear(self, sample): + 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 + + def calibrate_baseline(self, reader): + seconds = self.config.baseline_seconds + if seconds <= 0: + self.baseline = ForceBaseline() + return self.baseline + + left_values = [] + right_values = [] + left_shear_values = [] + right_shear_values = [] + left_shear_x_values = [] + left_shear_y_values = [] + right_shear_x_values = [] + right_shear_y_values = [] + deadline = time.perf_counter() + seconds + while time.perf_counter() < deadline: + left, right = reader.get_samples(timeout=0.2) + if left is not None: + left_values.append(self.normal_converter.convert(left["fnormal"])) + shear, shear_x, shear_y = self._convert_shear(left) + left_shear_values.append(shear) + left_shear_x_values.append(shear_x) + left_shear_y_values.append(shear_y) + if right is not None: + right_values.append(self.normal_converter.convert(right["fnormal"])) + shear, shear_x, shear_y = self._convert_shear(right) + right_shear_values.append(shear) + right_shear_x_values.append(shear_x) + right_shear_y_values.append(shear_y) + time.sleep(0.01) + + self.baseline = ForceBaseline( + left_normal=sum(left_values) / len(left_values) if left_values else 0.0, + right_normal=sum(right_values) / len(right_values) if right_values else 0.0, + left_shear=sum(left_shear_values) / len(left_shear_values) if left_shear_values else 0.0, + right_shear=sum(right_shear_values) / len(right_shear_values) if right_shear_values else 0.0, + left_shear_x=sum(left_shear_x_values) / len(left_shear_x_values) if left_shear_x_values else 0.0, + left_shear_y=sum(left_shear_y_values) / len(left_shear_y_values) if left_shear_y_values else 0.0, + right_shear_x=sum(right_shear_x_values) / len(right_shear_x_values) if right_shear_x_values else 0.0, + right_shear_y=sum(right_shear_y_values) / len(right_shear_y_values) if right_shear_y_values else 0.0, + ) + return self.baseline + + def read(self, reader, timeout=0.3): + left, right = reader.get_samples(timeout=timeout) + if left is None or right is None: + return None + + left_force = max( + 0.0, + self.normal_converter.convert(left["fnormal"]) - self.baseline.left_normal, + ) + right_force = max( + 0.0, + self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal, + ) + 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) + left_shear_x = raw_left_shear_x - self.baseline.left_shear_x + 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 + 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, + ) + right_shear = max( + 0.0, + self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear, + ) + + left_normal = self.left_normal_filter.update(left_force) + right_normal = self.right_normal_filter.update(right_force) + left_shear_filtered = self.left_shear_filter.update(left_shear) + right_shear_filtered = self.right_shear_filter.update(right_shear) + left_shear_x_filtered = self.left_shear_x_filter.update(left_shear_x) + left_shear_y_filtered = self.left_shear_y_filter.update(left_shear_y) + right_shear_x_filtered = self.right_shear_x_filter.update(right_shear_x) + right_shear_y_filtered = self.right_shear_y_filter.update(right_shear_y) + + return ForceFeedback( + left_normal=left_normal, + right_normal=right_normal, + min_normal=min(left_normal, right_normal), + max_normal=max(left_normal, right_normal), + normal_diff=left_normal - right_normal, + left_shear=left_shear_filtered, + right_shear=right_shear_filtered, + max_shear=max(left_shear_filtered, right_shear_filtered), + shear_diff=left_shear_filtered - right_shear_filtered, + left_shear_x=left_shear_x_filtered, + left_shear_y=left_shear_y_filtered, + right_shear_x=right_shear_x_filtered, + right_shear_y=right_shear_y_filtered, + ) diff --git a/gripper/filters.py b/gripper/filters.py new file mode 100644 index 0000000..52c34f1 --- /dev/null +++ b/gripper/filters.py @@ -0,0 +1,23 @@ +class ExponentialFilter: + def __init__(self, alpha): + self.alpha = max(0.0, min(1.0, float(alpha))) + self.value = 0.0 + self.initialized = False + + def update(self, sample): + sample = float(sample) + if not self.initialized: + self.value = sample + self.initialized = True + else: + self.value = self.alpha * sample + (1.0 - self.alpha) * self.value + return self.value + + def reset(self): + self.value = 0.0 + self.initialized = False + + +def clamp(value, min_value, max_value): + return max(min_value, min(max_value, value)) + diff --git a/gripper/force_reader.py b/gripper/force_reader.py new file mode 100644 index 0000000..28b7ae8 --- /dev/null +++ b/gripper/force_reader.py @@ -0,0 +1,456 @@ +import os +os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" + +import argparse +import inspect +import multiprocessing as mp +import queue +import time +from multiprocessing import Process, Queue + +import cv2 +import numpy as np + +from .paths import ensure_project_paths +from .rotation import ( + DEFAULT_ROTATION1, + DEFAULT_ROTATION2, + normalize_rotation_config, + write_temp_rotation_config, +) + +ensure_project_paths() + + +def parse_video_source(value): + if isinstance(value, str): + value = value.strip() + if value.isdigit(): + return int(value) + return value + + +def resolve_rotation_config_path(value, label): + if not isinstance(value, dict): + return value + return write_temp_rotation_config(value, label) + + +def build_sensor(orisys_module, vid_src, config_name, rotation_config, cuda, isstitch, backend, sensor_id): + candidates = { + "isstitch": isstitch, + "cuda": cuda, + "verbose": False, + "config_name": config_name, + "backend": backend, + "rotation_config": rotation_config, + } + + try: + params = inspect.signature(orisys_module.Sensor).parameters + except (TypeError, ValueError): + params = {} + + accepts_kwargs = any( + param.kind == inspect.Parameter.VAR_KEYWORD + for param in params.values() + ) + if accepts_kwargs: + kwargs = candidates + else: + kwargs = { + key: value + for key, value in candidates.items() + if key in params + } + + if "rotation_config_path" in params: + kwargs["rotation_config_path"] = resolve_rotation_config_path( + rotation_config, + f"sensor_{sensor_id}", + ) + + return orisys_module.Sensor(vid_src, **kwargs) + + +def _put_latest(result_queue, data): + try: + result_queue.put_nowait(data) + return + except queue.Full: + pass + + try: + result_queue.get_nowait() + except Exception: + pass + + try: + result_queue.put_nowait(data) + except queue.Full: + 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, + config_name, + rotation_config, + result_queue, + stop_event, + target_fps, + cuda, + isstitch, + backend, + motion_threshold, + include_visuals, + visual_size, +): + ensure_project_paths() + import orisys + + sensor = None + try: + sensor = build_sensor( + orisys, + vid_src, + config_name, + rotation_config, + cuda, + isstitch, + backend, + sensor_id, + ) + + frame_interval = 1.0 / target_fps if target_fps and target_fps > 0 else 0.0 + + while not stop_event.is_set(): + t_start = time.perf_counter() + img = sensor.get_img() + if img is None: + time.sleep(0.01) + continue + + is_contact = sensor.compute_deformation( + check_motion=True, + threshold=motion_threshold, + ) + 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 + + 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) + if sleep_time > 0.001: + time.sleep(sleep_time) + + except Exception as exc: + _put_latest( + result_queue, + { + "sensor_id": sensor_id, + "error": str(exc), + "timestamp": time.time(), + }, + ) + finally: + if sensor is not None: + try: + sensor.disconnect() + except Exception: + pass + + +class TwoSensorForceReader: + def __init__( + self, + video1=0, + video2=1, + config1="./config/ddjx01.json", + config2="./config/ddjx01.json", + rotation1=None, + rotation2=None, + target_fps=30, + cuda=True, + isstitch=True, + backend="auto", + motion_threshold=0, + queue_size=5, + include_visuals=False, + visual_size=320, + ): + self.sensor_configs = [ + { + "sensor_id": 0, + "vid_src": parse_video_source(video1), + "config_name": config1, + "rotation_config": normalize_rotation_config(rotation1, DEFAULT_ROTATION1), + }, + { + "sensor_id": 1, + "vid_src": parse_video_source(video2), + "config_name": config2, + "rotation_config": normalize_rotation_config(rotation2, DEFAULT_ROTATION2), + }, + ] + self.target_fps = target_fps + self.cuda = cuda + self.isstitch = isstitch + 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 = [] + self.processes = [] + self.latest = [None, None] + + def start(self): + if self.processes: + return + + self.result_queues = [Queue(maxsize=self.queue_size) for _ in self.sensor_configs] + self.stop_events = [mp.Event() for _ in self.sensor_configs] + self.latest = [None] * len(self.sensor_configs) + + for idx, config in enumerate(self.sensor_configs): + process = Process( + target=_sensor_force_worker, + args=( + config["sensor_id"], + config["vid_src"], + config["config_name"], + config["rotation_config"], + self.result_queues[idx], + self.stop_events[idx], + self.target_fps, + self.cuda, + self.isstitch, + self.backend, + self.motion_threshold, + self.include_visuals, + self.visual_size, + ), + ) + process.start() + self.processes.append(process) + + def update(self): + for idx, result_queue in enumerate(self.result_queues): + data = None + while True: + try: + data = result_queue.get_nowait() + except queue.Empty: + break + except Exception: + break + + if data is not None: + if "error" in data: + raise RuntimeError( + f"sensor {data.get('sensor_id', idx)} failed: {data['error']}" + ) + self.latest[idx] = data + + return self.latest + + def get_samples(self, timeout=0.0): + deadline = time.perf_counter() + max(0.0, float(timeout)) + while True: + self.update() + if all(sample is not None for sample in self.latest): + return self.latest[0], self.latest[1] + if time.perf_counter() >= deadline: + return self.latest[0], self.latest[1] + time.sleep(0.005) + + def get_forces(self, timeout=0.0): + left, right = self.get_samples(timeout=timeout) + left_force = None if left is None else left["fnormal"] + right_force = None if right is None else right["fnormal"] + return left_force, right_force + + def stop(self): + for event in self.stop_events: + event.set() + + for process in self.processes: + process.join(timeout=2.0) + if process.is_alive(): + process.terminate() + process.join(timeout=1.0) + + self.processes = [] + self.stop_events = [] + self.result_queues = [] + + def __enter__(self): + self.start() + return self + + def __exit__(self, exc_type, exc, traceback): + self.stop() + + +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("--rotation1", "-r1", type=str, default=None) + parser.add_argument("--rotation2", "-r2", type=str, default=None) + parser.add_argument("--target-fps", type=float, default=30) + parser.add_argument("--motion-threshold", type=float, default=0) + parser.add_argument("--cpu", action="store_true") + args = parser.parse_args() + + reader = TwoSensorForceReader( + video1=args.video1, + video2=args.video2, + config1=args.config1, + config2=args.config2, + rotation1=args.rotation1, + rotation2=args.rotation2, + target_fps=args.target_fps, + cuda=not args.cpu, + motion_threshold=args.motion_threshold, + ) + + print("Starting two-sensor force reader. Press Ctrl+C to stop.") + reader.start() + try: + while True: + left_sample, right_sample = reader.get_samples(timeout=1.0) + left_force, right_force = reader.get_forces() + left_fps = 0.0 if left_sample is None else left_sample["fps"] + right_fps = 0.0 if right_sample is None else right_sample["fps"] + left_contact = False if left_sample is None else left_sample["is_contact"] + right_contact = False if right_sample is None else right_sample["is_contact"] + + print( + "left={:>10} right={:>10} fps=({:5.1f}, {:5.1f}) contact=({}, {})".format( + "None" if left_force is None else f"{left_force:.4f}", + "None" if right_force is None else f"{right_force:.4f}", + left_fps, + right_fps, + int(left_contact), + int(right_contact), + ) + ) + time.sleep(0.05) + except KeyboardInterrupt: + print("\nStopping.") + finally: + reader.stop() + + +if __name__ == "__main__": + mp.freeze_support() + main() diff --git a/gripper/hardware.py b/gripper/hardware.py new file mode 100644 index 0000000..27fde63 --- /dev/null +++ b/gripper/hardware.py @@ -0,0 +1,119 @@ +from .filters import clamp +from .paths import ensure_project_paths + + +class GripperClient: + def __init__(self, config): + self.config = config + self.motor = None + + def connect(self): + if self.config.dry_run: + return self + + ensure_project_paths() + from changingtek_p_rtu_Servo import MotorController + + self.motor = MotorController( + self.config.port, + self.config.slave_id, + baudrate=self.config.baudrate, + timeout=self.config.serial_timeout, + ) + return self + + 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={speed_pct}, force={force_pct}, " + f"accel={accel}, decel={decel}" + ) + return + if self.motor is None: + raise RuntimeError( + "gripper motor is not connected; check serial port, conda environment, " + "and changingtek_p_rtu_Servo.py" + ) + + self.motor.temp_move( + position_mm=int(position), + speed_pct=int(speed_pct), + force_pct=int(force_pct), + accel=int(accel), + decel=int(decel), + trigger=True, + ) + + 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) + + def set_force(self, force_pct): + config = self.config + force_pct = int(clamp(force_pct, config.force_min, config.force_max)) + if config.dry_run: + print(f"[dry-run] set_target_force {force_pct}") + return force_pct + + self.motor.set_target_force(force_pct) + if config.trigger_force_update: + self.motor.trigger_motion() + return force_pct + + def read_position(self): + if self.config.dry_run: + return None + try: + return self.motor.read_real_position() + except Exception as exc: + print(f"read_real_position failed: {exc}") + return None + + def hold_current_position(self, force_pct): + current_pos = self.read_position() + if current_pos is None: + current_pos = self.config.hold_fallback_pos + + if current_pos is None: + self.set_force(force_pct) + self._stop_by_speed() + return None + + self.move(int(current_pos), force_pct, "clamp-hold") + return int(current_pos) + + def final_open_if_needed(self): + if not self.config.open_at_end: + return + if not self.config.dry_run and self.motor is None: + return + final_force = int(clamp( + self.config.initial_force, + self.config.force_min, + self.config.force_max, + )) + self.open(final_force, "open-at-end") + + def _stop_by_speed(self): + if self.config.dry_run: + return + try: + self.motor.set_target_speed(0) + self.motor.trigger_motion() + except Exception as exc: + print(f"stop by speed=0 failed: {exc}") diff --git a/gripper/main.py b/gripper/main.py new file mode 100644 index 0000000..f0bca07 --- /dev/null +++ b/gripper/main.py @@ -0,0 +1,62 @@ +import multiprocessing as mp + +from .calibration import ForceConverter +from .feedback import TactileFeedbackProcessor +from .force_reader import TwoSensorForceReader +from .hardware import GripperClient +from .paths import ensure_project_paths + +from .controller import SideTriggerGripController +from .settings import Demo02Config + + +def build_controller( + argv=None, + status_callback=None, + include_visuals=False, + visual_size=320, +): + ensure_project_paths() + config = Demo02Config.from_args(argv) + + normal_converter = ForceConverter.from_config( + config.normal_force_calibration, + "normal_force_calibration", + ) + shear_converter = ForceConverter.from_config( + config.shear_force_calibration, + "shear_force_calibration", + ) + + reader = TwoSensorForceReader( + video1=config.video1, + video2=config.video2, + config1=config.sensor_config1, + config2=config.sensor_config2, + rotation1=config.rotation1, + rotation2=config.rotation2, + 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, + status_callback=status_callback, + ) + + +def main(argv=None): + controller = build_controller(argv) + controller.run() + + +if __name__ == "__main__": + mp.freeze_support() + main() diff --git a/gripper/paths.py b/gripper/paths.py new file mode 100644 index 0000000..5407f0e --- /dev/null +++ b/gripper/paths.py @@ -0,0 +1,16 @@ +from pathlib import Path +import sys + + +PACKAGE_DIR = Path(__file__).resolve().parent +REPO_ROOT = PACKAGE_DIR.parent +SDK_DIR = REPO_ROOT / "sdk" +SRC_DIR = REPO_ROOT / "src" + + +def ensure_project_paths(): + for path in (SDK_DIR, SRC_DIR, REPO_ROOT): + path_text = str(path) + if path_text not in sys.path: + sys.path.insert(0, path_text) + diff --git a/gripper/rotation.py b/gripper/rotation.py new file mode 100644 index 0000000..54d9ad5 --- /dev/null +++ b/gripper/rotation.py @@ -0,0 +1,38 @@ +import copy +import json +import os +import tempfile + + +DEFAULT_ROTATION1 = { + "rotation_angle": 90, + "flip_x": True, + "flip_y": False, +} + +DEFAULT_ROTATION2 = { + "rotation_angle": 90, + "flip_x": False, + "flip_y": False, +} + + +def normalize_rotation_config(value, default_config): + if value is None: + return copy.deepcopy(default_config) + if isinstance(value, dict): + return copy.deepcopy(value) + if isinstance(value, str): + text = value.strip() + if not text or text.lower() == "default": + return copy.deepcopy(default_config) + if text.startswith("{"): + return json.loads(text) + return value + + +def write_temp_rotation_config(config, label): + path = os.path.join(tempfile.gettempdir(), f"gripper_{label}_rotation.json") + with open(path, "w", encoding="utf-8") as f: + json.dump(config, f, ensure_ascii=False, indent=2) + return path diff --git a/gripper/settings.py b/gripper/settings.py new file mode 100644 index 0000000..43c608e --- /dev/null +++ b/gripper/settings.py @@ -0,0 +1,211 @@ +import argparse +import copy +from dataclasses import dataclass, field + +from .calibration import ( + DEFAULT_NORMAL_FORCE_CALIBRATION, + DEFAULT_SHEAR_FORCE_CALIBRATION, +) +from .config_loader import load_control_config +from .paths import REPO_ROOT +from .rotation import ( + DEFAULT_ROTATION1, + DEFAULT_ROTATION2, + normalize_rotation_config, +) + + +DEFAULT_CONTROL_CONFIG = str( + REPO_ROOT / "config" / "gripper.json" +) + + +def _config_default(config, key, default): + return config.get(key, config.get(key.replace("_", "-"), default)) + + +@dataclass +class Demo02Config: + control_config: str = DEFAULT_CONTROL_CONFIG + + video1: str = "0" + 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_fps: float = 30.0 + motion_threshold: float = 0.0 + sensor_cpu: bool = False + + port: str = "COM5" + slave_id: int = 1 + baudrate: int = 115200 + serial_timeout: float = 0.2 + + open_pos: int = 0 + close_pos: int = 9000 + 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 + grip_start_force: int = 10 + hold_force: int = 15 + initial_force: int = 10 + force_min: int = 10 + force_max: int = 60 + + control_hz: float = 30.0 + baseline_seconds: float = 1.0 + 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 + open_wait_stable_samples: int = 12 + open_wait_stable_range_n: float = 0.03 + open_wait_stable_trend_n: float = 0.015 + grip_normal_force: float = 0.05 + grip_requires_both: bool = True + close_timeout_seconds: float = 5.0 + closing_command_interval: float = 0.2 + force_ramp_step: int = 1 + force_ramp_interval: float = 0.1 + grip_settle_seconds: float = 0.1 + adaptive_grip_enabled: bool = True + adaptive_shear_start_force: float = 0.10 + adaptive_shear_full_force: float = 0.60 + adaptive_force_min: int = 15 + 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 + 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 + open_at_end: bool = True + dry_run: bool = False + + normal_force_calibration: dict = field( + default_factory=lambda: copy.deepcopy(DEFAULT_NORMAL_FORCE_CALIBRATION) + ) + shear_force_calibration: dict = field( + default_factory=lambda: copy.deepcopy(DEFAULT_SHEAR_FORCE_CALIBRATION) + ) + + @classmethod + def from_args(cls, argv=None): + bootstrap = argparse.ArgumentParser(add_help=False) + bootstrap.add_argument("--control-config", default=DEFAULT_CONTROL_CONFIG) + bootstrap_args, _ = bootstrap.parse_known_args(argv) + config_data = load_control_config(bootstrap_args.control_config) + + parser = argparse.ArgumentParser() + add = parser.add_argument + get = lambda key, default: _config_default(config_data, key, default) + + add("--control-config", type=str, default=bootstrap_args.control_config) + add("--video1", "-v1", type=str, default=get("video1", cls.video1)) + add("--video2", "-v2", type=str, default=get("video2", cls.video2)) + add("--rotation1", "-r1", type=str, default=get("rotation1", copy.deepcopy(DEFAULT_ROTATION1))) + add("--rotation2", "-r2", type=str, default=get("rotation2", copy.deepcopy(DEFAULT_ROTATION2))) + add("--sensor-config1", type=str, default=get("sensor_config1", cls.sensor_config1)) + add("--sensor-config2", type=str, default=get("sensor_config2", cls.sensor_config2)) + add("--sensor-fps", type=float, default=get("sensor_fps", cls.sensor_fps)) + add("--motion-threshold", type=float, default=get("motion_threshold", cls.motion_threshold)) + add("--sensor-cpu", action="store_true", default=get("sensor_cpu", cls.sensor_cpu)) + + add("--port", type=str, default=get("port", cls.port)) + add("--slave-id", type=int, default=get("slave_id", cls.slave_id)) + add("--baudrate", type=int, default=get("baudrate", cls.baudrate)) + add("--serial-timeout", type=float, default=get("serial_timeout", cls.serial_timeout)) + + add("--open-pos", type=int, default=get("open_pos", cls.open_pos)) + add("--close-pos", type=int, default=get("close_pos", cls.close_pos)) + 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)) + add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_force)) + add("--hold-force", type=int, default=get("hold_force", cls.hold_force)) + add("--force-min", type=int, default=get("force_min", cls.force_min)) + add("--force-max", type=int, default=get("force_max", cls.force_max)) + + add("--control-hz", type=float, default=get("control_hz", cls.control_hz)) + 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("--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("--open-wait-stable-samples", type=int, default=get("open_wait_stable_samples", cls.open_wait_stable_samples)) + add("--open-wait-stable-range-n", type=float, default=get("open_wait_stable_range_n", cls.open_wait_stable_range_n)) + add("--open-wait-stable-trend-n", type=float, default=get("open_wait_stable_trend_n", cls.open_wait_stable_trend_n)) + 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") + add("--close-timeout-seconds", type=float, default=get("close_timeout_seconds", cls.close_timeout_seconds)) + add("--closing-command-interval", type=float, default=get("closing_command_interval", cls.closing_command_interval)) + 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("--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)) + add("--adaptive-shear-full-force", type=float, default=get("adaptive_shear_full_force", cls.adaptive_shear_full_force)) + add("--adaptive-force-min", type=int, default=get("adaptive_force_min", cls.adaptive_force_min)) + add("--adaptive-force-max", type=int, default=get("adaptive_force_max", cls.adaptive_force_max)) + 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-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("--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)) + 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)) + add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end)) + add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run)) + + args = parser.parse_args(argv) + values = vars(args) + values["initial_force"] = values["open_force"] + values["rotation1"] = normalize_rotation_config(values["rotation1"], DEFAULT_ROTATION1) + values["rotation2"] = normalize_rotation_config(values["rotation2"], DEFAULT_ROTATION2) + values["normal_force_calibration"] = copy.deepcopy( + config_data.get("normal_force_calibration", DEFAULT_NORMAL_FORCE_CALIBRATION) + ) + values["shear_force_calibration"] = copy.deepcopy( + config_data.get("shear_force_calibration", DEFAULT_SHEAR_FORCE_CALIBRATION) + ) + return cls(**values) diff --git a/gripper/visualizer.py b/gripper/visualizer.py new file mode 100644 index 0000000..b0480da --- /dev/null +++ b/gripper/visualizer.py @@ -0,0 +1,877 @@ +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 QEasingCurve, QPropertyAnimation, Qt, QTimer + from PyQt5.QtGui import QFont, QImage, QPixmap + from PyQt5.QtWidgets import ( + QApplication, + QFrame, + QGraphicsOpacityEffect, + QGridLayout, + QHBoxLayout, + QLabel, + QMainWindow, + QPushButton, + QSizePolicy, + QSplitter, + QVBoxLayout, + QWidget, + ) +except Exception as exc: + _QT_IMPORT_ERROR = exc + Qt = QTimer = QPropertyAnimation = QEasingCurve = QFont = QImage = QPixmap = QApplication = None + QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = QSplitter = None + QVBoxLayout = QWidget = None + QFrame = QGraphicsOpacityEffect = 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 +PLOT_FORCE_Y_MIN_N = 0.0 +PLOT_FORCE_Y_MAX_N = 3.0 +PLOT_SHEAR_Y_MIN_N = -3.0 +PLOT_SHEAR_Y_MAX_N = 3.0 +FORCE_DISPLAY_ZERO_THRESHOLD_N = 0.05 +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 apply_visual_transform(img, transform): + if img is None: + return None + angle = int(transform.get("rotation_angle", 0)) % 360 + if angle == 90: + img = cv2.rotate(img, cv2.ROTATE_90_CLOCKWISE) + elif angle == 180: + img = cv2.rotate(img, cv2.ROTATE_180) + elif angle == 270: + img = cv2.rotate(img, cv2.ROTATE_90_COUNTERCLOCKWISE) + + flip_x = bool(transform.get("flip_x")) + flip_y = bool(transform.get("flip_y")) + if flip_x and flip_y: + img = cv2.flip(img, -1) + elif flip_x: + img = cv2.flip(img, 1) + elif flip_y: + img = cv2.flip(img, 0) + return img + + +def force_display_value(value): + value = float(value) + if abs(value) < FORCE_DISPLAY_ZERO_THRESHOLD_N: + return 0.0 + return value + + +def fmt(value, unit="", width=0, precision=3): + if value is None: + text = "--" + else: + value = force_display_value(value) + text = "0" if value == 0.0 else f"{value:.{precision}f}" + if width: + text = text.rjust(width) + return f"{text}{unit}" + + +STATE_TEXT = { + "open_wait": "等待触发", + "closing": "闭合中", + "gripping": "夹持加力", + "hold_check": "夹持检测", + "open_recover": "张开恢复", + "manual_close": "手动闭合", + "manual_hold": "手动保持", + "error": "错误", +} + +ACTION_TEXT = { + "wait": "等待", + "waiting-for-sensor-samples": "等待传感器数据", + "open-wait-stabilizing": "等待稳定基准", + "open-wait-armed": "已就绪", + "side-trigger-close": "侧边触发闭合", + "closing-continue": "继续闭合", + "closing-to-grip": "闭合寻找物体", + "object-detected-low-force": "检测到物体", + "close-timeout-open": "闭合超时张开", + "grip-ramp-up": "夹持加力", + "grip-hold": "夹持保持", + "hold-reference-armed": "记录夹持基准", + "adaptive-grip-ramp": "自适应加力", + "adaptive-grip-wait": "等待自适应加力", + "release-arm-delay": "释放检测延时", + "release-reference-armed": "释放检测就绪", + "hold-check": "保持检测", + "force-change-release-open": "力变化张开", + "open-recover-position": "张开位置恢复", + "open-recover-position-stabilizing": "张开位置稳定中", + "open-ready-position": "张开到位", + "open-recover-quiet": "力稳定恢复", + "open-recover-stabilizing": "恢复稳定中", + "open-recover-wait-force-clear": "等待力恢复", + "open-ready": "恢复完成", + "manual-open": "手动张开", + "manual-close": "手动闭合", + "manual-hold": "手动保持", + "manual-release": "手动释放", +} + + +def label_with_cn(value, mapping): + value = str(value or "--") + cn = mapping.get(value) + if cn is None: + for prefix, text in mapping.items(): + if value.startswith(f"{prefix} "): + cn = text + break + if cn is None: + return value + return f"{value}（{cn}）" + + +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; +} +QFrame#StatusPanel { + background-color: #101010; + border: 1px solid #303030; + border-radius: 4px; +} +QLabel#StatusMain { + color: #00e5ff; + font-family: "Cascadia Code", "Consolas"; + font-size: 17px; + font-weight: 600; +} +QLabel#StatusSub { + color: #9a9a9a; + font-family: "Cascadia Code", "Consolas"; + font-size: 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: #171717; + color: #d8d8d8; + border: 1px solid #363636; + border-radius: 4px; + padding: 7px 16px; + font-size: 13px; +} +QPushButton:hover { + background-color: #242424; + color: #ffffff; + border: 1px solid #5a5a5a; +} +QPushButton:pressed { + background-color: #0f0f0f; + border: 1px solid #8a8a8a; + padding-top: 8px; + padding-bottom: 6px; +} +QPushButton:disabled { + background-color: #101010; + color: #606060; + border: 1px solid #242424; +} +QPushButton#StartButton { + background-color: #171717; + color: #d8d8d8; + border: 1px solid #363636; +} +QPushButton#StopButton { + background-color: #171717; + color: #d8d8d8; + border: 1px solid #363636; +} +QPushButton#RestartButton { + background-color: #171717; + color: #d8d8d8; + border: 1px solid #363636; +} +QPushButton#ManualButton { + background-color: #171717; + color: #d8d8d8; + border: 1px solid #363636; +} +QPushButton#HoldButton { + background-color: #171717; + color: #d8d8d8; + border: 1px solid #363636; +} +QSplitter::handle { + background-color: #151515; +} +QSplitter::handle:hover { + background-color: #2a2a2a; +} +QSplitter::handle:horizontal { + width: 8px; +} +QSplitter::handle:vertical { + height: 8px; +} +""" + + +if QPushButton is not None: + class AnimatedButton(QPushButton): + def __init__(self, text=""): + super().__init__(text) + self._opacity = QGraphicsOpacityEffect(self) + self._opacity.setOpacity(1.0) + self.setGraphicsEffect(self._opacity) + self._animation = QPropertyAnimation(self._opacity, b"opacity", self) + self._animation.setEasingCurve(QEasingCurve.OutCubic) + + def _animate_opacity(self, target, duration): + self._animation.stop() + self._animation.setDuration(duration) + self._animation.setStartValue(self._opacity.opacity()) + self._animation.setEndValue(target) + self._animation.start() + + def mousePressEvent(self, event): + if self.isEnabled(): + self._animate_opacity(0.72, 70) + super().mousePressEvent(event) + + def mouseReleaseEvent(self, event): + if self.isEnabled(): + self._animate_opacity(1.0, 140) + super().mouseReleaseEvent(event) +else: + AnimatedButton = None + + +class MetricBox(QFrame): + def __init__(self, title, value="--"): + super().__init__() + self.setObjectName("Panel") + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 8, 10, 8) + layout.setSpacing(4) + self.title_label = QLabel(title) + self.title_label.setObjectName("MetricLabel") + self.value_label = QLabel(value) + self.value_label.setObjectName("MetricValue") + layout.addWidget(self.title_label) + layout.addWidget(self.value_label) + + def set_value(self, value, sub=None): + self.value_label.setText(value) + + +class ImageBox(QFrame): + def __init__(self, title, on_rotate=None, on_flip_x=None, on_flip_y=None, on_reset=None): + super().__init__() + self.title = title + 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) + controls = QHBoxLayout() + controls.setSpacing(6) + self.rotate_button = AnimatedButton("旋转") + self.flip_x_button = AnimatedButton("水平") + self.flip_y_button = AnimatedButton("垂直") + self.reset_button = AnimatedButton("重置") + for button, callback in ( + (self.rotate_button, on_rotate), + (self.flip_x_button, on_flip_x), + (self.flip_y_button, on_flip_y), + (self.reset_button, on_reset), + ): + button.setObjectName("ManualButton") + if callback is not None: + button.clicked.connect(callback) + controls.addWidget(button) + layout.addLayout(controls) + + def set_image(self, img): + self.image_label.setPixmap(cv2_to_pixmap(img)) + + def set_transform_label(self, transform): + angle = int(transform.get("rotation_angle", 0)) % 360 + flip_x = "X1" if transform.get("flip_x") else "X0" + flip_y = "Y1" if transform.get("flip_y") else "Y0" + self.title_label.setText(f"{self.title} {angle}deg {flip_x} {flip_y}") + + +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.restart_pending = False + self.button_state = "idle" + self.start_time = time.time() + self.force_history = [] + self.argv = argv + self.visual_transforms = { + "left": {"rotation_angle": 0, "flip_x": False, "flip_y": False}, + "right": {"rotation_angle": 0, "flip_x": False, "flip_y": False}, + } + + 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("Orisys 夹爪") + title.setObjectName("Title") + self.status_panel = QFrame() + self.status_panel.setObjectName("StatusPanel") + status_layout = QVBoxLayout(self.status_panel) + status_layout.setContentsMargins(10, 6, 10, 6) + status_layout.setSpacing(1) + self.status_chip = QLabel("state=--") + self.status_chip.setObjectName("StatusMain") + self.action_label = QLabel("action=--") + self.action_label.setObjectName("StatusSub") + status_layout.addWidget(self.status_chip) + status_layout.addWidget(self.action_label) + self.start_button = AnimatedButton("启动") + self.start_button.setObjectName("StartButton") + self.start_button.clicked.connect(self.start_control) + self.stop_button = AnimatedButton("停止") + self.stop_button.setObjectName("StopButton") + self.stop_button.clicked.connect(self.stop_control) + self.stop_button.setEnabled(False) + self.restart_button = AnimatedButton("重启") + self.restart_button.setObjectName("RestartButton") + self.restart_button.clicked.connect(self.restart_control) + self.manual_open_button = AnimatedButton("夹爪张开") + self.manual_open_button.setObjectName("ManualButton") + self.manual_open_button.clicked.connect(lambda: self.send_manual_command("open")) + self.manual_close_button = AnimatedButton("夹爪闭合") + self.manual_close_button.setObjectName("ManualButton") + self.manual_close_button.clicked.connect(lambda: self.send_manual_command("close")) + self.manual_hold_button = AnimatedButton("保持") + self.manual_hold_button.setObjectName("HoldButton") + self.manual_hold_button.clicked.connect(self.toggle_manual_hold) + self._set_button_state("idle") + top.addWidget(title) + top.addWidget(self.status_panel) + top.addStretch(1) + top.addWidget(self.start_button) + top.addWidget(self.stop_button) + top.addWidget(self.restart_button) + top.addWidget(self.manual_open_button) + top.addWidget(self.manual_close_button) + top.addWidget(self.manual_hold_button) + outer.addLayout(top) + + main_splitter = QSplitter(Qt.Horizontal) + main_splitter.setChildrenCollapsible(False) + outer.addWidget(main_splitter, 1) + + flow_splitter = QSplitter(Qt.Vertical) + flow_splitter.setChildrenCollapsible(False) + self.left_flow = ImageBox( + "左侧光流", + on_rotate=lambda: self.adjust_visual_transform("left", "rotate"), + on_flip_x=lambda: self.adjust_visual_transform("left", "flip_x"), + on_flip_y=lambda: self.adjust_visual_transform("left", "flip_y"), + on_reset=lambda: self.adjust_visual_transform("left", "reset"), + ) + self.right_flow = ImageBox( + "右侧光流", + on_rotate=lambda: self.adjust_visual_transform("right", "rotate"), + on_flip_x=lambda: self.adjust_visual_transform("right", "flip_x"), + on_flip_y=lambda: self.adjust_visual_transform("right", "flip_y"), + on_reset=lambda: self.adjust_visual_transform("right", "reset"), + ) + self.left_flow.set_transform_label(self.visual_transforms["left"]) + self.right_flow.set_transform_label(self.visual_transforms["right"]) + flow_splitter.addWidget(self.left_flow) + flow_splitter.addWidget(self.right_flow) + flow_splitter.setStretchFactor(0, 1) + flow_splitter.setStretchFactor(1, 1) + flow_splitter.setSizes([360, 360]) + main_splitter.addWidget(flow_splitter) + + side_widget = QWidget() + side = QVBoxLayout(side_widget) + side.setContentsMargins(0, 0, 0, 0) + side.setSpacing(8) + main_splitter.addWidget(side_widget) + main_splitter.setStretchFactor(0, 2) + main_splitter.setStretchFactor(1, 4) + main_splitter.setSizes([520, 980]) + + metrics_grid = QGridLayout() + metrics_grid.setSpacing(8) + side.addLayout(metrics_grid) + self.left_force = MetricBox("左法向力", "--") + self.right_force = MetricBox("右法向力", "--") + self.left_shear_box = MetricBox("左切向力", "--") + self.right_shear_box = MetricBox("右切向力", "--") + self.force_pct_box = MetricBox("夹爪当前力比例（%）", "--") + self.target_force_box = MetricBox("夹爪目标力比例（%）", "--") + self.speed_box = MetricBox("速度", "--") + self.trigger_box = MetricBox("是否触发/系统是否允许触发", "--") + self.contact_box = MetricBox("接触", "--") + self.release_box = MetricBox("释放检测", "--") + boxes = [ + self.left_force, + self.right_force, + self.left_shear_box, + self.right_shear_box, + self.force_pct_box, + self.target_force_box, + self.speed_box, + self.trigger_box, + self.contact_box, + self.release_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) + + def _configure_plot(self): + self.plots = [] + self.curves = {} + plot_specs = [ + ("left_normal", "左法向力", "#00E5FF", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N), + ("right_normal", "右法向力", "#ffb000", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N), + ("left_shear_x", "左切向 X", "#61d394", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), + ("left_shear_y", "左切向 Y", "#f07178", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), + ("right_shear_x", "右切向 X", "#8ab4f8", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), + ("right_shear_y", "右切向 Y", "#d19a66", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N), + ] + for idx, (key, title, color, y_min, y_max) in enumerate(plot_specs): + row = idx // 2 + col = idx % 2 + plot = self.plot_widget.addPlot(row=row, col=col) + plot.setTitle(title, color="#999999", size="11pt") + plot.titleLabel.setFont(QFont("SimHei", 11)) + plot.showAxis("top", False) + plot.showAxis("right", False) + plot.showGrid(x=False, y=True, alpha=0.12) + plot.setLabel("bottom", "时间", units="s") + plot.setLabel("left", "力", units="N") + plot.getAxis("bottom").setTickSpacing(major=1.0, minor=1.0) + plot.setYRange(y_min, y_max, padding=0) + plot.setLimits(yMin=y_min, yMax=y_max) + plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False) + self.plots.append(plot) + self.curves[key] = plot.plot(pen=pg.mkPen(color, width=2)) + + def _set_button_state(self, state): + labels = { + "idle": ("启动", "停止", "重启"), + "starting": ("启动中", "停止", "重启"), + "running": ("启动", "停止", "重启"), + "stopping": ("启动", "停止中", "重启"), + "restarting": ("启动", "停止", "重启中"), + } + enabled = { + "idle": (True, False, True), + "starting": (False, True, False), + "running": (False, True, True), + "stopping": (False, False, False), + "restarting": (False, False, False), + } + self.button_state = state + start_text, stop_text, restart_text = labels[state] + start_enabled, stop_enabled, restart_enabled = enabled[state] + self.start_button.setText(start_text) + self.stop_button.setText(stop_text) + self.restart_button.setText(restart_text) + self.start_button.setEnabled(start_enabled) + self.stop_button.setEnabled(stop_enabled) + self.restart_button.setEnabled(restart_enabled) + manual_enabled = state == "running" + self.manual_open_button.setEnabled(manual_enabled) + self.manual_close_button.setEnabled(manual_enabled) + self.manual_hold_button.setEnabled(manual_enabled) + + def adjust_visual_transform(self, side, action): + transform = self.visual_transforms[side] + if action == "rotate": + transform["rotation_angle"] = (int(transform["rotation_angle"]) + 90) % 360 + elif action == "flip_x": + transform["flip_x"] = not bool(transform["flip_x"]) + elif action == "flip_y": + transform["flip_y"] = not bool(transform["flip_y"]) + elif action == "reset": + transform.update({"rotation_angle": 0, "flip_x": False, "flip_y": False}) + + if side == "left": + self.left_flow.set_transform_label(transform) + else: + self.right_flow.set_transform_label(transform) + + def send_manual_command(self, command): + if self.controller is None or self.control_thread is None or not self.control_thread.is_alive(): + return + try: + self.controller.request_manual_command(command) + except Exception as exc: + print(f"manual command failed: {exc}") + + def toggle_manual_hold(self): + command = "release" if self.manual_hold_button.text() == "释放" else "hold" + self.send_manual_command(command) + + def start_control(self, from_restart=False): + if self.control_thread is not None and self.control_thread.is_alive(): + return + self.force_history.clear() + self.start_time = time.time() + self._set_button_state("restarting" if from_restart else "starting") + self.controller_finished = False + + 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._set_button_state("stopping") + + def restart_control(self): + if self.control_thread is not None and self.control_thread.is_alive(): + self.restart_pending = True + self._set_button_state("restarting") + if self.controller is not None: + self.controller.stop() + return + self.restart_pending = False + self.start_control(from_restart=True) + + 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() + if ( + self.control_thread is not None + and self.control_thread.is_alive() + and not self.restart_pending + and self.button_state in ("starting", "restarting") + ): + self._set_button_state("running") + + 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={label_with_cn(state, STATE_TEXT)}") + self.action_label.setText(f"action={label_with_cn(action, ACTION_TEXT)}") + self.manual_hold_button.setText( + "释放" if bool(status.get("manual_hold_active")) else "保持" + ) + + self.left_force.set_value( + fmt(status.get("left_normal"), unit), + ) + self.right_force.set_value( + fmt(status.get("right_normal"), unit), + ) + self.left_shear_box.set_value( + fmt(status.get("left_shear"), shear_unit), + ) + self.right_shear_box.set_value( + fmt(status.get("right_shear"), shear_unit), + ) + self.force_pct_box.set_value( + str(status.get("force_pct", "--")), + ) + self.target_force_box.set_value( + str(status.get("adaptive_target_force", "--")), + ) + self.speed_box.set_value( + str(status.get("speed_pct", "--")), + ) + self.trigger_box.set_value( + f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}", + ) + self.contact_box.set_value( + str(int(bool(status.get("grip_contact")))), + ) + self.release_box.set_value( + str(int(bool(status.get("release_armed")))), + ) + + left_sample = status.get("left_sample") or {} + right_sample = status.get("right_sample") or {} + self.left_flow.set_image( + apply_visual_transform(left_sample.get("flow_view"), self.visual_transforms["left"]) + ) + self.right_flow.set_image( + apply_visual_transform(right_sample.get("flow_view"), self.visual_transforms["right"]) + ) + + elapsed = time.time() - self.start_time + self.force_history.append( + ( + elapsed, + force_display_value(status.get("left_normal", 0.0) or 0.0), + force_display_value(status.get("right_normal", 0.0) or 0.0), + force_display_value(status.get("left_shear_x", 0.0) or 0.0), + force_display_value(status.get("left_shear_y", 0.0) or 0.0), + force_display_value(status.get("right_shear_x", 0.0) or 0.0), + force_display_value(status.get("right_shear_y", 0.0) or 0.0), + ) + ) + if len(self.force_history) > PLOT_POINTS: + self.force_history = self.force_history[-PLOT_POINTS:] + self._update_plot() + + def _update_plot(self): + if not self.force_history: + return + data = np.asarray(self.force_history, dtype=float) + x = data[:, 0] + keys = [ + "left_normal", + "right_normal", + "left_shear_x", + "left_shear_y", + "right_shear_x", + "right_shear_y", + ] + for idx, key in enumerate(keys, start=1): + self.curves[key].setData(x, data[:, idx]) + if len(x) >= 2: + x_max = max(6.0, float(np.ceil(x[-1]))) + x_min = max(0.0, x_max - 6.0) + for plot in self.plots: + plot.setXRange(x_min, x_max, padding=0) + + def _update_thread_buttons(self): + if not self.controller_finished: + return + self.controller_finished = False + if self.control_thread is not None: + self.control_thread.join(timeout=0.1) + self.control_thread = None + if self.restart_pending: + self.restart_pending = False + self.start_control(from_restart=True) + return + self._set_button_state("idle") + + 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.restart_pending = False + 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/sdk/__pycache__/changingtek_p_rtu_Servo.cpython-311.pyc b/sdk/__pycache__/changingtek_p_rtu_Servo.cpython-311.pyc new file mode 100644 index 0000000..c77b8be Binary files /dev/null and b/sdk/__pycache__/changingtek_p_rtu_Servo.cpython-311.pyc differ diff --git a/sdk/changingtek_p_rtu_Servo.py b/sdk/changingtek_p_rtu_Servo.py new file mode 100644 index 0000000..75a8055 --- /dev/null +++ b/sdk/changingtek_p_rtu_Servo.py @@ -0,0 +1,284 @@ +""" +CTAG2F90D（Servo）控制器SDK (基于Modbus RTU的RS-485通信) +--------------------------------------------- +一个轻量级的Python SDK，用于通过minimalmodbus库控制电机， +具有线程安全、清晰的寄存器访问方式和实时数据监控功能。 + +依赖安装: + pip install minimalmodbus pyserial + +作者: 知行机器人 +""" + +import time +import threading +import minimalmodbus +import serial + +# ----------------------------- +# 寄存器映射 (保持寄存器，功能码0x03/0x06) +# ----------------------------- +# 写入寄存器 (目标值/控制) +REG_TARGET_POS_HIGH = 0x0102 # 目标位置（高16位） +REG_TARGET_POS_LOW = 0x0103 # 目标位置（低16位） +REG_TARGET_SPEED = 0x0104 # 目标速度 +REG_TARGET_FORCE = 0x0105 # 目标力/力矩 +REG_TARGET_ACCELERATION = 0x0106 # 目标加速度 +REG_TARGET_DECELERATION = 0x0107 # 目标减速度 +REG_MOTION_TRIGGER = 0x0108 # 运动触发（0: 空闲, 1: 触发） + +# 读取寄存器 (反馈/状态) +REG_REAL_POS_HIGH = 0x0418 # 实时位置（高16位） +REG_REAL_POS_LOW = 0x0419 # 实时位置（低16位） +REG_REAL_SPEED = 0x041A # 实时速度反馈 +REG_REAL_CURRENT = 0x041B # 实时电流反馈 + + +class MotorController: + def __init__(self, port: str, slave_id: int = 1, baudrate: int = 115200, timeout: float = 1.0): + """初始化与电机控制器的Modbus RTU通信 + + 参数: + port: 串口端口（例如，Windows上为'COM3'，Linux上为'/dev/ttyUSB0'） + slave_id: Modbus从机地址（默认: 1） + baudrate: 串口波特率（默认: 115200） + timeout: 读写超时时间（秒，默认: 1.0） + """ + self.instrument = minimalmodbus.Instrument(port, slave_id) + self.instrument.serial.baudrate = baudrate + self.instrument.serial.bytesize = 8 + self.instrument.serial.parity = serial.PARITY_NONE + self.instrument.serial.stopbits = 1 + self.instrument.serial.timeout = timeout + self.instrument.mode = minimalmodbus.MODE_RTU + self._lock = threading.Lock() # 线程锁，确保串口操作线程安全 + self._monitor_running = False # 监控线程运行标志 + self._monitor_thread = None # 监控线程对象 + + # ----------------------------- + # 低级寄存器操作工具方法 + # ----------------------------- + def _write_register(self, addr: int, value: int) -> None: + """写入单个16位保持寄存器（Modbus功能码0x06） + + 参数: + addr: 寄存器地址 + value: 要写入的值 + """ + with self._lock: + self.instrument.write_register(addr, value, functioncode=6) + + def _read_register(self, addr: int) -> int: + """读取单个16位保持寄存器（Modbus功能码0x03） + + 参数: + addr: 寄存器地址 + + 返回: + 寄存器的值 + """ + with self._lock: + return self.instrument.read_register(addr, functioncode=3) + + def _write_registers(self, addr: int, values: list) -> None: + """写入多个16位保持寄存器（Modbus功能码0x10） + + 参数: + addr: 起始寄存器地址 + values: 要写入的值列表 + """ + with self._lock: + self.instrument.write_registers(addr, values) + + def _read_registers(self, addr: int, count: int) -> list: + """读取多个16位保持寄存器（Modbus功能码0x03） + + 参数: + addr: 起始寄存器地址 + count: 要读取的寄存器数量 + + 返回: + 寄存器值的列表 + """ + with self._lock: + return self.instrument.read_registers(addr, count, functioncode=3) + + # ----------------------------- + # 目标值/控制写入操作 + # ----------------------------- + def set_target_position(self, position: int) -> None: + """设置目标位置（组合高低16位寄存器） + + 参数: + position: 目标位置值 + """ + hi = (position >> 16) & 0xFFFF # 提取高16位 + lo = position & 0xFFFF # 提取低16位 + self._write_registers(REG_TARGET_POS_HIGH, [hi, lo]) + + def set_target_speed(self, speed: int) -> None: + """设置目标速度 + + 参数: + speed: 目标速度值 + """ + self._write_register(REG_TARGET_SPEED, speed) + + def set_target_force(self, force: int) -> None: + """设置目标力/力矩 + + 参数: + force: 目标力/力矩值 + """ + self._write_register(REG_TARGET_FORCE, force) + + def set_target_acceleration(self, acceleration: int) -> None: + """设置目标加速度 + + 参数: + acceleration: 目标加速度值 + """ + self._write_register(REG_TARGET_ACCELERATION, acceleration) + + def set_target_deceleration(self, deceleration: int) -> None: + """设置目标减速度 + + 参数: + deceleration: 目标减速度值 + """ + self._write_register(REG_TARGET_DECELERATION, deceleration) + + def trigger_motion(self) -> None: + """使用已配置的目标参数触发运动""" + self._write_register(REG_MOTION_TRIGGER, 1) + + def temp_move(self, position_mm: int, speed_pct: int, force_pct: int, + accel: int, decel: int, trigger: bool = True) -> None: + """临时运动控制方法，一次性设置所有运动参数并可选触发运动 + + 参数: + position_mm: 目标位置(毫米) + speed_pct: 速度百分比 + force_pct: 力/力矩百分比 + accel: 加速度值 + decel: 减速度值 + trigger: 是否立即触发运动，默认为True + """ + self.set_target_position(position_mm) + self.set_target_speed(speed_pct) + self.set_target_force(force_pct) + self.set_target_acceleration(accel) + self.set_target_deceleration(decel) + + if trigger: + self.trigger_motion() + print(f"已触发运动到位置 {position_mm}，速度 {speed_pct}%，力 {force_pct}%") + + # ----------------------------- + # 反馈/状态读取操作 + # ----------------------------- + def read_real_position(self) -> int: + """读取实时位置（组合高低16位寄存器） + + 返回: + 组合后的32位实时位置（处理有符号值） + """ + regs = self._read_registers(REG_REAL_POS_HIGH, 2) + hi, lo = regs[0], regs[1] + combined = (hi << 16) | lo + # 处理32位有符号整数 + if combined & 0x80000000: # 最高位为1表示负数 + combined = combined - 0x100000000 + return combined + + def read_real_speed(self) -> int: + """读取实时速度反馈 + + 返回: + 实时速度值 + """ + return self._read_register(REG_REAL_SPEED) + + def read_real_current(self) -> int: + """读取实时电流反馈 + + 返回: + 实时电流值 + """ + return self._read_register(REG_REAL_CURRENT) + + # ----------------------------- + # 实时监控线程 + # ----------------------------- + def _monitor_loop(self, interval: float = 0.5) -> None: + """实时数据监控的后台循环""" + while self._monitor_running: + try: + pos = self.read_real_position() + speed = self.read_real_speed() + current = self.read_real_current() + print(f"[实时数据] 位置: {pos:6d}, 速度: {speed:3d}, 电流: {current:3d}") + except Exception as e: + print(f"监控错误: {e}") + time.sleep(interval) + + def start_monitoring(self, interval: float = 0.01) -> None: + """启动实时数据监控线程 + + 参数: + interval: 监控间隔（秒，默认: 0.01） + """ + if not self._monitor_running: + self._monitor_running = True + self._monitor_thread = threading.Thread( + target=self._monitor_loop, + args=(interval,) + ) + self._monitor_thread.daemon = True # 守护线程，主程序退出时自动结束 + self._monitor_thread.start() + print("实时监控已启动。") + + def stop_monitoring(self) -> None: + """停止实时数据监控线程""" + self._monitor_running = False + if self._monitor_thread: + self._monitor_thread.join(timeout=1.0) # 等待线程结束 + print("实时监控已停止。") + + +if __name__ == "__main__": + # 示例用法（根据实际设备调整端口/从机ID） + PORT = "COM5" + SLAVE_ID = 1 + sdk = MotorController(PORT, SLAVE_ID, baudrate=115200, timeout=1.0) + + try: + # 启动实时监控 + sdk.start_monitoring(interval=0.5) + + # 循环控制：5次往复运动 + # 假设 9000 为闭合位置，0 为张开位置 + # 最后一次循环结束后，确保处于张开位置 (0) + for i in range(5): + print(f"\n--- 第 {i+1}/5 次循环 ---") + + # 阶段1: 运动到位置 9000 (闭合?) + sdk.temp_move(position_mm=9000, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True) + time.sleep(5) # 等待运动完成 + + # 阶段2: 运动到位置 0 (张开?) + sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True) + time.sleep(5) # 等待运动完成 + + print("\n循环结束，确认最终状态为张开 (位置 0)...") + # 再次发送指令确保最终状态为 0 (虽然循环最后一步已经是 0，但这保证了"最后要张开"的要求) + sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True) + time.sleep(1) + + except KeyboardInterrupt: + print("\n程序被用户中断。") + except Exception as e: + print(f"执行过程中出错: {e}") + finally: + sdk.stop_monitoring() + print("程序已完成。") diff --git a/start_gripper.bat b/start_gripper.bat new file mode 100644 index 0000000..841313a --- /dev/null +++ b/start_gripper.bat @@ -0,0 +1,11 @@ +@echo off +setlocal +cd /d "%~dp0" +call conda activate py311 +if errorlevel 1 ( + echo Failed to activate conda environment: py311 + pause + exit /b 1 +) +python -m gripper.visualizer %* +pause