""" JE-Skin DevKit — Python gRPC Sensor Server 提供两个服务： 1. SensorPush (streaming) — 接收实时传感器帧 2. ExportProcessor (unary) — 处理导出的 CSV 文件：梯度过滤、xlsx 转换 安装依赖： pip install grpcio grpcio-tools openpyxl 生成 gRPC 代码： python -m grpc_tools.protoc -I../src-tauri/proto --python_out=. --grpc_python_out=. ../src-tauri/proto/sensor_stream.proto 启动： python sensor_server.py [--port 50051] """ from __future__ import annotations import argparse import csv import os import signal import statistics import sys import time from concurrent import futures from pathlib import Path import grpc import sensor_stream_pb2 import sensor_stream_pb2_grpc # ── 梯度过滤逻辑（来自用户的 main.py） ───────────────────────── def load_rows(path: Path) -> list[list[str]]: with path.open("r", encoding="utf-8-sig", newline="") as f: return [row for row in csv.reader(f) if row] def row_sum(row: list[str]) -> float: return sum(float(v) for v in row[1:] if v.strip()) def find_threshold(sum_values: list[float]) -> float: if len(sum_values) < 2: raise ValueError("At least two rows are required.") sorted_v = sorted(sum_values) idx = max( range(len(sorted_v) - 1), key=lambda i: sorted_v[i + 1] - sorted_v[i], ) return (sorted_v[idx] + sorted_v[idx + 1]) / 2.0 def extract_press_groups( rows: list[list[str]], sum_values: list[float], threshold: float ) -> tuple[list[list[str]], list[float]]: filtered: list[list[str]] = [] group_means: list[float] = [] current_group: list[float] = [] for row, total in zip(rows, sum_values): if total >= threshold: filtered.append(row) current_group.append(total) continue if current_group: group_means.append(statistics.fmean(current_group)) current_group = [] if current_group: group_means.append(statistics.fmean(current_group)) return filtered, group_means def write_csv(path: Path, rows: list[list[str]]) -> Path: out = path.with_name(f"{path.stem}_filtered.csv") with out.open("w", encoding="utf-8-sig", newline="") as f: csv.writer(f).writerows(rows) return out def write_xlsx(path: Path, rows: list[list[str]], stats: dict) -> Path: """将过滤后的数据和统计信息写入 xlsx""" try: import openpyxl except ImportError: raise RuntimeError("openpyxl is required for xlsx output. Install it with: pip install openpyxl") from openpyxl.styles import Font, PatternFill, Alignment, Border, Side wb = openpyxl.Workbook() # Sheet 1: 过滤后的数据 ws_data = wb.active ws_data.title = "Filtered Data" for row in rows: ws_data.append([float(c) if c.strip().replace(".", "").replace("-", "").isdigit() else c for c in row]) # Sheet 2: 统计信息 ws_stats = wb.create_sheet("Statistics") header_font = Font(bold=True, size=11) header_fill = PatternFill(start_color="E0E0E0", end_color="E0E0E0", fill_type="solid") ws_stats.append(["Parameter", "Value"]) ws_stats["A1"].font = header_font ws_stats["A1"].fill = header_fill ws_stats["B1"].font = header_font ws_stats["B1"].fill = header_fill stats_rows = [ ("Source File", stats.get("source_file", "")), ("Total Rows", stats.get("rows_total", 0)), ("Filtered Rows", stats.get("rows_kept", 0)), ("Groups Used", stats.get("groups_used", 0)), ("Mean Value", f"{stats.get('mean_value', 0):.3f}"), ("Threshold", f"{stats.get('threshold', 0):.3f}"), ("Process Time", stats.get("process_time", "")), ] for label, value in stats_rows: ws_stats.append([label, value]) ws_stats.column_dimensions["A"].width = 18 ws_stats.column_dimensions["B"].width = 30 out = path.with_name(f"{path.stem}_filtered.xlsx") wb.save(str(out)) return out def process_csv(csv_path: str, save_as_xlsx: bool) -> dict: """执行梯度过滤，返回结果统计""" path = Path(csv_path) if not path.is_file(): raise FileNotFoundError(f"CSV file not found: {csv_path}") rows = load_rows(path) if not rows: raise ValueError("CSV file is empty.") sum_values = [row_sum(r) for r in rows] threshold = find_threshold(sum_values) filtered_rows, group_means = extract_press_groups(rows, sum_values, threshold) if not filtered_rows: raise ValueError("No large press-down data was found.") overall_mean = statistics.fmean(group_means) stats = { "source_file": path.name, "rows_total": len(rows), "rows_kept": len(filtered_rows), "groups_used": len(group_means), "mean_value": overall_mean, "threshold": threshold, "process_time": time.strftime("%Y-%m-%d %H:%M:%S"), } if save_as_xlsx: output_path = write_xlsx(path, filtered_rows, stats) # 删除源 CSV try: path.unlink() except OSError: pass else: output_path = write_csv(path, filtered_rows) # 用过滤后的文件替换源文件 try: path.unlink() output_path.rename(path) output_path = path except OSError: pass # 追加一行到汇总 xlsx _append_analysis_log(csv_path, stats) return { "ok": True, "output_path": str(output_path), "groups_used": len(group_means), "mean_value": overall_mean, "threshold": threshold, "rows_total": len(rows), "rows_kept": len(filtered_rows), "message": "OK", } def _append_analysis_log(source_csv: str, stats: dict): """将处理结果追加到 devkit_analysis_results.xlsx""" try: import openpyxl except ImportError: return # openpyxl 不可用时跳过 log_path = Path(source_csv).parent / "devkit_analysis_results.xlsx" if log_path.exists(): wb = openpyxl.load_workbook(str(log_path)) ws = wb.active else: wb = openpyxl.Workbook() ws = wb.active ws.title = "Analysis Log" ws.append(["Time", "Source File", "Total Rows", "Kept Rows", "Groups", "Mean Value", "Threshold", "Output File"]) ws.append([ stats.get("process_time", ""), stats.get("source_file", ""), stats.get("rows_total", 0), stats.get("rows_kept", 0), stats.get("groups_used", 0), round(stats.get("mean_value", 0), 3), round(stats.get("threshold", 0), 3), f"{Path(stats.get('source_file', '')).stem}_filtered", ]) wb.save(str(log_path)) # ── gRPC 服务实现 ──────────────────────────────────────────────── class SensorPushServicer(sensor_stream_pb2_grpc.SensorPushServicer): """接收实时传感器帧（streaming）""" def __init__(self): self.frame_count = 0 self.last_report_time = time.time() self.last_angle = None self.last_state = 0 self.last_magnitude = 0.0 def Upload(self, request_iterator, context): print("[SensorPush] Client connected, waiting for frames...") reset_baseline() self.last_angle = None self.last_state = 0 self.last_magnitude = 0.0 for frame in request_iterator: self.frame_count += 1 angle = 0.0 magnitude = 0.0 state = 0 cop_x = 0.0 cop_y = 0.0 base_x = 0.0 base_y = 0.0 total_press = 0.0 threshold = 0.0 ok = True message = "OK" if len(frame.matrix) == SENSOR_ROWS * SENSOR_COLS: try: result = get_pzt_angle(frame.matrix, float(frame.dts_ms)) angle, magnitude, state, cop_x, cop_y, base_x, base_y, total_press, threshold = result threshold = threshold or 0.0 self.last_angle = angle self.last_state = state self.last_magnitude = magnitude if self.frame_count <= 10 or self.frame_count % 30 == 0: print( f"[SensorPush] PZT angle frame #{frame.seq} " f"dts={frame.dts_ms} angle={angle:.2f} " f"mag={magnitude:.2f} state={state} " f"cop=({cop_x:.2f},{cop_y:.2f}) " f"base=({base_x:.2f},{base_y:.2f}) " f"total={total_press:.2f} threshold={threshold:.2f}" ) except Exception as e: ok = False message = str(e) print(f"[SensorPush] PZT compute error on frame #{frame.seq}: {e}") else: ok = False message = f"Invalid matrix length: {len(frame.matrix)}" yield sensor_stream_pb2.PztAngleResponse( seq=frame.seq, timestamp_ms=frame.timestamp_ms, angle=angle, dts_ms=frame.dts_ms, ok=ok, message=message, magnitude=magnitude, state=state, cop_x=cop_x, cop_y=cop_y, base_x=base_x, base_y=base_y, total_press=total_press, threshold=threshold or 0.0, ) if self.frame_count % 100 == 0: now = time.time() elapsed = now - self.last_report_time fps = 100 / elapsed if elapsed > 0 else 0 self.last_report_time = now angle_text = ( f"{self.last_angle:.2f}" if self.last_angle is not None else "n/a" ) print( f"[SensorPush] Frame #{frame.seq} | " f"{frame.rows}x{frame.cols} | " f"angle={angle_text} | " f"mag={self.last_magnitude:.2f} | " f"state={self.last_state} | " f"force={frame.resultant_force:.1f} | " f"total={self.frame_count} | ~{fps:.1f} fps" ) print(f"[SensorPush] Stream ended. Total: {self.frame_count}") class ExportProcessorServicer(sensor_stream_pb2_grpc.ExportProcessorServicer): """处理导出的 CSV 文件（unary）""" def ProcessFile(self, request, context): csv_path = request.csv_path save_as_xlsx = request.save_as_xlsx print(f"[ExportProcessor] Processing: {csv_path} (xlsx={save_as_xlsx})") try: result = process_csv(csv_path, save_as_xlsx) return sensor_stream_pb2.ProcessResponse( ok=result["ok"], output_path=result["output_path"], groups_used=result["groups_used"], mean_value=result["mean_value"], threshold=result["threshold"], rows_total=result["rows_total"], rows_kept=result["rows_kept"], message=result["message"], ) except Exception as e: print(f"[ExportProcessor] Error: {e}") return sensor_stream_pb2.ProcessResponse( ok=False, output_path="", message=str(e), ) # ── 启动 ──────────────────────────────────────────────────────── def serve(port: int): server = grpc.server(futures.ThreadPoolExecutor(max_workers=4)) sensor_stream_pb2_grpc.add_SensorPushServicer_to_server(SensorPushServicer(), server) sensor_stream_pb2_grpc.add_ExportProcessorServicer_to_server(ExportProcessorServicer(), server) listen_addr = f"0.0.0.0:{port}" server.add_insecure_port(listen_addr) server.start() print(f"[DevKit Server] gRPC listening on {listen_addr}") print(f"[DevKit Server] Services: SensorPush (streaming), ExportProcessor (unary)") def shutdown(signum, frame): print("\n[DevKit Server] Shutting down...") server.stop(grace=5) sys.exit(0) signal.signal(signal.SIGINT, shutdown) signal.signal(signal.SIGTERM, shutdown) server.wait_for_termination() import numpy as np import threading from collections import deque # ===================== 切向力算法参数 ====================== COP_INIT_MEDIAN_FRAMES = 1 NOISE_COLLECT_FRAMES = 20 THRESH_K = 5 SENSOR_ROWS = 12 SENSOR_COLS = 7 SNAP_CENTER_X, SNAP_CENTER_Y = 3.0, 5.5 SNAP_RANGE_X = 0.0 SNAP_RANGE_Y = 0.0 POST_INIT_WINDOW_CNT = 600000 POST_INIT_STABLE_CNT = 500 POST_INIT_STABLE_THRESH = 0.1 # ===================== 线程安全全局状态 ====================== first_frame = None first_frame_lock = threading.Lock() first_contact_CoP_x = None first_contact_CoP_y = None contact_initialized = False cop_init_x_buf = deque(maxlen=COP_INIT_MEDIAN_FRAMES) cop_init_y_buf = deque(maxlen=COP_INIT_MEDIAN_FRAMES) noise_sum_buf = deque(maxlen=NOISE_COLLECT_FRAMES) dynamic_thresh = None post_init_frame_cnt = 0 post_stable_cnt = 0 post_refined_flag = False post_cand_x = None post_cand_y = None # ===================== 基线减除 ===================== def subtract_baseline(current_frame): global first_frame current_frame = np.array(current_frame, dtype=np.float32).flatten() with first_frame_lock: if first_frame is None: first_frame = current_frame.copy() diff = current_frame - first_frame return np.clip(diff, 0, None) # ===================== 重置CoP状态 ===================== def _legacy_reset_cop_state(): global first_contact_CoP_x, first_contact_CoP_y, contact_initialized global post_init_frame_cnt, post_stable_cnt, post_refined_flag global post_cand_x, post_cand_y first_contact_CoP_x = None first_contact_CoP_y = None contact_initialized = False cop_init_x_buf.clear() cop_init_y_buf.clear() post_init_frame_cnt = 0 post_stable_cnt = 0 post_refined_flag = False post_cand_x = None post_cand_y = None # ===================== CoP压力中心计算（新算法） ===================== def _legacy_compute_pressure_direction(baseline_subtracted_frame): global first_contact_CoP_x, first_contact_CoP_y, contact_initialized global post_init_frame_cnt, post_stable_cnt, post_refined_flag global post_cand_x, post_cand_y global noise_sum_buf, dynamic_thresh rows, cols = SENSOR_ROWS, SENSOR_COLS frame_flat = np.asarray(baseline_subtracted_frame, dtype=np.float32).flatten() frame2d = frame_flat.reshape(rows, cols) total_pressure = np.sum(frame2d) if dynamic_thresh is None: noise_sum_buf.append(total_pressure) if len(noise_sum_buf) >= NOISE_COLLECT_FRAMES: sums = np.array(noise_sum_buf) dynamic_thresh = THRESH_K * float(np.mean(sums)) if total_pressure == 0 or (dynamic_thresh is not None and total_pressure < dynamic_thresh): if contact_initialized and dynamic_thresh is not None: _legacy_reset_cop_state() return 0.0, 0.0, 0, rows-1, 0, cols-1, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0.0, dynamic_thresh x_grid = np.tile(np.arange(cols), (rows, 1)) y_grid = np.repeat(np.arange(rows), cols).reshape(rows, cols) cop_x = np.sum(frame2d * x_grid) / total_pressure cop_y = np.sum(frame2d * y_grid) / total_pressure delta_CoP_x = 0.0 delta_CoP_y = 0.0 base_x = cop_x base_y = cop_y if not contact_initialized: cop_init_x_buf.append(cop_x) cop_init_y_buf.append(cop_y) if len(cop_init_x_buf) >= COP_INIT_MEDIAN_FRAMES: first_contact_CoP_x = float(np.median(cop_init_x_buf)) first_contact_CoP_y = float(np.median(cop_init_y_buf)) contact_initialized = True cop_init_x_buf.clear() cop_init_y_buf.clear() if (abs(first_contact_CoP_x - SNAP_CENTER_X) <= SNAP_RANGE_X and abs(first_contact_CoP_y - SNAP_CENTER_Y) <= SNAP_RANGE_Y): first_contact_CoP_x = SNAP_CENTER_X first_contact_CoP_y = SNAP_CENTER_Y else: post_init_frame_cnt += 1 if not post_refined_flag and post_init_frame_cnt <= POST_INIT_WINDOW_CNT: if post_cand_x is not None: dist_val = np.hypot(cop_x - post_cand_x, cop_y - post_cand_y) if dist_val <= POST_INIT_STABLE_THRESH: post_stable_cnt += 1 else: post_cand_x = cop_x post_cand_y = cop_y post_stable_cnt = 1 else: post_cand_x = cop_x post_cand_y = cop_y post_stable_cnt = 1 if post_stable_cnt >= POST_INIT_STABLE_CNT: first_contact_CoP_x = post_cand_x first_contact_CoP_y = post_cand_y post_refined_flag = True else: post_refined_flag = True delta_CoP_x = cop_x - first_contact_CoP_x delta_CoP_y = first_contact_CoP_y - cop_y base_x = first_contact_CoP_x base_y = first_contact_CoP_y magnitude = np.hypot(delta_CoP_x, delta_CoP_y) if not contact_initialized: state = 0 elif not post_refined_flag: state = 1 else: state = 2 return (cop_x, cop_y, 0, rows-1, 0, cols-1, delta_CoP_x, delta_CoP_y, base_x, base_y, magnitude, state, total_pressure, dynamic_thresh) # ===================== 角度计算核心 ===================== def _legacy_compute_vector_angle(x: float, y: float) -> tuple[float, float]: epsilon = 1e-8 mag = np.hypot(x, y) angle = np.degrees(np.arctan2(y, x + epsilon)) if angle < 0: angle += 360 return angle, mag def _legacy_compute_PZT_angle(Px: float, Py: float) -> tuple[float, float]: return _legacy_compute_vector_angle(Px, Py) # ===================== 核心入口函数 ===================== def _legacy_get_pzt_angle(adc_data): if len(adc_data) != 84: raise ValueError("ADC数据长度必须为84") result = _legacy_compute_pressure_direction(adc_data) cop_x, cop_y = result[0], result[1] dx, dy = result[6], result[7] base_x, base_y = result[8], result[9] magnitude = result[10] state = int(result[11]) total_press = result[12] threshold = result[13] pzt_angle, _ = _legacy_compute_PZT_angle(dx, dy) return pzt_angle, magnitude, state, cop_x, cop_y, base_x, base_y, total_press, threshold # ===================== 重置基线（校准用） ===================== def _legacy_reset_baseline(): global first_frame, noise_sum_buf, dynamic_thresh with first_frame_lock: first_frame = None noise_sum_buf.clear() dynamic_thresh = None _legacy_reset_cop_state() from dataclasses import dataclass from enum import IntEnum from typing import Optional, Tuple ADC_LEN = SENSOR_ROWS * SENSOR_COLS class CoPState(IntEnum): NO_CONTACT = 0 INIT_COLLECTING = 1 POST_REFINING = 2 READY = 3 @dataclass class CoPResult: cop_x: float cop_y: float row_min: int row_max: int col_min: int col_max: int dx: float dy: float base_x: float base_y: float magnitude: float state: int total_pressure: float threshold: Optional[float] angle: float @dataclass class CoPConfig: rows: int = SENSOR_ROWS cols: int = SENSOR_COLS noise_collect_ms: float = 300.0 thresh_k: float = 5.0 min_threshold: float = 50.0 contact_confirm_ms: float = 20.0 release_confirm_ms: float = 50.0 init_collect_ms: float = 80.0 snap_enable: bool = True snap_center_x: float = 3.0 snap_center_y: float = 5.5 snap_range_x: float = 0.25 snap_range_y: float = 0.25 post_refine_enable: bool = True post_refine_window_ms: float = 800.0 post_stable_ms: float = 200.0 post_stable_thresh: float = 0.1 cop_lpf_alpha: float = 0.25 epsilon: float = 1e-8 class PressureDirectionEstimator: def __init__(self, config: CoPConfig = CoPConfig()): self.cfg = config self.reset_all() def reset_all(self): self.dynamic_thresh: Optional[float] = None self.noise_samples = [] self.noise_start_ms: Optional[float] = None self.reset_contact_state() def reset_contact_state(self): self.first_contact_cop_x: Optional[float] = None self.first_contact_cop_y: Optional[float] = None self.state = CoPState.NO_CONTACT self.init_x_buf = [] self.init_y_buf = [] self.init_start_ms: Optional[float] = None self.post_start_ms: Optional[float] = None self.post_stable_start_ms: Optional[float] = None self.post_cand_x: Optional[float] = None self.post_cand_y: Optional[float] = None self.post_refined = False self.contact_candidate_start_ms: Optional[float] = None self.release_candidate_start_ms: Optional[float] = None self.filtered_cop_x: Optional[float] = None self.filtered_cop_y: Optional[float] = None def update(self, adc_data, timestamp_ms: float) -> CoPResult: frame2d = self._prepare_frame(adc_data) total_pressure = float(np.sum(frame2d)) self._update_dynamic_threshold(total_pressure, timestamp_ms) raw_contact = self._is_raw_contact(total_pressure) contact_valid = self._debounce_contact(raw_contact, timestamp_ms) if not contact_valid: self._handle_no_contact() return self._make_empty_result(total_pressure) cop_x, cop_y = self._compute_cop(frame2d, total_pressure) cop_x, cop_y = self._filter_cop(cop_x, cop_y) self._update_state_machine(cop_x, cop_y, timestamp_ms) if self.first_contact_cop_x is None or self.first_contact_cop_y is None: dx = 0.0 dy = 0.0 base_x = cop_x base_y = cop_y else: base_x = self.first_contact_cop_x base_y = self.first_contact_cop_y dx = cop_x - base_x dy = base_y - cop_y magnitude = float(np.hypot(dx, dy)) angle = self.compute_vector_angle(dx, dy)[0] return CoPResult( cop_x=float(cop_x), cop_y=float(cop_y), row_min=0, row_max=self.cfg.rows - 1, col_min=0, col_max=self.cfg.cols - 1, dx=float(dx), dy=float(dy), base_x=float(base_x), base_y=float(base_y), magnitude=magnitude, state=int(self.state), total_pressure=total_pressure, threshold=self.dynamic_thresh, angle=float(angle), ) def _prepare_frame(self, adc_data) -> np.ndarray: arr = np.asarray(adc_data, dtype=np.float32).flatten() expected_len = self.cfg.rows * self.cfg.cols if len(arr) != expected_len: raise ValueError(f"ADC数据长度必须为{expected_len}，当前为{len(arr)}") arr = np.clip(arr, 0, None) return arr.reshape(self.cfg.rows, self.cfg.cols) def _update_dynamic_threshold(self, total_pressure: float, timestamp_ms: float): if self.dynamic_thresh is not None: return if self.noise_start_ms is None: self.noise_start_ms = timestamp_ms self.noise_samples.append(total_pressure) if timestamp_ms - self.noise_start_ms >= self.cfg.noise_collect_ms: samples = np.asarray(self.noise_samples, dtype=np.float32) mean_val = float(np.mean(samples)) std_val = float(np.std(samples)) thresh = mean_val + self.cfg.thresh_k * std_val self.dynamic_thresh = max(thresh, self.cfg.min_threshold) def _is_raw_contact(self, total_pressure: float) -> bool: if self.dynamic_thresh is None: return False return total_pressure >= self.dynamic_thresh def _debounce_contact(self, raw_contact: bool, timestamp_ms: float) -> bool: currently_in_contact = self.state != CoPState.NO_CONTACT if raw_contact: self.release_candidate_start_ms = None if currently_in_contact: return True if self.contact_candidate_start_ms is None: self.contact_candidate_start_ms = timestamp_ms return timestamp_ms - self.contact_candidate_start_ms >= self.cfg.contact_confirm_ms self.contact_candidate_start_ms = None if not currently_in_contact: return False if self.release_candidate_start_ms is None: self.release_candidate_start_ms = timestamp_ms if timestamp_ms - self.release_candidate_start_ms >= self.cfg.release_confirm_ms: return False return True def _handle_no_contact(self): if self.state != CoPState.NO_CONTACT: self.reset_contact_state() def _compute_cop(self, frame2d: np.ndarray, total_pressure: float) -> Tuple[float, float]: rows, cols = self.cfg.rows, self.cfg.cols x_grid = np.tile(np.arange(cols, dtype=np.float32), (rows, 1)) y_grid = np.repeat(np.arange(rows, dtype=np.float32), cols).reshape(rows, cols) cop_x = float(np.sum(frame2d * x_grid) / total_pressure) cop_y = float(np.sum(frame2d * y_grid) / total_pressure) return cop_x, cop_y def _filter_cop(self, cop_x: float, cop_y: float) -> Tuple[float, float]: alpha = self.cfg.cop_lpf_alpha if alpha <= 0.0: return cop_x, cop_y if self.filtered_cop_x is None or self.filtered_cop_y is None: self.filtered_cop_x = cop_x self.filtered_cop_y = cop_y else: self.filtered_cop_x = alpha * cop_x + (1.0 - alpha) * self.filtered_cop_x self.filtered_cop_y = alpha * cop_y + (1.0 - alpha) * self.filtered_cop_y return self.filtered_cop_x, self.filtered_cop_y def _update_state_machine(self, cop_x: float, cop_y: float, timestamp_ms: float): if self.state == CoPState.NO_CONTACT: self.state = CoPState.INIT_COLLECTING self.init_start_ms = timestamp_ms self.init_x_buf.clear() self.init_y_buf.clear() if self.state == CoPState.INIT_COLLECTING: self.init_x_buf.append(cop_x) self.init_y_buf.append(cop_y) if self.init_start_ms is None: self.init_start_ms = timestamp_ms if timestamp_ms - self.init_start_ms >= self.cfg.init_collect_ms: base_x = float(np.median(self.init_x_buf)) base_y = float(np.median(self.init_y_buf)) base_x, base_y = self._apply_center_snap(base_x, base_y) self.first_contact_cop_x = base_x self.first_contact_cop_y = base_y self.post_start_ms = timestamp_ms self.post_cand_x = None self.post_cand_y = None self.post_stable_start_ms = None if self.cfg.post_refine_enable: self.state = CoPState.POST_REFINING else: self.post_refined = True self.state = CoPState.READY return if self.state == CoPState.POST_REFINING: self._post_refine(cop_x, cop_y, timestamp_ms) def _apply_center_snap(self, base_x: float, base_y: float) -> Tuple[float, float]: if not self.cfg.snap_enable: return base_x, base_y if ( abs(base_x - self.cfg.snap_center_x) <= self.cfg.snap_range_x and abs(base_y - self.cfg.snap_center_y) <= self.cfg.snap_range_y ): return self.cfg.snap_center_x, self.cfg.snap_center_y return base_x, base_y def _post_refine(self, cop_x: float, cop_y: float, timestamp_ms: float): if self.post_start_ms is None: self.post_start_ms = timestamp_ms if timestamp_ms - self.post_start_ms >= self.cfg.post_refine_window_ms: self.post_refined = True self.state = CoPState.READY return if self.post_cand_x is None or self.post_cand_y is None: self.post_cand_x = cop_x self.post_cand_y = cop_y self.post_stable_start_ms = timestamp_ms return dist = float(np.hypot(cop_x - self.post_cand_x, cop_y - self.post_cand_y)) if dist <= self.cfg.post_stable_thresh: if self.post_stable_start_ms is None: self.post_stable_start_ms = timestamp_ms if timestamp_ms - self.post_stable_start_ms >= self.cfg.post_stable_ms: refined_x, refined_y = self._apply_center_snap(self.post_cand_x, self.post_cand_y) self.first_contact_cop_x = float(refined_x) self.first_contact_cop_y = float(refined_y) self.post_refined = True self.state = CoPState.READY else: self.post_cand_x = cop_x self.post_cand_y = cop_y self.post_stable_start_ms = timestamp_ms def _make_empty_result(self, total_pressure: float) -> CoPResult: return CoPResult( cop_x=0.0, cop_y=0.0, row_min=0, row_max=self.cfg.rows - 1, col_min=0, col_max=self.cfg.cols - 1, dx=0.0, dy=0.0, base_x=0.0, base_y=0.0, magnitude=0.0, state=int(CoPState.NO_CONTACT), total_pressure=float(total_pressure), threshold=self.dynamic_thresh, angle=0.0, ) def compute_vector_angle(self, x: float, y: float) -> Tuple[float, float]: mag = float(np.hypot(x, y)) angle = float(np.degrees(np.arctan2(y, x + self.cfg.epsilon))) if angle < 0: angle += 360.0 return angle, mag @dataclass class LocalForceResult: angle: float magnitude: float planar_x: float planar_y: float confidence: float contact_active: bool reportable: bool total_pressure: float peak: float cop_x: float cop_y: float threshold: float class LocalTangentialForceEstimator: CONTACT_ENTER_TOTAL_THRESHOLD = 520.0 CONTACT_ENTER_PEAK_THRESHOLD = 50.0 CONTACT_EXIT_TOTAL_THRESHOLD = 260.0 CONTACT_EXIT_PEAK_THRESHOLD = 28.0 CONTACT_ENTER_FRAMES_REQUIRED = 2 CONTACT_EXIT_FRAMES_REQUIRED = 8 BASELINE_IDLE_ALPHA = 0.035 BASELINE_BOOTSTRAP_ALPHA = 1.0 BASELINE_NOISE_FLOOR = 5.0 ACTIVE_CELL_MIN_VALUE = 18.0 ACTIVE_CELL_PEAK_RATIO = 0.14 MIN_ACTIVE_CELLS = 3 VECTOR_SMOOTHING_ALPHA = 0.16 REPORT_MAGNITUDE_ENTER = 0.12 REPORT_MAGNITUDE_EXIT = 0.045 REPORT_CONFIDENCE_ENTER = 0.14 REPORT_CONFIDENCE_EXIT = 0.06 REPORT_HOLD_FRAMES = 10 ASYMMETRY_WEIGHT = 1.1 DRIFT_WEIGHT = 0.65 MOTION_WEIGHT = 0.25 EDGE_ASYMMETRY_DAMPING = 0.35 EDGE_INWARD_ROLLING_BIAS = 0.55 EDGE_START_COP_THRESHOLD = 0.45 EDGE_START_BIAS_WEIGHT = 1.1 ROLLING_FRICTION_ALPHA = 0.68 ROLLING_FRICTION_MIN_MAGNITUDE = 0.05 def __init__(self): self.reset_all() def reset_all(self): self.baseline_frame = None self.reset_contact_state() def reset_contact_state(self): self.contact_active = False self.contact_enter_counter = 0 self.contact_exit_counter = 0 self.anchor_cop_x = None self.anchor_cop_y = None self.last_cop_x = None self.last_cop_y = None self.edge_start_bias_x = 0.0 self.edge_start_bias_y = 0.0 self.smoothed_x = 0.0 self.smoothed_y = 0.0 self.report_active = False self.report_hold_counter = 0 self.held_report = None def update(self, adc_data, timestamp_ms: float) -> LocalForceResult: raw = np.asarray(adc_data, dtype=np.float32).flatten() if len(raw) != ADC_LEN: raise ValueError(f"ADC data length must be {ADC_LEN}") baseline_subtracted = self._subtract_baseline(raw) if not self._update_contact_state(raw, baseline_subtracted): return self._inactive_result(float(np.sum(baseline_subtracted)), float(np.max(baseline_subtracted, initial=0.0))) stats = self._compute_contact_stats(baseline_subtracted) if stats is None: return self._stabilize_report(self._weak_contact_result(float(np.sum(baseline_subtracted)), float(np.max(baseline_subtracted, initial=0.0)))) if self.anchor_cop_x is None: self.anchor_cop_x = stats["cop_x"] self.anchor_cop_y = stats["cop_y"] self.last_cop_x = stats["cop_x"] self.last_cop_y = stats["cop_y"] self.edge_start_bias_x, self.edge_start_bias_y = self._edge_start_bias(stats) return self._stabilize_report(self._weak_contact_result(stats["total"], stats["peak"], stats["cop_x"], stats["cop_y"])) anchor_x = self.anchor_cop_x anchor_y = self.anchor_cop_y if self.anchor_cop_y is not None else stats["cop_y"] last_x = self.last_cop_x if self.last_cop_x is not None else stats["cop_x"] last_y = self.last_cop_y if self.last_cop_y is not None else stats["cop_y"] drift_x = stats["cop_x"] - anchor_x drift_y = stats["cop_y"] - anchor_y motion_x = stats["cop_x"] - last_x motion_y = stats["cop_y"] - last_y kinematic_x = drift_x * self.DRIFT_WEIGHT + motion_x * self.MOTION_WEIGHT kinematic_y = drift_y * self.DRIFT_WEIGHT + motion_y * self.MOTION_WEIGHT asymmetry_x, asymmetry_y = self._damp_edge_asymmetry( stats, kinematic_x + self.edge_start_bias_x, kinematic_y + self.edge_start_bias_y, ) combined_x = asymmetry_x + kinematic_x + self.edge_start_bias_x combined_y = asymmetry_y + kinematic_y + self.edge_start_bias_y combined_x, combined_y = self._apply_rolling_friction( self.smoothed_x, self.smoothed_y, combined_x, combined_y, ) self.smoothed_x += (combined_x - self.smoothed_x) * self.VECTOR_SMOOTHING_ALPHA self.smoothed_y += (combined_y - self.smoothed_y) * self.VECTOR_SMOOTHING_ALPHA self.last_cop_x = stats["cop_x"] self.last_cop_y = stats["cop_y"] planar_x = self.smoothed_x planar_y = -self.smoothed_y angle, magnitude = self.compute_vector_angle(planar_x, planar_y) active_span_rows = (stats["max_row"] - stats["min_row"] + 1) / SENSOR_ROWS active_span_cols = (stats["max_col"] - stats["min_col"] + 1) / SENSOR_COLS activity = min(max(stats["active_cells"] / ADC_LEN, 0.0), 1.0) span = min(max((active_span_rows + active_span_cols) * 0.5, 0.0), 1.0) pressure_ratio = min(max(stats["active_total"] / max(stats["total"], 1.0), 0.0), 1.0) peak_ratio = min(max(stats["peak"] / (stats["total"] / stats["active_cells"] + 1.0), 0.0), 1.0) confidence = min(max(activity * 0.35 + span * 0.2 + pressure_ratio * 0.3 + peak_ratio * 0.15, 0.0), 1.0) return self._stabilize_report(LocalForceResult( angle=angle, magnitude=magnitude, planar_x=planar_x, planar_y=planar_y, confidence=confidence, contact_active=True, reportable=False, total_pressure=stats["total"], peak=stats["peak"], cop_x=stats["cop_x"], cop_y=stats["cop_y"], threshold=self.CONTACT_ENTER_TOTAL_THRESHOLD, )) def _update_idle_baseline(self, raw_frame, alpha: float): if self.baseline_frame is None: self.baseline_frame = np.array(raw_frame, dtype=np.float32).copy() return self.baseline_frame += (raw_frame - self.baseline_frame) * alpha def _subtract_baseline(self, raw_frame): if self.baseline_frame is None: self._update_idle_baseline(raw_frame, self.BASELINE_BOOTSTRAP_ALPHA) diff = raw_frame - self.baseline_frame - self.BASELINE_NOISE_FLOOR return np.clip(diff, 0, None) def _pressure_metrics(self, frame): return float(np.sum(frame)), float(np.max(frame, initial=0.0)) def _update_contact_state(self, raw_frame, frame) -> bool: total, peak = self._pressure_metrics(frame) enter = total >= self.CONTACT_ENTER_TOTAL_THRESHOLD and peak >= self.CONTACT_ENTER_PEAK_THRESHOLD exit_frame = total <= self.CONTACT_EXIT_TOTAL_THRESHOLD or peak <= self.CONTACT_EXIT_PEAK_THRESHOLD if self.contact_active: if exit_frame: self.contact_exit_counter += 1 if self.contact_exit_counter >= self.CONTACT_EXIT_FRAMES_REQUIRED: self._update_idle_baseline(raw_frame, self.BASELINE_IDLE_ALPHA) self.reset_contact_state() return False else: self.contact_exit_counter = 0 return True if enter: self.contact_enter_counter += 1 if self.contact_enter_counter >= self.CONTACT_ENTER_FRAMES_REQUIRED: self.contact_active = True self.contact_enter_counter = 0 self.contact_exit_counter = 0 return True return False self.contact_enter_counter = 0 self._update_idle_baseline(raw_frame, self.BASELINE_IDLE_ALPHA) return False def _compute_contact_stats(self, frame): total, peak = self._pressure_metrics(frame) if total <= 0.0 or peak <= 0.0: return None active_threshold = max(peak * self.ACTIVE_CELL_PEAK_RATIO, self.ACTIVE_CELL_MIN_VALUE) frame2d = np.asarray(frame, dtype=np.float32).reshape(SENSOR_ROWS, SENSOR_COLS) active_mask = frame2d >= active_threshold active_cells = int(np.count_nonzero(active_mask)) if active_cells < self.MIN_ACTIVE_CELLS: return None active_values = frame2d[active_mask] active_total = float(np.sum(active_values)) if active_total <= 0.0: return None rows, cols = np.nonzero(active_mask) cop_x = float(np.sum(active_values * cols) / active_total) cop_y = float(np.sum(active_values * rows) / active_total) min_row, max_row = int(np.min(rows)), int(np.max(rows)) min_col, max_col = int(np.min(cols)), int(np.max(cols)) bbox_center_x = (min_col + max_col) * 0.5 bbox_center_y = (min_row + max_row) * 0.5 half_width = max(max_col - min_col, 1) * 0.5 half_height = max(max_row - min_row, 1) * 0.5 asymmetry_x = float(np.sum(active_values * ((cols - bbox_center_x) / half_width)) / active_total) asymmetry_y = float(np.sum(active_values * ((rows - bbox_center_y) / half_height)) / active_total) return { "total": total, "peak": peak, "active_total": active_total, "active_cells": active_cells, "min_row": min_row, "max_row": max_row, "min_col": min_col, "max_col": max_col, "cop_x": cop_x, "cop_y": cop_y, "asymmetry_x": asymmetry_x, "asymmetry_y": asymmetry_y, } def _contact_touches_edge(self, stats) -> bool: return ( stats["min_row"] == 0 or stats["max_row"] == SENSOR_ROWS - 1 or stats["min_col"] == 0 or stats["max_col"] == SENSOR_COLS - 1 ) def _damp_edge_asymmetry(self, stats, kinematic_x: float, kinematic_y: float): asymmetry_x = stats["asymmetry_x"] * self.ASYMMETRY_WEIGHT asymmetry_y = stats["asymmetry_y"] * self.ASYMMETRY_WEIGHT if stats["min_col"] == 0 and asymmetry_x < 0.0: asymmetry_x = -asymmetry_x * self.EDGE_INWARD_ROLLING_BIAS if stats["max_col"] == SENSOR_COLS - 1 and asymmetry_x > 0.0: asymmetry_x = -asymmetry_x * self.EDGE_INWARD_ROLLING_BIAS if stats["min_row"] == 0 and asymmetry_y < 0.0: asymmetry_y = -asymmetry_y * self.EDGE_INWARD_ROLLING_BIAS if stats["max_row"] == SENSOR_ROWS - 1 and asymmetry_y > 0.0: asymmetry_y = -asymmetry_y * self.EDGE_INWARD_ROLLING_BIAS opposing_dot = asymmetry_x * kinematic_x + asymmetry_y * kinematic_y kinematic_mag = float(np.hypot(kinematic_x, kinematic_y)) if self._contact_touches_edge(stats) and opposing_dot < 0.0 and kinematic_mag >= self.ROLLING_FRICTION_MIN_MAGNITUDE: asymmetry_x *= self.EDGE_ASYMMETRY_DAMPING asymmetry_y *= self.EDGE_ASYMMETRY_DAMPING return asymmetry_x, asymmetry_y def _edge_start_bias(self, stats): center_x = (SENSOR_COLS - 1) * 0.5 center_y = (SENSOR_ROWS - 1) * 0.5 normalized_x = min(max((stats["cop_x"] - center_x) / max(center_x, 1.0), -1.0), 1.0) normalized_y = min(max((stats["cop_y"] - center_y) / max(center_y, 1.0), -1.0), 1.0) bias_x = self._edge_start_axis_bias(normalized_x) if stats["min_col"] == 0 or stats["max_col"] == SENSOR_COLS - 1 else 0.0 bias_y = self._edge_start_axis_bias(normalized_y) if stats["min_row"] == 0 or stats["max_row"] == SENSOR_ROWS - 1 else 0.0 return bias_x, bias_y def _edge_start_axis_bias(self, normalized_axis: float) -> float: distance = abs(normalized_axis) if distance <= self.EDGE_START_COP_THRESHOLD: return 0.0 strength = min(max((distance - self.EDGE_START_COP_THRESHOLD) / (1.0 - self.EDGE_START_COP_THRESHOLD), 0.0), 1.0) return -np.sign(normalized_axis) * strength * self.EDGE_START_BIAS_WEIGHT def _apply_rolling_friction(self, previous_x: float, previous_y: float, current_x: float, current_y: float): previous_mag = float(np.hypot(previous_x, previous_y)) current_mag = float(np.hypot(current_x, current_y)) if previous_mag < self.ROLLING_FRICTION_MIN_MAGNITUDE or current_mag < self.ROLLING_FRICTION_MIN_MAGNITUDE: return current_x, current_y dot = previous_x * current_x + previous_y * current_y if dot >= 0.0: return current_x, current_y mixed_x = current_x * (1.0 - self.ROLLING_FRICTION_ALPHA) + previous_x * self.ROLLING_FRICTION_ALPHA mixed_y = current_y * (1.0 - self.ROLLING_FRICTION_ALPHA) + previous_y * self.ROLLING_FRICTION_ALPHA if mixed_x * previous_x + mixed_y * previous_y >= 0.0: return mixed_x, mixed_y keep_mag = min(previous_mag, current_mag) * 0.5 return previous_x / previous_mag * keep_mag, previous_y / previous_mag * keep_mag def _inactive_result(self, total_pressure=0.0, peak=0.0): return LocalForceResult(0.0, 0.0, 0.0, 0.0, 0.0, False, False, total_pressure, peak, 0.0, 0.0, self.CONTACT_ENTER_TOTAL_THRESHOLD) def _weak_contact_result(self, total_pressure=0.0, peak=0.0, cop_x=0.0, cop_y=0.0): return LocalForceResult(0.0, 0.0, 0.0, 0.0, 0.0, True, False, total_pressure, peak, cop_x, cop_y, self.CONTACT_ENTER_TOTAL_THRESHOLD) def _store_report(self, result: LocalForceResult): result.reportable = True self.report_active = True self.report_hold_counter = 0 self.held_report = result return result def _hold_or_drop_report(self): if self.report_active and self.report_hold_counter < self.REPORT_HOLD_FRAMES and self.held_report is not None: self.report_hold_counter += 1 held = self.held_report held.reportable = True return held self.report_active = False self.report_hold_counter = 0 self.held_report = None return self._weak_contact_result() def _stabilize_report(self, result: LocalForceResult): if not result.contact_active: self.report_active = False self.report_hold_counter = 0 self.held_report = None return result can_enter = result.magnitude >= self.REPORT_MAGNITUDE_ENTER and result.confidence >= self.REPORT_CONFIDENCE_ENTER can_stay = result.magnitude >= self.REPORT_MAGNITUDE_EXIT and result.confidence >= self.REPORT_CONFIDENCE_EXIT if self.report_active: if can_stay: return self._store_report(result) return self._hold_or_drop_report() if can_enter: return self._store_report(result) return result def compute_vector_angle(self, x: float, y: float) -> Tuple[float, float]: magnitude = float(np.hypot(x, y)) if magnitude <= np.finfo(np.float32).eps: return 0.0, 0.0 angle = float(np.degrees(np.arctan2(y, x))) if angle < 0.0: angle += 360.0 return angle, magnitude _estimator = LocalTangentialForceEstimator() def reset_cop_state(): _estimator.reset_contact_state() def reset_all_state(): _estimator.reset_all() def compute_pressure_direction(adc_data, timestamp_ms: float): result = _estimator.update(adc_data, timestamp_ms) return ( result.cop_x, result.cop_y, 0, SENSOR_ROWS - 1, 0, SENSOR_COLS - 1, result.planar_x, result.planar_y, 0.0, 0.0, result.magnitude, 1 if result.reportable else 0, result.total_pressure, result.threshold, ) def compute_vector_angle(x: float, y: float) -> Tuple[float, float]: return _estimator.compute_vector_angle(x, y) def compute_PZT_angle(Px: float, Py: float) -> Tuple[float, float]: return compute_vector_angle(Px, Py) def get_pzt_angle(adc_data, timestamp_ms: float): if len(adc_data) != ADC_LEN: raise ValueError(f"ADC数据长度必须为{ADC_LEN}") result = _estimator.update(adc_data, timestamp_ms) return ( result.angle, result.magnitude, 1 if result.reportable else 0, result.cop_x, result.cop_y, 0.0, 0.0, result.total_pressure, result.threshold, ) def reset_baseline(): reset_all_state() if __name__ == "__main__": parser = argparse.ArgumentParser(description="JE-Skin DevKit gRPC Server") parser.add_argument("--port", type=int, default=50051, help="gRPC listen port (default: 50051)") args = parser.parse_args() serve(args.port)