Выстроена готовая 3Д сцена

README.md

@@ -37,6 +37,9 @@ IIWA_GENESIS_SMOKE=1 uv run pytest tests/test_genesis_smoke.py
 
 Headless по умолчанию выполняет 120 шагов, viewer работает до закрытия окна,
 Ctrl+C или Q/Esc в окне кадров. `output.preview` включает окна RGB и глубины.
+Превью работает в отдельном процессе: оконные циклы OpenCV и Genesis не мешают
+друг другу. Если показ не успевает за симуляцией, пропускаются только кадры
+превью; запись на диск продолжает получать все запланированные кадры.
 30 FPS — частота по времени симуляции; реальная скорость зависит от оборудования.
 Headless всё равно требует рабочий OpenGL/EGL для RGB. Используется EGL, если
 `PYOPENGL_PLATFORM` не задан пользователем. GPU требует совместимого драйвера;
@@ -59,8 +62,14 @@ Python overlays, а pytest не загружает внешние плагины
 заданы относительно `camera_optical_frame`: вперёд +Z, вверх изображения -Y.
 Трансформы крепления сохранены из исходного описания. Направление оптической
 оси можно уточнять через локальные `lookat` и `up` без правки URDF.
+Исходный optical link расположен у начала CAD-корпуса, поэтому апертура
+сенсора дополнительно смещена на `[0, 0.014, 0.027]` м: передняя плоскость
+находится за корпусом толщиной 26 мм. Это геометрическое приближение,
+исключающее измерение собственного корпуса, а не заводская калибровка.
 Нулевая гравитация удерживает выбранную позу без контроллера: v1 предназначена
 для осмотра сцены и получения изображений, а не динамического управления.
+При нулевых углах суставов фланец и камера инструмента направлены вверх.
+Для осмотра стола этой камерой задайте соответствующую рабочую позу в `robot.joints`.
 
 ## Камеры и запись
 

configs/scene.yaml

@@ -1,5 +1,6 @@
 scene:
-  mode: without_gripper # with_gripper automatically adds the tool camera
+  # mode: without_gripper # with_gripper automatically adds the tool camera
+  mode: with_gripper # without_gripper automatically adds the tool camera
   backend: cpu # gpu on a machine with a supported GPU
   viewer: true
   dt: 0.008333333333333333 # 120 Hz simulation clock
@@ -25,7 +26,7 @@ cameras:
   overhead:
     position: [-0.263, 0.0, 2.8]
     lookat: [-0.263, 0.0, 1.2]
-    up: [0.0, 1.0, 0.0]
+    up: [1.0, 0.0, 0.0]
     resolution: [640, 480]
     fps: 30
     horizontal_fov: 1.3089969 # radians, legacy simulated profile
@@ -38,9 +39,10 @@ cameras:
       depth_stddev: 0.02 # metres
   tool:
     link: camera_optical_frame
-    # Local camera axes in the existing link: forward +Z, image up -Y.
+    # Existing optical link is at the CAD housing origin. Place the simulated
-    position: [0.0, 0.0, 0.0]
+    # aperture in front of its 26 mm deep body, avoiding self-hits.
-    lookat: [0.0, 0.0, 1.0]
+    position: [0.0, 0.014, 0.027]
+    lookat: [0.0, 0.014, 1.027]
     up: [0.0, -1.0, 0.0]
     resolution: [640, 480]
     fps: 30

iiwa_genesis_description/README.md

@@ -4,6 +4,9 @@
 из `iiwa_description` с сохранением кинематики, инерций, суставных ограничений
 и геометрии. `table/table.urdf` — копия стола с локальными путями.
 Meshes находятся в `urdf/meshes`; исходная папка не изменена.
+DAE звеньев iiwa созданы в миллиметрах. Их visual mesh в URDF имеет
+`scale="0.001 0.001 0.001"`: Genesis не применяет COLLADA unit автоматически.
+STL коллизий, стол и инструмент уже заданы в метрах и не уменьшаются.
 
 Воспроизводимый экспорт именно этих исходных макросов:
 

iiwa_genesis_description/urdf/iiwa_with_gripper.urdf

@@ -10,7 +10,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/base_link.DAE" />
+        <mesh filename="meshes/iiwa/visual/base_link.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -32,7 +32,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link1.DAE" />
+        <mesh filename="meshes/iiwa/visual/link1.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -54,7 +54,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link2.DAE" />
+        <mesh filename="meshes/iiwa/visual/link2.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -76,7 +76,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link3.DAE" />
+        <mesh filename="meshes/iiwa/visual/link3.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -98,7 +98,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link4.DAE" />
+        <mesh filename="meshes/iiwa/visual/link4.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -120,7 +120,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link5.DAE" />
+        <mesh filename="meshes/iiwa/visual/link5.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -142,7 +142,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link6.DAE" />
+        <mesh filename="meshes/iiwa/visual/link6.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -164,7 +164,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link7.DAE" />
+        <mesh filename="meshes/iiwa/visual/link7.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />

iiwa_genesis_description/urdf/iiwa_without_gripper.urdf

@@ -10,7 +10,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/base_link.DAE" />
+        <mesh filename="meshes/iiwa/visual/base_link.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -32,7 +32,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link1.DAE" />
+        <mesh filename="meshes/iiwa/visual/link1.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -54,7 +54,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link2.DAE" />
+        <mesh filename="meshes/iiwa/visual/link2.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -76,7 +76,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link3.DAE" />
+        <mesh filename="meshes/iiwa/visual/link3.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -98,7 +98,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link4.DAE" />
+        <mesh filename="meshes/iiwa/visual/link4.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -120,7 +120,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link5.DAE" />
+        <mesh filename="meshes/iiwa/visual/link5.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -142,7 +142,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link6.DAE" />
+        <mesh filename="meshes/iiwa/visual/link6.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
@@ -164,7 +164,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="meshes/iiwa/visual/link7.DAE" />
+        <mesh filename="meshes/iiwa/visual/link7.DAE" scale="0.001 0.001 0.001" />
       </geometry>
       <material name="iiwa_material">
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />

src/iiwa_genesis/camera.py

@@ -2,6 +2,7 @@
 from dataclasses import dataclass
 import json
 import math
+import warnings
 
 import numpy as np
 from PIL import Image
@@ -72,10 +73,15 @@ class D455:
         ray_to_optical = np.eye(4)
         ray_to_optical[:3, :3] = [[0, -1, 0], [0, 0, -1], [1, 0, 0]]
         ray_pose = self.local_pose @ ray_to_optical
+        # Looking vertically down is a valid pose at an Euler singularity. SciPy's
+        # chosen third angle of zero represents exactly the same rotation.
+        with warnings.catch_warnings():
+            warnings.filterwarnings("ignore", message="Gimbal lock detected", category=UserWarning)
+            ray_euler = Rotation.from_matrix(ray_pose[:3, :3]).as_euler("xyz", degrees=True)
         self.depth = scene.add_sensor(gs.sensors.DepthCamera(
             pattern=gs.sensors.DepthCameraPattern(res=cfg["resolution"], fov_horizontal=math.degrees(cfg["horizontal_fov"])),
             pos_offset=tuple(ray_pose[:3, 3]),
-            euler_offset=tuple(Rotation.from_matrix(ray_pose[:3, :3]).as_euler("xyz", degrees=True)),
+            euler_offset=tuple(ray_euler),
             min_range=0.0, max_range=cfg["max_range"] * np.linalg.norm(optical_rays(cfg), axis=-1).max(),
             no_hit_value=float("inf"), return_points=False, **mount))
 

src/iiwa_genesis/cli.py

@@ -35,10 +35,10 @@ def main():
     from .scene import build_scene
 
     gs.init(backend=getattr(gs, args.backend or cfg["scene"]["backend"]), seed=cfg["scene"]["seed"], logging_level="warning")
-    preview = viewer and cfg["output"]["preview"]
+    from .preview import Preview
-    if preview:
+    preview = Preview() if viewer and cfg["output"]["preview"] else None
-        import cv2
     try:
+        print("Building scene; first launch compiles Genesis kernels...", flush=True)
         scene, robot, cameras = build_scene(gs, cfg, viewer)
         directory = cfg["output"]["directory"] / datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%S_%fZ")
         print(f"Mode: {cfg['scene']['mode']}; cameras: {', '.join(c.name for c in cameras)}; Genesis {gs.__version__}; backend {gs.backend}")
@@ -62,13 +62,10 @@ def main():
                 if save:
                     camera.save(frame, directory, counts[camera.name], cfg["output"]["pointcloud"])
                 if preview:
-                    import numpy as np
+                    preview.submit(camera.name, frame, camera.cfg["max_range"])
-                    depth_view = cv2.applyColorMap(np.uint8(np.clip(frame.depth / camera.cfg["max_range"], 0, 1) * 255), cv2.COLORMAP_TURBO)
-                    depth_view[~frame.valid] = 0
-                    cv2.imshow(camera.name, np.hstack((cv2.cvtColor(frame.rgb, cv2.COLOR_RGB2BGR), depth_view)))
                 counts[camera.name] += 1
                 next_capture[camera.name] = (counts[camera.name] + 1) / camera.cfg["fps"]
-            if preview and cv2.waitKey(1) & 0xff in (27, ord("q")):
+            if preview and preview.stop.is_set():
                 break
             if viewer:
                 time.sleep(max(0, min(dt, start + timestamp - time.monotonic())))
@@ -77,7 +74,7 @@ def main():
         pass
     finally:
         if preview:
-            cv2.destroyAllWindows()
+            preview.close()
         gs.destroy()
 
 

src/iiwa_genesis/preview.py

@@ -0,0 +1,48 @@
+"""Keep OpenCV's GUI event loop separate from Genesis/pyglet's OpenGL contexts."""
+import multiprocessing as mp
+from queue import Empty, Full
+
+
+def _display(queue, stop):
+    import cv2
+    import numpy as np
+
+    try:
+        while not stop.is_set():
+            try:
+                name, rgb, depth, valid, max_range = queue.get(timeout=.05)
+            except Empty:
+                if cv2.waitKey(1) & 0xff in (27, ord("q")):
+                    stop.set()
+                continue
+            depth_view = cv2.applyColorMap(np.uint8(np.clip(depth / max_range, 0, 1) * 255), cv2.COLORMAP_TURBO)
+            depth_view[~valid] = 0
+            cv2.imshow(name, np.hstack((cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR), depth_view)))
+            if cv2.waitKey(1) & 0xff in (27, ord("q")):
+                stop.set()
+    finally:
+        cv2.destroyAllWindows()
+
+
+class Preview:
+    def __init__(self):
+        ctx = mp.get_context("spawn")
+        self.queue = ctx.Queue(maxsize=4)
+        self.stop = ctx.Event()
+        self.process = ctx.Process(target=_display, args=(self.queue, self.stop), daemon=True)
+        self.process.start()
+
+    def submit(self, name, frame, max_range):
+        try:
+            self.queue.put_nowait((name, frame.rgb, frame.depth, frame.valid, max_range))
+        except Full:
+            pass # A slow display drops preview frames, never recorded frames.
+
+    def close(self):
+        self.stop.set()
+        self.process.join(timeout=2)
+        if self.process.is_alive():
+            self.process.terminate()
+            self.process.join(timeout=2)
+        self.queue.cancel_join_thread()
+        self.queue.close()

src/iiwa_genesis/scene.py

@@ -32,7 +32,7 @@ def build_scene(gs, cfg, viewer):
                     robot if name == "tool" else None)
                for i, (name, cam) in enumerate(cfg["cameras"].items())]
     scene.build()
-    indices = [robot.get_joint(f"joint{i}").dof_idx_local for i in range(1, 8)]
+    indices = [robot.get_joint(f"joint{i}").dofs_idx_local[0] for i in range(1, 8)]
     if robot.n_dofs != 7:
         raise ValueError(f"Expected seven robot DOFs, got {robot.n_dofs}")
     robot.set_dofs_position([cfg["robot"]["joints"][f"joint{i}"] for i in range(1, 8)], dofs_idx_local=indices)

tests/test_genesis_smoke.py

@@ -41,3 +41,6 @@ def test_cpu_rgbd_scene(tmp_path, mode, names):
         np.testing.assert_allclose(np.array(meta["optical_from_world"]) @ meta["world_from_optical"], np.eye(4), atol=1e-6)
         if name == "overhead":
             assert valid.any()
+        else:
+            # The aperture must not sit inside the camera's 26 mm collision box.
+            assert (data["depth"] > .05).any()

tests/test_mesh_scale.py

@@ -0,0 +1,21 @@
+import xml.etree.ElementTree as ET
+
+import numpy as np
+import pytest
+import trimesh
+
+from iiwa_genesis.config import DESCRIPTION
+
+
+@pytest.mark.parametrize("name", ["base_link"] + [f"link{i}" for i in range(1, 8)])
+def test_visual_matches_collision_in_metres(name):
+    path = DESCRIPTION / "urdf/iiwa_with_gripper.urdf"
+    link = ET.parse(path).find(f"link[@name='{name}']")
+    sizes = []
+    for tag in ("visual", "collision"):
+        node = link.find(f"{tag}/geometry/mesh")
+        mesh = trimesh.load(path.parent / node.get("filename"), force="scene")
+        scale = np.fromstring(node.get("scale", "1 1 1"), sep=" ")
+        sizes.append(mesh.extents * scale)
+    np.testing.assert_allclose(sizes[0], sizes[1], rtol=.05, atol=.003)
+    assert max(sizes[0]) < .6

tools/export_descriptions.py

@@ -57,6 +57,14 @@ def export():
             target.parent.mkdir(parents=True, exist_ok=True)
             shutil.copy2(SOURCE / "resource/meshes" / relative, target)
             mesh.set("filename", ("meshes/" if path.parent.name == "urdf" else "../urdf/meshes/") + relative)
+            # Genesis/trimesh retain COLLADA units as metadata, without scaling vertices.
+            # Convert millimetre-authored visual meshes into the URDF's metres explicitly.
+            if target.suffix.lower() == ".dae":
+                unit = ET.parse(target).find(".//{*}unit")
+                metres = float(unit.get("meter", "1")) if unit is not None else 1.0
+                if metres != 1.0:
+                    scale = [float(v) * metres for v in mesh.get("scale", "1 1 1").split()]
+                    mesh.set("scale", " ".join(f"{v:g}" for v in scale))
         ET.indent(robot)
         ET.ElementTree(robot).write(path, encoding="utf-8", xml_declaration=True)