加入独立参考力接口与离线恒力反馈骨架

复用法向力投影，区分持续补偿与固定位置松弛记录。189 项定向检查及 lint 通过；扩大检查的失败与限制见架构审查。
2026-10-18 17:18:47 +08:00
parent 06e06126ca
commit 797235c6a4
6 changed files with 563 additions and 45 deletions
@@ -0,0 +1,162 @@
+"""Offline force-hold policy. Outputs are advice, never motor commands.
+
+Depth is measured indentation from an established contact zero, positive into
+the rubber, in mm. It is not a geometric contact height or a motor coordinate.
+The caller owns serialized updates, synchronized depth readback and stop on
+fault. A future executor must revalidate motion limits and require a watchdog;
+zero advice does not cancel a previously issued hardware velocity command.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, fields
+from typing import Literal
+import math
+
+from .reference_force import NormalForceSample
+
+
+@dataclass(frozen=True)
+class ForceControlConfig:
+    target_n: float
+    kp_mm_per_n_s: float
+    ki_mm_per_n_s2: float
+    kd_mm_per_n: float
+    max_force_n: float
+    max_speed_mm_s: float
+    max_depth_mm: float
+    max_sample_age_s: float
+    max_duration_s: float
+
+    def __post_init__(self) -> None:
+        for item in fields(self):
+            value = getattr(self, item.name)
+            if not math.isfinite(value) or value < 0:
+                raise ValueError(f"{item.name} must be finite and nonnegative")
+            if item.name.startswith("max_") and value == 0:
+                raise ValueError(f"{item.name} must be positive")
+        if self.target_n > self.max_force_n:
+            raise ValueError("target_n exceeds max_force_n")
+
+
+@dataclass(frozen=True)
+class ForceDecision:
+    """One recordable observation; positive velocity increases indentation."""
+
+    sample: NormalForceSample
+    depth_mm: float
+    target_n: float
+    velocity_mm_s: float = 0.0
+    error_n: float | None = None
+    force_rate_n_s: float | None = None
+    fault: str = ""
+
+
+class ForceController:
+    """PID hold or fixed-position relaxation recording after contact acquisition.
+
+    Gains have explicit dimensions and no equipment defaults. Derivative acts
+    on measured force; conditional integration prevents speed-limit windup.
+    This is not a contact search, an unload sequence or a real-time scheduler.
+    """
+
+    def __init__(
+        self,
+        config: ForceControlConfig,
+        *,
+        mode: Literal["compensate", "record_relaxation"] = "record_relaxation",
+    ) -> None:
+        if mode not in ("compensate", "record_relaxation"):
+            raise ValueError("unknown relaxation mode")
+        self.config = config
+        self.mode = mode
+        self.reset()
+
+    def reset(self) -> None:
+        """Begin a new bounded hold; the caller must separately stop hardware."""
+        self._previous: NormalForceSample | None = None
+        self._start_ns: int | None = None
+        self._integral = 0.0
+        self._fault = ""
+
+    def update(
+        self, sample: NormalForceSample, *, depth_mm: float, now_ns: int
+    ) -> ForceDecision:
+        """Consume a fresh calibrated sample in the host monotonic time domain.
+
+        Caller must invoke on missing-data ticks too (with its last sample), so
+        age detection still runs. Faults latch until explicit reset.
+        """
+        cfg = self.config
+        fault = self._fault
+        previous = self._previous
+        age_s = (now_ns - sample.t_host_mono_ns) / 1e9
+        dt = (
+            0.0
+            if previous is None
+            else (sample.t_host_mono_ns - previous.t_host_mono_ns) / 1e9
+        )
+        if not fault:
+            if (
+                not sample.valid
+                or not sample.source_id
+                or not sample.calibration_id
+                or not math.isfinite(sample.force_n)
+                or sample.t_host_mono_ns < 0
+            ):
+                fault = "invalid_reference"
+            elif not 0 <= age_s <= cfg.max_sample_age_s:
+                fault = "stale_or_future_reference"
+            elif previous is not None and not 0 < dt <= cfg.max_sample_age_s:
+                fault = "reference_time_discontinuity"
+            elif previous is not None and (sample.source_id, sample.calibration_id) != (
+                previous.source_id,
+                previous.calibration_id,
+            ):
+                fault = "reference_identity_changed"
+            elif abs(sample.force_n) > cfg.max_force_n:
+                fault = "force_limit"
+            elif not math.isfinite(depth_mm) or not 0 <= depth_mm <= cfg.max_depth_mm:
+                fault = "depth_limit"
+            elif (
+                self._start_ns is not None
+                and (now_ns - self._start_ns) / 1e9 > cfg.max_duration_s
+            ):
+                fault = "hold_timeout"
+        if fault:
+            self._fault = fault
+            return ForceDecision(sample, depth_mm, cfg.target_n, fault=fault)
+
+        if self._start_ns is None:
+            self._start_ns = now_ns
+        error = cfg.target_n - sample.force_n
+        rate = None if previous is None else (sample.force_n - previous.force_n) / dt
+        self._previous = sample
+        velocity = 0.0
+        if self.mode == "compensate":
+            candidate = self._integral + error * dt
+            proportional_derivative = cfg.kp_mm_per_n_s * error - cfg.kd_mm_per_n * (
+                rate or 0.0
+            )
+            requested = proportional_derivative + cfg.ki_mm_per_n_s2 * candidate
+            if not all(math.isfinite(value) for value in (candidate, requested)):
+                self._fault = "numerical_failure"
+                return ForceDecision(sample, depth_mm, cfg.target_n, fault=self._fault)
+            # Bound the next sample-age horizon, using measured depth, not integrated advice.
+            lower = max(-cfg.max_speed_mm_s, -depth_mm / cfg.max_sample_age_s)
+            upper = min(
+                cfg.max_speed_mm_s, (cfg.max_depth_mm - depth_mm) / cfg.max_sample_age_s
+            )
+            if not (
+                (requested > upper and error > 0) or (requested < lower and error < 0)
+            ):
+                self._integral = candidate
+            velocity = min(upper, max(lower, requested))
+            if (requested > 0 and depth_mm >= cfg.max_depth_mm) or (
+                requested < 0 and depth_mm <= 0
+            ):
+                self._fault = "depth_limit"
+                velocity = 0.0
+        return ForceDecision(
+            sample, depth_mm, cfg.target_n, velocity, error, rate, self._fault
+        )
@@ -46,7 +46,8 @@ from .recording_options import (
    make_record_config,
    require_codec_available,
 )
-from .sensor.frames import PLATFORM_AXES, TOOL_AXIS
+from .reference_force import normal_force_n as normal_force_n, parse_rotation as parse_rotation
+from .sensor.frames import TOOL_AXIS

 SENSOR_MODEL = "finger"

@@ -352,50 +353,6 @@ def load_steps(
    ]]


-def parse_rotation(values: list[float]) -> tuple[tuple[float, float, float], ...]:
-    """Force-sensor frame to tool frame, row major. Nothing is filled in for you."""
-    if len(values) != 9:
-        raise ValueError("the force-sensor to tool rotation needs 9 numbers, row major")
-    numbers = tuple(_finite(value, "rotation") for value in values)
-    rows = (numbers[0:3], numbers[3:6], numbers[6:9])
-    columns = tuple(tuple(row[index] for row in rows) for index in range(3))
-
-    def dot(left: tuple[float, ...], right: tuple[float, ...]) -> float:
-        return sum(a * b for a, b in zip(left, right))
-
-    for index, column in enumerate(columns):
-        if abs(dot(column, column) - 1.0) > 1e-6:
-            raise ValueError("each column of the rotation must have length 1")
-        for earlier in columns[:index]:
-            if abs(dot(column, earlier)) > 1e-6:
-                raise ValueError("the columns of the rotation must be perpendicular")
-    determinant = (
-        columns[0][0] * (columns[1][1] * columns[2][2] - columns[1][2] * columns[2][1])
-        - columns[0][1] * (columns[1][0] * columns[2][2] - columns[1][2] * columns[2][0])
-        + columns[0][2] * (columns[1][0] * columns[2][1] - columns[1][1] * columns[2][0])
-    )
-    if abs(determinant - 1.0) > 1e-6:
-        raise ValueError("the transform must be a rotation with determinant +1")
-    return rows
-
-
-def normal_force_n(
-    fx: float,
-    fy: float,
-    fz: float,
-    rotation: tuple[tuple[float, float, float], ...],
-) -> float:
-    """Project a raw force onto the tool axis after the entered rotation.
-
-    ``F_tool = R @ F_sensor`` and ``F_normal = F_tool · tool_axis``. ``tool_axis``
-    is the platform axis the pose solver aligns the surface normal to. The raw
-    components are not readable as the normal without that product.
-    """
-    raw = (_finite(fx, "fx"), _finite(fy, "fy"), _finite(fz, "fz"))
-    transformed = tuple(sum(a * b for a, b in zip(row, raw)) for row in rotation)
-    return transformed[PLATFORM_AXES.index(TOOL_AXIS)]
-
-
 def _finite(value: float, label: str) -> float:
    number = float(value)
    if not math.isfinite(number):
@@ -0,0 +1,133 @@
+"""Reference-force boundary in N / N m, independent of acquisition and motion.
+
+Adapters own open/tare/calibration and convert device clocks to host monotonic
+nanoseconds at receipt. A missing reading is None, never a zero-force sample.
+One acquisition thread owns read/close; pass immutable samples to consumers.
+Keep raw wrench samples alongside projected ones when recording an experiment.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Protocol
+import math
+
+from .sensor.frames import PLATFORM_AXES, TOOL_AXIS
+
+
+@dataclass(frozen=True)
+class NormalForceSample:
+    """Calibrated normal force, positive in compression along the tool axis."""
+
+    force_n: float
+    t_host_mono_ns: int
+    source_id: str
+    calibration_id: str
+    valid: bool = True
+
+
+@dataclass(frozen=True)
+class WrenchSample:
+    """Force and optional torque in the named transducer frame (not tactile raw)."""
+
+    force_n: tuple[float, float, float]
+    t_host_mono_ns: int
+    source_id: str
+    calibration_id: str
+    frame: str
+    torque_nm: tuple[float, float, float] | None = None
+    valid: bool = True
+
+
+class ReferenceForceSensor(Protocol):
+    """A configured single-axis or FT adapter; read must be nonblocking."""
+
+    def read(self) -> NormalForceSample | WrenchSample | None: ...
+
+    def close(self) -> None: ...
+
+
+def project_wrench(
+    sample: WrenchSample,
+    *,
+    rotation: tuple[tuple[float, float, float], ...],
+    expected_frame: str,
+    compression_sign: int,
+) -> NormalForceSample:
+    """Apply transducer-to-tool rotation and explicit reaction/load polarity.
+
+    Translation does not affect force. Torque is retained in the raw sample;
+    transforming moments about a different origin also needs the lever arm.
+    A transducer attached to a rotating stage needs a pose-dependent rotation.
+    """
+    if not expected_frame or sample.frame != expected_frame:
+        raise ValueError("reference force frame mismatch")
+    if compression_sign not in (-1, 1):
+        raise ValueError("compression_sign must be +1 or -1")
+    parsed = parse_rotation([value for row in rotation for value in row])
+    if len(sample.force_n) != 3:
+        raise ValueError("force needs 3 components in N")
+    force = normal_force_n(*sample.force_n, parsed) * compression_sign
+    valid = sample.valid
+    if sample.torque_nm is not None:
+        valid = (
+            valid
+            and len(sample.torque_nm) == 3
+            and all(math.isfinite(value) for value in sample.torque_nm)
+        )
+    return NormalForceSample(
+        force, sample.t_host_mono_ns, sample.source_id, sample.calibration_id, valid
+    )
+
+
+def parse_rotation(values: list[float]) -> tuple[tuple[float, float, float], ...]:
+    """Force-sensor frame to tool frame, row major. Nothing is filled in for you."""
+    if len(values) != 9:
+        raise ValueError("the force-sensor to tool rotation needs 9 numbers, row major")
+    numbers = tuple(_finite(value, "rotation") for value in values)
+    rows = (numbers[0:3], numbers[3:6], numbers[6:9])
+    columns = tuple(tuple(row[index] for row in rows) for index in range(3))
+
+    def dot(left: tuple[float, ...], right: tuple[float, ...]) -> float:
+        return sum(a * b for a, b in zip(left, right))
+
+    for index, column in enumerate(columns):
+        if abs(dot(column, column) - 1.0) > 1e-6:
+            raise ValueError("each column of the rotation must have length 1")
+        for earlier in columns[:index]:
+            if abs(dot(column, earlier)) > 1e-6:
+                raise ValueError("the columns of the rotation must be perpendicular")
+    determinant = (
+        columns[0][0] * (columns[1][1] * columns[2][2] - columns[1][2] * columns[2][1])
+        - columns[0][1]
+        * (columns[1][0] * columns[2][2] - columns[1][2] * columns[2][0])
+        + columns[0][2]
+        * (columns[1][0] * columns[2][1] - columns[1][1] * columns[2][0])
+    )
+    if abs(determinant - 1.0) > 1e-6:
+        raise ValueError("the transform must be a rotation with determinant +1")
+    return rows
+
+
+def normal_force_n(
+    fx: float,
+    fy: float,
+    fz: float,
+    rotation: tuple[tuple[float, float, float], ...],
+) -> float:
+    """Project a raw force onto the tool axis after the entered rotation.
+
+    ``F_tool = R @ F_sensor`` and ``F_normal = F_tool · tool_axis``. ``tool_axis``
+    is the platform axis the pose solver aligns the surface normal to. The raw
+    components are not readable as the normal without that product.
+    """
+    raw = (_finite(fx, "fx"), _finite(fy, "fy"), _finite(fz, "fz"))
+    transformed = tuple(sum(a * b for a, b in zip(row, raw)) for row in rotation)
+    return transformed[PLATFORM_AXES.index(TOOL_AXIS)]
+
+
+def _finite(value: float, label: str) -> float:
+    number = float(value)
+    if not math.isfinite(number):
+        raise ValueError(f"{label} must be a finite number")
+    return number