update csv export and added fixed mesh axis drawing

examples/pipeline/service.py

@@ -6,11 +6,12 @@ import time
 from dataclasses import dataclass
 
 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 .config import PipelineConfig
+from .csv_recorder import CsvMeasurement
 from .frame import FingerFrameView
 from .profiling import StageProfiler, run_deformation_timed
 
@@ -40,6 +41,12 @@ class FingerPipeline:
         self.profiler: StageProfiler | 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
     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:
             return
         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":
             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)
 
+    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:
         if self.profiler is not None and self.profiler.frame_count:
             self.profiler.print_report(prefix="[profile final]")
@@ -83,6 +106,12 @@ class FingerPipeline:
             self.runtime.close()
             self.runtime = 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:
         if self.runtime is not None:
@@ -165,3 +194,53 @@ class FingerPipeline:
 
     def stop_recording(self) -> None:
         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,
+        )