update csv export and added fixed mesh axis drawing #7
+1
-1
@@ -59,7 +59,7 @@ pip install -e ".[gui]"
|
|||||||
## Notes
|
## Notes
|
||||||
|
|
||||||
- `finger_pipeline_3dview.py` is the main interactive demo. It uses `gui/app_paths.py` so the same entry point works in editable and packaged runs.
|
- `finger_pipeline_3dview.py` is the main interactive demo. It uses `gui/app_paths.py` so the same entry point works in editable and packaged runs.
|
||||||
- Dashboard controls: click **启动** to start acquisition; `Q` quits, `R` resets, `S` starts recording, `E` stops recording, and `Esc` toggles fullscreen.
|
- Dashboard controls: click **启动** to start acquisition; use **Record CSV / Stop CSV** and the CSV save-directory browser to log contact/force data; `Q` quits, `R` resets, `S` starts video recording, `E` stops video recording, and `Esc` toggles fullscreen.
|
||||||
- `finger_pipeline.py` is the leanest finger example if you want to validate UV unroll + force/contact processing without the full dashboard.
|
- `finger_pipeline.py` is the leanest finger example if you want to validate UV unroll + force/contact processing without the full dashboard.
|
||||||
- `finger_pipeline.py` uses `orisys.finger.FingerRuntime` as the high-level entry point, then runs tactile analysis through `runtime.tactile`.
|
- `finger_pipeline.py` uses `orisys.finger.FingerRuntime` as the high-level entry point, then runs tactile analysis through `runtime.tactile`.
|
||||||
- In `--profile` mode, the example splits the loop into capture, UV unroll, deformation, contact, and display stages so you can measure each step separately.
|
- In `--profile` mode, the example splits the loop into capture, UV unroll, deformation, contact, and display stages so you can measure each step separately.
|
||||||
|
|||||||
@@ -2,6 +2,7 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
import sys
|
import sys
|
||||||
import time
|
import time
|
||||||
|
|
||||||
@@ -17,6 +18,7 @@ from PyQt5.QtWidgets import (
|
|||||||
)
|
)
|
||||||
|
|
||||||
from pipeline.config import PipelineConfig
|
from pipeline.config import PipelineConfig
|
||||||
|
from pipeline.csv_recorder import CsvRecorder
|
||||||
from pipeline.service import FingerPipeline
|
from pipeline.service import FingerPipeline
|
||||||
|
|
||||||
from .app_paths import resource_path
|
from .app_paths import resource_path
|
||||||
@@ -54,6 +56,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self.config = config
|
self.config = config
|
||||||
self.layout = layout
|
self.layout = layout
|
||||||
self.pipeline = FingerPipeline(config)
|
self.pipeline = FingerPipeline(config)
|
||||||
|
self.csv_recorder = CsvRecorder(config.csv_save_dir)
|
||||||
self._running = False
|
self._running = False
|
||||||
self._frame_index = 0
|
self._frame_index = 0
|
||||||
self._wall_fps = 0.0
|
self._wall_fps = 0.0
|
||||||
@@ -103,6 +106,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self.settings_panel.set_config_path(self.config.config)
|
self.settings_panel.set_config_path(self.config.config)
|
||||||
self.settings_panel.set_calibration_path(self.config.calib or "")
|
self.settings_panel.set_calibration_path(self.config.calib or "")
|
||||||
self.settings_panel.set_finger_export_dir(self.config.export_dir)
|
self.settings_panel.set_finger_export_dir(self.config.export_dir)
|
||||||
|
self.settings_panel.set_csv_save_dir(self.config.csv_save_dir)
|
||||||
|
|
||||||
self.settings_panel.log_visibility_changed.connect(self._set_log_panel_visible)
|
self.settings_panel.log_visibility_changed.connect(self._set_log_panel_visible)
|
||||||
self.settings_panel.free_rotation_changed.connect(self.mesh_viewport.set_free_rotation)
|
self.settings_panel.free_rotation_changed.connect(self.mesh_viewport.set_free_rotation)
|
||||||
@@ -112,9 +116,12 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self.settings_panel.browse_video_requested.connect(self._select_camera)
|
self.settings_panel.browse_video_requested.connect(self._select_camera)
|
||||||
self.settings_panel.browse_config_requested.connect(self._select_config)
|
self.settings_panel.browse_config_requested.connect(self._select_config)
|
||||||
self.settings_panel.browse_finger_assets_requested.connect(self._select_finger_assets)
|
self.settings_panel.browse_finger_assets_requested.connect(self._select_finger_assets)
|
||||||
|
self.settings_panel.browse_csv_dir_requested.connect(self._select_csv_dir)
|
||||||
|
self.settings_panel.csv_record_toggled.connect(self._on_csv_record_toggled)
|
||||||
self.settings_panel.btn_play_pause.clicked.connect(self._on_play_pause_clicked)
|
self.settings_panel.btn_play_pause.clicked.connect(self._on_play_pause_clicked)
|
||||||
self.settings_panel.btn_reset_tracker.clicked.connect(self._on_reset)
|
self.settings_panel.btn_reset_tracker.clicked.connect(self._on_reset)
|
||||||
self.settings_panel.set_play_pause_state("run")
|
self.settings_panel.set_play_pause_state("run")
|
||||||
|
self.settings_panel.set_csv_controls_for_pipeline(False)
|
||||||
|
|
||||||
self._panel_widgets: dict[str, QWidget] = {}
|
self._panel_widgets: dict[str, QWidget] = {}
|
||||||
self._panel_widgets["mesh"] = self.mesh_viewport
|
self._panel_widgets["mesh"] = self.mesh_viewport
|
||||||
@@ -314,6 +321,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
cal = self.settings_panel.calibration_path()
|
cal = self.settings_panel.calibration_path()
|
||||||
self.config.calib = cal or None
|
self.config.calib = cal or None
|
||||||
self.config.export_dir = self.settings_panel.finger_export_dir()
|
self.config.export_dir = self.settings_panel.finger_export_dir()
|
||||||
|
self.config.csv_save_dir = self.settings_panel.csv_save_dir() or "./outputs/csv"
|
||||||
|
|
||||||
try:
|
try:
|
||||||
self.pipeline = FingerPipeline(self.config)
|
self.pipeline = FingerPipeline(self.config)
|
||||||
@@ -334,6 +342,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self.timer.start(self.layout.timer_interval_ms)
|
self.timer.start(self.layout.timer_interval_ms)
|
||||||
self._set_play_pause_button("pause")
|
self._set_play_pause_button("pause")
|
||||||
self.settings_panel.btn_reset_tracker.setEnabled(True)
|
self.settings_panel.btn_reset_tracker.setEnabled(True)
|
||||||
|
self.settings_panel.set_csv_controls_for_pipeline(True)
|
||||||
print(
|
print(
|
||||||
f"▶ 启动 — 摄像机={self.config.video}, 配置={self.config.config}, "
|
f"▶ 启动 — 摄像机={self.config.video}, 配置={self.config.config}, "
|
||||||
f"标定={self.config.calib or '(none)'}, 资产={self.config.export_dir}"
|
f"标定={self.config.calib or '(none)'}, 资产={self.config.export_dir}"
|
||||||
@@ -342,6 +351,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
def _on_stop(self) -> None:
|
def _on_stop(self) -> None:
|
||||||
if not self._running:
|
if not self._running:
|
||||||
return
|
return
|
||||||
|
self._stop_csv_recording(announce=True)
|
||||||
self._running = False
|
self._running = False
|
||||||
self.timer.stop()
|
self.timer.stop()
|
||||||
self.log_handler.clear_status_line()
|
self.log_handler.clear_status_line()
|
||||||
@@ -354,6 +364,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self._set_play_pause_button("run")
|
self._set_play_pause_button("run")
|
||||||
self.settings_panel.btn_reset_tracker.setEnabled(False)
|
self.settings_panel.btn_reset_tracker.setEnabled(False)
|
||||||
self.settings_panel.set_finger_export_dir_enabled(True)
|
self.settings_panel.set_finger_export_dir_enabled(True)
|
||||||
|
self.settings_panel.set_csv_controls_for_pipeline(False)
|
||||||
|
|
||||||
def _on_reset(self) -> None:
|
def _on_reset(self) -> None:
|
||||||
if not self._running:
|
if not self._running:
|
||||||
@@ -413,10 +424,26 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
profile.record("total", total_ms)
|
profile.record("total", total_ms)
|
||||||
self.pipeline.record_total_frame(total_ms)
|
self.pipeline.record_total_frame(total_ms)
|
||||||
|
|
||||||
|
measurement = self.pipeline.measure_for_csv(frame, self._frame_index)
|
||||||
|
if self.csv_recorder.is_recording:
|
||||||
|
self.csv_recorder.write(measurement)
|
||||||
|
|
||||||
flow_mag = 0.0
|
flow_mag = 0.0
|
||||||
if frame.flow.size:
|
if frame.flow.size:
|
||||||
flow_mag = float((frame.flow[:, :, 0] ** 2 + frame.flow[:, :, 1] ** 2).mean() ** 0.5)
|
flow_mag = float((frame.flow[:, :, 0] ** 2 + frame.flow[:, :, 1] ** 2).mean() ** 0.5)
|
||||||
|
|
||||||
|
contact_xyz_mm = None
|
||||||
|
if (
|
||||||
|
math.isfinite(measurement.contact_x)
|
||||||
|
and math.isfinite(measurement.contact_y)
|
||||||
|
and math.isfinite(measurement.contact_z)
|
||||||
|
):
|
||||||
|
contact_xyz_mm = (
|
||||||
|
measurement.contact_x,
|
||||||
|
measurement.contact_y,
|
||||||
|
measurement.contact_z,
|
||||||
|
)
|
||||||
|
|
||||||
log_cfg = layout.log_config
|
log_cfg = layout.log_config
|
||||||
if log_cfg.status_enabled:
|
if log_cfg.status_enabled:
|
||||||
self.log_handler.set_status_line(
|
self.log_handler.set_status_line(
|
||||||
@@ -428,6 +455,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
flow_rms=flow_mag,
|
flow_rms=flow_mag,
|
||||||
mask_coverage=frame.mask_coverage,
|
mask_coverage=frame.mask_coverage,
|
||||||
wall_fps=self._wall_fps,
|
wall_fps=self._wall_fps,
|
||||||
|
contact_xyz_mm=contact_xyz_mm,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -460,6 +488,50 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
if path:
|
if path:
|
||||||
self.settings_panel.set_finger_export_dir(path)
|
self.settings_panel.set_finger_export_dir(path)
|
||||||
|
|
||||||
|
def _select_csv_dir(self) -> None:
|
||||||
|
path = QFileDialog.getExistingDirectory(
|
||||||
|
self,
|
||||||
|
"选择 CSV 保存目录",
|
||||||
|
self.settings_panel.csv_save_dir() or "./outputs/csv",
|
||||||
|
)
|
||||||
|
if path:
|
||||||
|
self.settings_panel.set_csv_save_dir(path)
|
||||||
|
self.config.csv_save_dir = path
|
||||||
|
|
||||||
|
def _on_csv_record_toggled(self, start: bool) -> None:
|
||||||
|
if start:
|
||||||
|
self._start_csv_recording()
|
||||||
|
else:
|
||||||
|
self._stop_csv_recording(announce=True)
|
||||||
|
|
||||||
|
def _start_csv_recording(self) -> None:
|
||||||
|
if not self._running:
|
||||||
|
print("请先启动传感器后再记录 CSV。")
|
||||||
|
self.settings_panel.set_csv_recording(False)
|
||||||
|
return
|
||||||
|
save_dir = self.settings_panel.csv_save_dir() or "./outputs/csv"
|
||||||
|
self.config.csv_save_dir = save_dir
|
||||||
|
try:
|
||||||
|
path = self.csv_recorder.start(save_dir)
|
||||||
|
except OSError as exc:
|
||||||
|
print(f"无法开始 CSV 记录:{exc}")
|
||||||
|
self.settings_panel.set_csv_recording(False)
|
||||||
|
return
|
||||||
|
self.settings_panel.set_csv_recording(True)
|
||||||
|
print(f"开始 CSV 记录:{path}")
|
||||||
|
|
||||||
|
def _stop_csv_recording(self, *, announce: bool = False) -> None:
|
||||||
|
if not self.csv_recorder.is_recording:
|
||||||
|
self.settings_panel.set_csv_recording(False)
|
||||||
|
return
|
||||||
|
path, count = self.csv_recorder.stop()
|
||||||
|
self.settings_panel.set_csv_recording(False)
|
||||||
|
if announce:
|
||||||
|
if path is not None:
|
||||||
|
print(f"CSV 记录结束:{path},总行数:{count}")
|
||||||
|
else:
|
||||||
|
print(f"CSV 记录结束,总行数:{count}")
|
||||||
|
|
||||||
def keyPressEvent(self, event):
|
def keyPressEvent(self, event):
|
||||||
key = event.key()
|
key = event.key()
|
||||||
if key == Qt.Key_Escape:
|
if key == Qt.Key_Escape:
|
||||||
@@ -483,6 +555,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
|
|
||||||
def closeEvent(self, event):
|
def closeEvent(self, event):
|
||||||
self.timer.stop()
|
self.timer.stop()
|
||||||
|
self._stop_csv_recording(announce=True)
|
||||||
self.mesh_viewport.close_renderer()
|
self.mesh_viewport.close_renderer()
|
||||||
self.pipeline.close()
|
self.pipeline.close()
|
||||||
event.accept()
|
event.accept()
|
||||||
@@ -493,6 +566,7 @@ def run_dashboard(config: PipelineConfig, *, layout: DashboardLayout = LAYOUT) -
|
|||||||
from .app import get_or_create_qapp
|
from .app import get_or_create_qapp
|
||||||
|
|
||||||
print("\n按键说明:Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏")
|
print("\n按键说明:Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏")
|
||||||
|
print("GUI:Record CSV / Stop CSV 写入接触点与力到所选目录。")
|
||||||
if config.profile:
|
if config.profile:
|
||||||
print(f"Profiling enabled (report every {config.profile_every} frames).")
|
print(f"Profiling enabled (report every {config.profile_every} frames).")
|
||||||
print("Log FPS= is end-to-end dashboard rate; sdk= is Sensor internal rate.")
|
print("Log FPS= is end-to-end dashboard rate; sdk= is Sensor internal rate.")
|
||||||
|
|||||||
+30
-5
@@ -19,6 +19,9 @@ MeshLighting (below) controls the 3D mesh panel sun light:
|
|||||||
|
|
||||||
MESH_BACKGROUND (below) sets the 3D mesh panel clear color (RGB 0–255).
|
MESH_BACKGROUND (below) sets the 3D mesh panel clear color (RGB 0–255).
|
||||||
|
|
||||||
|
MESH_AXIS_OVERLAY (below) controls the fixed top-right pre-rendered 3D axis triad:
|
||||||
|
enabled, padding_px, size_px, opacity
|
||||||
|
|
||||||
MESH_CONTACT_ARROW (below) controls the yellow 3D contact arrow on the mesh panel:
|
MESH_CONTACT_ARROW (below) controls the yellow 3D contact arrow on the mesh panel:
|
||||||
reference_normal_force — Fn at which arrow reaches full size (log/power/linear curve)
|
reference_normal_force — Fn at which arrow reaches full size (log/power/linear curve)
|
||||||
force_strength_mode — "log" | "power" | "linear"
|
force_strength_mode — "log" | "power" | "linear"
|
||||||
@@ -87,7 +90,7 @@ TOP_ROW = PanelRow(
|
|||||||
PanelSlot("mesh", "3D mesh", width_px=400, height_px=400),
|
PanelSlot("mesh", "3D mesh", width_px=400, height_px=400),
|
||||||
PanelSlot("arrows", "UV Unrolled", width_px=400, height_px=400, enabled=True),
|
PanelSlot("arrows", "UV Unrolled", width_px=400, height_px=400, enabled=True),
|
||||||
PanelSlot("charts", "Force charts", width_px=300, height_px=400),
|
PanelSlot("charts", "Force charts", width_px=300, height_px=400),
|
||||||
PanelSlot("settings", "Settings", width_px=200, height_px=0),
|
PanelSlot("settings", "Settings", width_px=250, height_px=0),
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -142,6 +145,20 @@ class MeshBackground:
|
|||||||
|
|
||||||
MESH_BACKGROUND = MeshBackground(color=(0, 0, 0))
|
MESH_BACKGROUND = MeshBackground(color=(0, 0, 0))
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MeshAxisOverlay:
|
||||||
|
"""Fixed pre-rendered XYZ triad HUD on the 3D mesh RGB output."""
|
||||||
|
|
||||||
|
enabled: bool = True
|
||||||
|
padding_px: int = 12
|
||||||
|
size_px: int = 96
|
||||||
|
opacity: float = 1.0
|
||||||
|
|
||||||
|
|
||||||
|
MESH_AXIS_OVERLAY = MeshAxisOverlay()
|
||||||
|
|
||||||
|
|
||||||
# 3D mesh contact arrow — edit here for dashboard tuning (see ContactArrowStyle in viz/contact_arrow.py).
|
# 3D mesh contact arrow — edit here for dashboard tuning (see ContactArrowStyle in viz/contact_arrow.py).
|
||||||
MESH_CONTACT_ARROW = ContactArrowStyle(
|
MESH_CONTACT_ARROW = ContactArrowStyle(
|
||||||
reference_normal_force=10000.0,
|
reference_normal_force=10000.0,
|
||||||
@@ -197,6 +214,7 @@ class LogConfig:
|
|||||||
flow_rms: float = 0.0,
|
flow_rms: float = 0.0,
|
||||||
mask_coverage: float = 0.0,
|
mask_coverage: float = 0.0,
|
||||||
wall_fps: float | None = None,
|
wall_fps: float | None = None,
|
||||||
|
contact_xyz_mm: tuple[float, float, float] | None = None,
|
||||||
) -> str:
|
) -> str:
|
||||||
shear_mag = float((fshearx**2 + fsheary**2) ** 0.5)
|
shear_mag = float((fshearx**2 + fsheary**2) ** 0.5)
|
||||||
if wall_fps is not None:
|
if wall_fps is not None:
|
||||||
@@ -206,11 +224,17 @@ class LogConfig:
|
|||||||
parts.extend(
|
parts.extend(
|
||||||
[
|
[
|
||||||
f"Fn={fnormal:.1f}",
|
f"Fn={fnormal:.1f}",
|
||||||
f"|Fs|={shear_mag:.1f}",
|
f"|(Fx,Fy)|={shear_mag:.1f}",
|
||||||
f"Fs=({fshearx:.1f}, {fsheary:.1f})",
|
f"Fx, Fy=({fshearx:.1f}, {fsheary:.1f})",
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
return " ".join(parts)
|
force_line = " ".join(parts)
|
||||||
|
if contact_xyz_mm is None:
|
||||||
|
contact_line = "contact=---"
|
||||||
|
else:
|
||||||
|
cx, cy, cz = contact_xyz_mm
|
||||||
|
contact_line = f"contact=({cx:.1f}, {cy:.1f}, {cz:.1f}) mm"
|
||||||
|
return f"{force_line}\n{contact_line}"
|
||||||
|
|
||||||
|
|
||||||
LOG_CONFIG = LogConfig()
|
LOG_CONFIG = LogConfig()
|
||||||
@@ -224,7 +248,7 @@ class DashboardLayout:
|
|||||||
|
|
||||||
window_title: str = "Orisys Finger Dashboard"
|
window_title: str = "Orisys Finger Dashboard"
|
||||||
window_width: int = 1300
|
window_width: int = 1300
|
||||||
window_height: int = 520
|
window_height: int = 560
|
||||||
|
|
||||||
root_margin_px: int = 6
|
root_margin_px: int = 6
|
||||||
root_spacing_px: int = 6
|
root_spacing_px: int = 6
|
||||||
@@ -249,6 +273,7 @@ class DashboardLayout:
|
|||||||
mesh_background: MeshBackground = MESH_BACKGROUND
|
mesh_background: MeshBackground = MESH_BACKGROUND
|
||||||
mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW
|
mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW
|
||||||
mesh_flow_arrow: FlowArrowStyle = MESH_FLOW_ARROW
|
mesh_flow_arrow: FlowArrowStyle = MESH_FLOW_ARROW
|
||||||
|
mesh_axis_overlay: MeshAxisOverlay = MESH_AXIS_OVERLAY
|
||||||
|
|
||||||
log_config: LogConfig = LOG_CONFIG
|
log_config: LogConfig = LOG_CONFIG
|
||||||
|
|
||||||
|
|||||||
@@ -22,12 +22,13 @@ class TeeStream:
|
|||||||
|
|
||||||
|
|
||||||
class WidgetLogHandler:
|
class WidgetLogHandler:
|
||||||
"""Mirror print output into a QPlainTextEdit with a live status line at the bottom."""
|
"""Mirror print output into a QPlainTextEdit with pinned status lines at the bottom."""
|
||||||
|
|
||||||
def __init__(self, widget: QPlainTextEdit):
|
def __init__(self, widget: QPlainTextEdit):
|
||||||
self.widget = widget
|
self.widget = widget
|
||||||
self.buf = ""
|
self.buf = ""
|
||||||
self._has_status_line = False
|
self._status_text: str | None = None
|
||||||
|
self._status_block_count = 0
|
||||||
|
|
||||||
def write(self, text: str):
|
def write(self, text: str):
|
||||||
self.buf += text
|
self.buf += text
|
||||||
@@ -42,34 +43,42 @@ class WidgetLogHandler:
|
|||||||
self.buf = ""
|
self.buf = ""
|
||||||
|
|
||||||
def set_status_line(self, text: str) -> None:
|
def set_status_line(self, text: str) -> None:
|
||||||
"""Update the pinned bottom line (FPS / forces) without scrolling the log."""
|
"""Update pinned bottom status (supports multi-line) without scrolling away."""
|
||||||
text = text.rstrip("\n")
|
text = text.rstrip("\n")
|
||||||
if self._has_status_line:
|
self._remove_status_blocks()
|
||||||
cursor = QTextCursor(self.widget.document().lastBlock())
|
self.widget.appendPlainText(text)
|
||||||
cursor.select(QTextCursor.BlockUnderCursor)
|
self._status_text = text
|
||||||
cursor.insertText(text)
|
self._status_block_count = max(1, text.count("\n") + 1)
|
||||||
else:
|
|
||||||
self.widget.appendPlainText(text)
|
|
||||||
self._has_status_line = True
|
|
||||||
scrollbar = self.widget.verticalScrollBar()
|
scrollbar = self.widget.verticalScrollBar()
|
||||||
scrollbar.setValue(scrollbar.maximum())
|
scrollbar.setValue(scrollbar.maximum())
|
||||||
|
|
||||||
def clear_status_line(self) -> None:
|
def clear_status_line(self) -> None:
|
||||||
if not self._has_status_line:
|
self._remove_status_blocks()
|
||||||
|
self._status_text = None
|
||||||
|
self._status_block_count = 0
|
||||||
|
|
||||||
|
def _remove_status_blocks(self) -> None:
|
||||||
|
if self._status_block_count <= 0:
|
||||||
return
|
return
|
||||||
cursor = QTextCursor(self.widget.document().lastBlock())
|
doc = self.widget.document()
|
||||||
cursor.select(QTextCursor.BlockUnderCursor)
|
end = QTextCursor(doc)
|
||||||
|
end.movePosition(QTextCursor.End)
|
||||||
|
start = QTextCursor(doc)
|
||||||
|
start.movePosition(QTextCursor.End)
|
||||||
|
for _ in range(self._status_block_count):
|
||||||
|
start.movePosition(QTextCursor.StartOfBlock)
|
||||||
|
if start.position() > 0:
|
||||||
|
start.movePosition(QTextCursor.Left)
|
||||||
|
cursor = QTextCursor(doc)
|
||||||
|
cursor.setPosition(start.position())
|
||||||
|
cursor.setPosition(end.position(), QTextCursor.KeepAnchor)
|
||||||
cursor.removeSelectedText()
|
cursor.removeSelectedText()
|
||||||
self._has_status_line = False
|
self._status_block_count = 0
|
||||||
|
|
||||||
def _append_log_line(self, line: str) -> None:
|
def _append_log_line(self, line: str) -> None:
|
||||||
status = None
|
status = self._status_text
|
||||||
if self._has_status_line:
|
if status is not None:
|
||||||
status = self.widget.document().lastBlock().text()
|
self.clear_status_line()
|
||||||
cursor = QTextCursor(self.widget.document().lastBlock())
|
|
||||||
cursor.select(QTextCursor.BlockUnderCursor)
|
|
||||||
cursor.removeSelectedText()
|
|
||||||
self._has_status_line = False
|
|
||||||
self.widget.appendPlainText(line)
|
self.widget.appendPlainText(line)
|
||||||
if status is not None:
|
if status is not None:
|
||||||
self.set_status_line(status)
|
self.set_status_line(status)
|
||||||
|
|||||||
@@ -120,6 +120,7 @@ class MeshViewportWidget(ImagePanel):
|
|||||||
mesh_background=self._layout.mesh_background.color,
|
mesh_background=self._layout.mesh_background.color,
|
||||||
contact_arrow_style=self._layout.mesh_contact_arrow,
|
contact_arrow_style=self._layout.mesh_contact_arrow,
|
||||||
flow_arrow_style=self._layout.mesh_flow_arrow,
|
flow_arrow_style=self._layout.mesh_flow_arrow,
|
||||||
|
axis_overlay=self._layout.mesh_axis_overlay,
|
||||||
)
|
)
|
||||||
self._renderer.flow_arrow.set_enabled(self._mesh_state.flow_arrows_enabled)
|
self._renderer.flow_arrow.set_enabled(self._mesh_state.flow_arrows_enabled)
|
||||||
|
|
||||||
|
|||||||
@@ -41,6 +41,8 @@ class SettingsPanelWidget(QWidget):
|
|||||||
browse_video_requested = pyqtSignal()
|
browse_video_requested = pyqtSignal()
|
||||||
browse_config_requested = pyqtSignal()
|
browse_config_requested = pyqtSignal()
|
||||||
browse_finger_assets_requested = pyqtSignal()
|
browse_finger_assets_requested = pyqtSignal()
|
||||||
|
browse_csv_dir_requested = pyqtSignal()
|
||||||
|
csv_record_toggled = pyqtSignal(bool)
|
||||||
|
|
||||||
def __init__(
|
def __init__(
|
||||||
self,
|
self,
|
||||||
@@ -57,8 +59,8 @@ class SettingsPanelWidget(QWidget):
|
|||||||
self.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)
|
self.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)
|
||||||
|
|
||||||
root = QVBoxLayout(self)
|
root = QVBoxLayout(self)
|
||||||
root.setContentsMargins(10, 10, 10, 10)
|
root.setContentsMargins(8, 8, 8, 8)
|
||||||
root.setSpacing(10)
|
root.setSpacing(6)
|
||||||
|
|
||||||
title = QLabel(layout.settings_panel_title)
|
title = QLabel(layout.settings_panel_title)
|
||||||
title.setObjectName("settingsTitle")
|
title.setObjectName("settingsTitle")
|
||||||
@@ -125,10 +127,10 @@ class SettingsPanelWidget(QWidget):
|
|||||||
config_row.addWidget(self.btn_browse_config)
|
config_row.addWidget(self.btn_browse_config)
|
||||||
root.addLayout(config_row)
|
root.addLayout(config_row)
|
||||||
|
|
||||||
|
# Optional calibration path is kept for future re-enablement but hidden for now.
|
||||||
self.cal_edit = QLineEdit("")
|
self.cal_edit = QLineEdit("")
|
||||||
self.cal_edit.setPlaceholderText("Optional calibration path")
|
self.cal_edit.setPlaceholderText("Optional calibration path")
|
||||||
root.addWidget(self._caption("Calibration"))
|
self.cal_edit.hide()
|
||||||
root.addWidget(self.cal_edit)
|
|
||||||
|
|
||||||
root.addWidget(self._caption("Finger assets"))
|
root.addWidget(self._caption("Finger assets"))
|
||||||
finger_assets_row = QHBoxLayout()
|
finger_assets_row = QHBoxLayout()
|
||||||
@@ -143,7 +145,28 @@ class SettingsPanelWidget(QWidget):
|
|||||||
finger_assets_row.addWidget(self.btn_browse_finger_assets)
|
finger_assets_row.addWidget(self.btn_browse_finger_assets)
|
||||||
root.addLayout(finger_assets_row)
|
root.addLayout(finger_assets_row)
|
||||||
|
|
||||||
root.addSpacing(8)
|
root.addWidget(self._caption("CSV save directory"))
|
||||||
|
csv_dir_row = QHBoxLayout()
|
||||||
|
csv_dir_row.setSpacing(6)
|
||||||
|
self.csv_dir_edit = QLineEdit("./outputs/csv")
|
||||||
|
self.csv_dir_edit.setPlaceholderText("Directory for CSV recordings")
|
||||||
|
csv_dir_row.addWidget(self.csv_dir_edit, 1)
|
||||||
|
self.btn_browse_csv_dir = QPushButton("…")
|
||||||
|
self.btn_browse_csv_dir.setFixedWidth(32)
|
||||||
|
self.btn_browse_csv_dir.setToolTip("Browse CSV save directory")
|
||||||
|
self.btn_browse_csv_dir.clicked.connect(self.browse_csv_dir_requested.emit)
|
||||||
|
csv_dir_row.addWidget(self.btn_browse_csv_dir)
|
||||||
|
root.addLayout(csv_dir_row)
|
||||||
|
|
||||||
|
self.btn_csv_record = QPushButton("Record CSV")
|
||||||
|
self.btn_csv_record.setToolTip("Start writing contact/force CSV")
|
||||||
|
self.btn_csv_record.setEnabled(False)
|
||||||
|
self.btn_csv_record.clicked.connect(self._on_csv_toggle_clicked)
|
||||||
|
root.addWidget(self.btn_csv_record)
|
||||||
|
self._csv_recording = False
|
||||||
|
self._pipeline_running = False
|
||||||
|
|
||||||
|
root.addSpacing(4)
|
||||||
rotate_row = QHBoxLayout()
|
rotate_row = QHBoxLayout()
|
||||||
rotate_row.setSpacing(6)
|
rotate_row.setSpacing(6)
|
||||||
self.free_rotate_check = QCheckBox("Free rotate 3D view")
|
self.free_rotate_check = QCheckBox("Free rotate 3D view")
|
||||||
@@ -211,6 +234,45 @@ class SettingsPanelWidget(QWidget):
|
|||||||
self.finger_assets_edit.setEnabled(enabled)
|
self.finger_assets_edit.setEnabled(enabled)
|
||||||
self.btn_browse_finger_assets.setEnabled(enabled)
|
self.btn_browse_finger_assets.setEnabled(enabled)
|
||||||
|
|
||||||
|
def set_csv_save_dir(self, value: str) -> None:
|
||||||
|
self.csv_dir_edit.setText(value)
|
||||||
|
|
||||||
|
def set_csv_controls_for_pipeline(self, running: bool) -> None:
|
||||||
|
"""Update CSV controls from pipeline start/stop."""
|
||||||
|
self._pipeline_running = bool(running)
|
||||||
|
if not self._pipeline_running:
|
||||||
|
self._csv_recording = False
|
||||||
|
self._refresh_csv_controls()
|
||||||
|
|
||||||
|
def set_csv_recording(self, recording: bool) -> None:
|
||||||
|
"""Update CSV controls after a successful start/stop of recording."""
|
||||||
|
self._csv_recording = bool(recording) and self._pipeline_running
|
||||||
|
self._refresh_csv_controls()
|
||||||
|
|
||||||
|
def _refresh_csv_controls(self) -> None:
|
||||||
|
self.btn_csv_record.setEnabled(self._pipeline_running)
|
||||||
|
if self._csv_recording:
|
||||||
|
self.btn_csv_record.setText("Stop CSV")
|
||||||
|
self.btn_csv_record.setToolTip("Stop CSV recording")
|
||||||
|
self.btn_csv_record.setProperty("recording", True)
|
||||||
|
else:
|
||||||
|
self.btn_csv_record.setText("Record CSV")
|
||||||
|
self.btn_csv_record.setToolTip("Start writing contact/force CSV")
|
||||||
|
self.btn_csv_record.setProperty("recording", False)
|
||||||
|
style = self.btn_csv_record.style()
|
||||||
|
style.unpolish(self.btn_csv_record)
|
||||||
|
style.polish(self.btn_csv_record)
|
||||||
|
self.btn_csv_record.update()
|
||||||
|
|
||||||
|
dir_enabled = not self._csv_recording
|
||||||
|
self.csv_dir_edit.setEnabled(dir_enabled)
|
||||||
|
self.btn_browse_csv_dir.setEnabled(dir_enabled)
|
||||||
|
|
||||||
|
def _on_csv_toggle_clicked(self) -> None:
|
||||||
|
if not self._pipeline_running:
|
||||||
|
return
|
||||||
|
self.csv_record_toggled.emit(not self._csv_recording)
|
||||||
|
|
||||||
def _emit_arrow_background_changed(self, _index: int) -> None:
|
def _emit_arrow_background_changed(self, _index: int) -> None:
|
||||||
value = self.arrow_background_combo.currentData()
|
value = self.arrow_background_combo.currentData()
|
||||||
if value:
|
if value:
|
||||||
@@ -239,3 +301,6 @@ class SettingsPanelWidget(QWidget):
|
|||||||
|
|
||||||
def finger_export_dir(self) -> str:
|
def finger_export_dir(self) -> str:
|
||||||
return self.finger_assets_edit.text().strip()
|
return self.finger_assets_edit.text().strip()
|
||||||
|
|
||||||
|
def csv_save_dir(self) -> str:
|
||||||
|
return self.csv_dir_edit.text().strip()
|
||||||
|
|||||||
@@ -1,10 +1,14 @@
|
|||||||
"""Finger example pipeline: runtime acquisition without GUI dependencies."""
|
"""Finger example pipeline: runtime acquisition without GUI dependencies."""
|
||||||
|
|
||||||
from .config import PipelineConfig, mesh_threshold
|
from .config import PipelineConfig, mesh_threshold
|
||||||
|
from .csv_recorder import CSV_COLUMNS, CsvMeasurement, CsvRecorder
|
||||||
from .frame import FingerFrameView
|
from .frame import FingerFrameView
|
||||||
from .service import FingerPipeline
|
from .service import FingerPipeline
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
|
"CSV_COLUMNS",
|
||||||
|
"CsvMeasurement",
|
||||||
|
"CsvRecorder",
|
||||||
"FingerFrameView",
|
"FingerFrameView",
|
||||||
"FingerPipeline",
|
"FingerPipeline",
|
||||||
"PipelineConfig",
|
"PipelineConfig",
|
||||||
|
|||||||
@@ -20,6 +20,7 @@ class PipelineConfig:
|
|||||||
config: str = "./config/dd02-ov.json"
|
config: str = "./config/dd02-ov.json"
|
||||||
calib: str | None = None
|
calib: str | None = None
|
||||||
export_dir: str = DEFAULT_FINGER_EXPORT_DIR
|
export_dir: str = DEFAULT_FINGER_EXPORT_DIR
|
||||||
|
csv_save_dir: str = "./outputs/csv"
|
||||||
verbose: bool = False
|
verbose: bool = False
|
||||||
mesh_map: str = "depth"
|
mesh_map: str = "depth"
|
||||||
threshold: float | None = None
|
threshold: float | None = None
|
||||||
@@ -45,6 +46,7 @@ class PipelineConfig:
|
|||||||
lowpass_cutoff_hz=getattr(args, "cutoff", 5.0),
|
lowpass_cutoff_hz=getattr(args, "cutoff", 5.0),
|
||||||
profile=getattr(args, "profile", False),
|
profile=getattr(args, "profile", False),
|
||||||
profile_every=getattr(args, "profile_every", 30),
|
profile_every=getattr(args, "profile_every", 30),
|
||||||
|
csv_save_dir=getattr(args, "csv_save_dir", "./outputs/csv"),
|
||||||
)
|
)
|
||||||
|
|
||||||
@property
|
@property
|
||||||
|
|||||||
@@ -0,0 +1,114 @@
|
|||||||
|
"""CSV recorder for DD02 contact / force logging."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import csv
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from datetime import datetime
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
|
||||||
|
CSV_COLUMNS = (
|
||||||
|
"frame_index",
|
||||||
|
"timestamp_s",
|
||||||
|
"contact_x_mm",
|
||||||
|
"contact_y_mm",
|
||||||
|
"contact_z_mm",
|
||||||
|
"normal_x",
|
||||||
|
"normal_y",
|
||||||
|
"normal_z",
|
||||||
|
"force_local_fn",
|
||||||
|
"force_local_fx",
|
||||||
|
"force_local_fy",
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class CsvMeasurement:
|
||||||
|
frame_index: int
|
||||||
|
timestamp_s: float
|
||||||
|
contact_x: float
|
||||||
|
contact_y: float
|
||||||
|
contact_z: float
|
||||||
|
normal_x: float
|
||||||
|
normal_y: float
|
||||||
|
normal_z: float
|
||||||
|
force_local_fn: float
|
||||||
|
force_local_fx: float
|
||||||
|
force_local_fy: float
|
||||||
|
|
||||||
|
|
||||||
|
def measurement_to_csv_row(row: CsvMeasurement) -> dict[str, float | int]:
|
||||||
|
return {
|
||||||
|
"frame_index": row.frame_index,
|
||||||
|
"timestamp_s": row.timestamp_s,
|
||||||
|
"contact_x_mm": row.contact_x,
|
||||||
|
"contact_y_mm": row.contact_y,
|
||||||
|
"contact_z_mm": row.contact_z,
|
||||||
|
"normal_x": row.normal_x,
|
||||||
|
"normal_y": row.normal_y,
|
||||||
|
"normal_z": row.normal_z,
|
||||||
|
"force_local_fn": row.force_local_fn,
|
||||||
|
"force_local_fx": row.force_local_fx,
|
||||||
|
"force_local_fy": row.force_local_fy,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class CsvRecorder:
|
||||||
|
"""Write one contact/force measurement row per frame."""
|
||||||
|
|
||||||
|
def __init__(self, save_dir: str | Path = "./outputs/csv") -> None:
|
||||||
|
self.save_dir = Path(save_dir)
|
||||||
|
self._file = None
|
||||||
|
self._writer: csv.DictWriter | None = None
|
||||||
|
self._path: Path | None = None
|
||||||
|
self._row_count = 0
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_recording(self) -> bool:
|
||||||
|
return self._writer is not None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def path(self) -> Path | None:
|
||||||
|
return self._path
|
||||||
|
|
||||||
|
@property
|
||||||
|
def row_count(self) -> int:
|
||||||
|
return self._row_count
|
||||||
|
|
||||||
|
def start(self, save_dir: str | Path | None = None) -> Path:
|
||||||
|
if self.is_recording:
|
||||||
|
self.stop()
|
||||||
|
|
||||||
|
if save_dir is not None:
|
||||||
|
self.save_dir = Path(save_dir)
|
||||||
|
self.save_dir.mkdir(parents=True, exist_ok=True)
|
||||||
|
|
||||||
|
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||||
|
self._path = self.save_dir / f"dd02_record_{stamp}.csv"
|
||||||
|
self._file = self._path.open("w", newline="", encoding="utf-8")
|
||||||
|
self._writer = csv.DictWriter(self._file, fieldnames=CSV_COLUMNS)
|
||||||
|
self._writer.writeheader()
|
||||||
|
self._file.flush()
|
||||||
|
self._row_count = 0
|
||||||
|
return self._path
|
||||||
|
|
||||||
|
def write(self, row: CsvMeasurement) -> None:
|
||||||
|
if self._writer is None:
|
||||||
|
return
|
||||||
|
self._writer.writerow(measurement_to_csv_row(row))
|
||||||
|
self._row_count += 1
|
||||||
|
if self._file is not None and self._row_count % 30 == 0:
|
||||||
|
self._file.flush()
|
||||||
|
|
||||||
|
def stop(self) -> tuple[Path | None, int]:
|
||||||
|
count = self._row_count
|
||||||
|
saved_path = self._path
|
||||||
|
if self._file is not None:
|
||||||
|
self._file.flush()
|
||||||
|
self._file.close()
|
||||||
|
self._file = None
|
||||||
|
self._writer = None
|
||||||
|
self._path = None
|
||||||
|
self._row_count = 0
|
||||||
|
return saved_path, count
|
||||||
@@ -6,11 +6,12 @@ import time
|
|||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from orisys.finger import FingerRuntime
|
from orisys.finger import FingerRuntime, compute_uv_surface_frames
|
||||||
|
|
||||||
from viz import FrameRecorder, build_uv_surface_mesh, resolve_viz_pose_path
|
from viz import FrameRecorder, build_uv_surface_mesh, resolve_viz_pose_path
|
||||||
|
|
||||||
from .config import PipelineConfig
|
from .config import PipelineConfig
|
||||||
|
from .csv_recorder import CsvMeasurement
|
||||||
from .frame import FingerFrameView
|
from .frame import FingerFrameView
|
||||||
from .profiling import StageProfiler, run_deformation_timed
|
from .profiling import StageProfiler, run_deformation_timed
|
||||||
|
|
||||||
@@ -40,6 +41,12 @@ class FingerPipeline:
|
|||||||
self.profiler: StageProfiler | None = (
|
self.profiler: StageProfiler | None = (
|
||||||
StageProfiler(report_every=config.profile_every) if config.profile else None
|
StageProfiler(report_every=config.profile_every) if config.profile else None
|
||||||
)
|
)
|
||||||
|
self._world_xyz: np.ndarray | None = None
|
||||||
|
self._surface_mask: np.ndarray | None = None
|
||||||
|
self._geometric_center: np.ndarray | None = None
|
||||||
|
self._tangent_x: np.ndarray | None = None
|
||||||
|
self._tangent_y: np.ndarray | None = None
|
||||||
|
self._normals: np.ndarray | None = None
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def is_open(self) -> bool:
|
def is_open(self) -> bool:
|
||||||
@@ -72,10 +79,26 @@ class FingerPipeline:
|
|||||||
if domain.surface is None or getattr(domain.surface, "world_xyz", None) is None:
|
if domain.surface is None or getattr(domain.surface, "world_xyz", None) is None:
|
||||||
return
|
return
|
||||||
self.uv_mesh = build_uv_surface_mesh(domain.surface.world_xyz, domain.valid_mask)
|
self.uv_mesh = build_uv_surface_mesh(domain.surface.world_xyz, domain.valid_mask)
|
||||||
|
self._cache_surface_geometry(domain.surface.world_xyz, domain.valid_mask)
|
||||||
if self.config.viz_pose.lower() != "none":
|
if self.config.viz_pose.lower() != "none":
|
||||||
self.world_to_cam = np.load(resolve_viz_pose_path(self.config.viz_pose))
|
self.world_to_cam = np.load(resolve_viz_pose_path(self.config.viz_pose))
|
||||||
self.mesh_lookat = self.uv_mesh.rest_vertices.mean(axis=0)
|
self.mesh_lookat = self.uv_mesh.rest_vertices.mean(axis=0)
|
||||||
|
|
||||||
|
def _cache_surface_geometry(self, world_xyz: np.ndarray, valid_mask: np.ndarray) -> None:
|
||||||
|
world_xyz = np.asarray(world_xyz, dtype=np.float64)
|
||||||
|
valid_mask = np.asarray(valid_mask, dtype=bool)
|
||||||
|
finite = np.all(np.isfinite(world_xyz), axis=2)
|
||||||
|
mask = valid_mask & finite
|
||||||
|
self._world_xyz = world_xyz
|
||||||
|
self._surface_mask = mask
|
||||||
|
points = world_xyz[mask]
|
||||||
|
self._geometric_center = (
|
||||||
|
points.mean(axis=0) if len(points) else np.zeros(3, dtype=np.float64)
|
||||||
|
)
|
||||||
|
self._tangent_x, self._tangent_y, self._normals = compute_uv_surface_frames(
|
||||||
|
world_xyz, mask
|
||||||
|
)
|
||||||
|
|
||||||
def close(self) -> None:
|
def close(self) -> None:
|
||||||
if self.profiler is not None and self.profiler.frame_count:
|
if self.profiler is not None and self.profiler.frame_count:
|
||||||
self.profiler.print_report(prefix="[profile final]")
|
self.profiler.print_report(prefix="[profile final]")
|
||||||
@@ -83,6 +106,12 @@ class FingerPipeline:
|
|||||||
self.runtime.close()
|
self.runtime.close()
|
||||||
self.runtime = None
|
self.runtime = None
|
||||||
self._start_time = None
|
self._start_time = None
|
||||||
|
self._world_xyz = None
|
||||||
|
self._surface_mask = None
|
||||||
|
self._geometric_center = None
|
||||||
|
self._tangent_x = None
|
||||||
|
self._tangent_y = None
|
||||||
|
self._normals = None
|
||||||
|
|
||||||
def reset(self) -> None:
|
def reset(self) -> None:
|
||||||
if self.runtime is not None:
|
if self.runtime is not None:
|
||||||
@@ -165,3 +194,53 @@ class FingerPipeline:
|
|||||||
|
|
||||||
def stop_recording(self) -> None:
|
def stop_recording(self) -> None:
|
||||||
self.recorder.stop()
|
self.recorder.stop()
|
||||||
|
|
||||||
|
def measure_for_csv(self, frame: FingerFrameView, frame_index: int) -> CsvMeasurement:
|
||||||
|
"""Lift contact + local forces into global-frame measurements for CSV export."""
|
||||||
|
nan = float("nan")
|
||||||
|
contact = (nan, nan, nan)
|
||||||
|
normal = (nan, nan, nan)
|
||||||
|
|
||||||
|
fn = float(frame.fnormal)
|
||||||
|
fx = float(frame.fshearx)
|
||||||
|
fy = float(frame.fsheary)
|
||||||
|
|
||||||
|
if (
|
||||||
|
frame.centroid_found
|
||||||
|
and frame.centroid is not None
|
||||||
|
and np.asarray(frame.centroid).size >= 2
|
||||||
|
and self._world_xyz is not None
|
||||||
|
and self._geometric_center is not None
|
||||||
|
and self._normals is not None
|
||||||
|
and self._tangent_x is not None
|
||||||
|
and self._tangent_y is not None
|
||||||
|
):
|
||||||
|
cx = int(round(float(np.asarray(frame.centroid).reshape(-1)[0])))
|
||||||
|
cy = int(round(float(np.asarray(frame.centroid).reshape(-1)[1])))
|
||||||
|
h, w = self._world_xyz.shape[:2]
|
||||||
|
if 0 <= cy < h and 0 <= cx < w:
|
||||||
|
if self._surface_mask is None or bool(self._surface_mask[cy, cx]):
|
||||||
|
world = self._world_xyz[cy, cx]
|
||||||
|
if np.all(np.isfinite(world)):
|
||||||
|
centered = world - self._geometric_center
|
||||||
|
contact = (float(centered[0]), float(centered[1]), float(centered[2]))
|
||||||
|
|
||||||
|
n = self._normals[cy, cx]
|
||||||
|
n_norm = float(np.linalg.norm(n))
|
||||||
|
if n_norm > 1e-8:
|
||||||
|
n_u = n / n_norm
|
||||||
|
normal = (float(n_u[0]), float(n_u[1]), float(n_u[2]))
|
||||||
|
|
||||||
|
return CsvMeasurement(
|
||||||
|
frame_index=int(frame_index),
|
||||||
|
timestamp_s=float(frame.timestamp),
|
||||||
|
contact_x=contact[0],
|
||||||
|
contact_y=contact[1],
|
||||||
|
contact_z=contact[2],
|
||||||
|
normal_x=normal[0],
|
||||||
|
normal_y=normal[1],
|
||||||
|
normal_z=normal[2],
|
||||||
|
force_local_fn=fn,
|
||||||
|
force_local_fx=fx,
|
||||||
|
force_local_fy=fy,
|
||||||
|
)
|
||||||
|
|||||||
@@ -84,6 +84,168 @@ def _background_rgb01(color: tuple[int, int, int]) -> np.ndarray:
|
|||||||
return np.asarray(_background_rgba01(color)[:3], dtype=np.float64)
|
return np.asarray(_background_rgba01(color)[:3], dtype=np.float64)
|
||||||
|
|
||||||
|
|
||||||
|
# Cached pre-rendered mesh-axis triads keyed by size_px.
|
||||||
|
_AXIS_TRIAD_CACHE: dict[int, np.ndarray] = {}
|
||||||
|
|
||||||
|
|
||||||
|
def _project_mesh_axis_to_screen(point_xyz: np.ndarray, *, size_px: int) -> tuple[int, int, float]:
|
||||||
|
"""Project mesh coords to HUD pixels.
|
||||||
|
|
||||||
|
Screen mapping for the fixed mesh-axis reference:
|
||||||
|
+X -> down, +Y -> left, +Z -> inward (into the page).
|
||||||
|
|
||||||
|
Mild foreshortening keeps +Z visible as a short into-page axis.
|
||||||
|
Returns (px, py, depth) where larger depth is farther from the viewer.
|
||||||
|
"""
|
||||||
|
x, y, z = float(point_xyz[0]), float(point_xyz[1]), float(point_xyz[2])
|
||||||
|
# Camera looks predominantly along +Z (into page): u right, v down.
|
||||||
|
# Soft shear so Z has a short foreshortened stub.
|
||||||
|
shear = 0.34
|
||||||
|
u = -y - shear * z
|
||||||
|
v = x - shear * z
|
||||||
|
scale = size_px * 0.30
|
||||||
|
# Origin slightly lower-right-of-center so labeled tips stay in-frame.
|
||||||
|
cx = size_px * 0.58
|
||||||
|
cy = size_px * 0.48
|
||||||
|
return int(round(cx + u * scale)), int(round(cy + v * scale)), z
|
||||||
|
|
||||||
|
|
||||||
|
def _render_axis_triad_rgba(size_px: int) -> np.ndarray:
|
||||||
|
"""Build (and cache) a transparent RGBA 3D-looking XYZ triad."""
|
||||||
|
import cv2
|
||||||
|
|
||||||
|
size_px = max(48, int(size_px))
|
||||||
|
cached = _AXIS_TRIAD_CACHE.get(size_px)
|
||||||
|
if cached is not None:
|
||||||
|
return cached
|
||||||
|
|
||||||
|
rgba = np.zeros((size_px, size_px, 4), dtype=np.uint8)
|
||||||
|
rgb = rgba[:, :, :3]
|
||||||
|
alpha = rgba[:, :, 3]
|
||||||
|
|
||||||
|
# Soft circular plate for contrast on bright contact heatmaps.
|
||||||
|
plate = np.zeros((size_px, size_px), dtype=np.uint8)
|
||||||
|
center = (size_px // 2, size_px // 2)
|
||||||
|
radius = int(size_px * 0.46)
|
||||||
|
cv2.circle(plate, center, radius, 255, thickness=-1, lineType=cv2.LINE_AA)
|
||||||
|
rgba[:, :, 0:3][plate > 0] = (18, 18, 18)
|
||||||
|
alpha[plate > 0] = 140
|
||||||
|
|
||||||
|
thickness = max(2, size_px // 42)
|
||||||
|
font = cv2.FONT_HERSHEY_SIMPLEX
|
||||||
|
font_scale = max(0.35, size_px / 220.0)
|
||||||
|
origin = np.zeros(3, dtype=np.float64)
|
||||||
|
o_xy = _project_mesh_axis_to_screen(origin, size_px=size_px)[:2]
|
||||||
|
|
||||||
|
axes = (
|
||||||
|
("X", np.array([1.0, 0.0, 0.0]), (230, 70, 70)),
|
||||||
|
("Y", np.array([0.0, 1.0, 0.0]), (70, 210, 95)),
|
||||||
|
("Z", np.array([0.0, 0.0, 1.0]), (70, 140, 255)),
|
||||||
|
)
|
||||||
|
# Paint far axes first so nearer shafts occlude correctly.
|
||||||
|
painted = []
|
||||||
|
for label, direction, color in axes:
|
||||||
|
tip = direction * 1.0
|
||||||
|
px, py, depth = _project_mesh_axis_to_screen(tip, size_px=size_px)
|
||||||
|
painted.append((depth, label, color, (px, py)))
|
||||||
|
painted.sort(key=lambda item: item[0], reverse=True)
|
||||||
|
|
||||||
|
layer = np.zeros((size_px, size_px, 3), dtype=np.uint8)
|
||||||
|
for _, label, color, tip_xy in painted:
|
||||||
|
cv2.arrowedLine(
|
||||||
|
layer,
|
||||||
|
o_xy,
|
||||||
|
tip_xy,
|
||||||
|
color,
|
||||||
|
thickness,
|
||||||
|
line_type=cv2.LINE_AA,
|
||||||
|
tipLength=0.28,
|
||||||
|
)
|
||||||
|
(tw, th), _ = cv2.getTextSize(label, font, font_scale, max(1, thickness - 1))
|
||||||
|
# Offset label slightly past the arrow tip, away from origin.
|
||||||
|
dx = tip_xy[0] - o_xy[0]
|
||||||
|
dy = tip_xy[1] - o_xy[1]
|
||||||
|
norm = max(1.0, float(np.hypot(dx, dy)))
|
||||||
|
lx = int(tip_xy[0] + 8 * dx / norm - tw // 2)
|
||||||
|
ly = int(tip_xy[1] + 8 * dy / norm + th // 2)
|
||||||
|
lx = min(max(1, lx), size_px - tw - 1)
|
||||||
|
ly = min(max(th + 1, ly), size_px - 2)
|
||||||
|
cv2.putText(
|
||||||
|
layer,
|
||||||
|
label,
|
||||||
|
(lx, ly),
|
||||||
|
font,
|
||||||
|
font_scale,
|
||||||
|
color,
|
||||||
|
max(1, thickness - 1),
|
||||||
|
cv2.LINE_AA,
|
||||||
|
)
|
||||||
|
# Origin hub on the same contiguous buffer.
|
||||||
|
cv2.circle(layer, o_xy, max(2, thickness), (235, 235, 235), -1, lineType=cv2.LINE_AA)
|
||||||
|
|
||||||
|
mask = np.any(layer > 0, axis=2)
|
||||||
|
rgb[mask] = layer[mask]
|
||||||
|
alpha[mask] = 255
|
||||||
|
|
||||||
|
_AXIS_TRIAD_CACHE[size_px] = rgba
|
||||||
|
return rgba
|
||||||
|
|
||||||
|
|
||||||
|
def draw_axis_overlay(
|
||||||
|
rgb: np.ndarray,
|
||||||
|
*,
|
||||||
|
enabled: bool = True,
|
||||||
|
padding_px: int = 12,
|
||||||
|
size_px: int = 96,
|
||||||
|
opacity: float = 1.0,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Composite a fixed top-right mesh-axis triad onto an RGB frame.
|
||||||
|
|
||||||
|
The triad is pre-rendered and cached; orientation is locked to true mesh
|
||||||
|
axes (+X down, +Y left, +Z inward) and does not follow free-rotate.
|
||||||
|
"""
|
||||||
|
if not enabled or rgb is None or rgb.size == 0:
|
||||||
|
return rgb
|
||||||
|
|
||||||
|
out = np.ascontiguousarray(rgb)
|
||||||
|
if out.ndim != 3 or out.shape[2] < 3:
|
||||||
|
return out
|
||||||
|
|
||||||
|
triad = _render_axis_triad_rgba(size_px)
|
||||||
|
th, tw = triad.shape[:2]
|
||||||
|
pad = max(0, int(padding_px))
|
||||||
|
h, w = out.shape[:2]
|
||||||
|
x0 = max(0, w - tw - pad)
|
||||||
|
y0 = max(0, pad)
|
||||||
|
x1 = min(w, x0 + tw)
|
||||||
|
y1 = min(h, y0 + th)
|
||||||
|
if x1 <= x0 or y1 <= y0:
|
||||||
|
return out
|
||||||
|
|
||||||
|
roi = out[y0:y1, x0:x1]
|
||||||
|
overlay = triad[: y1 - y0, : x1 - x0]
|
||||||
|
alpha = (overlay[:, :, 3].astype(np.float32) / 255.0) * float(np.clip(opacity, 0.0, 1.0))
|
||||||
|
if alpha.ndim == 2:
|
||||||
|
alpha = alpha[:, :, None]
|
||||||
|
rgb_o = overlay[:, :, :3].astype(np.float32)
|
||||||
|
base = roi.astype(np.float32)
|
||||||
|
blended = rgb_o * alpha + base * (1.0 - alpha)
|
||||||
|
roi[:] = np.clip(blended, 0, 255).astype(np.uint8)
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def _apply_axis_overlay(rgb: np.ndarray, axis_overlay) -> np.ndarray:
|
||||||
|
if axis_overlay is None:
|
||||||
|
return draw_axis_overlay(rgb)
|
||||||
|
return draw_axis_overlay(
|
||||||
|
rgb,
|
||||||
|
enabled=bool(getattr(axis_overlay, "enabled", True)),
|
||||||
|
padding_px=int(getattr(axis_overlay, "padding_px", 12)),
|
||||||
|
size_px=int(getattr(axis_overlay, "size_px", 96)),
|
||||||
|
opacity=float(getattr(axis_overlay, "opacity", 1.0)),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
class _FilamentOffscreenBackend:
|
class _FilamentOffscreenBackend:
|
||||||
"""Filament OffscreenRenderer (Linux headless / platforms with EGL)."""
|
"""Filament OffscreenRenderer (Linux headless / platforms with EGL)."""
|
||||||
|
|
||||||
@@ -104,6 +266,7 @@ class _FilamentOffscreenBackend:
|
|||||||
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
||||||
contact_arrow_style=None,
|
contact_arrow_style=None,
|
||||||
flow_arrow_style=None,
|
flow_arrow_style=None,
|
||||||
|
axis_overlay=None,
|
||||||
):
|
):
|
||||||
import open3d as o3d
|
import open3d as o3d
|
||||||
|
|
||||||
@@ -119,6 +282,7 @@ class _FilamentOffscreenBackend:
|
|||||||
self.lookat = lookat
|
self.lookat = lookat
|
||||||
self.mesh_lighting = mesh_lighting
|
self.mesh_lighting = mesh_lighting
|
||||||
self.mesh_background = mesh_background
|
self.mesh_background = mesh_background
|
||||||
|
self.axis_overlay = axis_overlay
|
||||||
self._shade_factors = (
|
self._shade_factors = (
|
||||||
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
||||||
if mesh_lighting is not None
|
if mesh_lighting is not None
|
||||||
@@ -257,7 +421,8 @@ class _FilamentOffscreenBackend:
|
|||||||
mesh_to_o3d=lambda v, t, c: _mesh_to_o3d(o3d, v, t, c),
|
mesh_to_o3d=lambda v, t, c: _mesh_to_o3d(o3d, v, t, c),
|
||||||
)
|
)
|
||||||
|
|
||||||
return _numpy_o3d_image_to_rgb(self._renderer.render_to_image())
|
rgb = _numpy_o3d_image_to_rgb(self._renderer.render_to_image())
|
||||||
|
return _apply_axis_overlay(rgb, self.axis_overlay)
|
||||||
|
|
||||||
def close(self) -> None:
|
def close(self) -> None:
|
||||||
self._renderer = None
|
self._renderer = None
|
||||||
@@ -283,6 +448,7 @@ class _HiddenVisualizerBackend:
|
|||||||
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
||||||
contact_arrow_style=None,
|
contact_arrow_style=None,
|
||||||
flow_arrow_style=None,
|
flow_arrow_style=None,
|
||||||
|
axis_overlay=None,
|
||||||
):
|
):
|
||||||
import open3d as o3d
|
import open3d as o3d
|
||||||
|
|
||||||
@@ -298,6 +464,7 @@ class _HiddenVisualizerBackend:
|
|||||||
self.lookat = lookat
|
self.lookat = lookat
|
||||||
self.mesh_lighting = mesh_lighting
|
self.mesh_lighting = mesh_lighting
|
||||||
self.mesh_background = mesh_background
|
self.mesh_background = mesh_background
|
||||||
|
self.axis_overlay = axis_overlay
|
||||||
self._shade_factors = (
|
self._shade_factors = (
|
||||||
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
||||||
if mesh_lighting is not None
|
if mesh_lighting is not None
|
||||||
@@ -441,7 +608,8 @@ class _HiddenVisualizerBackend:
|
|||||||
|
|
||||||
self.vis.poll_events()
|
self.vis.poll_events()
|
||||||
self.vis.update_renderer()
|
self.vis.update_renderer()
|
||||||
return _numpy_o3d_image_to_rgb(self.vis.capture_screen_float_buffer(do_render=False))
|
rgb = _numpy_o3d_image_to_rgb(self.vis.capture_screen_float_buffer(do_render=False))
|
||||||
|
return _apply_axis_overlay(rgb, self.axis_overlay)
|
||||||
|
|
||||||
def close(self) -> None:
|
def close(self) -> None:
|
||||||
if self.vis is not None:
|
if self.vis is not None:
|
||||||
@@ -478,6 +646,7 @@ class OffscreenFingerMeshRenderer:
|
|||||||
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
||||||
contact_arrow_style=None,
|
contact_arrow_style=None,
|
||||||
flow_arrow_style=None,
|
flow_arrow_style=None,
|
||||||
|
axis_overlay=None,
|
||||||
):
|
):
|
||||||
lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0)
|
lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0)
|
||||||
self._backend = _create_backend(
|
self._backend = _create_backend(
|
||||||
@@ -495,6 +664,7 @@ class OffscreenFingerMeshRenderer:
|
|||||||
mesh_background=mesh_background,
|
mesh_background=mesh_background,
|
||||||
contact_arrow_style=contact_arrow_style,
|
contact_arrow_style=contact_arrow_style,
|
||||||
flow_arrow_style=flow_arrow_style,
|
flow_arrow_style=flow_arrow_style,
|
||||||
|
axis_overlay=axis_overlay,
|
||||||
)
|
)
|
||||||
|
|
||||||
@property
|
@property
|
||||||
|
|||||||
Reference in New Issue
Block a user