Feature/force cal #17

Merged
zhangyijian merged 5 commits from feature/force_cal into dev 2026-08-26 20:22:28 +08:00
10 changed files with 272 additions and 33 deletions
Showing only changes of commit 577e3fe528 - Show all commits
+8
View File
@@ -0,0 +1,8 @@
# Non-Python files needed for sdist -> wheel builds from an unpacked sdist.
include pyproject.toml
include setup.py
include README.md
recursive-include config *.json
recursive-include sensors *
recursive-include build/common *.py
recursive-include build/wheel *.py
+36 -1
View File
@@ -4,6 +4,7 @@ from __future__ import annotations
import ast
import importlib.util
import shutil
import subprocess
import sys
from pathlib import Path
@@ -102,7 +103,11 @@ def bundle_items(repo_root: Path) -> list[dict]:
def bundle_data_files(repo_root: Path) -> list[tuple[str, list[str]]]:
"""Map bundle items marked dev_install=true to setuptools data_files entries."""
"""Map bundle items marked ``dev_install=true`` to install destinations.
Kept for tests / tooling. Prefer :func:`stage_dev_bundle_into` for pip
installs so files land inside ``site-packages/orisys/``.
"""
resolve_bundle_source_files = _load_wheel_layout_module().resolve_bundle_source_files
grouped: dict[str, list[str]] = {}
@@ -123,3 +128,33 @@ def bundle_data_files(repo_root: Path) -> list[tuple[str, list[str]]]:
grouped.setdefault(dest, []).append(rel_src)
return [(dest, sorted(files)) for dest, files in sorted(grouped.items())]
def stage_dev_bundle_into(repo_root: Path, pkg_dir: Path) -> list[str]:
"""Copy ``dev_install`` bundle files under ``pkg_dir`` (the orisys package).
Returns package-relative paths that were written (e.g. ``sensors/box/...``).
"""
resolve_bundle_source_files = _load_wheel_layout_module().resolve_bundle_source_files
packaged: list[str] = []
for item in bundle_items(repo_root):
if not item.get("dev_install", False):
continue
src_dir = repo_root / item["source"]
dst_dir = pkg_dir / item["target"]
dst_dir.mkdir(parents=True, exist_ok=True)
for src_file in resolve_bundle_source_files(repo_root, item):
if item.get("preserve_tree", False):
rel = src_file.relative_to(src_dir)
dst_file = dst_dir / rel
else:
rel = Path(src_file.name)
dst_file = dst_dir / src_file.name
dst_file.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(src_file, dst_file)
packaged.append(f"{item['target']}/{rel.as_posix()}")
return packaged
+12 -1
View File
@@ -6,12 +6,19 @@ import shutil
from pathlib import Path
# Required files that must ship with the wheel for ORY-FINGER runtime/GUI.
# Accept both the flat layout and a nested assets/finger/ checkout tree.
REQUIRED_ASSET_RELPATHS = (
"finger/assets/30fps-0703/mesh.npz",
"finger/assets/30fps-0703/camera_intrinsics.npz",
"finger/assets/30fps-0703/world_to_cam.npy",
"finger/assets/visualization/world_to_cam.npy",
)
REQUIRED_ASSET_RELPATH_ALTERNATES = {
"finger/assets/30fps-0703/mesh.npz": "finger/assets/finger/30fps-0703/mesh.npz",
"finger/assets/30fps-0703/camera_intrinsics.npz": "finger/assets/finger/30fps-0703/camera_intrinsics.npz",
"finger/assets/30fps-0703/world_to_cam.npy": "finger/assets/finger/30fps-0703/world_to_cam.npy",
"finger/assets/visualization/world_to_cam.npy": "finger/assets/finger/visualization/world_to_cam.npy",
}
def resolve_bundle_source_files(repo_root: Path, item: dict) -> list[Path]:
@@ -61,7 +68,11 @@ def verify_required_assets(repo_root: Path, items: list[dict]) -> None:
continue
resolved.add(rel)
missing = [rel for rel in REQUIRED_ASSET_RELPATHS if rel not in resolved]
missing = []
for rel in REQUIRED_ASSET_RELPATHS:
alt = REQUIRED_ASSET_RELPATH_ALTERNATES.get(rel)
if rel not in resolved and (alt is None or alt not in resolved):
missing.append(rel)
if missing:
joined = ", ".join(missing)
raise FileNotFoundError(
+27 -6
View File
@@ -49,8 +49,23 @@ def main():
sensor.compute_contact()
# 步骤 2.4:读取结果
fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = sensor.read_info(
# 步骤 2.4:读取结果(FNORMAL/FSHEAR* 在有 sensors/box/force_calibration.json 时为标定后牛顿力;
# *_RAW 始终为原始积分量)
(
fps,
flow,
vnormal,
img,
contour,
centroid,
depth_map,
fnormal,
fshearx,
fsheary,
fnormal_raw,
fshearx_raw,
fsheary_raw,
) = sensor.read_info(
sensor.info.FPS,
sensor.info.VRAW,
sensor.info.VNORMAL,
@@ -60,7 +75,10 @@ def main():
sensor.info.DEPTH,
sensor.info.FNORMAL,
sensor.info.FSHEARX,
sensor.info.FSHEARY
sensor.info.FSHEARY,
sensor.info.FNORMAL_RAW,
sensor.info.FSHEARX_RAW,
sensor.info.FSHEARY_RAW,
)
# 步骤 3:可视化显示
@@ -89,10 +107,13 @@ def main():
# 应用伪彩色映射
cv2.imshow("depth", cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))
# 步骤 3.4:打印关键结果
# 步骤 3.4:打印关键结果(标定力 + 原始力)
print(
f"FPS={fps:.2f}, 法向力={fnormal:.4f}, 切向力X={fshearx:.4f}, "
f"切向力Y={fsheary:.4f}, 深度图尺寸={depth_map.shape}, 原始分辨率={sensor.img_size_raw}"
f"FPS={fps:.2f}, "
f"法向力={fnormal:.4f} N (raw={fnormal_raw:.4f}), "
f"切向力X={fshearx:.4f} N (raw={fshearx_raw:.4f}), "
f"切向力Y={fsheary:.4f} N (raw={fsheary_raw:.4f}), "
f"深度图尺寸={depth_map.shape}, 原始分辨率={sensor.img_size_raw}"
)
# 步骤 3.5:键盘控制
+10 -1
View File
@@ -79,7 +79,16 @@ dev_install = true
name = "sensors"
source = "sensors"
target = "sensors"
patterns = ["**/*.json", "finger/assets/30fps*/*", "finger/assets/visualization/*"]
# JSON bundles (sensor.json, force_calibration.json) plus finger mesh/viz assets.
# Prefer the flat layout sensors/finger/assets/{30fps-*,visualization}/;
# also accept a nested assets/finger/ tree if present in a checkout.
patterns = [
"**/*.json",
"finger/assets/30fps*/*",
"finger/assets/visualization/*",
"finger/assets/finger/30fps*/*",
"finger/assets/finger/visualization/*",
]
preserve_tree = true
dev_install = true
-8
View File
@@ -1,8 +0,0 @@
{
"normal_force": {
"model": "linear",
"slope": 3.92685554668795e-05,
"offset": 0.0217596967278535,
"unit": "N"
}
}
+33 -4
View File
@@ -1,13 +1,42 @@
"""Setuptools entry point; bundle rules are read from pyproject.toml."""
"""Setuptools entry point; bundle rules are read from pyproject.toml.
``dev_install`` bundle items (configs/sensors) are staged into the ``orisys``
package during build so ``pip install .`` places them at
``site-packages/orisys/...`` (importlib / configs resolvers), not under
``sys.prefix/orisys`` via deprecated ``data_files``.
"""
from __future__ import annotations
from pathlib import Path
import sys
from pathlib import Path
from setuptools import setup
from setuptools.command.build_py import build_py as _build_py
REPO_ROOT = Path(__file__).resolve().parent
sys.path.insert(0, str(REPO_ROOT / "build" / "common"))
from pyproject_utils import bundle_data_files
from pyproject_utils import stage_dev_bundle_into # noqa: E402
setup(data_files=bundle_data_files(REPO_ROOT))
class build_py(_build_py):
def run(self) -> None:
super().run()
# After pure modules are copied, stage configs/sensors into build/lib/orisys.
pkg_dir = Path(self.build_lib) / "orisys"
pkg_dir.mkdir(parents=True, exist_ok=True)
stage_dev_bundle_into(REPO_ROOT, pkg_dir)
setup(
cmdclass={"build_py": build_py},
# Declared so wheels record the staged files; actual copy happens in build_py.
package_data={
"orisys": [
"configs/*.json",
"sensors/**/*",
],
},
include_package_data=True,
)
+62 -7
View File
@@ -11,6 +11,7 @@ import numpy as np
import cv2
import time
from enum import Enum, auto
from pathlib import Path
from typing import Optional, Tuple
# from threading import Thread, Event
# from queue import Queue, Empty
@@ -22,7 +23,12 @@ from . import configs
from .get_depth import Contact3D
from .backend_availability import (check_cupy_available, check_opencl_available)
from .frame_processors import create_frame_processor
from .signal_processing import DEFAULT_SAMPLE_RATE_HZ, LowpassFilter
from .signal_processing import (
DEFAULT_SAMPLE_RATE_HZ,
LowpassFilter,
calibrate_normal_force,
calibrate_shear_components,
)
from .capture import (
CaptureConfig,
CaptureWorker,
@@ -148,6 +154,7 @@ class Sensor:
self.n_boxes = None
self.fnormal = self.fshearx = self.fsheary = self.ftor = 0.0
self._force_raw = (0.0, 0.0, 0.0)
self._force_cal_sensor_name = self._resolve_force_calibration_sensor_name()
self._lowpass_enabled = False
self._lowpass_cutoff_hz = 0.0
self._lowpass_sample_rate_hz = float(DEFAULT_SAMPLE_RATE_HZ)
@@ -158,6 +165,13 @@ class Sensor:
order=self._lowpass_order,
n_channels=3,
)
if self.verbose:
if self._force_cal_sensor_name:
print(
f"Force calibration: sensors/{self._force_cal_sensor_name}/force_calibration.json"
)
else:
print("Force calibration: raw (no force_calibration.json)")
# Contact Data Variables (public)
self.centroid = (0, 0)
@@ -790,17 +804,58 @@ class Sensor:
self._update_hhd_mask_cache()
return self
def _resolve_force_calibration_sensor_name(self) -> Optional[str]:
"""Return sensors/<name> when force_calibration.json exists; else None.
Lookup order:
1. ``config_name`` stem / product alias (e.g. ``box``)
2. ``sensors/<name>/...`` path segment in ``config_name``
Missing JSON means ``FNORMAL/FSHEAR*`` stay raw (no hardcoded fallback).
"""
text = str(self.config_name or "").strip().replace("\\", "/")
if not text:
return None
candidates: list[str] = []
stem = Path(text).stem if text else ""
if stem:
candidates.append(stem)
parts = [p for p in text.split("/") if p]
if "sensors" in parts:
idx = parts.index("sensors")
if idx + 1 < len(parts):
candidates.append(parts[idx + 1])
for name in configs.PRODUCT_BUNDLES:
if text == name or text.endswith(f"/sensors/{name}") or text.endswith(f"/sensors/{name}/sensor.json"):
candidates.append(name)
seen: set[str] = set()
for candidate in candidates:
alias = configs.CONFIG_ALIASES.get(candidate, candidate)
for name in (alias, candidate):
if name in seen:
continue
seen.add(name)
if configs.load_force_calibration(name) is not None:
return name
return None
def _apply_force_filter(self):
raw = np.asarray(self._force_raw, dtype=np.float64)
if self._lowpass_enabled and self._lowpass_cutoff_hz > 0:
filtered = self._force_filter.process(raw)
self.fnormal, self.fshearx, self.fsheary = (float(v) for v in filtered)
fn, fx, fy = (float(v) for v in filtered)
else:
self.fnormal, self.fshearx, self.fsheary = (
float(raw[0]),
float(raw[1]),
float(raw[2]),
)
fn, fx, fy = float(raw[0]), float(raw[1]), float(raw[2])
# Apply sensors/<name>/force_calibration.json when present; otherwise keep raw.
name = self._force_cal_sensor_name
if name is not None:
fn = float(calibrate_normal_force(fn, sensor_name=name))
fx, fy = calibrate_shear_components(fx, fy, sensor_name=name)
fx, fy = float(fx), float(fy)
self.fnormal, self.fshearx, self.fsheary = fn, fx, fy
# def export_config(self, path="./config/exported_config.json"):
# """Return current configuration dict."""
+22 -2
View File
@@ -57,13 +57,33 @@ def test_pyproject_bundle_manifest(packaging_utils):
assert all("/" in path and "*" not in path for path in config_files)
assert any(path.endswith(".json") for path in config_files)
sensor_dirs = [dest for dest in data_files if dest.startswith("orisys/sensors/")]
sensor_dirs = [dest for dest in data_files if dest.startswith("orisys/sensors")]
assert sensor_dirs
assert any("finger/assets" in dest for dest in sensor_dirs)
all_sensor_files = [path for files in (data_files[d] for d in sensor_dirs) for path in files]
assert any(path.endswith("force_calibration.json") for path in all_sensor_files)
assert any("finger" in path and path.endswith(".json") for path in all_sensor_files)
# Finger mesh/viz assets (flat or nested assets/finger/ checkout).
assert any("finger/assets" in dest or any("finger/assets" in p for p in data_files[dest]) for dest in sensor_dirs)
assert all("*" not in path for files in data_files.values() for path in files)
assert "examples" not in data_files
def test_stage_dev_bundle_into_writes_sensors_under_package(tmp_path):
import sys
build_dir = REPO_ROOT / "build"
if str(build_dir) not in sys.path:
sys.path.insert(0, str(build_dir))
from common.pyproject_utils import stage_dev_bundle_into
pkg = tmp_path / "orisys"
packaged = stage_dev_bundle_into(REPO_ROOT, pkg)
assert any(p.replace("\\", "/").endswith("sensors/box/force_calibration.json") for p in packaged)
assert (pkg / "sensors" / "box" / "force_calibration.json").is_file()
assert (pkg / "configs").is_dir()
assert any((pkg / "configs").glob("*.json"))
def test_bundle_explicit_files():
import sys
+62 -3
View File
@@ -129,7 +129,16 @@ def test_configure_lowpass_filters_read_info_and_keeps_raw():
sensor.info.FSHEARX,
sensor.info.FSHEARY,
)
assert (fn, fx, fy) == pytest.approx((1000.0, -200.0, 50.0))
fn_raw, fx_raw, fy_raw = sensor.read_info(
sensor.info.FNORMAL_RAW,
sensor.info.FSHEARX_RAW,
sensor.info.FSHEARY_RAW,
)
assert (fn_raw, fx_raw, fy_raw) == pytest.approx((1000.0, -200.0, 50.0))
assert fn == pytest.approx(calibrate_normal_force(1000.0, sensor_name="box"))
assert (fx, fy) == pytest.approx(
calibrate_shear_components(-200.0, 50.0, sensor_name="box")
)
sensor.configure(lowpass_cutoff_hz=5.0, lowpass_sample_rate_hz=30.0)
noisy = [(1000.0 + (200.0 if i % 2 else -200.0), -200.0, 50.0) for i in range(40)]
@@ -166,7 +175,10 @@ def test_configure_lowpass_disabled_or_zero_cutoff_returns_raw():
sensor.info.FSHEARX,
sensor.info.FSHEARY,
)
assert (fn, fx, fy) == pytest.approx((111.0, 222.0, 333.0))
assert fn == pytest.approx(calibrate_normal_force(111.0, sensor_name="box"))
assert (fx, fy) == pytest.approx(
calibrate_shear_components(222.0, 333.0, sensor_name="box")
)
sensor.configure(lowpass_cutoff_hz=5.0, lowpass_enabled=False)
sensor._force_raw = (7.0, 8.0, 9.0)
@@ -177,12 +189,59 @@ def test_configure_lowpass_disabled_or_zero_cutoff_returns_raw():
sensor.info.FSHEARY,
)
fn_raw = sensor.read_info(sensor.info.FNORMAL_RAW)[0]
assert (fn, fx, fy) == pytest.approx((7.0, 8.0, 9.0))
assert fn == pytest.approx(calibrate_normal_force(7.0, sensor_name="box"))
assert (fx, fy) == pytest.approx(
calibrate_shear_components(8.0, 9.0, sensor_name="box")
)
assert fn_raw == pytest.approx(7.0)
finally:
sensor.disconnect()
def test_box_read_info_applies_force_calibration_json():
sensor = _external_sensor()
try:
assert sensor._force_cal_sensor_name == "box"
sensor._force_raw = (10000.0, 36000.0, -36000.0)
sensor._apply_force_filter()
fn, fx, fy = sensor.read_info(
sensor.info.FNORMAL,
sensor.info.FSHEARX,
sensor.info.FSHEARY,
)
fn_raw, fx_raw, fy_raw = sensor.read_info(
sensor.info.FNORMAL_RAW,
sensor.info.FSHEARX_RAW,
sensor.info.FSHEARY_RAW,
)
assert (fn_raw, fx_raw, fy_raw) == pytest.approx((10000.0, 36000.0, -36000.0))
assert fn == pytest.approx(0.377)
assert fx == pytest.approx(1.19)
assert fy == pytest.approx(-1.19)
finally:
sensor.disconnect()
def test_read_info_force_stays_raw_without_calibration_json(monkeypatch):
monkeypatch.setattr(
"orisys.sensor.configs.load_force_calibration",
lambda _name: None,
)
sensor = Sensor("external", config_name="box", verbose=False, backend="cpu")
try:
assert sensor._force_cal_sensor_name is None
sensor._force_raw = (111.0, -22.0, 33.0)
sensor._apply_force_filter()
fn, fx, fy = sensor.read_info(
sensor.info.FNORMAL,
sensor.info.FSHEARX,
sensor.info.FSHEARY,
)
assert (fn, fx, fy) == pytest.approx((111.0, -22.0, 33.0))
finally:
sensor.disconnect()
def test_configure_hhd_downsample_invalidates_solver_cache():
sensor = _external_sensor()
try: