Добавлены 3д объекты в виртуальный мир (молотки 5шт)
This commit is contained in:
@@ -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**, использующее профиль из согласованного плана:
|
||||
|
||||
@@ -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 |
@@ -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,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"]
|
||||
|
||||
@@ -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
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
@@ -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"]})
|
||||
Reference in New Issue
Block a user