Files
2026-11-03 19:46:08 +08:00

536 lines
16 KiB
Python

"""Interactive terrain visualizer using Viser.
Displays a 10-row grid of terrains with increasing difficulty.
Configurations and parameters are dynamically loaded from mjlab.terrains.config.
Run with:
uv run src/mjlab/scripts/visualize_terrain.py
"""
from __future__ import annotations
import dataclasses
import re
import time
from typing import Any, List, TypedDict
import mujoco
import numpy as np
import trimesh
import viser
from mjviser.conversions import (
create_primitive_mesh,
merge_geoms,
mujoco_mesh_to_trimesh,
)
from mujoco import mjtGeom
from mjlab.asset_zoo.robots import (
get_g1_robot_cfg,
get_go1_robot_cfg,
get_yam_robot_cfg,
)
from mjlab.terrains.config import ALL_TERRAINS_CFG
from mjlab.terrains.terrain_generator import (
TerrainGenerator,
TerrainGeneratorCfg,
)
# Supported robots for visualization.
ROBOT_CFG_GETTERS = {
"None": None,
"Unitree Go1": get_go1_robot_cfg,
"Unitree G1": get_g1_robot_cfg,
"Yam": get_yam_robot_cfg,
}
# Parameter range hints for sliders.
PARAM_HINTS = {
"octaves": (1, 10, 1),
"persistence": (0.0, 1.0, 0.05),
"lacunarity": (1.0, 5.0, 0.1),
"scale": (0.01, 0.5, 0.01),
"horizontal_scale": (0.001, 0.1, 0.001),
"resolution": (0.1, 2.0, 0.1),
"base_thickness_ratio": (0.1, 2.0, 0.1),
"border_width": (0.0, 2.0, 0.05),
"amplitude_range": (0.0, 1.0, 0.05),
"height_range": (0.0, 2.0, 0.05),
"num_waves": (1, 20, 1),
"num_obstacles": (1, 100, 1),
"obstacle_height_range": (0.0, 1.0, 0.05),
"obstacle_width_range": (0.1, 2.0, 0.05),
"box_width_range": (0.1, 2.0, 0.05),
"box_length_range": (0.1, 2.0, 0.05),
"slope_range": (0.0, 1.0, 0.05),
"platform_width": (0.1, 5.0, 0.1),
"step_height_range": (0.0, 0.5, 0.01),
"step_width": (0.1, 1.0, 0.05),
"grid_width": (0.1, 1.0, 0.05),
"grid_height_range": (0.0, 1.0, 0.05),
"height_merge_threshold": (0.01, 0.2, 0.01),
"max_merge_distance": (1, 10, 1),
"num_beams": (1, 64, 1),
"num_rings": (1, 32, 1),
"displacement_range": (0.0, 1.0, 0.005),
"stone_size_variation": (0.0, 1.0, 0.005),
"stone_height_variation": (0.0, 1.0, 0.005),
}
class _AppState(TypedDict):
preset_name: str
robot_name: str
seed: int
size: tuple[float, float]
params: dict[str, Any]
rows: int
cols: int
difficulty_range: tuple[float, float]
robot_handles: list[Any]
terrain_origins: np.ndarray | None
def main():
server = viser.ViserServer()
# Load available terrains from config.
available_presets = ALL_TERRAINS_CFG.sub_terrains
preset_names = ["All Terrains"] + list(available_presets.keys())
# State management.
state: _AppState = {
"preset_name": preset_names[0],
"robot_name": "None",
"seed": 42,
"size": ALL_TERRAINS_CFG.size,
"params": {},
"rows": 10,
"cols": 1,
"difficulty_range": (0.0, 1.0),
"robot_handles": [],
"terrain_origins": None,
}
# Handle for the terrain mesh in the scene.
terrain_handle: viser.SceneNodeHandle | None = None
# GUI for statistics.
gui_stats_folder = server.gui.add_folder("Statistics")
with gui_stats_folder:
status_label = server.gui.add_markdown("**Status:** Ready")
polygon_count_label = server.gui.add_markdown("**Number of Polygons:** -")
def update_robots():
# Clear old robot handles.
for h in state["robot_handles"]:
h.remove()
state["robot_handles"] = []
if state["robot_name"] == "None" or state["terrain_origins"] is None:
status_label.content = "**Status:** Ready"
return
status_label.content = f"**Status:** Spawning {state['robot_name']}..."
# Get robot config.
robot_cfg_getter = ROBOT_CFG_GETTERS[state["robot_name"]]
if robot_cfg_getter is None:
return
robot_cfg = robot_cfg_getter()
# Merge robot template mesh.
# We compile the robot spec standalone to get a clean local-space mesh template.
robot_spec = robot_cfg.spec_fn()
if robot_spec.worldbody.bodies:
robot_spec.worldbody.bodies[0].pos = (0, 0, 0)
if robot_spec.keys:
robot_spec.delete(robot_spec.keys[0])
robot_model = robot_spec.compile()
robot_data = mujoco.MjData(robot_model)
# Apply joint poses.
joint_pos = robot_cfg.init_state.joint_pos
if joint_pos is None:
joint_pos = {}
for pattern, val in joint_pos.items():
if not pattern.startswith("^"):
pattern = ".*" + pattern
for j_id in range(robot_model.njnt):
name = mujoco.mj_id2name(robot_model, mujoco.mjtObj.mjOBJ_JOINT, j_id)
if re.match(pattern, name):
adr = robot_model.jnt_qposadr[j_id]
robot_data.qpos[adr] = val
mujoco.mj_forward(robot_model, robot_data)
# Filter for visual geoms only (group < 3).
visual_geom_ids = [
i for i in range(robot_model.ngeom) if robot_model.geom_group[i] < 3
]
if not visual_geom_ids:
status_label.content = "**Status:** Error: No visual geoms for robot"
print(f"Error: No visual geoms found for {state['robot_name']}")
return
# Merge visual geoms in world space.
meshes = []
for gid in visual_geom_ids:
if robot_model.geom_type[gid] == mjtGeom.mjGEOM_MESH:
mesh = mujoco_mesh_to_trimesh(robot_model, gid)
else:
mesh = create_primitive_mesh(robot_model, gid)
xform = np.eye(4)
xform[:3, :3] = robot_data.geom_xmat[gid].reshape(3, 3)
xform[:3, 3] = robot_data.geom_xpos[gid]
mesh.apply_transform(xform)
meshes.append(mesh)
robot_mesh = trimesh.util.concatenate(meshes) if len(meshes) > 1 else meshes[0]
n_verts = len(robot_mesh.vertices)
n_faces = len(robot_mesh.faces)
print(f"Robot mesh: {n_verts} vertices, {n_faces} faces.")
# Prepare batched positions.
num_rows, num_cols = state["terrain_origins"].shape[:2]
batched_positions = []
for row in range(num_rows):
for col in range(num_cols):
origin = state["terrain_origins"][row, col]
pos = origin + np.array(robot_cfg.init_state.pos)
batched_positions.append(pos)
batched_positions = np.array(batched_positions)
n = len(batched_positions)
print(f"Instancing {n} robots at positions like {batched_positions[0]}")
batched_wxyzs = np.array([1.0, 0.0, 0.0, 0.0])[None].repeat(
len(batched_positions), axis=0
)
with server.atomic():
handle = server.scene.add_batched_meshes_trimesh(
"/robots_batched",
robot_mesh,
batched_positions=batched_positions,
batched_wxyzs=batched_wxyzs,
)
state["robot_handles"].append(handle)
status_label.content = "**Status:** Ready"
def update_terrain():
nonlocal terrain_handle
status_label.content = "**Status:** Generating terrain..."
if state["preset_name"] == "All Terrains":
# Create a copy with equal proportions to ensure all are shown once.
sub_terrains = {}
for name, cfg in available_presets.items():
new_cfg = dataclasses.replace(cfg, proportion=1.0)
sub_terrains[name] = new_cfg
num_cols = len(sub_terrains)
num_rows = state["rows"]
else:
selected_instance = available_presets[state["preset_name"]]
terrain_type = type(selected_instance)
# Instantiate sub-terrain config with current GUI state.
sub_cfg_params = {}
for field in dataclasses.fields(terrain_type):
if field.name in ["proportion", "size", "flat_patch_sampling"]:
sub_cfg_params[field.name] = getattr(selected_instance, field.name)
continue
if "range" in field.name and isinstance(
getattr(selected_instance, field.name), (tuple, list)
):
if field.name + "_min" in state["params"]:
sub_cfg_params[field.name] = (
state["params"][field.name + "_min"],
state["params"][field.name + "_max"],
)
else:
sub_cfg_params[field.name] = getattr(selected_instance, field.name)
elif field.name in state["params"]:
sub_cfg_params[field.name] = state["params"][field.name]
else:
sub_cfg_params[field.name] = getattr(selected_instance, field.name)
try:
sub_cfg = terrain_type(**sub_cfg_params)
except Exception as e:
print(f"Error creating config: {e}")
return
num_rows = state["rows"]
num_cols = state["cols"]
sub_terrains = {state["preset_name"]: sub_cfg}
generator_cfg = TerrainGeneratorCfg(
seed=state["seed"],
size=state["size"],
num_rows=num_rows,
num_cols=num_cols,
curriculum=True,
difficulty_range=state["difficulty_range"],
sub_terrains=sub_terrains,
add_lights=True,
)
generator = TerrainGenerator(generator_cfg)
spec = mujoco.MjSpec()
generator.compile(spec)
# Save terrain metadata for robot spawning.
state["terrain_origins"] = generator.terrain_origins
status_label.content = "**Status:** Compiling MuJoCo model..."
model = spec.compile()
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
# Find terrain geoms.
terrain_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "terrain")
terrain_geom_ids = [
i for i in range(model.ngeom) if model.geom_bodyid[i] == terrain_body_id
]
if not terrain_geom_ids:
status_label.content = "**Status:** Error: No terrain geoms found"
print("No terrain geoms found.")
return
status_label.content = (
f"**Status:** Merging {len(terrain_geom_ids)} terrain geoms..."
)
terrain_mesh = merge_geoms(model, terrain_geom_ids)
num_faces = len(terrain_mesh.faces)
polygon_count_label.content = f"**Number of Polygons:** {num_faces:,}"
# Remove old terrain mesh if exists.
if terrain_handle is not None:
terrain_handle.remove()
status_label.content = "**Status:** Uploading terrain mesh..."
terrain_handle = server.scene.add_mesh_trimesh(
"/terrain",
terrain_mesh,
)
# Trigger robot update.
update_robots()
# GUI Setup.
gui_params_folder = server.gui.add_folder("Terrain Parameters")
param_controls: List[Any] = []
def rebuild_gui():
nonlocal param_controls
for control in param_controls:
control.remove()
param_controls.clear()
if state["preset_name"] == "All Terrains":
with gui_params_folder:
md = server.gui.add_markdown(
"_Parameters not available for 'All Terrains' mode._"
)
param_controls.append(md)
return
selected_instance = available_presets[state["preset_name"]]
terrain_type = type(selected_instance)
fields = dataclasses.fields(terrain_type)
with gui_params_folder:
for field in fields:
if field.name in ["proportion", "size", "flat_patch_sampling"]:
continue
# Get type as string for comparison (handles future annotations).
type_str = str(field.type)
# Check for range tuples first.
if "range" in field.name and isinstance(
getattr(selected_instance, field.name), (tuple, list)
):
hint = PARAM_HINTS.get(field.name, (0.0, 1.0, 0.01))
val_min, val_max = getattr(selected_instance, field.name)
# Store in state if not present.
if field.name + "_min" not in state["params"]:
state["params"][field.name + "_min"] = val_min
state["params"][field.name + "_max"] = val_max
cur_min = state["params"][field.name + "_min"]
cur_max = state["params"][field.name + "_max"]
v_min, v_max, v_step = hint
# Ensure range is valid for sliders.
if cur_min is not None:
v_min = min(v_min, cur_min)
else:
cur_min = v_min
if cur_max is not None:
v_max = max(v_max, cur_max)
else:
cur_max = v_max
s_min = server.gui.add_slider(
f"{field.name} min",
min=v_min,
max=v_max,
step=v_step,
initial_value=cur_min,
)
s_max = server.gui.add_slider(
f"{field.name} max",
min=v_min,
max=v_max,
step=v_step,
initial_value=cur_max,
)
@s_min.on_update
def _(event, name=field.name):
state["params"][name + "_min"] = event.target.value
update_terrain()
@s_max.on_update
def _(event, name=field.name):
state["params"][name + "_max"] = event.target.value
update_terrain()
param_controls.extend([s_min, s_max])
elif "float" in type_str or "int" in type_str or field.type in [float, int]:
hint = PARAM_HINTS.get(field.name, (0.0, 10.0, 0.1))
val = getattr(selected_instance, field.name)
if field.name not in state["params"]:
state["params"][field.name] = val
cur_val = state["params"][field.name]
v_min, v_max, v_step = hint
if cur_val is not None:
v_min = min(v_min, cur_val)
v_max = max(v_max, cur_val)
else:
cur_val = (v_min + v_max) / 2.0
slider = server.gui.add_slider(
field.name, min=v_min, max=v_max, step=v_step, initial_value=cur_val
)
@slider.on_update
def _(
event, name=field.name, is_int=("int" in type_str) or field.type is int
):
val = event.target.value
if is_int:
val = int(val)
state["params"][name] = val
update_terrain()
param_controls.append(slider)
elif "bool" in type_str or field.type is bool:
val = getattr(selected_instance, field.name)
if field.name not in state["params"]:
state["params"][field.name] = val
checkbox = server.gui.add_checkbox(
field.name, initial_value=state["params"][field.name]
)
@checkbox.on_update
def _(event, name=field.name):
state["params"][name] = event.target.value
update_terrain()
param_controls.append(checkbox)
else:
# Fallback for other simple types if they have a default value.
try:
val = getattr(selected_instance, field.name)
if isinstance(val, (int, float)):
if field.name not in state["params"]:
state["params"][field.name] = val
slider = server.gui.add_slider(
field.name,
min=min(0.0, val),
max=max(10.0, val),
step=0.1,
initial_value=val,
)
@slider.on_update
def _(event, name=field.name):
state["params"][name] = event.target.value
update_terrain()
param_controls.append(slider)
except Exception:
pass
# Global Controls.
with server.gui.add_folder("Global Settings"):
preset_select = server.gui.add_dropdown(
"Preset", options=preset_names, initial_value=state["preset_name"]
)
@preset_select.on_update
def _(event):
state["preset_name"] = event.target.value
state["params"] = {} # Clear local overrides for new preset.
rebuild_gui()
update_terrain()
seed_input = server.gui.add_number("Seed", initial_value=int(state["seed"]))
@seed_input.on_update
def _(event):
state["seed"] = int(event.target.value)
update_terrain()
robot_select = server.gui.add_dropdown(
"Robot", options=list(ROBOT_CFG_GETTERS.keys()), initial_value=state["robot_name"]
)
@robot_select.on_update
def _(event):
state["robot_name"] = event.target.value
update_robots()
btn_randomize = server.gui.add_button("Randomize Seed")
@btn_randomize.on_click
def _(_):
new_seed = np.random.randint(0, 10000)
seed_input.value = new_seed
state["seed"] = int(new_seed)
update_terrain()
btn_reset_camera = server.gui.add_button("Reset Camera")
@btn_reset_camera.on_click
def _(_):
for client in server.get_clients().values():
client.camera.position = (10, 10, 10)
client.camera.look_at = (0, 0, 0)
# Initialize.
rebuild_gui()
update_terrain()
print("Viser Terrain Visualizer running...")
while True:
time.sleep(1.0)
if __name__ == "__main__":
main()