108 lines
4.2 KiB
Python
108 lines
4.2 KiB
Python
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"]})
|