67 lines
2.8 KiB
Python
67 lines
2.8 KiB
Python
"""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
|