加入独立参考力接口与离线恒力反馈骨架
复用法向力投影,区分持续补偿与固定位置松弛记录。189 项定向检查及 lint 通过;扩大检查的失败与限制见架构审查。
This commit is contained in:
@@ -28,6 +28,8 @@ Replaces the vendor example program it was ported from:
|
|||||||
|
|
||||||
## Capture workflow
|
## 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
|
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
|
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
|
step**, always stating why it is disabled. Alignment is only a request: the move
|
||||||
|
|||||||
@@ -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),覆盖非平凡旋转/反力符号、无效性、两种松弛模式、采样时间、身份变化、故障锁存、输出限幅、抗积分饱和及松弛模型。未连接的适配器与真实执行器不属于已验证能力。
|
||||||
@@ -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,
|
make_record_config,
|
||||||
require_codec_available,
|
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"
|
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:
|
def _finite(value: float, label: str) -> float:
|
||||||
number = float(value)
|
number = float(value)
|
||||||
if not math.isfinite(number):
|
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
|
||||||
@@ -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
|
||||||
Reference in New Issue
Block a user