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

tests/test_tool_physics.py

@@ -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