Добавлены 3д объекты в виртуальный мир (молотки 5шт)

This commit is contained in:
Даниил Грабарь
2026-09-15 13:08:17 +10:00
parent f7dff2e484
commit 71e669c2ed
23 changed files with 123769 additions and 5 deletions
+59 -2
View File
@@ -107,11 +107,68 @@ Python overlays, а pytest не загружает внешние плагины
сенсора дополнительно смещена на `[0, 0.014, 0.027]` м: передняя плоскость
находится за корпусом толщиной 26 мм. Это геометрическое приближение,
исключающее измерение собственного корпуса, а не заводская калибровка.
Сцена использует нулевую гравитацию и суставной position controller для
удержания позы и выполнения команд с ограниченной скоростью.
Сцена использует гравитацию `[0, 0, -9.81]` м/с². У робота включена полная
компенсация гравитации и суставной position controller для удержания позы и
выполнения команд с ограниченной скоростью. Свободный инструмент подчиняется
гравитации и контактам.
При нулевых углах суставов фланец и камера инструмента направлены вверх.
Для осмотра стола этой камерой задайте соответствующую рабочую позу в `robot.joints`.
## Случайный молоток на столе
В `configs/scene.yaml` перечислите допустимые имена файлов без расширений:
```yaml
tools:
allowed: [hammer1, hammer3, hammer5]
seed: null
```
При построении сцены равновероятно выбирается **ровно один** молоток из списка.
Он остаётся тем же объектом в течение запуска. `allowed: [hammer2]` всегда
выбирает второй молоток. Список не может быть пустым; повторы, неизвестные имена
и отсутствующие OBJ/MTL отклоняются при чтении конфигурации. Файлы загружаются
из `iiwa_genesis_description/tools/hammer/`. Конфиги без раздела `tools`
используют все пять молотков.
При `seed: null` выбор модели, положения и поворота производится заново при
каждом запуске; случайный выбор может повторить предыдущую модель. Целое
неотрицательное число, например `seed: 17`, повторяет выбор и начальную позу.
Этот seed независим от `scene.seed`, используемого симуляцией и камерами.
Выбранное имя выводится в терминале и используется как имя сущности Genesis.
Молоток лежит на свободной стороне стола: весь его контур находится внутри
`X=[0.06, 0.58]`, `Y=[-0.48, 0.48]` м в системе стола, за пределами монтажной
пластины и бортиков. Положение учитывает `table.position` и `table.euler`.
Нижняя точка появляется на 5 мм выше столешницы (локальная высота 0.800 м),
затем инструмент опускается на неё под действием гравитации. Область рассчитана
на поставляемый стол; произвольные начальные позы руки, пересекающие эту область,
могут сразу привести к контакту с инструментом.
Импорт учитывает Y-up экспорт Blender `(X, Y, Z) → (X, -Z, Y)` и смещения
молотков в исходной Blender-сцене; OBJ/MTL не изменяются. Масштаб остаётся 1:1.
Все детали одного OBJ образуют одно свободное твёрдое тело с шестью степенями
свободы. Коллизии строятся по выпуклым частям геометрии с разложением вогнутых
участков, а не по одному охватывающему параллелепипеду. Молоток может падать,
сдвигаться, вращаться и сталкиваться со столом и роботом.
Плотность 3000 кг/м³ и коэффициент трения 0.65 — начальные приближения;
масса и инерция вычисляются по геометрии. Для точного цифрового двойника нужно
отдельно откалибровать распределение массы и параметры контакта. Управление
захватом и сценарии захвата этим изменением не добавляются.
В текущих MTL отсутствуют `map_Kd` и другие текстурные карты: процедурные
материалы Blender не переносятся через такой OBJ-экспорт. Genesis загружает
доступные MTL; для сохранения древесных волокон и других текстур их нужно
запечь в изображения и экспортировать с материалами.
Проверка реальных контактов всех пяти OBJ со столом (может требовать несколько
минут на первичное разложение мешей и компиляцию):
```bash
IIWA_GENESIS_SMOKE=1 uv run pytest tests/test_tool_physics.py
```
## Камеры и запись
Это **приближение D455**, использующее профиль из согласованного плана:
+9
View File
@@ -22,6 +22,15 @@ table:
position: [0.0, 0.0, 0.0]
euler: [0.0, 0.0, 0.0]
tools:
# Exactly one name is chosen per launch. Names are OBJ/MTL stems in tools/hammer.
# allowed: [hammer1, hammer2, hammer3, hammer4, hammer5]
allowed: [hammer5]
# null: new random tool, position and yaw each launch; integer: repeatable spawn.
# Independent of scene.seed (which controls simulation/camera randomness).
seed: null
cameras:
overhead:
position: [-0.263, 0.0, 2.8]
@@ -0,0 +1,42 @@
# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
# www.blender.org
newmtl 01_|_circular_ground_face
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer1_basecolor.png
newmtl 01_|_exposed_cut_wood
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer1_basecolor.png
newmtl 01_|_honey_hickory_longitudinal_grain
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer1_basecolor.png
newmtl 01_|_satin_forged_steel
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer1_basecolor.png
File diff suppressed because it is too large Load Diff
Binary file not shown.

After

Width:  |  Height:  |  Size: 2.7 MiB

@@ -0,0 +1,42 @@
# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
# www.blender.org
newmtl 02_|_black_oxide_forged_steel
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer2_basecolor.png
newmtl 02_|_blue_glass_fibre_resin
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer2_basecolor.png
newmtl 02_|_graphite_moulded_rubber
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer2_basecolor.png
newmtl 02_|_machined_steel_face
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer2_basecolor.png
File diff suppressed because it is too large Load Diff
Binary file not shown.

After

Width:  |  Height:  |  Size: 4.2 MiB

@@ -0,0 +1,42 @@
# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
# www.blender.org
newmtl 03_|_ash_end_grain
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer3_basecolor.png
newmtl 03_|_blue_tempered_steel
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer3_basecolor.png
newmtl 03_|_bright_milled_steel
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer3_basecolor.png
newmtl 03_|_pale_ash_straight_grain
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer3_basecolor.png
File diff suppressed because it is too large Load Diff
Binary file not shown.

After

Width:  |  Height:  |  Size: 2.4 MiB

@@ -0,0 +1,42 @@
# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
# www.blender.org
newmtl 04_|_dark_walnut_grain
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer4_basecolor.png
newmtl 04_|_polished_ball_and_face
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer4_basecolor.png
newmtl 04_|_walnut_end_grain
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer4_basecolor.png
newmtl 04_|_warm_forged_steel
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer4_basecolor.png
File diff suppressed because it is too large Load Diff
Binary file not shown.

After

Width:  |  Height:  |  Size: 2.6 MiB

@@ -0,0 +1,42 @@
# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
# www.blender.org
newmtl 05_|_charcoal_anti_vibration_rubber
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer5_basecolor.png
newmtl 05_|_dark_textured_forged_coating
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer5_basecolor.png
newmtl 05_|_machined_impact_faces
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer5_basecolor.png
newmtl 05_|_orange_glass_fibre_resin
Ns 250.000000
Ka 1.000000 1.000000 1.000000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.500000
d 1.000000
illum 2
map_Kd hammer5_basecolor.png
File diff suppressed because it is too large Load Diff
Binary file not shown.

After

Width:  |  Height:  |  Size: 4.3 MiB

+26
View File
@@ -1,5 +1,6 @@
from pathlib import Path
import math
import re
import xml.etree.ElementTree as ET
import numpy as np
@@ -16,9 +17,34 @@ def vector(value, name):
return tuple(a)
def validate_tools(cfg):
tools = cfg.setdefault("tools", {"allowed": [f"hammer{i}" for i in range(1, 6)]})
if not isinstance(tools, dict):
raise ValueError("tools must contain an allowed list")
allowed = tools.get("allowed")
if (not isinstance(allowed, list) or not allowed
or any(not isinstance(name, str) or not re.fullmatch(r"[A-Za-z0-9_-]+", name)
for name in allowed)):
raise ValueError("tools.allowed must be a nonempty list of tool names without extensions")
if len(set(allowed)) != len(allowed):
raise ValueError("tools.allowed must not contain duplicate names")
directory = DESCRIPTION / "tools/hammer"
for name in allowed:
for extension in (".obj", ".mtl"):
path = directory / (name + extension)
if not path.is_file():
raise ValueError(f"tools.allowed: missing asset {path}")
seed = tools.setdefault("seed", None)
if seed is not None and (type(seed) is not int or seed < 0):
raise ValueError("tools.seed must be null or a nonnegative integer")
def load_config(path):
path = Path(path).resolve()
cfg = yaml.safe_load(path.read_text())
if not isinstance(cfg, dict):
raise ValueError("configuration must be a YAML mapping")
validate_tools(cfg)
mode = cfg["scene"]["mode"]
if mode not in ("without_gripper", "with_gripper"):
raise ValueError("scene.mode must be without_gripper or with_gripper")
+6 -3
View File
@@ -6,6 +6,7 @@ from scipy.spatial.transform import Rotation
from .camera import D455
from .config import DESCRIPTION
from .tools import add_table_tool
from .control import (
CartesianCommand,
JointCommand,
@@ -153,8 +154,7 @@ class SceneMotionController:
def build_scene(gs, cfg, viewer):
scene = gs.Scene(
show_viewer=viewer,
# Zero gravity supports pose inspection with bounded position control.
sim_options=gs.options.SimOptions(dt=cfg["scene"]["dt"], gravity=(0, 0, 0)),
sim_options=gs.options.SimOptions(dt=cfg["scene"]["dt"], gravity=(0, 0, -9.81)),
viewer_options=gs.options.ViewerOptions(camera_pos=(2.8, -3, 2.6), camera_lookat=(0, 0, 1.1)),
vis_options=gs.options.VisOptions(show_world_frame=True),
)
@@ -163,9 +163,12 @@ def build_scene(gs, cfg, viewer):
options = cfg[name]
entity = scene.add_entity(gs.morphs.URDF(
file=str(options["urdf"]), fixed=True, pos=options["position"], euler=options["euler"],
merge_fixed_links=False, decimate=False))
merge_fixed_links=False, decimate=False),
# Preserve gravity-free robot control while the free tool can fall.
material=gs.materials.Rigid(gravity_compensation=1 if name == "robot" else 0))
if name == "robot":
robot = entity
add_table_tool(gs, scene, cfg)
# Reuse the supplied D455 CAD mesh. Its lens face is at mesh Z=0; body extends +Z.
from .camera import optical_pose
overhead = cfg["cameras"]["overhead"]
+84
View File
@@ -0,0 +1,84 @@
"""Place one exported Y-up hammer on the free side of the supplied table."""
from dataclasses import dataclass
from pathlib import Path
import random
import numpy as np
from scipy.spatial.transform import Rotation
from .config import DESCRIPTION
# Table-local coordinates, metres. The robot mounting plate ends at X=-0.028.
# Keep the entire footprint clear of the plate and the tabletop's orange rim.
SPAWN_MIN = np.array([.06, -.48])
SPAWN_MAX = np.array([.58, .48])
TABLE_TOP = .800
DROP_CLEARANCE = .005
@dataclass(frozen=True)
class ToolSpawn:
name: str
path: Path
position: tuple[float, float, float]
quaternion: tuple[float, float, float, float]
def obj_vertices_zup(path):
"""Read bounds without Genesis initialization; match its Y-up conversion."""
vertices = []
with Path(path).open() as file:
for line in file:
parts = line.split()
if parts and parts[0] == "v":
try:
x, y, z = map(float, parts[1:4])
except ValueError as exc:
raise ValueError(f"Invalid OBJ vertex in {path}") from exc
vertices.append((x, -z, y))
result = np.asarray(vertices)
if (result.ndim != 2 or result.shape[1] != 3 or not np.isfinite(result).all()
or (np.ptp(result, axis=0) <= 0).any()):
raise ValueError(f"Tool must have finite, nonzero 3D geometry: {path}")
return result
def choose_tool_spawn(cfg):
# A separate generator avoids gs.init(seed=scene.seed) repeating the tool
# every launch. None uses OS entropy; an explicit tools.seed is repeatable.
rng = random.Random(cfg["tools"]["seed"])
name = rng.choice(cfg["tools"]["allowed"])
path = DESCRIPTION / "tools/hammer" / f"{name}.obj"
yaw = Rotation.from_euler("z", rng.uniform(-180, 180), degrees=True)
vertices = yaw.apply(obj_vertices_zup(path))
low, high = vertices.min(axis=0), vertices.max(axis=0)
# Translation intervals account for every vertex, including the authored
# offset from the five-hammer Blender layout. No asset files are rewritten.
lower, upper = SPAWN_MIN - low[:2], SPAWN_MAX - high[:2]
if (lower > upper).any():
raise ValueError(f"Tool {name} does not fit the free tabletop area")
local_position = np.array([rng.uniform(lower[0], upper[0]),
rng.uniform(lower[1], upper[1]),
TABLE_TOP + DROP_CLEARANCE - low[2]])
table = cfg["table"]
rotation = Rotation.from_euler("xyz", table["euler"], degrees=True)
position = rotation.apply(local_position) + table["position"]
quaternion = (rotation * yaw).as_quat(scalar_first=True)
return ToolSpawn(name, path, tuple(position), tuple(quaternion))
def add_table_tool(gs, scene, cfg):
spawn = choose_tool_spawn(cfg)
entity = scene.add_entity(
gs.morphs.Mesh(
file=str(spawn.path), pos=spawn.position, quat=spawn.quaternion,
file_meshes_are_zup=False, fixed=False, collision=True,
decimate=False, convexify=True, decompose_object_error_threshold=.05,
coacd_options=gs.options.CoacdOptions(threshold=.05, preprocess_resolution=80),
merge_submeshes_for_collision=False),
# Nominal bulk density: mass/inertia need calibration for a real tool.
material=gs.materials.Rigid(rho=3000, friction=.65, gravity_compensation=0),
name=spawn.name,
)
print(f"Table tool: {spawn.name}; OBJ: {spawn.path.name}; dynamic collision enabled", flush=True)
return entity
+2
View File
@@ -17,6 +17,7 @@ from iiwa_genesis.config import ROOT
def test_cpu_rgbd_scene(tmp_path, mode, names):
cfg = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
cfg["scene"]["mode"] = mode
cfg["tools"] = {"allowed": ["hammer1"], "seed": 17}
cfg["output"]["directory"] = str(tmp_path / "frames")
path = tmp_path / "scene.yaml"
path.write_text(yaml.safe_dump(cfg))
@@ -26,6 +27,7 @@ def test_cpu_rgbd_scene(tmp_path, mode, names):
"--headless", "--backend", "cpu", "--steps", "4", "--save-frames"],
env=env, capture_output=True, text=True, timeout=600)
assert result.returncode == 0, result.stdout + result.stderr
assert result.stdout.count("Table tool: hammer1;") == 1
captures = list((tmp_path / "frames").glob("*/*.npz"))
assert len(captures) == len(names)
for name in names:
+66
View File
@@ -0,0 +1,66 @@
"""Opt-in contact regression using real exported meshes and the real table."""
import os
import subprocess
import sys
import pytest
from iiwa_genesis.config import ROOT
@pytest.mark.skipif(os.environ.get("IIWA_GENESIS_SMOKE") != "1",
reason="set IIWA_GENESIS_SMOKE=1 for real tool/table contact tests")
def test_all_hammers_fall_rest_and_move_on_table():
script = r'''
import sys
import numpy as np
sys.path.insert(0, "src")
from iiwa_genesis.config import load_config
from iiwa_genesis.runtime import import_genesis
from iiwa_genesis.tools import add_table_tool
gs = import_genesis()
gs.init(backend=gs.cpu, seed=42, logging_level="warning")
try:
for number in range(1, 6):
cfg = load_config("configs/scene.yaml")
cfg["tools"] = {"allowed": [f"hammer{number}"], "seed": 10}
scene = gs.Scene(show_viewer=False, show_FPS=False,
sim_options=gs.options.SimOptions(dt=1/120, gravity=(0, 0, -9.81)))
scene.add_entity(gs.morphs.URDF(file=str(cfg["table"]["urdf"]), fixed=True,
merge_fixed_links=False, decimate=False))
tool = add_table_tool(gs, scene, cfg)
scene.build()
assert len(scene.entities) == 2 # one table and exactly one tool
assert tool.n_dofs == 6 and tool.n_links == 1 and tool.n_geoms > 1
assert float(tool.get_mass()) > 0
initial = tool.get_AABB().cpu().numpy()
np.testing.assert_allclose(initial[0, 2], .805, atol=.002)
assert initial[1, 2] < .87
for _ in range(120):
scene.step()
rest = tool.get_AABB().cpu().numpy()
assert np.isfinite(rest).all()
assert .797 < rest[0, 2] < .803, (number, initial, rest)
assert rest[0, 2] < initial[0, 2] - .001
assert np.linalg.norm(tool.get_vel().cpu().numpy()) < .03
# An imparted horizontal velocity moves the free rigid assembly; it
# stays supported by the tabletop and friction slows it down.
before = tool.get_pos().cpu().numpy().copy()
tool.set_dofs_velocity([.3, 0, 0, 0, 0, 0])
for _ in range(60):
scene.step()
after = tool.get_pos().cpu().numpy()
assert np.linalg.norm(after[:2] - before[:2]) > .001, (number, before, after)
assert .797 < float(tool.get_AABB()[0, 2]) < .803
print(f"hammer{number}: contact, rest and dynamic movement OK", flush=True)
scene.destroy()
finally:
gs.destroy()
'''
env = os.environ.copy()
env.pop("PYTHONPATH", None)
env.setdefault("PYOPENGL_PLATFORM", "egl")
result = subprocess.run([sys.executable, "-c", script], cwd=ROOT,
env=env, capture_output=True, text=True, timeout=900)
assert result.returncode == 0, result.stdout + result.stderr
+107
View File
@@ -0,0 +1,107 @@
import numpy as np
import pytest
from scipy.spatial.transform import Rotation
import yaml
from iiwa_genesis.config import ROOT, load_config
def tool_config(tmp_path, tools):
data = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
data["tools"] = tools
path = tmp_path / "scene.yaml"
path.write_text(yaml.safe_dump(data))
return load_config(path)
@pytest.mark.parametrize("allowed", [[], "hammer1", [1], ["hammer1", "hammer1"],
["missing"], ["../hammer1"], ["hammer1.obj"]])
def test_invalid_tool_allowlist_is_rejected(tmp_path, allowed):
with pytest.raises(ValueError, match="tools.allowed"):
tool_config(tmp_path, {"allowed": allowed})
@pytest.mark.parametrize("seed", [True, 1.5, "42", -1])
def test_invalid_tool_seed_is_rejected(tmp_path, seed):
with pytest.raises(ValueError, match="tools.seed"):
tool_config(tmp_path, {"allowed": ["hammer1"], "seed": seed})
@pytest.mark.parametrize("tools", [None, ["hammer1"]])
def test_invalid_tools_section_is_rejected(tmp_path, tools):
with pytest.raises(ValueError, match="tools"):
tool_config(tmp_path, tools)
def test_empty_config_reports_validation_error(tmp_path):
path = tmp_path / "empty.yaml"
path.write_text("")
with pytest.raises(ValueError, match="configuration"):
load_config(path)
def test_tool_defaults_and_singleton_selection(tmp_path):
from iiwa_genesis.tools import choose_tool_spawn
cfg = tool_config(tmp_path, {"allowed": ["hammer3"]})
assert cfg["tools"]["seed"] is None
assert choose_tool_spawn(cfg).name == "hammer3"
def test_seed_repeats_spawn_and_only_allowed_tools_can_be_selected(tmp_path):
from iiwa_genesis.tools import choose_tool_spawn
cfg = tool_config(tmp_path, {"allowed": ["hammer2", "hammer5"], "seed": 123})
first = choose_tool_spawn(cfg)
assert choose_tool_spawn(cfg) == first
cfg["scene"]["seed"] = 999
assert choose_tool_spawn(cfg) == first
choices = set()
positions = set()
for seed in range(12):
cfg["tools"]["seed"] = seed
spawn = choose_tool_spawn(cfg)
choices.add(spawn.name)
positions.add(spawn.position)
assert choices == {"hammer2", "hammer5"}
assert len(positions) > 1
@pytest.mark.parametrize("name", [f"hammer{i}" for i in range(1, 6)])
@pytest.mark.parametrize("table_pose", [((0, 0, 0), (0, 0, 0)),
((1.2, -.7, .2), (5, -3, 60))])
def test_exported_tool_fits_table_without_blender_offset(tmp_path, name, table_pose):
from iiwa_genesis.tools import choose_tool_spawn
cfg = tool_config(tmp_path, {"allowed": [name], "seed": 9})
cfg["table"]["position"], cfg["table"]["euler"] = table_pose
# Read the authored Y-up vertices independently of placement code.
raw = np.array([[float(x) for x in line.split()[1:4]]
for line in (ROOT / f"iiwa_genesis_description/tools/hammer/{name}.obj").read_text().splitlines()
if line.startswith("v ")])
zup = raw[:, [0, 2, 1]] * [1, -1, 1]
table_rotation = Rotation.from_euler("xyz", table_pose[1], degrees=True)
for seed in (0, 1, 9, 37):
cfg["tools"]["seed"] = seed
spawn = choose_tool_spawn(cfg)
world = Rotation.from_quat(spawn.quaternion, scalar_first=True).apply(zup) + spawn.position
local = table_rotation.inv().apply(world - table_pose[0])
assert local[:, 0].min() >= .06 - 1e-9
assert local[:, 0].max() <= .58 + 1e-9
assert local[:, 1].min() >= -.48 - 1e-9
assert local[:, 1].max() <= .48 + 1e-9
assert local[:, 2].min() == pytest.approx(.805)
assert local[:, 2].max() < .87 # lying flat, not standing on the handle
def test_missing_material_is_rejected(tmp_path, monkeypatch):
from iiwa_genesis import config
description = tmp_path / "description"
asset = description / "tools/hammer/hammer1.obj"
asset.parent.mkdir(parents=True)
asset.write_text("mtllib hammer1.mtl\nv 0 0 0\n")
(description / "urdf").symlink_to(config.DESCRIPTION / "urdf", target_is_directory=True)
monkeypatch.setattr(config, "DESCRIPTION", description)
with pytest.raises(ValueError, match="hammer1.mtl"):
tool_config(tmp_path, {"allowed": ["hammer1"]})