ykcheungab
225 lines
7.3 KiB
Python
225 lines
7.3 KiB
Python
"""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
|