grabardm/iiwa-genesis-word
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Даниил Грабарь
2026-09-13 14:26:50 +03:00
parent 397e33bd08
commit ae30c02ddd
12 changed files with 129 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+9
View File
@@ -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
+7 -5
View File
@@ -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
iiwa_genesis_description/README.md
+3
View File
@@ -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
+8 -8
View File
@@ -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
+8 -8
View File
@@ -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
+7 -1
View File
@@ -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
+6 -9
View File
@@ -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()
src/iiwa_genesis/preview.py
+48
View File
@@ -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
+1 -1
View File
@@ -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
+3
View File
@@ -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
+21
View File
@@ -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
+8
View File
@@ -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)