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

复用法向力投影，区分持续补偿与固定位置松弛记录。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,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