cheng
112 lines
3.4 KiB
Python
112 lines
3.4 KiB
Python
from dataclasses import dataclass
|
|
|
|
|
|
DEFAULT_FORCE_CALIBRATION_POINTS = [
|
|
{"raw": 1000.0, "force_n": 0.0},
|
|
{"raw": 10000.0, "force_n": 0.377},
|
|
{"raw": 30000.0, "force_n": 1.377},
|
|
{"raw": 62000.0, "force_n": 2.377},
|
|
]
|
|
|
|
DEFAULT_NORMAL_FORCE_CALIBRATION = {
|
|
"enabled": True,
|
|
"method": "piecewise_linear",
|
|
"extrapolate": True,
|
|
"clamp_output_min": 0.0,
|
|
"points": [
|
|
{"fnormal": 1000.0, "force_n": 0.0},
|
|
{"fnormal": 10000.0, "force_n": 0.377},
|
|
{"fnormal": 30000.0, "force_n": 1.377},
|
|
{"fnormal": 62000.0, "force_n": 2.377},
|
|
],
|
|
}
|
|
|
|
DEFAULT_SHEAR_FORCE_CALIBRATION = {
|
|
"enabled": True,
|
|
"method": "piecewise_linear",
|
|
"extrapolate": True,
|
|
"clamp_output_min": 0.0,
|
|
"points": DEFAULT_FORCE_CALIBRATION_POINTS,
|
|
}
|
|
|
|
|
|
def parse_calibration_point(point):
|
|
if isinstance(point, dict):
|
|
raw = point.get("fnormal", point.get("raw", point.get("x")))
|
|
force_n = point.get("force_n", point.get("n", point.get("y")))
|
|
return float(raw), float(force_n)
|
|
if isinstance(point, (list, tuple)) and len(point) >= 2:
|
|
return float(point[0]), float(point[1])
|
|
raise ValueError(f"invalid calibration point: {point!r}")
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class ForceConverter:
|
|
points: tuple
|
|
unit: str = "N"
|
|
enabled: bool = True
|
|
extrapolate: bool = True
|
|
clamp_output_min: float | None = 0.0
|
|
|
|
@classmethod
|
|
def from_config(cls, calibration, config_name):
|
|
if calibration is None:
|
|
calibration = DEFAULT_NORMAL_FORCE_CALIBRATION
|
|
if not calibration.get("enabled", True):
|
|
return cls(points=(), unit="raw", enabled=False)
|
|
|
|
method = str(calibration.get("method", "piecewise_linear")).lower()
|
|
if method != "piecewise_linear":
|
|
raise ValueError(f"unsupported {config_name} method: {method}")
|
|
|
|
points = tuple(sorted(
|
|
parse_calibration_point(point)
|
|
for point in calibration.get("points", [])
|
|
))
|
|
if len(points) < 2:
|
|
raise ValueError(f"{config_name}.points must contain at least two points")
|
|
|
|
return cls(
|
|
points=points,
|
|
enabled=True,
|
|
extrapolate=bool(calibration.get("extrapolate", True)),
|
|
clamp_output_min=calibration.get("clamp_output_min", 0.0),
|
|
)
|
|
|
|
def convert(self, raw):
|
|
if not self.enabled:
|
|
return float(raw)
|
|
|
|
raw = float(raw)
|
|
points = self.points
|
|
if raw <= points[0][0]:
|
|
segment = (points[0], points[1])
|
|
if not self.extrapolate:
|
|
return self._clamp(points[0][1])
|
|
elif raw >= points[-1][0]:
|
|
segment = (points[-2], points[-1])
|
|
if not self.extrapolate:
|
|
return self._clamp(points[-1][1])
|
|
else:
|
|
segment = None
|
|
for left, right in zip(points, points[1:]):
|
|
if left[0] <= raw <= right[0]:
|
|
segment = (left, right)
|
|
break
|
|
if segment is None:
|
|
return self._clamp(points[-1][1])
|
|
|
|
(raw0, n0), (raw1, n1) = segment
|
|
if raw1 == raw0:
|
|
value = n0
|
|
else:
|
|
ratio = (raw - raw0) / (raw1 - raw0)
|
|
value = n0 + ratio * (n1 - n0)
|
|
return self._clamp(value)
|
|
|
|
def _clamp(self, value):
|
|
if self.clamp_output_min is not None:
|
|
value = max(float(self.clamp_output_min), value)
|
|
return value
|
|
|