diff --git a/README.md b/README.md index 431c427..834ee3f 100644 --- a/README.md +++ b/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`. ## Камеры и запись diff --git a/configs/scene.yaml b/configs/scene.yaml index f5b90bd..8f715d2 100644 --- a/configs/scene.yaml +++ b/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. - position: [0.0, 0.0, 0.0] - lookat: [0.0, 0.0, 1.0] + # Existing optical link is at the CAD housing origin. Place the simulated + # aperture in front of its 26 mm deep body, avoiding self-hits. + 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 diff --git a/iiwa_genesis_description/README.md b/iiwa_genesis_description/README.md index 731098a..110227b 100644 --- a/iiwa_genesis_description/README.md +++ b/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 коллизий, стол и инструмент уже заданы в метрах и не уменьшаются. Воспроизводимый экспорт именно этих исходных макросов: diff --git a/iiwa_genesis_description/urdf/iiwa_with_gripper.urdf b/iiwa_genesis_description/urdf/iiwa_with_gripper.urdf index ffef51e..276d979 100644 --- a/iiwa_genesis_description/urdf/iiwa_with_gripper.urdf +++ b/iiwa_genesis_description/urdf/iiwa_with_gripper.urdf @@ -10,7 +10,7 @@ - + @@ -32,7 +32,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -76,7 +76,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -120,7 +120,7 @@ - + @@ -142,7 +142,7 @@ - + @@ -164,7 +164,7 @@ - + diff --git a/iiwa_genesis_description/urdf/iiwa_without_gripper.urdf b/iiwa_genesis_description/urdf/iiwa_without_gripper.urdf index 2848ac2..f45c6b0 100644 --- a/iiwa_genesis_description/urdf/iiwa_without_gripper.urdf +++ b/iiwa_genesis_description/urdf/iiwa_without_gripper.urdf @@ -10,7 +10,7 @@ - + @@ -32,7 +32,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -76,7 +76,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -120,7 +120,7 @@ - + @@ -142,7 +142,7 @@ - + @@ -164,7 +164,7 @@ - + diff --git a/src/iiwa_genesis/camera.py b/src/iiwa_genesis/camera.py index 6bb8583..2837d8e 100644 --- a/src/iiwa_genesis/camera.py +++ b/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)) diff --git a/src/iiwa_genesis/cli.py b/src/iiwa_genesis/cli.py index 97554db..589aaaf 100644 --- a/src/iiwa_genesis/cli.py +++ b/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"] - if preview: - import cv2 + from .preview import Preview + preview = Preview() if viewer and cfg["output"]["preview"] else None 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 - 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))) + preview.submit(camera.name, frame, camera.cfg["max_range"]) 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() diff --git a/src/iiwa_genesis/preview.py b/src/iiwa_genesis/preview.py new file mode 100644 index 0000000..c00eea2 --- /dev/null +++ b/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() diff --git a/src/iiwa_genesis/scene.py b/src/iiwa_genesis/scene.py index 2ec7dda..992eb2b 100644 --- a/src/iiwa_genesis/scene.py +++ b/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) diff --git a/tests/test_genesis_smoke.py b/tests/test_genesis_smoke.py index 0a3b884..ac64e40 100644 --- a/tests/test_genesis_smoke.py +++ b/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() diff --git a/tests/test_mesh_scale.py b/tests/test_mesh_scale.py new file mode 100644 index 0000000..10c0972 --- /dev/null +++ b/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 diff --git a/tools/export_descriptions.py b/tools/export_descriptions.py index 803245f..266ac4c 100644 --- a/tools/export_descriptions.py +++ b/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)