feat(simulation): add Worker AI polygon runtime and terrain navigation
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,170 @@
|
||||
"""Keep CMU height hazards, except an observed supported grade.
|
||||
|
||||
Height above a cell's low quantile is not step height. A smooth 15-degree ramp
|
||||
can exceed 10 cm across that cell. A 6 cm voxel mesh also quantizes a continuous
|
||||
grade. Admit that surface only with broad support, a bounded plane residual and
|
||||
no observed short-range height jump exceeding the qualified 10 cm step. Vertical
|
||||
surfaces, excessive roughness and sparse/unknown support retain the CMU cost.
|
||||
"""
|
||||
|
||||
import ctypes
|
||||
import hashlib
|
||||
import math
|
||||
from collections import OrderedDict
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
_NATIVE_PATH = Path("/opt/missioncore/libterrain_connectivity.so")
|
||||
_NATIVE = ctypes.CDLL(str(_NATIVE_PATH)) if _NATIVE_PATH.is_file() else None
|
||||
if _NATIVE is not None:
|
||||
for suffix, dtype in (("f32", ctypes.c_float), ("f64", ctypes.c_double)):
|
||||
function = getattr(_NATIVE, "terrain_connected_" + suffix)
|
||||
function.argtypes = [ctypes.POINTER(dtype), ctypes.c_int]
|
||||
function.restype = ctypes.c_int
|
||||
|
||||
|
||||
def connected_grade(nearby):
|
||||
if _NATIVE is not None and nearby.dtype in (np.dtype("float32"), np.dtype("float64")):
|
||||
points = np.ascontiguousarray(nearby[:, :3])
|
||||
dtype, suffix = (
|
||||
(ctypes.c_float, "f32") if points.dtype.itemsize == 4 else (ctypes.c_double, "f64")
|
||||
)
|
||||
return bool(
|
||||
getattr(_NATIVE, "terrain_connected_" + suffix)(
|
||||
points.ctypes.data_as(ctypes.POINTER(dtype)), len(points)
|
||||
)
|
||||
)
|
||||
# Reference implementation retained for portable CPU tests and comparison.
|
||||
dx = nearby[:, None, 0] - nearby[None, :, 0]
|
||||
dy = nearby[:, None, 1] - nearby[None, :, 1]
|
||||
separation = np.sqrt(dx * dx + dy * dy)
|
||||
jump = np.abs(nearby[:, None, 2] - nearby[None, :, 2])
|
||||
if np.any((separation <= 0.12) & (jump > 0.1001)):
|
||||
return False
|
||||
connected = separation <= 0.12
|
||||
reached = connected[np.argmin(np.linalg.norm(nearby[:, :2], axis=1))].copy()
|
||||
while True:
|
||||
expanded = np.any(connected[reached], axis=0)
|
||||
if np.array_equal(expanded, reached):
|
||||
return bool(reached.all())
|
||||
reached = expanded
|
||||
|
||||
|
||||
def underbody_support_costs(terrain, pose, contact_height_m=0.37):
|
||||
"""Reconcile low returns already inside the current chassis footprint.
|
||||
|
||||
CMU's neighbourhood ground reference can label the supported terrain under
|
||||
the chassis as a body collision. Use measured pose and the declared contact
|
||||
height only inside the body (with a 5 cm inset), never for terrain ahead.
|
||||
Retain drops, high returns and excessive tilt. The 8 cm band has 2 cm reserve
|
||||
below the physically qualified 10 cm step; raw terrain memory is unchanged.
|
||||
"""
|
||||
result = terrain.copy()
|
||||
x, y, z, w = pose[3:]
|
||||
up = np.array([2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)])
|
||||
if up[2] < math.cos(math.radians(25)):
|
||||
return result, 0
|
||||
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
|
||||
axes = np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])
|
||||
local = (terrain[:, :2] - pose[:2]) @ axes
|
||||
height = (terrain[:, :3] - pose[:3]) @ up + contact_height_m
|
||||
supported = (
|
||||
(np.abs(local) < 0.45).all(axis=1) & (np.abs(height) <= 0.08) & (terrain[:, 3] > 0.1)
|
||||
)
|
||||
result[supported, 3] = np.abs(height[supported])
|
||||
return result, int(supported.sum())
|
||||
|
||||
|
||||
def _neighborhoods(terrain, radius=0.4):
|
||||
"""Exact radius neighborhoods without scanning the whole accumulated map.
|
||||
|
||||
Returns in the nine adjacent cells include every possible neighbor. Keep
|
||||
source order so fitting and thresholds remain identical to the full scan.
|
||||
"""
|
||||
cells = np.floor(terrain[:, :2] / radius).astype(np.int64)
|
||||
buckets = {}
|
||||
for index, (x, y) in enumerate(cells):
|
||||
buckets.setdefault((x, y), []).append(index)
|
||||
cached = {}
|
||||
|
||||
def around(index):
|
||||
key = tuple(cells[index])
|
||||
if key not in cached:
|
||||
x, y = key
|
||||
cached[key] = np.array(
|
||||
sorted(
|
||||
i
|
||||
for dx in (-1, 0, 1)
|
||||
for dy in (-1, 0, 1)
|
||||
for i in buckets.get((x + dx, y + dy), ())
|
||||
),
|
||||
dtype=np.int64,
|
||||
)
|
||||
delta = terrain[cached[key], :3] - terrain[index, :3]
|
||||
return delta[np.linalg.norm(delta[:, :2], axis=1) <= radius]
|
||||
|
||||
return around
|
||||
|
||||
|
||||
def supported_slope_costs(terrain, cache=None):
|
||||
result = terrain.copy()
|
||||
if len(terrain) < 8:
|
||||
return result, 0
|
||||
around = _neighborhoods(terrain)
|
||||
corrected = 0
|
||||
for index in np.flatnonzero(terrain[:, 3] > 0.1):
|
||||
nearby = around(index)
|
||||
if len(nearby) < 8:
|
||||
continue
|
||||
# Any admitted plane spans at most a 0.8 m diameter at 25 degrees,
|
||||
# plus the two 7.5 cm residuals. Reject tall foliage/walls before fitting.
|
||||
if np.ptp(nearby[:, 2]) > 0.8 * math.tan(math.radians(25 + 1e-4)) + 0.15:
|
||||
continue
|
||||
key = None
|
||||
if cache is not None:
|
||||
key = hashlib.blake2b(nearby.tobytes(), digest_size=24).digest()
|
||||
if key in cache:
|
||||
if cache[key]:
|
||||
result[index, 3] = 0.0
|
||||
corrected += 1
|
||||
cache.move_to_end(key)
|
||||
continue
|
||||
cache[key] = False
|
||||
if len(cache) > 16384:
|
||||
cache.popitem(last=False)
|
||||
# No collinear strip, hidden region, multiple height layers or vertical
|
||||
# surface is admitted as a plane. All observed points must agree.
|
||||
covariance = np.cov(nearby[:, :2], rowvar=False)
|
||||
if np.linalg.eigvalsh(covariance)[0] < 0.0036:
|
||||
continue
|
||||
matrix = np.column_stack((nearby[:, :2], np.ones(len(nearby))))
|
||||
plane = np.linalg.lstsq(matrix, nearby[:, 2], rcond=None)[0]
|
||||
slope = math.degrees(math.atan(np.linalg.norm(plane[:2])))
|
||||
if not 2 <= slope <= 25 + 1e-4 or abs(plane[2]) > 0.05:
|
||||
continue
|
||||
if np.max(np.abs(matrix @ plane - nearby[:, 2])) > 0.075:
|
||||
continue
|
||||
# A permissive fit alone could erase a real ledge. Test close measured
|
||||
# returns explicitly: even a narrow step/drop must retain its hazard.
|
||||
if not connected_grade(nearby):
|
||||
continue # Do not fit a road across a gap with no returns.
|
||||
result[index, 3] = 0.0
|
||||
corrected += 1
|
||||
if cache is not None:
|
||||
cache[key] = True
|
||||
return result, corrected
|
||||
|
||||
|
||||
class TerrainCostNormalizer:
|
||||
"""Reuse fits only for byte-identical observed neighborhoods, bounded in RAM.
|
||||
|
||||
New or changed returns always trigger a new fit. This stores no occupancy
|
||||
belief and clears with the owning ROS node on every episode/reset.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
self.cache = OrderedDict()
|
||||
|
||||
def __call__(self, terrain):
|
||||
return supported_slope_costs(terrain, self.cache)
|
||||
Reference in New Issue
Block a user