480 lines
19 KiB
Python
480 lines
19 KiB
Python
"""Validated, dependency-free browser training/deployment contract (version 1)."""
|
||
|
||
from __future__ import annotations
|
||
|
||
import copy
|
||
import math
|
||
import random
|
||
from typing import Any
|
||
|
||
FLAT_TASK = "Unitree-Go2-Flat"
|
||
ROUGH_TASK = "Unitree-Go2-Rough"
|
||
OBSTACLE_TASK = "Unitree-Go2-ObstacleAvoidance"
|
||
TERRAIN_PRESETS = ("plane", "discrete_obstacles", "rough", "pyramid_stairs", "wave", "custom_boxes")
|
||
# Range metadata is also the sole validation source for browser configuration.
|
||
TERRAIN_PARAMETERS = {
|
||
"size": {"min": 8, "max": 24, "default": 12},
|
||
"obstacle_count": {"min": 1, "max": 100, "default": 24, "integer": True},
|
||
"obstacle_height_min": {"min": 0.05, "max": 1.5, "default": 0.2},
|
||
"obstacle_height_max": {"min": 0.05, "max": 1.5, "default": 0.6},
|
||
"spacing": {"min": 0.6, "max": 3, "default": 1.2},
|
||
"friction": {"min": 0.2, "max": 2, "default": 0.8},
|
||
"roughness": {"min": 0.01, "max": 0.2, "default": 0.06},
|
||
"step_height": {"min": 0.03, "max": 0.2, "default": 0.08},
|
||
"wave_amplitude": {"min": 0.01, "max": 0.2, "default": 0.08},
|
||
}
|
||
SENSOR_PARAMETERS = {
|
||
"fov": {"min": 30, "max": 120, "default": 90},
|
||
"maxDistance": {"min": 1, "max": 5, "default": 4},
|
||
"safetyDistance": {"min": 0.1, "max": 1, "default": 0.5},
|
||
"avoidanceWeight": {"min": 0, "max": 10, "default": 2},
|
||
}
|
||
JOINT_NAMES = [
|
||
f"{leg}_{joint}_joint" for leg in ("FL", "FR", "RL", "RR") for joint in ("hip", "thigh", "calf")
|
||
]
|
||
DEFAULT_JOINT_POSITION = [-0.1, 0.9, -1.8, 0.1, 0.9, -1.8] * 2
|
||
|
||
|
||
class TaskConfigError(ValueError):
|
||
pass
|
||
|
||
|
||
def _parameters(value: Any, schema: dict, label: str) -> dict:
|
||
if not isinstance(value, dict) or value.keys() - schema.keys():
|
||
raise TaskConfigError(f"{label} 包含未知参数或不是对象")
|
||
result = {}
|
||
for name, bounds in schema.items():
|
||
number = value.get(name, bounds["default"])
|
||
if (
|
||
isinstance(number, bool)
|
||
or not isinstance(number, (int, float))
|
||
or not bounds["min"] <= number <= bounds["max"]
|
||
or not math.isfinite(number)
|
||
or (bounds.get("integer") and not isinstance(number, int))
|
||
):
|
||
raise TaskConfigError(f"{label}.{name} 超出允许的有限数值范围")
|
||
result[name] = number
|
||
return result
|
||
|
||
|
||
def sensor_pattern(mode: str, fov: float) -> dict:
|
||
if mode not in ("single_ring_raycast", "multi_ring_raycast"):
|
||
raise TaskConfigError("不支持的 sensorMode")
|
||
multi = mode == "multi_ring_raycast"
|
||
count = 16 if multi else 32
|
||
return {
|
||
"sensorMode": mode,
|
||
"rayCount": 48 if multi else 32,
|
||
"pitchAngles": [0, -20, -45] if multi else [0],
|
||
"yawCount": count,
|
||
"yawAngles": [-fov / 2 + i * fov / (count - 1) for i in range(count)],
|
||
"angleUnit": "deg",
|
||
"rayOrder": "layer-major",
|
||
}
|
||
|
||
|
||
def validate_sensor_config(raw: dict) -> dict:
|
||
pattern_keys = set(sensor_pattern("single_ring_raycast", 90))
|
||
sensor = _parameters(
|
||
{k: v for k, v in raw.items() if k not in pattern_keys | {"type"}},
|
||
SENSOR_PARAMETERS,
|
||
"sensorCfg",
|
||
)
|
||
pattern = sensor_pattern(raw.get("sensorMode", "single_ring_raycast"), sensor["fov"])
|
||
for key, expected in pattern.items():
|
||
if key not in raw:
|
||
continue
|
||
actual = raw[key]
|
||
if isinstance(expected, list):
|
||
valid = (
|
||
isinstance(actual, list)
|
||
and len(actual) == len(expected)
|
||
and all(
|
||
not isinstance(a, bool)
|
||
and isinstance(a, (int, float))
|
||
and math.isfinite(a)
|
||
and abs(a - b) <= 1e-10
|
||
for a, b in zip(actual, expected, strict=True)
|
||
)
|
||
)
|
||
else:
|
||
valid = type(actual) is type(expected) and actual == expected
|
||
if not valid:
|
||
raise TaskConfigError(f"sensorCfg.{key} 与mode/FOV矛盾")
|
||
return {**sensor, "type": "raycast", **pattern}
|
||
|
||
|
||
def validate_custom_terrain(value: Any) -> dict:
|
||
"""Untrusted full layout: exact schema, no RNG, no geometry repair or clipping."""
|
||
fields = {
|
||
"representation",
|
||
"approximation",
|
||
"size",
|
||
"friction",
|
||
"boxes",
|
||
"spawn",
|
||
"spawnQuaternion",
|
||
"target",
|
||
"actualObstacleCount",
|
||
}
|
||
if not isinstance(value, dict) or set(value) != fields:
|
||
raise TaskConfigError("customTerrainBoxes 字段缺失或未知（不接受路径/MJCF）")
|
||
|
||
def number(v, lo, hi):
|
||
if (
|
||
isinstance(v, bool)
|
||
or not isinstance(v, (int, float))
|
||
or not lo <= v <= hi
|
||
or not math.isfinite(v)
|
||
):
|
||
raise TaskConfigError("customTerrainBoxes 必须使用范围内有限数值")
|
||
return v
|
||
|
||
def vector(v, n, lo=-12, hi=12):
|
||
if not isinstance(v, list) or len(v) != n:
|
||
raise TaskConfigError("customTerrainBoxes 向量长度无效")
|
||
return [number(x, lo, hi) for x in v]
|
||
|
||
size = number(value["size"], 8, 24)
|
||
number(value["friction"], 0.2, 2)
|
||
if value["representation"] != "boxes-v1" or value["approximation"] is not True:
|
||
raise TaskConfigError(
|
||
"custom_boxes 必须声明 boxes-v1 和 approximation=true（AABB/底板标准化）"
|
||
)
|
||
boxes = value["boxes"]
|
||
if not isinstance(boxes, list) or not 1 <= len(boxes) <= 257:
|
||
raise TaskConfigError("customTerrainBoxes 只能包含底板及最多256障碍")
|
||
for box in boxes:
|
||
if not isinstance(box, dict) or set(box) != {"pos", "size", "yaw"}:
|
||
raise TaskConfigError("box 字段缺失或未知")
|
||
center = vector(box["pos"], 3)
|
||
half = vector(box["size"], 3, 0, 12)
|
||
number(box["yaw"], 0, 0)
|
||
if any(x <= 0 for x in half):
|
||
raise TaskConfigError("box 半尺寸必须严格大于零")
|
||
if any(abs(center[i]) + half[i] > size / 2 + 1e-6 for i in range(2)):
|
||
raise TaskConfigError("box 超出世界地图边界")
|
||
if center[2] - half[2] < -0.2 - 1e-6 or center[2] + half[2] > 12:
|
||
raise TaskConfigError("box 高度超出边界")
|
||
if boxes[0] != {"pos": [0, 0, -0.1], "size": [size / 2, size / 2, 0.1], "yaw": 0}:
|
||
raise TaskConfigError("必须使用标准 floor z=[-0.2,0]")
|
||
count = value["actualObstacleCount"]
|
||
if isinstance(count, bool) or not isinstance(count, int) or count != len(boxes) - 1:
|
||
raise TaskConfigError("actualObstacleCount 与布局不一致")
|
||
spawn = vector(value["spawn"], 3)
|
||
target = vector(value["target"], 2)
|
||
if spawn[2] != 0.32:
|
||
raise TaskConfigError("出生高度必须为0.32")
|
||
quaternion = vector(value["spawnQuaternion"], 4, -1, 1)
|
||
if abs(sum(x * x for x in quaternion) - 1) > 1e-6:
|
||
raise TaskConfigError("出生四元数必须归一化")
|
||
for point in (spawn, target):
|
||
if any(abs(point[i]) > size / 2 - 0.5 for i in range(2)):
|
||
raise TaskConfigError("起终点0.5m安全区超出地图")
|
||
for box in boxes[1:]:
|
||
distance_sq = sum(
|
||
max(abs(point[i] - box["pos"][i]) - box["size"][i], 0) ** 2 for i in range(2)
|
||
)
|
||
if distance_sq <= 0.5**2:
|
||
raise TaskConfigError("障碍物侵占起终点0.5m圆形安全区；请修改坐标，不会清除障碍")
|
||
return copy.deepcopy(value)
|
||
|
||
|
||
def validate_task_config(task_id: str, payload: dict, seed: int) -> dict | None:
|
||
"""Validate preset parameters or an authoritative boxes-v1 layout, never paths/XML."""
|
||
custom_keys = {
|
||
"terrainPreset",
|
||
"terrainParams",
|
||
"sensorCfg",
|
||
"sensorType",
|
||
"customTerrainBoxes",
|
||
}
|
||
if task_id != OBSTACLE_TASK and not custom_keys.intersection(payload):
|
||
return None
|
||
if task_id not in (FLAT_TASK, ROUGH_TASK, OBSTACLE_TASK):
|
||
raise TaskConfigError("该任务不支持自定义地形")
|
||
preset = payload.get(
|
||
"terrainPreset", "discrete_obstacles" if task_id == OBSTACLE_TASK else "plane"
|
||
)
|
||
if not isinstance(preset, str) or preset not in TERRAIN_PRESETS:
|
||
raise TaskConfigError("不支持的 terrainPreset")
|
||
layout = None
|
||
if preset == "custom_boxes":
|
||
layout = validate_custom_terrain(payload.get("customTerrainBoxes"))
|
||
expected = {"size": layout["size"], "friction": layout["friction"]}
|
||
if "terrainParams" in payload and (
|
||
not isinstance(payload["terrainParams"], dict)
|
||
or payload["terrainParams"] != expected
|
||
or any(
|
||
isinstance(v, bool) or not isinstance(v, (int, float))
|
||
for v in payload["terrainParams"].values()
|
||
)
|
||
):
|
||
raise TaskConfigError("custom_boxes terrainParams 必须与布局size/friction完全一致")
|
||
terrain = expected
|
||
else:
|
||
if "customTerrainBoxes" in payload:
|
||
raise TaskConfigError("customTerrainBoxes 仅允许 custom_boxes，不能降级预设")
|
||
terrain = _parameters(payload.get("terrainParams", {}), TERRAIN_PARAMETERS, "terrainParams")
|
||
if not layout and terrain["obstacle_height_min"] > terrain["obstacle_height_max"]:
|
||
raise TaskConfigError("障碍物最小高度不能超过最大高度")
|
||
raw_sensor = payload.get("sensorCfg", {})
|
||
if not isinstance(raw_sensor, dict):
|
||
raise TaskConfigError("sensorCfg 必须是对象")
|
||
sensor_type = payload.get("sensorType", raw_sensor.get("type", "raycast"))
|
||
if sensor_type != "raycast" or raw_sensor.get("type", "raycast") != "raycast":
|
||
raise TaskConfigError("首版只支持 raycast，未实现 camera_depth")
|
||
if task_id != OBSTACLE_TASK and (raw_sensor or "sensorType" in payload):
|
||
raise TaskConfigError("只有避障任务支持 sensorCfg")
|
||
sensor = None
|
||
if task_id == OBSTACLE_TASK:
|
||
sensor = validate_sensor_config(raw_sensor)
|
||
sensor["type"] = "raycast"
|
||
if sensor["safetyDistance"] >= sensor["maxDistance"]:
|
||
raise TaskConfigError("安全距离必须小于探测距离")
|
||
return {
|
||
"terrainPreset": preset,
|
||
"terrainParams": terrain,
|
||
"sensorCfg": sensor,
|
||
"seed": seed,
|
||
**({"customTerrainBoxes": layout} if layout else {}),
|
||
}
|
||
|
||
|
||
def navigation_candidates(
|
||
layout: dict, spacing: float = 0.5, clearance: float = 0.55, min_distance: float = 2.0
|
||
) -> dict:
|
||
"""Build connected, collision-free point-goal samples for episode resets.
|
||
|
||
Obstacles are inflated by ``clearance`` and the map is sampled on a regular grid.
|
||
Only components containing a pair at least ``min_distance`` apart are retained, so
|
||
runtime sampling cannot place a goal across an impassable wall.
|
||
"""
|
||
size = layout["size"]
|
||
lo, hi = -size / 2 + clearance, size / 2 - clearance
|
||
count = max(1, int(math.floor((hi - lo) / spacing)) + 1)
|
||
axis = [lo + i * (hi - lo) / max(count - 1, 1) for i in range(count)]
|
||
obstacles = layout["boxes"][1:]
|
||
|
||
def free(x: float, y: float) -> bool:
|
||
return all(
|
||
math.hypot(
|
||
max(abs(x - box["pos"][0]) - box["size"][0], 0),
|
||
max(abs(y - box["pos"][1]) - box["size"][1], 0),
|
||
)
|
||
> clearance
|
||
for box in obstacles
|
||
)
|
||
|
||
cells = {(i, j) for i, x in enumerate(axis) for j, y in enumerate(axis) if free(x, y)}
|
||
components = []
|
||
while cells:
|
||
pending = [cells.pop()]
|
||
component = []
|
||
while pending:
|
||
cell = pending.pop()
|
||
component.append(cell)
|
||
i, j = cell
|
||
for neighbour in ((i - 1, j), (i + 1, j), (i, j - 1), (i, j + 1)):
|
||
if neighbour in cells:
|
||
cells.remove(neighbour)
|
||
pending.append(neighbour)
|
||
points = [(axis[i], axis[j]) for i, j in sorted(component)]
|
||
extrema = [
|
||
(
|
||
min(points, key=lambda point: point[axis_index]),
|
||
max(points, key=lambda point: point[axis_index]),
|
||
)
|
||
for axis_index in (0, 1)
|
||
]
|
||
first, second = max(extrema, key=lambda pair: math.dist(*pair))
|
||
if math.dist(first, second) >= min_distance:
|
||
components.append((points, first, second))
|
||
if not components:
|
||
raise TaskConfigError(
|
||
f"地图没有可用于随机起终点的连通自由区域（至少需要{min_distance:g}m间距）"
|
||
)
|
||
flattened = []
|
||
starts = []
|
||
counts = []
|
||
fallbacks = []
|
||
for points, first, second in components:
|
||
starts.append(len(flattened))
|
||
counts.append(len(points))
|
||
flattened.extend(points)
|
||
fallbacks.append((first, second))
|
||
return {
|
||
"points": flattened,
|
||
"componentStarts": starts,
|
||
"componentCounts": counts,
|
||
"fallbackPairs": fallbacks,
|
||
"minDistance": min_distance,
|
||
"clearance": clearance,
|
||
"spacing": spacing,
|
||
}
|
||
|
||
|
||
def build_terrain_layout(config: dict) -> dict:
|
||
"""Emit exact world-centered boxes; consumers do not reimplement RNG/terrain presets.
|
||
|
||
size values are MuJoCo half-extents, pos is the center, yaw is always zero.
|
||
TerrainGenerator uses one patch, shifted back to these coordinates. All parallel
|
||
environments are independent worlds with the same map and spawn, not a grid.
|
||
"""
|
||
if config["terrainPreset"] == "custom_boxes":
|
||
return validate_custom_terrain(config.get("customTerrainBoxes"))
|
||
p = config["terrainParams"]
|
||
size = p["size"]
|
||
rng = random.Random(config["seed"])
|
||
boxes = [{"pos": [0, 0, -0.1], "size": [size / 2, size / 2, 0.1], "yaw": 0}]
|
||
preset = config["terrainPreset"]
|
||
|
||
def box(x: float, y: float, sx: float, sy: float, height: float) -> None:
|
||
boxes.append({"pos": [x, y, height / 2], "size": [sx, sy, height / 2], "yaw": 0})
|
||
|
||
# Leave two flat end strips for the spawn and goal; no rejection sampling.
|
||
if preset == "discrete_obstacles":
|
||
spacing = p["spacing"]
|
||
nx = max(1, int((size - 4) / spacing))
|
||
ny = max(1, int((size - 2) / spacing))
|
||
cells = [
|
||
(
|
||
-size / 2 + 2 + (i + 0.5) * (size - 4) / nx,
|
||
-size / 2 + 1 + (j + 0.5) * (size - 2) / ny,
|
||
)
|
||
for i in range(nx)
|
||
for j in range(ny)
|
||
]
|
||
rng.shuffle(cells)
|
||
for x, y in cells[: p["obstacle_count"]]:
|
||
box(x, y, 0.2, 0.2, rng.uniform(p["obstacle_height_min"], p["obstacle_height_max"]))
|
||
elif preset in ("rough", "wave"):
|
||
# 16x16 bounded box approximation, deliberately not the editor heightfield.
|
||
nx = ny = 16
|
||
sx, sy = (size - 4) / nx, size / ny
|
||
for i in range(nx):
|
||
for j in range(ny):
|
||
height = (
|
||
rng.uniform(0.005, p["roughness"])
|
||
if preset == "rough"
|
||
else 0.005 + p["wave_amplitude"] * (1 + math.sin(i * math.pi / 4)) / 2
|
||
)
|
||
box(
|
||
-size / 2 + 2 + (i + 0.5) * sx,
|
||
-size / 2 + (j + 0.5) * sy,
|
||
sx / 2,
|
||
sy / 2,
|
||
height,
|
||
)
|
||
elif preset == "pyramid_stairs":
|
||
for i in range(4):
|
||
half = (size - 4) / 2 - i * (size - 4) / 10
|
||
box(0, 0, half, half, p["step_height"] * (i + 1))
|
||
return {
|
||
"representation": "boxes-v1",
|
||
"approximation": preset in ("rough", "wave", "pyramid_stairs"),
|
||
"size": size,
|
||
"friction": p["friction"],
|
||
"boxes": boxes,
|
||
"spawn": [-size / 2 + 1, 0, 0.32],
|
||
"spawnQuaternion": [1, 0, 0, 0],
|
||
"target": [size / 2 - 1, 0],
|
||
"actualObstacleCount": len(boxes) - 1,
|
||
}
|
||
|
||
|
||
def deployment_metadata(task_id: str, config: dict | None, seed: int) -> dict:
|
||
obstacle = task_id == OBSTACLE_TASK
|
||
metadata = {
|
||
"version": 1,
|
||
"taskId": task_id,
|
||
"browserCompatible": task_id in (FLAT_TASK, OBSTACLE_TASK),
|
||
"observationSize": (49 + config["sensorCfg"]["rayCount"])
|
||
if obstacle
|
||
else (234 if task_id == ROUGH_TASK else 47),
|
||
"actionSize": 12,
|
||
"controlHz": 50,
|
||
"gaitPeriod": 0.6,
|
||
"jointNames": JOINT_NAMES,
|
||
"defaultJointPosition": DEFAULT_JOINT_POSITION,
|
||
"actionScale": [0.25] * 12,
|
||
"stiffness": [20, 20, 40] * 4,
|
||
"damping": [1, 1, 2] * 4,
|
||
"effortLimits": [23.5, 23.5, 45] * 4,
|
||
"observationTerms": [
|
||
"base_ang_vel",
|
||
"projected_gravity",
|
||
"command",
|
||
"phase",
|
||
"joint_pos",
|
||
"joint_vel",
|
||
"actions",
|
||
]
|
||
+ (
|
||
["forward_depth", "target_error"]
|
||
if obstacle
|
||
else (["height_scan"] if task_id == ROUGH_TASK else [])
|
||
),
|
||
"seed": seed,
|
||
}
|
||
if task_id == ROUGH_TASK:
|
||
metadata["incompatibilityReason"] = "旧 Rough actor 含向下高度扫描；浏览器未实现此部署契约"
|
||
if config is not None:
|
||
metadata.update(
|
||
{
|
||
"terrainPreset": config["terrainPreset"],
|
||
"terrainParams": config["terrainParams"],
|
||
"terrain": build_terrain_layout(config),
|
||
}
|
||
)
|
||
if obstacle:
|
||
assert config is not None
|
||
metadata["sensorCfg"] = {
|
||
**config["sensorCfg"],
|
||
"offset": [0.3, 0, 0.05],
|
||
"alignment": "base",
|
||
"terrainOnly": True,
|
||
"includeGround": True,
|
||
}
|
||
metadata["navigation"] = {
|
||
"speed": 0.6,
|
||
"arrivalRadius": 0.5,
|
||
"distanceScale": config["terrainParams"]["size"],
|
||
"headingScale": math.pi,
|
||
"yawGain": 1.0,
|
||
"maxYawRate": 1.0,
|
||
"episodeSeconds": 20,
|
||
"onArrival": "stop",
|
||
"onReset": "respawn",
|
||
"trainingReset": "random-connected-free-pair",
|
||
"trainingMinGoalDistance": 2.0,
|
||
"trainingClearance": 0.55,
|
||
}
|
||
return metadata
|
||
|
||
|
||
def task_metadata(tasks: tuple[str, ...]) -> list[dict]:
|
||
names = {
|
||
FLAT_TASK: "平地速度控制",
|
||
ROUGH_TASK: "地形自适应（仅后端）",
|
||
OBSTACLE_TASK: "前视射线避障导航",
|
||
}
|
||
return [
|
||
{
|
||
"id": task,
|
||
"name": names.get(task, task),
|
||
"browserCompatible": task in (FLAT_TASK, OBSTACLE_TASK),
|
||
"terrainPresets": list(TERRAIN_PRESETS) if task in names else [],
|
||
"terrainParameters": TERRAIN_PARAMETERS if task in names else {},
|
||
"sensorTypes": ["raycast"] if task == OBSTACLE_TASK else [],
|
||
"sensorModes": ["single_ring_raycast", "multi_ring_raycast"]
|
||
if task == OBSTACLE_TASK
|
||
else [],
|
||
"sensorParameters": SENSOR_PARAMETERS if task == OBSTACLE_TASK else {},
|
||
"mapSyncScope": (
|
||
"已应用静态碰撞场景→custom_boxes（boxes-v1）；AABB与标准底板近似；不支持mesh/hfield"
|
||
),
|
||
}
|
||
for task in tasks
|
||
]
|