Refactor depth snapshot processing in mujoco_server.py for metric depth representation

- Updated the capture_depth_snapshot function to convert depth values from meters to millimeters, ensuring precision in depth rendering.
- Introduced dynamic handling of maximum depth based on the MuJoCo visualization settings, improving adaptability to different environments.
- Enhanced depth normalization and clipping to handle invalid values more effectively, resulting in more accurate depth images.

mujoco_server.py

@@ -1957,22 +1957,22 @@ def capture_depth_snapshot(camera_name: str) -> Optional[bytes]:
             # Close the temporary renderer
             depth_renderer.close()
 
-            # Convert depth to 16-bit PNG for precision
-            # Normalize to 0-65535 range
-            # Handle invalid/infinite values
-            depth_min = np.min(depth_array[np.isfinite(depth_array)])
-            depth_max = np.max(depth_array[np.isfinite(depth_array)])
-
-            if depth_max > depth_min:
-                depth_normalized = np.clip(
-                    (depth_array - depth_min) / (depth_max - depth_min),
-                    0, 1
-                )
-            else:
-                depth_normalized = np.zeros_like(depth_array)
-
-            # Convert to 16-bit
-            depth_16bit = (depth_normalized * 65535).astype(np.uint16)
+            # Convert depth to 16-bit PNG in millimeters (metric depth).
+            # MuJoCo renderer returns depth in meters; preserve metric scale.
+            max_depth_m = 10.0
+            try:
+                vis_map = getattr(env.model, "vis", None)
+                vis_map = getattr(vis_map, "map", None) if vis_map is not None else None
+                zfar = getattr(vis_map, "zfar", None) if vis_map is not None else None
+                if zfar is not None:
+                    max_depth_m = float(zfar)
+            except Exception:
+                pass
+            # uint16 in mm can represent up to 65.535 m
+            max_depth_m = min(max_depth_m, 65.535)
+            depth_m = np.nan_to_num(depth_array, nan=0.0, posinf=max_depth_m, neginf=0.0)
+            depth_m = np.clip(depth_m, 0.0, max_depth_m)
+            depth_16bit = (depth_m * 1000.0).round().astype(np.uint16)
 
             # Encode as PNG
             ret, buffer = cv2.imencode('.png', depth_16bit)