feat(simulation): add Worker AI polygon runtime and terrain navigation
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,390 @@
|
||||
import importlib.util
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from k1link.observatory.modular_composition import CompositionError
|
||||
from k1link.simulation.ai_polygon.composition import compose, registry
|
||||
from k1link.simulation.ai_polygon.terrain_contract import terrain_matches
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1] / "simulation/ai-polygon"
|
||||
|
||||
|
||||
def module(name):
|
||||
spec = importlib.util.spec_from_file_location(name, ROOT / (name + ".py"))
|
||||
result = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(result)
|
||||
return result
|
||||
|
||||
|
||||
def test_simulation_composition_has_causal_dependencies_and_separate_authority():
|
||||
graph = compose(ROOT)
|
||||
assert graph.as_dict()["execution"]["mode"] == "worker-local-simulation"
|
||||
motion = graph.nodes[-1]
|
||||
assert motion.module.group == "motion"
|
||||
assert dict(motion.inputs)["segmentation.surface"] == "simulation-segformer-ade"
|
||||
assert dict(motion.inputs)["detection.boxes"] == "simulation-rf-detr"
|
||||
assert motion.module.state_policy == "causal-reset-at-source-start"
|
||||
selection = graph.selection_document()
|
||||
selection["selections"] = [r for r in selection["selections"] if r["group"] != "segmentation"]
|
||||
with pytest.raises(CompositionError, match="segmentation.surface"):
|
||||
compose(ROOT, selection)
|
||||
|
||||
|
||||
def test_surface_providers_are_interchangeable_in_the_shared_constructor():
|
||||
selection = compose(ROOT).selection_document()
|
||||
reference = next(m for m in registry(ROOT).modules if m.module_id == "simulation-ddrnet-goose")
|
||||
for row in selection["selections"]:
|
||||
if row["group"] == "segmentation":
|
||||
row.update(module_id=reference.module_id, module_sha256=reference.sha256)
|
||||
graph = compose(ROOT, selection)
|
||||
assert dict(graph.nodes[-1].inputs)["segmentation.surface"] == reference.module_id
|
||||
|
||||
|
||||
def test_composition_rejects_stale_module_identity():
|
||||
selection = compose(ROOT).selection_document()
|
||||
selection["selections"][0]["module_sha256"] = "0" * 64
|
||||
with pytest.raises(CompositionError, match="not installed"):
|
||||
compose(ROOT, selection)
|
||||
|
||||
|
||||
def test_shared_constructor_import_needs_no_core_or_third_party_runtime():
|
||||
# -S removes site-packages, as in the minimal Windows coordinator. Loading
|
||||
# a contract must not load the POSIX-only artifact gateway through __init__.
|
||||
source = str(ROOT.parents[1] / "src")
|
||||
result = subprocess.run(
|
||||
[
|
||||
sys.executable,
|
||||
"-S",
|
||||
"-c",
|
||||
f"import sys; sys.path.insert(0, {source!r}); "
|
||||
"from k1link.observatory.modular_composition import ModuleRegistry; "
|
||||
"from k1link.simulation.ai_polygon.composition import compose; "
|
||||
"assert 'k1link.artifact_gateway' not in sys.modules",
|
||||
],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
assert result.returncode == 0, result.stderr
|
||||
|
||||
|
||||
def test_semantic_goal_uses_range_and_correct_square_camera_crop():
|
||||
nav = module("navigation_client")
|
||||
points = np.array(
|
||||
[[x, y, 0] for x in np.linspace(1.5, 3, 20) for y in np.linspace(-0.3, 0.3, 9)],
|
||||
dtype=np.float32,
|
||||
)
|
||||
pose = [0, 0, 0.27, 0, 0, 0, 1]
|
||||
calibration = {
|
||||
"origin": [0.38, 0, 0.8],
|
||||
"rotation": np.eye(3).reshape(-1).tolist(),
|
||||
"intrinsics": [800 * 24 / 36, 800 * 24 / 36, 400, 300],
|
||||
}
|
||||
leaves = np.ones((512, 512), dtype=bool)
|
||||
goal = nav.visual_goal(leaves, points, pose, calibration)
|
||||
assert goal is not None and 1.5 < goal[0] < 3 and abs(goal[1]) < 0.3
|
||||
assert nav.visual_goal(np.zeros_like(leaves), points, pose, calibration) is None
|
||||
assert nav.visual_goal(leaves, points + [0, 0, 2], pose, calibration) is None
|
||||
# A previously valid goal cannot authorize motion through newly unknown RGB.
|
||||
assert nav.visual_goal(np.zeros_like(leaves), points, pose, calibration, goal) is None
|
||||
# An explicit-route waypoint entering the camera blind strip is retained,
|
||||
# but losing all current visual surface support still forbids movement.
|
||||
close = [0.65, 0, 0]
|
||||
assert nav.visual_goal(leaves, points, pose, calibration, close, target=[0.65, 0]) == close
|
||||
assert (
|
||||
nav.visual_goal(np.zeros_like(leaves), points, pose, calibration, close, target=[0.65, 0])
|
||||
is None
|
||||
)
|
||||
|
||||
|
||||
def test_ground_placement_uses_actual_triangle_intersection():
|
||||
terrain = module("terrain")
|
||||
vertices = np.array([[0, 0, 0], [1, 0, 0.2], [0, 1, 0]], dtype=np.float32)
|
||||
faces = np.array([[0, 1, 2]], dtype=np.int32)
|
||||
assert terrain.ground_intersections(vertices, faces, 0.25, 0.25)[0] == pytest.approx(0.05)
|
||||
assert len(terrain.ground_intersections(vertices, faces, 0.9, 0.9)) == 0
|
||||
|
||||
|
||||
def test_observed_route_goal_keeps_task_position_and_cannot_run_away_from_it():
|
||||
choose = module("navigation_client").visual_goal
|
||||
points = np.array(
|
||||
[[x, y, 0] for x in np.arange(1.2, 3.1, 0.05) for y in np.arange(-0.5, 0.51, 0.05)]
|
||||
)
|
||||
pose = [0, 0, 0.37, 0, 0, 0, 1]
|
||||
calibration = {
|
||||
"origin": [0.38, 0, 0.8],
|
||||
"rotation": np.eye(3).reshape(-1).tolist(),
|
||||
"intrinsics": [800 * 24 / 36, 800 * 24 / 36, 400, 300],
|
||||
"body_contact_height_m": 0.37,
|
||||
}
|
||||
surface = np.ones((512, 512), dtype=bool)
|
||||
target = [2.03, 0.27]
|
||||
assert choose(surface, points, pose, calibration, target=target) == pytest.approx([*target, 0])
|
||||
# The close, already observed waypoint can enter the camera blind strip.
|
||||
advanced = [1.5, 0, 0.37, 0, 0, 0, 1]
|
||||
camera = {**calibration, "origin": [1.88, 0, 0.8]}
|
||||
prior = [*target, 0]
|
||||
assert choose(surface, points + [1.5, 0, 0], advanced, camera, prior, target) == prior
|
||||
# The actual camera loses nearby ground beyond the old hardcoded 0.8 m.
|
||||
advanced = [1.1, 0, 0.37, 0, 0, 0, 1]
|
||||
camera = {**calibration, "origin": [1.48, 0, 0.8]}
|
||||
assert choose(surface, points + [1.1, 0, 0], advanced, camera, prior, target) == prior
|
||||
assert choose(np.zeros_like(surface), points, pose, calibration, target=target) is None
|
||||
# Clear road ahead is not permission to drive away from a missed waypoint.
|
||||
assert choose(surface, points, pose, calibration, target=[-1, 0]) is None
|
||||
# A distant task may still use an observed local goal towards it.
|
||||
far = choose(surface, points, pose, calibration, target=[8, 0])
|
||||
assert far is not None and 1.2 <= far[0] <= 3.1
|
||||
|
||||
|
||||
def test_collision_identity_follows_geometry_and_tile_coverage_not_camera_or_start():
|
||||
settings = dict(
|
||||
meters_per_unit=1,
|
||||
rotation_degrees=[-90, 0, 180],
|
||||
spawn_xy=[0, 0],
|
||||
ground_z=0,
|
||||
camera_height_m=0.8,
|
||||
max_speed_mps=0.15,
|
||||
)
|
||||
terrain = dict(source_sha256="a" * 64, generator_sha256="b" * 64, settings=settings)
|
||||
world = dict(sha256="a" * 64, settings={**settings, "spawn_xy": [1, 1], "camera_height_m": 1})
|
||||
assert terrain_matches(terrain, world, "b" * 64)
|
||||
assert not terrain_matches(terrain, world, "c" * 64)
|
||||
assert not terrain_matches(terrain, {**world, "sha256": "d" * 64})
|
||||
for change in ({"spawn_xy": [20, 0]}, {"meters_per_unit": 2}, {"rotation_degrees": [0, 0, 0]}):
|
||||
assert not terrain_matches(terrain, {**world, "settings": {**world["settings"], **change}})
|
||||
|
||||
|
||||
def test_paired_full_scene_does_not_inherit_generated_tile_bounds():
|
||||
settings = dict(meters_per_unit=1, rotation_degrees=[90, 0, 0], spawn_xy=[0, 0], ground_z=5)
|
||||
terrain = dict(
|
||||
generator="paired-source",
|
||||
source_sha256="a" * 64,
|
||||
collider_sha256="b" * 64,
|
||||
settings=settings,
|
||||
)
|
||||
world = dict(
|
||||
sha256="a" * 64,
|
||||
collider_sha256="b" * 64,
|
||||
settings={**settings, "spawn_xy": [210, 30], "ground_z": 1.5},
|
||||
)
|
||||
assert terrain_matches(terrain, world)
|
||||
assert not terrain_matches(terrain, {**world, "collider_sha256": "c" * 64})
|
||||
assert not terrain_matches(
|
||||
terrain, {**world, "settings": {**world["settings"], "meters_per_unit": 2}}
|
||||
)
|
||||
assert not terrain_matches({**terrain, "generator": "generated-tile"}, world)
|
||||
|
||||
|
||||
def test_square_footprint_fits_straight_corridor_and_rejects_corner_sweep():
|
||||
check = module("navigation/footprint").swept_footprint_clear
|
||||
pose = [0, 0, 0.27, 0, 0, 0, 1]
|
||||
path = [[0, 0, 0], [0.5, 0, 0], [1, 0, 0]]
|
||||
walls = np.array([[x, y, 0.5, 0.5] for x in np.arange(-1, 2, 0.1) for y in [-0.65, 0.65]])
|
||||
assert check(path, walls, pose)
|
||||
walls[:, 1] *= 0.45 / 0.65
|
||||
assert not check(path, walls, pose)
|
||||
# A diagonal turn sweeps a square corner into this obstacle, even though
|
||||
# the chassis at its initial and final straight poses does not contain it.
|
||||
assert not check([[0, 0, 0], [0.5, 0.5, 0]], [[0.7, 0, 0.5, 0.5]], pose)
|
||||
|
||||
|
||||
def test_command_monitor_covers_deadman_braking_distance_and_rotation():
|
||||
check = module("navigation/footprint").command_footprint_clear
|
||||
pose = [0, 0, 0.27, 0, 0, 0, 1]
|
||||
assert check(0.15, 0, [[1.5, 0, 0.5, 0.5]], pose)
|
||||
assert not check(0.15, 0, [[0.7, 0, 0.5, 0.5]], pose)
|
||||
assert not check(0, 0.8, [[0.7, 0, 0.5, 0.5]], pose)
|
||||
|
||||
|
||||
def test_smooth_slope_is_distinct_from_a_step_or_vertical_terrain():
|
||||
costs = module("navigation/terrain_costs").supported_slope_costs
|
||||
xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.1) for y in np.arange(-1, 1.01, 0.1)])
|
||||
slope = np.column_stack((xy, xy[:, 0] * np.tan(np.radians(20)), np.full(len(xy), 0.15)))
|
||||
normalized, corrected = costs(slope)
|
||||
assert corrected > len(slope) * 0.9
|
||||
assert normalized[len(slope) // 2, 3] == 0
|
||||
assert np.array_equal(normalized[:, :3], slope[:, :3])
|
||||
# A 15 cm ledge across the initial footprint cannot become a traversable ramp.
|
||||
step = slope.copy()
|
||||
step[:, 2] = np.where(step[:, 0] >= 0, 0.15, 0)
|
||||
assert costs(step)[1] == 0
|
||||
cliff = slope.copy()
|
||||
cliff[:, 2] = np.where(cliff[:, 0] >= 0, -0.4, 0)
|
||||
assert costs(cliff)[1] == 0
|
||||
steep = slope.copy()
|
||||
steep[:, 2] = steep[:, 0] * np.tan(np.radians(35))
|
||||
assert costs(steep)[1] == 0
|
||||
assert costs(slope[np.abs(slope[:, 1]) < 0.01])[1] == 0 # Unobserved lateral support.
|
||||
|
||||
|
||||
def test_underbody_support_does_not_clear_future_terrain_walls_or_drops():
|
||||
correct = module("navigation/terrain_costs").underbody_support_costs
|
||||
terrain = np.array(
|
||||
[
|
||||
[-0.375, -0.28, 0.066, 0.103], # Low return already under the chassis.
|
||||
[0.46, 0, 0.066, 0.103], # Inset excludes the leading edge.
|
||||
[0.75, 0, 0.066, 0.103], # Never change future terrain from body pose.
|
||||
[0, 0, 0.12, 0.12], # A real step within the footprint remains blocked.
|
||||
[0, 0, 0.5, 0.5],
|
||||
[0, 0, -0.4, 0.4],
|
||||
]
|
||||
)
|
||||
original = terrain.copy()
|
||||
result, count = correct(terrain, [0, 0, 0.37, 0, 0, 0, 1])
|
||||
assert count == 1 and result[0, 3] == pytest.approx(0.066)
|
||||
assert np.array_equal(result[1:], original[1:])
|
||||
assert np.array_equal(terrain, original) # Never erase the causal raw map.
|
||||
# Rotate both observations and the measured chassis; the result must agree.
|
||||
yaw = np.pi / 2
|
||||
rotated = terrain.copy()
|
||||
rotated[:, :2] = terrain[:, :2] @ np.array([[0, 1], [-1, 0]]) + [3, 4]
|
||||
pose = [3, 4, 0.37, 0, 0, np.sin(yaw / 2), np.cos(yaw / 2)]
|
||||
assert np.allclose(correct(rotated, pose)[0][:, 3], result[:, 3])
|
||||
assert correct(terrain, [0, 0, 0.37, 0, np.sin(np.pi / 12), 0, np.cos(np.pi / 12)])[1] == 0
|
||||
|
||||
|
||||
def test_retreat_requires_observed_full_width_support_and_no_drop_or_step():
|
||||
choose = module("navigation_client").recovery_goal
|
||||
points = np.array(
|
||||
[[x, y, 0.0] for x in np.arange(-1.5, -0.39, 0.05) for y in np.arange(-0.85, 0.86, 0.05)]
|
||||
)
|
||||
pose = [0, 0, 0.37, 0, 0, 0, 1]
|
||||
calibration = {"body_contact_height_m": 0.37}
|
||||
assert choose(points, pose, calibration) == pytest.approx([-0.65, 0, 0])
|
||||
assert choose(points[points[:, 1] > -0.2], pose, calibration) is None
|
||||
assert choose(points[points[:, 0] < -0.9], pose, calibration) is None
|
||||
for height in (-0.4, 0.15):
|
||||
discontinuous = points.copy()
|
||||
discontinuous[points[:, 0] < -0.9, 2] = height
|
||||
assert choose(discontinuous, pose, calibration) is None
|
||||
slope = points.copy()
|
||||
slope[:, 2] = slope[:, 0] * np.tan(np.radians(10))
|
||||
assert choose(slope, pose, calibration) is not None
|
||||
assert choose(points, pose, calibration, [-0.65, 0.3, 0]) is None
|
||||
|
||||
|
||||
def test_reverse_monitor_checks_behind_the_body():
|
||||
check = module("navigation/footprint").command_footprint_clear
|
||||
pose = [0, 0, 0.37, 0, 0, 0, 1]
|
||||
assert check(-0.1, 0, [[-1.5, 0, 0.5, 0.5]], pose)
|
||||
assert not check(-0.1, 0, [[-0.65, 0, 0.5, 0.5]], pose)
|
||||
|
||||
|
||||
def test_legacy_coordinate_migration_is_an_exact_rigid_rotation():
|
||||
import json
|
||||
import struct
|
||||
|
||||
migrate = module("navigation/migrate_terrain_coordinates")
|
||||
positions = [[1.0, -2.0, -3.0], [2.0, -2.0, -3.0], [1.0, -1.0, -3.0]]
|
||||
binary = struct.pack("<9f3I", *(v for p in positions for v in p), 0, 1, 2)
|
||||
document = {
|
||||
"nodes": [{"mesh": 0}],
|
||||
"meshes": [{"primitives": [{"attributes": {"POSITION": 0}, "indices": 1}]}],
|
||||
"accessors": [
|
||||
{
|
||||
"bufferView": 0,
|
||||
"componentType": 5126,
|
||||
"type": "VEC3",
|
||||
"count": 3,
|
||||
"min": [1, -2, -3],
|
||||
"max": [2, -1, -3],
|
||||
},
|
||||
{"bufferView": 1, "componentType": 5125, "type": "SCALAR", "count": 3},
|
||||
],
|
||||
"bufferViews": [{"byteOffset": 0, "byteLength": 36}, {"byteOffset": 36, "byteLength": 12}],
|
||||
}
|
||||
raw = json.dumps(document).encode()
|
||||
raw += b" " * ((-len(raw)) % 4)
|
||||
glb = (
|
||||
struct.pack("<III", 0x46546C67, 2, 28 + len(raw) + len(binary))
|
||||
+ struct.pack("<II", len(raw), 0x4E4F534A)
|
||||
+ raw
|
||||
+ struct.pack("<II", len(binary), 0x004E4942)
|
||||
+ binary
|
||||
)
|
||||
corrected = migrate.rotate_glb(glb)
|
||||
length = struct.unpack_from("<I", corrected, 12)[0]
|
||||
result = np.array(struct.unpack_from("<9f", corrected, 28 + length)).reshape(-1, 3)
|
||||
assert np.array_equal(result, np.array(positions) * [-1, 1, -1])
|
||||
assert np.array_equal(
|
||||
result[:, [0, 2, 1]] * [1, -1, 1], [[-1, -3, -2], [-2, -3, -2], [-1, -3, -1]]
|
||||
)
|
||||
assert struct.unpack_from("<3I", corrected, 28 + length + 36) == (0, 1, 2)
|
||||
|
||||
|
||||
def test_voxel_quantized_grade_does_not_become_a_wall_but_ledge_remains():
|
||||
costs = module("navigation/terrain_costs").supported_slope_costs
|
||||
xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.06) for y in np.arange(-1, 1.01, 0.06)])
|
||||
height = np.round(xy[:, 0] * np.tan(np.radians(20)) / 0.06) * 0.06
|
||||
surface = np.column_stack((xy, height, np.full(len(xy), 0.15)))
|
||||
assert costs(surface)[1] > len(surface) * 0.8
|
||||
for discontinuity in (0.12, 0.15, -0.4):
|
||||
ledge = surface.copy()
|
||||
ledge[:, 2] = np.where(xy[:, 0] >= 0, discontinuity, 0)
|
||||
corrected, _ = costs(ledge)
|
||||
near_edge = np.abs(xy[:, 0]) < 0.12
|
||||
assert np.all(corrected[near_edge, 3] > 0.1)
|
||||
stone = surface.copy()
|
||||
stone[:, 2] = 0
|
||||
stone[(abs(xy[:, 0]) < 0.12) & (abs(xy[:, 1]) < 0.12), 2] = 0.15
|
||||
assert costs(stone)[1] == 0
|
||||
|
||||
|
||||
def test_grade_fit_cannot_bridge_an_unobserved_gap():
|
||||
costs = module("navigation/terrain_costs").supported_slope_costs
|
||||
points = np.array(
|
||||
[
|
||||
[x, y, 0 if x < 0 else 0.15, 0.15]
|
||||
for x in [-0.3, -0.2, 0.2, 0.3]
|
||||
for y in np.arange(-0.3, 0.31, 0.1)
|
||||
]
|
||||
)
|
||||
assert costs(points)[1] == 0
|
||||
|
||||
|
||||
def test_existing_reserve_overlap_only_allows_departure_not_approach_or_body_overlap():
|
||||
check = module("navigation/footprint").command_footprint_clear
|
||||
pose = [0, 0, 0.37, 0, 0, 0, 1]
|
||||
behind = [[-0.53, 0, 0.5, 0.5]]
|
||||
assert check(0.15, 0, behind, pose)
|
||||
assert not check(-0.1, 0, behind, pose)
|
||||
assert not check(0, 0.35, behind, pose)
|
||||
assert not check(0.15, 0, [[-0.49, 0, 0.5, 0.5]], pose)
|
||||
assert not check(0.15, 0, [[0.53, 0, 0.5, 0.5]], pose)
|
||||
# A longer admitted camera age must also enlarge the collision envelope.
|
||||
assert not check(0.15, 0, [[0.80, 0, 0.5, 0.5]], pose)
|
||||
|
||||
|
||||
def test_terrain_fit_cache_invalidates_when_a_new_obstacle_is_observed():
|
||||
costs = module("navigation/terrain_costs")
|
||||
normalize = costs.TerrainCostNormalizer()
|
||||
xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.1) for y in np.arange(-1, 1.01, 0.1)])
|
||||
grade = np.column_stack((xy, xy[:, 0] * np.tan(np.radians(20)), np.full(len(xy), 0.15)))
|
||||
clear, count = normalize(grade)
|
||||
assert count > len(grade) * 0.9
|
||||
assert np.array_equal(normalize(grade)[0], clear)
|
||||
changed = np.vstack((grade, [0.02, 0.02, 0.3, 0.3]))
|
||||
cached, count = normalize(changed)
|
||||
fresh, expected_count = costs.supported_slope_costs(changed)
|
||||
assert np.array_equal(cached, fresh) and count == expected_count
|
||||
assert cached[len(grade) // 2, 3] > 0.1
|
||||
assert np.array_equal(normalize(grade)[0], clear)
|
||||
|
||||
|
||||
def test_velocity_regulation_keeps_the_selected_arc_and_its_braking_clearance():
|
||||
monitor = module("navigation/footprint")
|
||||
pose = [0, 0, 0.37, 0, 0, 0, 1]
|
||||
hazard = [[0.834, -0.08, 0.15, 0.15]]
|
||||
speed, yaw, scale = monitor.regulate_command(0.15, -0.245, hazard, pose)
|
||||
assert 0.2 <= scale < 1
|
||||
assert speed / yaw == pytest.approx(0.15 / -0.245)
|
||||
assert monitor.command_footprint_clear(speed, yaw, hazard, pose)
|
||||
assert monitor.regulate_command(0.15, 0, [[0.56, 0, 0.2, 0.2]], pose) == (0, 0, 0)
|
||||
assert monitor.regulate_command(0.15, 0, [[0.49, 0, 0.2, 0.2]], pose) == (0, 0, 0)
|
||||
reverse, _, scale = monitor.regulate_command(-0.1, 0, [[-0.68, 0, 0.2, 0.2]], pose)
|
||||
assert 0.2 <= scale < 1 and reverse < 0
|
||||
assert monitor.command_footprint_clear(reverse, 0, [[-0.68, 0, 0.2, 0.2]], pose)
|
||||
Reference in New Issue
Block a user