File size: 1,671 Bytes
8c7a278 6686473 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 | """Tests for buffer allocation (strategy-layer temporary parking).
Uses a headless sim so footprint/reachability queries hit real PyBullet, but
objects are teleported into controlled layouts.
"""
import math
import os
import sys
import pybullet as pb
import pytest
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from llm_panda import config
@pytest.fixture
def sim():
from main import PandaSimulation
simulation = PandaSimulation(gui=False, seed=3)
yield simulation
simulation.disconnect()
def _isolate(sim, keep):
"""Park every object except those named in `keep` far off the table."""
for oid in sim.scene.object_ids:
if sim.scene.get_name(oid) not in keep:
pb.resetBasePositionAndOrientation(oid, [0.5, 0.62, 0.34], [0, 0, 0, 1])
# --------------------------------------------------------------------------- #
# Strategy: buffer allocation
# --------------------------------------------------------------------------- #
def test_buffer_spot_is_clear_of_avoided_object(sim):
_isolate(sim, {"banana"})
ban = sim.scene.name_to_id("banana")
ban_pos = [0.50, 0.0, 0.33]
pb.resetBasePositionAndOrientation(ban, ban_pos, [0, 0, 0, 1])
for _ in range(30):
pb.stepSimulation()
spot = sim.scene.find_buffer_spot(
(0.08, 0.08), exclude_ids=[], avoid_positions=[ban_pos],
)
assert spot is not None
d = math.dist(spot[:2], ban_pos[:2])
assert d >= config.BUFFER_MIN_CLEARANCE - 1e-6
# And it must be a real spot on the table top.
assert abs(spot[2] - config.TABLE_TOP_Z) < 1e-6
|