From 797235c6a497ba6b482656ac1c7681d2450489cc Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Sun, 18 Oct 2026 17:18:47 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E7=8B=AC=E7=AB=8B=E5=8F=82?= =?UTF-8?q?=E8=80=83=E5=8A=9B=E6=8E=A5=E5=8F=A3=E4=B8=8E=E7=A6=BB=E7=BA=BF?= =?UTF-8?q?=E6=81=92=E5=8A=9B=E5=8F=8D=E9=A6=88=E9=AA=A8=E6=9E=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 复用法向力投影，区分持续补偿与固定位置松弛记录。189 项定向检查及 lint 通过；扩大检查的失败与限制见架构审查。 --- apps/autocal/README.md | 2 + .../autocal/docs/force-control-scaffold.zh.md | 40 ++++ .../src/orisys_autocal/force_control.py | 162 +++++++++++++ apps/autocal/src/orisys_autocal/multipoint.py | 47 +--- .../src/orisys_autocal/reference_force.py | 133 +++++++++++ apps/autocal/tests/test_force_control.py | 224 ++++++++++++++++++ 6 files changed, 563 insertions(+), 45 deletions(-) create mode 100644 apps/autocal/docs/force-control-scaffold.zh.md create mode 100644 apps/autocal/src/orisys_autocal/force_control.py create mode 100644 apps/autocal/src/orisys_autocal/reference_force.py create mode 100644 apps/autocal/tests/test_force_control.py diff --git a/apps/autocal/README.md b/apps/autocal/README.md index d418f56..ee6d67d 100644 --- a/apps/autocal/README.md +++ b/apps/autocal/README.md @@ -28,6 +28,8 @@ Replaces the vendor example program it was ported from: ## Capture workflow +[Offline force-feedback scaffold](docs/force-control-scaffold.zh.md). + The 标定采集 page is one queue, not two panels — a single point is a queue with one row. `Ready` lists the six facts a run needs and the **primary button follows the step**, always stating why it is disabled. Alignment is only a request: the move diff --git a/apps/autocal/docs/force-control-scaffold.zh.md b/apps/autocal/docs/force-control-scaffold.zh.md new file mode 100644 index 0000000..aa42b65 --- /dev/null +++ b/apps/autocal/docs/force-control-scaffold.zh.md @@ -0,0 +1,40 @@ +# 独立参考力与反馈骨架 + +用途：当前代码契约。状态：离线可测试；未接设备、运动循环、界面或生产记录。架构问题与实施顺序见[审查](../../../../docs/platform/autocal-architecture-review.zh.md)。 + +## 边界与单位 + +[`reference_force.py`](../src/orisys_autocal/reference_force.py) 定义 `ReferenceForceSensor`：由适配器建立连接、去皮和校准，单个采集线程拥有非阻塞 `read()` 与 `close()`；无新数据返回 `None`。异常、断开和设备生命周期由未来适配器处理，不能转成零力。 + +单轴适配器输出不可变的 `NormalForceSample`，`force_n` 为 N，压缩为正。FT 适配器输出 `WrenchSample`，三个力分量为 N、可选三个力矩分量为 N m，明确 `frame`。两者携带 `source_id`、`calibration_id`、有效性和接收时的 `t_host_mono_ns`。设备原生时钟以后另存，不能直接冒充主机时钟。 + +`project_wrench()` 必须显式给出 transducer→tool 的旋转、预期输入坐标系和 `compression_sign`。它验证正交性、行列式 +1，并复用 `normal_force_n()`：`F_normal = compression_sign * (R @ F_sensor)[TOOL_AXIS]`。单轴传感器的安装方向由其适配器标定；不得输入触觉算法的 raw 值。FT 安装于旋转台时，R 必须随姿态更新；固定工具端才可采用已验证的固定 R。 + +力投影不需要平移；转换不同力矩原点时还需力臂，本次未实现力矩变换。保留原始 wrench，不能把只投影法向力称作完整 wrench 变换。原 `multipoint` 的 `parse_rotation/normal_force_n` 导入保持兼容，算法只有一个实现。 + +## 控制输入和输出 + +[`force_control.py`](../src/orisys_autocal/force_control.py) 的 `ForceController.update()` 消费已校准法向力、同步压入量和当前主机时间，返回 `ForceDecision`。`depth_mm` 是接触零点之后、向橡胶内部为正的压入量，范围为 `[0,max_depth_mm]`；它既不是几何高度，也不是 Z 轴绝对坐标。运动适配层必须用 `PressAxis` 的方向/零点及当次接触零点建立转换。 + +输出 `velocity_mm_s` 是压入速度建议，正值增加压入。样本、压入量、目标、误差、力变化率和故障原因均保留在结果中，可由未来记录器逐样本保存；当前不会自动写文件。配置中的增益量纲为：P `mm/(N s)`、I `mm/(N s²)`、D `mm/N`，全部显式输入，没有真实机器默认增益。 + +| 模式 | 行为 | +|---|---| +| `record_relaxation`（默认） | 输出速度为零，记录力衰减和压入量。实际位置保持由执行器完成。 | +| `compensate` | 误差驱动 PID 速度建议；橡胶松弛造成欠力时允许继续压入，始终受限。 | + +导数作用于测量值，积分在输出受限且误差继续推动饱和时冻结。速度同时受最大速度及剩余压入行程/最大样本间隔约束。每次保持有最长时限，不无限追赶松弛。模型测试覆盖标准线性固体的松弛行为，只证明算法路径可用，不代表实际橡胶模型已识别。 + +## 故障与执行责任 + +无效/非有限数据、缺少身份、未来/过期时间戳、重复/倒序/过大采样间隔、传感器或校准身份改变、力幅值超限、压入越界、保持超时及数值溢出都会锁存零输出，需显式 `reset()` 开始新保持。校准时保留有符号力；超力检查使用绝对值，不取绝对值伪造正法向力。 + +调用者必须在每个控制周期推进检查。没有新样本时可用最后一帧调用以触发停止；该版本也将重复样本作为故障，因此控制更新频率应跟随参考采样，而独立看门狗负责无数据时停止。接触尚未建立时不能运行该保持策略。 + +零速度建议不等于停止已经发出的运动。未来执行器必须保证唯一运动所有权、控制器端看门狗、限速/加速度与行程保护、命令确认及实际回读；故障或取消时发停止并核实停止。不得将每个周期的建议堆进现有 GUI 命令队列，也不能声称 `MotionPoller` 的 100 ms 刷新提供确定实时周期。本次骨架没有调用 DLL 或绕过 `MotionService`。 + +## 后续接入验收 + +先做硬件禁用状态的“适配器→投影→策略→记录”链路：证明掉线、饱和、过期、时钟跳变与取消保留原因，实际执行反馈可追溯到建议；然后才接受授权范围内的台架测试。保持实验分别记录恒力跟踪误差、峰值力、额外压入量和固定位置衰减曲线。PID 参数、滤波延迟、采样率、去皮、重力补偿和机械方向均须按具体设备实测。 + +当前测试位于 [`test_force_control.py`](../tests/test_force_control.py)，覆盖非平凡旋转/反力符号、无效性、两种松弛模式、采样时间、身份变化、故障锁存、输出限幅、抗积分饱和及松弛模型。未连接的适配器与真实执行器不属于已验证能力。 diff --git a/apps/autocal/src/orisys_autocal/force_control.py b/apps/autocal/src/orisys_autocal/force_control.py new file mode 100644 index 0000000..fac8574 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/force_control.py @@ -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 + ) diff --git a/apps/autocal/src/orisys_autocal/multipoint.py b/apps/autocal/src/orisys_autocal/multipoint.py index 6f90266..123535a 100644 --- a/apps/autocal/src/orisys_autocal/multipoint.py +++ b/apps/autocal/src/orisys_autocal/multipoint.py @@ -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): diff --git a/apps/autocal/src/orisys_autocal/reference_force.py b/apps/autocal/src/orisys_autocal/reference_force.py new file mode 100644 index 0000000..90b5881 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/reference_force.py @@ -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 diff --git a/apps/autocal/tests/test_force_control.py b/apps/autocal/tests/test_force_control.py new file mode 100644 index 0000000..d75db3a --- /dev/null +++ b/apps/autocal/tests/test_force_control.py @@ -0,0 +1,224 @@ +"""Offline force feedback contracts; no controller or sensor is opened.""" + +from dataclasses import replace +import math + +import pytest + +from orisys_autocal.force_control import ForceControlConfig, ForceController +from orisys_autocal.reference_force import ( + NormalForceSample, + WrenchSample, + project_wrench, +) +from orisys_autocal.multipoint import normal_force_n, parse_rotation + + +def config(**overrides): + values = dict( + target_n=2.0, + kp_mm_per_n_s=0.2, + ki_mm_per_n_s2=0.1, + kd_mm_per_n=0.0, + max_force_n=5.0, + max_speed_mm_s=0.5, + max_depth_mm=1.0, + max_sample_age_s=0.2, + max_duration_s=10.0, + ) + return ForceControlConfig(**(values | overrides)) + + +def sample(force=2.0, seconds=1.0, **overrides): + return NormalForceSample( + force_n=force, + t_host_mono_ns=int(seconds * 1e9), + source_id="reference-1", + calibration_id="tare-1", + **overrides, + ) + + +def update(controller, force=2.0, seconds=1.0, depth=0.3, **overrides): + return controller.update( + sample(force, seconds, **overrides), depth_mm=depth, now_ns=int(seconds * 1e9) + ) + + +def test_ft_projection_uses_rotation_polarity_and_preserves_identity(): + # Proper rotation: sensor +X -> tool +Z; tool reaction is negative compression. + rotation = parse_rotation([0, 0, -1, 0, 1, 0, 1, 0, 0]) + raw = WrenchSample( + (-3.0, 7.0, 8.0), 123, "ft-1", "tare-4", "ft", torque_nm=(0.1, 0.2, 0.3) + ) + measured = project_wrench( + raw, rotation=rotation, expected_frame="ft", compression_sign=-1 + ) + assert measured.force_n == 3.0 + assert measured.t_host_mono_ns == 123 + assert measured.source_id == "ft-1" and measured.calibration_id == "tare-4" + assert normal_force_n(-3, 7, 8, rotation) == -3.0 + assert raw.torque_nm == (0.1, 0.2, 0.3) + with pytest.raises(ValueError, match="frame"): + project_wrench( + raw, rotation=rotation, expected_frame="platform", compression_sign=-1 + ) + + +def test_projection_rejects_mirror_and_keeps_invalid_sample_invalid(): + raw = WrenchSample((1.0, 2.0, 3.0), 1, "ft", "tare", "ft", valid=False) + projected = project_wrench( + raw, + rotation=((1, 0, 0), (0, 1, 0), (0, 0, 1)), + expected_frame="ft", + compression_sign=1, + ) + assert not projected.valid + with pytest.raises(ValueError, match="determinant"): + project_wrench( + raw, + rotation=((-1, 0, 0), (0, 1, 0), (0, 0, 1)), + expected_frame="ft", + compression_sign=1, + ) + + +def test_force_drop_is_compensated_or_recorded_without_motion(): + active = ForceController(config(), mode="compensate") + passive = ForceController(config(), mode="record_relaxation") + for controller in (active, passive): + assert update(controller).velocity_mm_s == 0 + compensation = update(active, force=1.8, seconds=1.1) + observation = update(passive, force=1.8, seconds=1.1) + assert compensation.velocity_mm_s > 0 + assert observation.velocity_mm_s == 0 + assert observation.force_rate_n_s == pytest.approx(-2.0) + assert observation.sample.force_n == 1.8 and observation.depth_mm == 0.3 + assert observation.error_n == pytest.approx(0.2) + + +@pytest.mark.parametrize( + "field,value", + [ + ("kp_mm_per_n_s", math.nan), + ("max_depth_mm", 0), + ("max_sample_age_s", -1), + ("target_n", 6), + ("ki_mm_per_n_s2", -1), + ("max_duration_s", math.inf), + ], +) +def test_bad_configuration_is_rejected(field, value): + with pytest.raises(ValueError): + config(**{field: value}) + + +@pytest.mark.parametrize( + "fault", + [ + "invalid", + "stale", + "future", + "duplicate", + "gap", + "overforce", + "depth", + "nonfinite", + "identity", + "timeout", + ], +) +def test_faults_latch_zero_output_until_explicit_reset(fault): + controller = ForceController(config()) + update(controller) + item, now, depth = sample(1.0, 1.1), 1_100_000_000, 0.3 + if fault == "invalid": + item = replace(item, valid=False) + elif fault == "stale": + now = 1_400_000_000 + elif fault == "future": + now = 1_000_000_000 + elif fault == "duplicate": + item = sample(1.0, 1.0) + elif fault == "gap": + item, now = sample(1.0, 1.4), 1_400_000_000 + elif fault == "overforce": + item = sample(-6.0, 1.1) + elif fault == "depth": + depth = 1.1 + elif fault == "nonfinite": + item = sample(math.nan, 1.1) + elif fault == "identity": + item = replace(item, calibration_id="new-tare") + elif fault == "timeout": + controller = ForceController(config(max_duration_s=0.05)) + update(controller) + result = controller.update(item, depth_mm=depth, now_ns=now) + assert result.fault and result.velocity_mm_s == 0 + assert update(controller, seconds=1.2).fault == result.fault + controller.reset() + assert not update(controller, seconds=1.3).fault + + +def test_derivative_is_on_measurement_and_uses_sample_time(): + controller = ForceController( + config(kp_mm_per_n_s=0, ki_mm_per_n_s2=0, kd_mm_per_n=0.1), mode="compensate" + ) + update(controller) + assert update(controller, force=1.9, seconds=1.1).velocity_mm_s == pytest.approx( + 0.1 + ) + + +def test_saturation_does_not_accumulate_integral(): + controller = ForceController( + config(kp_mm_per_n_s=1, ki_mm_per_n_s2=1), mode="compensate" + ) + for index in range(10): + assert update(controller, force=0, seconds=1 + index * 0.1).velocity_mm_s == 0.5 + assert update(controller, force=2.1, seconds=2).velocity_mm_s < 0 + + +def test_pure_integral_can_saturate_without_losing_its_output(): + controller = ForceController( + config(kp_mm_per_n_s=0, ki_mm_per_n_s2=10), mode="compensate" + ) + update(controller, force=0) + assert update(controller, force=0, seconds=1.1).velocity_mm_s == 0.5 + assert update(controller, force=2.1, seconds=1.2).velocity_mm_s < 0 + + +def test_numerical_overflow_faults_instead_of_returning_a_velocity(): + controller = ForceController(config(kp_mm_per_n_s=1e308), mode="compensate") + assert update(controller, force=-2).fault == "numerical_failure" + + +def test_depth_horizon_limits_velocity_and_boundary_faults(): + controller = ForceController(config(), mode="compensate") + result = update(controller, force=0, depth=0.99) + assert result.velocity_mm_s <= 0.05 + 1e-12 + assert update(controller, force=0, seconds=1.1, depth=1.0).fault + + +def test_relaxing_rubber_model_tracks_force_without_unbounded_depth(): + # Standard linear solid: F = k_inf*x + q, q' = k_relax*x' - q/tau. + controller = ForceController( + config(max_sample_age_s=0.1, kp_mm_per_n_s=0.25, ki_mm_per_n_s2=0.25), + mode="compensate", + ) + depth, transient = 0.4, 1.2 + initial_force = 2 * depth + transient + peak = initial_force + for index in range(400): + force = 2 * depth + transient + decision = update( + controller, force=force, seconds=1 + index * 0.02, depth=depth + ) + assert not decision.fault + change = decision.velocity_mm_s * 0.02 + depth += change + transient += 3 * change - transient * 0.02 / 0.5 + peak = max(peak, force) + assert abs(force - 2.0) < 0.08 + assert 0.4 < depth <= 1.0 + assert peak < 2.3