251 lines
9.2 KiB
Python
251 lines
9.2 KiB
Python
"""Rough terrain curriculum for explicitly sized legged robots.
|
|
|
|
The nominal dimensions are a starting preset. Supply standing height, fore-aft
|
|
support span and swing clearance in metres for the intended gait. Terrain scan
|
|
observations and sensors belong to the training task.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import math
|
|
import os
|
|
from dataclasses import replace
|
|
|
|
from .registry import Headlight, Scene
|
|
|
|
NOMINAL_ROBOT_HEIGHT = 0.25
|
|
NOMINAL_STANCE = 0.40
|
|
NOMINAL_FOOT_LIFT = 0.04
|
|
|
|
# Reference dimensions for mjlab's obstacle recipe.
|
|
_BASELINE_HEIGHT = 0.278
|
|
_BASELINE_STANCE = 0.3762
|
|
MAX_SLOPE = 0.4
|
|
DIFFICULTY = 1.25
|
|
ROUGHNESS_DIFFICULTY = 1.5
|
|
TILE = 12.0
|
|
_HF_BORDER = 0.25
|
|
_WAVE_SAMPLE = 0.1
|
|
_HEIGHT_STEP = 0.005
|
|
_MAX_HEIGHTFIELD_VALUE = 32767 * _HEIGHT_STEP
|
|
_NOISE_FLOOR = 0.005
|
|
|
|
NUM_ROWS = 10
|
|
TERRAIN_ROW_ENV = "MJRL_TERRAIN_ROW"
|
|
TERRAIN_COL_ENV = "MJRL_TERRAIN_COL"
|
|
|
|
# Minimum terrain budgets. Individual models may require larger allocations.
|
|
NCONMAX = 256
|
|
NJMAX = 2048
|
|
|
|
|
|
def _positive_metres(value: float | str, name: str) -> float:
|
|
message = f"{name} must be a finite positive number of metres"
|
|
try:
|
|
metres = float(value)
|
|
except (TypeError, ValueError, OverflowError):
|
|
raise ValueError(message) from None
|
|
if isinstance(value, bool) or not math.isfinite(metres) or metres <= 0:
|
|
raise ValueError(message)
|
|
return metres
|
|
|
|
|
|
def _parse_metres(value: str) -> float:
|
|
try:
|
|
return _positive_metres(value, "distance")
|
|
except ValueError as exc:
|
|
raise argparse.ArgumentTypeError(str(exc)) from None
|
|
|
|
|
|
def _terrain(robot_height: float, stance: float, foot_lift: float):
|
|
from mjlab.terrains.config import (
|
|
box_random_grid,
|
|
hf_pyramid_slope,
|
|
hf_pyramid_slope_inv,
|
|
pyramid_stairs,
|
|
pyramid_stairs_inv,
|
|
random_rough,
|
|
wave_terrain,
|
|
)
|
|
from mjlab.terrains.terrain_generator import TerrainGeneratorCfg
|
|
|
|
horizontal_scale = stance / _BASELINE_STANCE
|
|
wave_length = ((8.0 - 2 * _HF_BORDER) / 4) * horizontal_scale
|
|
step_width = 0.30 * horizontal_scale
|
|
platform_width = 3.0 * horizontal_scale
|
|
stair_border = 1.0 * horizontal_scale
|
|
if wave_length < 2 * _WAVE_SAMPLE:
|
|
raise ValueError("stance is too small for the wave resolution")
|
|
if platform_width + 2 * (stair_border + step_width) > TILE:
|
|
raise ValueError("stance is too large for the stair geometry")
|
|
wave_amplitude = (0.20 * DIFFICULTY / _BASELINE_HEIGHT) * robot_height
|
|
noise_ceiling = _NOISE_FLOOR + foot_lift * ROUGHNESS_DIFFICULTY
|
|
if wave_amplitude > _MAX_HEIGHTFIELD_VALUE:
|
|
raise ValueError("robot_height is too large for the terrain heightfield")
|
|
if noise_ceiling > _MAX_HEIGHTFIELD_VALUE:
|
|
raise ValueError("foot_lift is too large for the terrain recipe")
|
|
|
|
# Scale riser height and tread width independently. The number of treads
|
|
# under a stance controls the height difference between its support points.
|
|
stairs = dict(
|
|
proportion=0.2,
|
|
step_height_range=(0.0, 0.10 * DIFFICULTY / _BASELINE_HEIGHT * robot_height),
|
|
step_width=step_width,
|
|
platform_width=platform_width,
|
|
border_width=stair_border,
|
|
)
|
|
# Slope is a gradient. The spawn platform scales with the support span.
|
|
slopes = dict(
|
|
proportion=0.1,
|
|
slope_range=(0.0, MAX_SLOPE * DIFFICULTY),
|
|
platform_width=2.0 * horizontal_scale,
|
|
)
|
|
return TerrainGeneratorCfg(
|
|
curriculum=True,
|
|
# Keep the tile size independent of obstacle dimensions. It must hold
|
|
# the distance traveled per episode to keep curriculum samples on their row.
|
|
size=(TILE, TILE),
|
|
border_width=5.0,
|
|
num_rows=NUM_ROWS,
|
|
num_cols=20,
|
|
color_scheme="none",
|
|
sub_terrains={
|
|
# Keep the paving separate to avoid large box/mesh contact pairs.
|
|
"flat": box_random_grid(
|
|
proportion=0.2,
|
|
grid_width=1.0,
|
|
grid_height_range=(0.0, 0.0),
|
|
merge_similar_heights=False,
|
|
),
|
|
"pyramid_stairs": pyramid_stairs(**stairs),
|
|
"pyramid_stairs_inv": pyramid_stairs_inv(**stairs),
|
|
"hf_pyramid_slope": hf_pyramid_slope(**slopes),
|
|
"hf_pyramid_slope_inv": hf_pyramid_slope_inv(**slopes),
|
|
# The range sets the relief before heightfield quantization.
|
|
"random_rough": random_rough(
|
|
proportion=0.1,
|
|
scale_with_difficulty=True,
|
|
noise_range=(_NOISE_FLOOR, noise_ceiling),
|
|
noise_step=_HEIGHT_STEP,
|
|
vertical_scale=_HEIGHT_STEP,
|
|
),
|
|
# Derive wave count from wavelength and interior tile width.
|
|
"wave_terrain": wave_terrain(
|
|
proportion=0.1,
|
|
amplitude_range=(0.0, wave_amplitude),
|
|
num_waves=max(round((TILE - 2 * _HF_BORDER) / wave_length), 1),
|
|
border_width=_HF_BORDER,
|
|
horizontal_scale=_WAVE_SAMPLE,
|
|
vertical_scale=_HEIGHT_STEP,
|
|
),
|
|
},
|
|
add_lights=True,
|
|
)
|
|
|
|
|
|
def cli_args(group):
|
|
"""Register scene-specific options and return their destination names."""
|
|
group.add_argument(
|
|
"--terrain-row", default=os.environ.get(TERRAIN_ROW_ENV, ""), metavar="N",
|
|
help=f"pin the curriculum to row 0 (flat) through {NUM_ROWS - 1} (hardest)",
|
|
)
|
|
group.add_argument(
|
|
"--terrain-col", default=os.environ.get(TERRAIN_COL_ENV, ""), metavar="NAME",
|
|
help="pin the curriculum to one of flat, pyramid_stairs, pyramid_stairs_inv, "
|
|
"hf_pyramid_slope, hf_pyramid_slope_inv, random_rough, wave_terrain",
|
|
)
|
|
group.add_argument(
|
|
"--terrain-robot-height", dest="robot_height", type=_parse_metres,
|
|
default=NOMINAL_ROBOT_HEIGHT, metavar="M",
|
|
help=f"nominal standing height in metres (default {NOMINAL_ROBOT_HEIGHT:g})",
|
|
)
|
|
group.add_argument(
|
|
"--terrain-stance", dest="stance", type=_parse_metres,
|
|
default=NOMINAL_STANCE, metavar="M",
|
|
help=f"fore-aft support span in metres (default {NOMINAL_STANCE:g})",
|
|
)
|
|
group.add_argument(
|
|
"--terrain-foot-lift", dest="foot_lift", type=_parse_metres,
|
|
default=NOMINAL_FOOT_LIFT, metavar="M",
|
|
help=f"nominal swing clearance in metres (default {NOMINAL_FOOT_LIFT:g})",
|
|
)
|
|
return ("terrain_row", "terrain_col", "robot_height", "stance", "foot_lift")
|
|
|
|
|
|
def _pin(cfg, row: str, col: str):
|
|
"""Narrow the curriculum while preserving the selected generator and difficulty."""
|
|
changes: dict = {}
|
|
if col:
|
|
if col not in cfg.sub_terrains:
|
|
raise SystemExit(
|
|
f"[scene] --terrain-col {col!r} is not a sub-terrain of the rough "
|
|
f"scene. Available: {', '.join(sorted(cfg.sub_terrains))}"
|
|
)
|
|
changes["sub_terrains"] = {col: cfg.sub_terrains[col]}
|
|
if row:
|
|
try:
|
|
index = int(row)
|
|
except ValueError:
|
|
raise SystemExit(
|
|
f"[scene] --terrain-row {row!r} is not an integer. Rows are 0 "
|
|
f"(flat) to {cfg.num_rows - 1} (hardest)."
|
|
) from None
|
|
if not 0 <= index < cfg.num_rows:
|
|
raise SystemExit(
|
|
f"[scene] --terrain-row {index} is outside 0..{cfg.num_rows - 1}."
|
|
)
|
|
# The difficulty this row would have had in the full grid. The generator
|
|
# spreads `difficulty_range` across `num_rows`, so a single row has to be
|
|
# handed the value rather than the index.
|
|
difficulty = index / (cfg.num_rows - 1) if cfg.num_rows > 1 else 0.0
|
|
changes["num_rows"] = 1
|
|
changes["difficulty_range"] = (difficulty, difficulty)
|
|
if not changes:
|
|
return cfg
|
|
pinned = replace(cfg, **changes)
|
|
print(
|
|
"[scene] rough pinned to"
|
|
+ (f" row {row} (difficulty {pinned.difficulty_range[0]:.3f})" if row else "")
|
|
+ (f" column {col!r}" if col else "")
|
|
)
|
|
return pinned
|
|
|
|
|
|
def scene(
|
|
terrain_row: str = "",
|
|
terrain_col: str = "",
|
|
*,
|
|
robot_height: float = NOMINAL_ROBOT_HEIGHT,
|
|
stance: float = NOMINAL_STANCE,
|
|
foot_lift: float = NOMINAL_FOOT_LIFT,
|
|
) -> Scene:
|
|
"""Build rough terrain with nominal dimensions in metres and optional pins."""
|
|
robot_height = _positive_metres(robot_height, "robot_height")
|
|
stance = _positive_metres(stance, "stance")
|
|
foot_lift = _positive_metres(foot_lift, "foot_lift")
|
|
terrain = _pin(_terrain(robot_height, stance, foot_lift), terrain_row, terrain_col)
|
|
|
|
from mjlab.utils import spec_config as sc
|
|
|
|
return Scene(
|
|
headlight=Headlight(ambient=(0.10, 0.11, 0.12), diffuse=(0.06, 0.06, 0.07)),
|
|
textures=(
|
|
sc.TextureCfg(
|
|
name="rough_sky", type="skybox", builtin="gradient",
|
|
rgb1=(0.45, 0.48, 0.55), rgb2=(0.62, 0.66, 0.74),
|
|
width=256, height=256,
|
|
),
|
|
),
|
|
materials=(),
|
|
lights=(
|
|
sc.LightCfg(name="sun", type="directional", pos=(-4.0, -1.0, 2.5),
|
|
dir=(0.8, 0.2, -0.6), castshadow=True,
|
|
diffuse=(0.55, 0.54, 0.51), ambient=(0.10, 0.11, 0.13)),
|
|
),
|
|
terrain_type="generator",
|
|
terrain_generator=terrain,
|
|
nconmax=NCONMAX,
|
|
njmax=NJMAX,
|
|
)
|