Добавлены 3д объекты в виртуальный мир (молотки 5шт)
This commit is contained in:
@@ -107,11 +107,68 @@ Python overlays, а pytest не загружает внешние плагины
|
|||||||
сенсора дополнительно смещена на `[0, 0.014, 0.027]` м: передняя плоскость
|
сенсора дополнительно смещена на `[0, 0.014, 0.027]` м: передняя плоскость
|
||||||
находится за корпусом толщиной 26 мм. Это геометрическое приближение,
|
находится за корпусом толщиной 26 мм. Это геометрическое приближение,
|
||||||
исключающее измерение собственного корпуса, а не заводская калибровка.
|
исключающее измерение собственного корпуса, а не заводская калибровка.
|
||||||
Сцена использует нулевую гравитацию и суставной position controller для
|
Сцена использует гравитацию `[0, 0, -9.81]` м/с². У робота включена полная
|
||||||
удержания позы и выполнения команд с ограниченной скоростью.
|
компенсация гравитации и суставной position controller для удержания позы и
|
||||||
|
выполнения команд с ограниченной скоростью. Свободный инструмент подчиняется
|
||||||
|
гравитации и контактам.
|
||||||
При нулевых углах суставов фланец и камера инструмента направлены вверх.
|
При нулевых углах суставов фланец и камера инструмента направлены вверх.
|
||||||
Для осмотра стола этой камерой задайте соответствующую рабочую позу в `robot.joints`.
|
Для осмотра стола этой камерой задайте соответствующую рабочую позу в `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**, использующее профиль из согласованного плана:
|
Это **приближение D455**, использующее профиль из согласованного плана:
|
||||||
|
|||||||
@@ -22,6 +22,15 @@ table:
|
|||||||
position: [0.0, 0.0, 0.0]
|
position: [0.0, 0.0, 0.0]
|
||||||
euler: [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:
|
cameras:
|
||||||
overhead:
|
overhead:
|
||||||
position: [-0.263, 0.0, 2.8]
|
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
|
from pathlib import Path
|
||||||
import math
|
import math
|
||||||
|
import re
|
||||||
import xml.etree.ElementTree as ET
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -16,9 +17,34 @@ def vector(value, name):
|
|||||||
return tuple(a)
|
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):
|
def load_config(path):
|
||||||
path = Path(path).resolve()
|
path = Path(path).resolve()
|
||||||
cfg = yaml.safe_load(path.read_text())
|
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"]
|
mode = cfg["scene"]["mode"]
|
||||||
if mode not in ("without_gripper", "with_gripper"):
|
if mode not in ("without_gripper", "with_gripper"):
|
||||||
raise ValueError("scene.mode must be without_gripper or 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 .camera import D455
|
||||||
from .config import DESCRIPTION
|
from .config import DESCRIPTION
|
||||||
|
from .tools import add_table_tool
|
||||||
from .control import (
|
from .control import (
|
||||||
CartesianCommand,
|
CartesianCommand,
|
||||||
JointCommand,
|
JointCommand,
|
||||||
@@ -153,8 +154,7 @@ class SceneMotionController:
|
|||||||
def build_scene(gs, cfg, viewer):
|
def build_scene(gs, cfg, viewer):
|
||||||
scene = gs.Scene(
|
scene = gs.Scene(
|
||||||
show_viewer=viewer,
|
show_viewer=viewer,
|
||||||
# Zero gravity supports pose inspection with bounded position control.
|
sim_options=gs.options.SimOptions(dt=cfg["scene"]["dt"], gravity=(0, 0, -9.81)),
|
||||||
sim_options=gs.options.SimOptions(dt=cfg["scene"]["dt"], gravity=(0, 0, 0)),
|
|
||||||
viewer_options=gs.options.ViewerOptions(camera_pos=(2.8, -3, 2.6), camera_lookat=(0, 0, 1.1)),
|
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),
|
vis_options=gs.options.VisOptions(show_world_frame=True),
|
||||||
)
|
)
|
||||||
@@ -163,9 +163,12 @@ def build_scene(gs, cfg, viewer):
|
|||||||
options = cfg[name]
|
options = cfg[name]
|
||||||
entity = scene.add_entity(gs.morphs.URDF(
|
entity = scene.add_entity(gs.morphs.URDF(
|
||||||
file=str(options["urdf"]), fixed=True, pos=options["position"], euler=options["euler"],
|
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":
|
if name == "robot":
|
||||||
robot = entity
|
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.
|
# Reuse the supplied D455 CAD mesh. Its lens face is at mesh Z=0; body extends +Z.
|
||||||
from .camera import optical_pose
|
from .camera import optical_pose
|
||||||
overhead = cfg["cameras"]["overhead"]
|
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):
|
def test_cpu_rgbd_scene(tmp_path, mode, names):
|
||||||
cfg = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
|
cfg = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
|
||||||
cfg["scene"]["mode"] = mode
|
cfg["scene"]["mode"] = mode
|
||||||
|
cfg["tools"] = {"allowed": ["hammer1"], "seed": 17}
|
||||||
cfg["output"]["directory"] = str(tmp_path / "frames")
|
cfg["output"]["directory"] = str(tmp_path / "frames")
|
||||||
path = tmp_path / "scene.yaml"
|
path = tmp_path / "scene.yaml"
|
||||||
path.write_text(yaml.safe_dump(cfg))
|
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"],
|
"--headless", "--backend", "cpu", "--steps", "4", "--save-frames"],
|
||||||
env=env, capture_output=True, text=True, timeout=600)
|
env=env, capture_output=True, text=True, timeout=600)
|
||||||
assert result.returncode == 0, result.stdout + result.stderr
|
assert result.returncode == 0, result.stdout + result.stderr
|
||||||
|
assert result.stdout.count("Table tool: hammer1;") == 1
|
||||||
captures = list((tmp_path / "frames").glob("*/*.npz"))
|
captures = list((tmp_path / "frames").glob("*/*.npz"))
|
||||||
assert len(captures) == len(names)
|
assert len(captures) == len(names)
|
||||||
for name in 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