Files
UniversalTrain-ai/scenes/soft.py
T
chenlin d5e9004f6c
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
docs: add AI agent workflow guide and clean up residual single-robot references
2026-11-05 11:52:07 +08:00

241 lines
11 KiB
Python

"""Compliant ground with optional explicitly named foot drag.
This scene changes contact physics. MuJoCo selects the whole contact parameter
set from the higher-priority geom; terrain priority=2 overrides lower-priority
feet, but does not automatically override every imported robot. Inspect the
compiled contact parameters when using an asset with different priorities.
solimp controls the impedance ramp with penetration depth. solref controls the
restoring time constant and damping. The values below are a nominal simulation
preset, not a measured material model or a calibrated property of a robot.
Foot-friction randomization is ineffective wherever the ground priority wins.
The surface is compliant, not plastically deformable: it retains no footprints.
The optional GranularDrag event requires explicit foot_bodies and adds a Python
force. That event cannot be exported as plain MJCF. Without named feet only the
native contact-compliance preset is applied.
"""
from __future__ import annotations
from typing import Any
from .registry import GROUND, Headlight, Scene
def _drag_event(foot_bodies):
"""The drag as an mjlab event term, running every step."""
from mjlab.managers.event_manager import EventTermCfg
return EventTermCfg(
func=GranularDrag,
mode="step",
params={"drag_k": DRAG_K, "max_force": DRAG_MAX_FORCE,
"foot_bodies": tuple(foot_bodies)},
)
#: Nominal impedance ramp: d0, dwidth, width in metres, midpoint, power.
SOFT_SOLIMP = (0.5, 0.95, 0.03, 0.5, 2.0)
#: Restoring time constant in seconds and damping ratio.
SOFT_SOLREF = (0.05, 1.0)
#: Dominates lower-priority contact geoms; inspect imported models explicitly.
SOFT_PRIORITY = 2
#: Illustrative sliding, torsional and rolling friction for this material preset.
SOFT_FRICTION = (1.2, 0.01, 0.01)
def _soften(spec: Any) -> None:
"""Give every ground geom the soft contact parameters, and the priority to
make them count.
Matches `registry.GROUND` -- the same pattern the scenes' materials bind
with, so a scene that renders on the ground also softens exactly that ground.
**Raises if it matches nothing**, which is the difference between this and
the material binding it borrows from: an unbound material is a cosmetic
surprise, but silently rigid ground here would be a scene that claims to
change the physics and does not, and every measurement taken on it would be
wrong without looking wrong.
"""
import re
# **`spec.geoms`, not `spec.worldbody.geoms`.** The terrain geom is not on the
# worldbody at the point `decorate` runs -- written the obvious way first, the
# worldbody's geom list was empty and the guard below fired on the very first
# build. `spec.geoms` is every geom in the spec regardless of which body owns
# it, which is what the name pattern is meant to be matched against anyway.
pats = [re.compile(p) for p in GROUND]
hits = 0
for geom in spec.geoms:
name = geom.name or ""
if not any(p.search(name) for p in pats):
continue
geom.solref = list(SOFT_SOLREF)
geom.solimp = list(SOFT_SOLIMP)
geom.friction = list(SOFT_FRICTION)
geom.priority = SOFT_PRIORITY
hits += 1
if not hits:
names = [g.name for g in spec.geoms]
raise RuntimeError(
f"scene 'soft' matched no ground geom with {GROUND!r}; worldbody has "
f"{names}. The scene would have left the ground rigid and said nothing."
)
class GranularDrag:
"""Resist a sunk foot's sideways motion, in proportion to how deep it is.
**MuJoCo's contact model cannot express this, which is why it is a force term
rather than a parameter.** `solimp` governs the normal direction only;
tangential force is `mu * N`, and `N` at rest is the robot's weight however
deep the foot is. A plane has no material to displace. So the soft ground
built above is a soft trampoline -- vertically compliant, and sideways exactly
as slippery as concrete. Walking on sand is not like that: the resistance
comes from having to push grains out of the way, and it grows with burial.
F = -k * depth * v_xy per foot, applied at the foot body
`depth` is how far the foot is below the surface, from the task's own
`foot_height_scan` sensor. `v_xy` is the foot's horizontal velocity.
**This is a phenomenological model and `DRAG_K` has no measured basis.** Real
granular intrusion is closer to rate-independent and depth-proportional at
walking speeds, i.e. Coulomb-like rather than viscous. Viscous was chosen
anyway, and the reason is numerical rather than physical: a rate-independent
law needs `sign(v)`, which chatters when a planted foot's velocity hovers
around zero and would fight the contact solver at every stance. Linear in
velocity is smooth through zero. The cost is that it under-resists slow
motion and over-resists fast, which is the wrong way round for sand.
Everything else in this repository has a number from a real run behind it.
This one does not, and it should be tuned by watching rather than trusted:
the observable is that a foot planted deep should be hard to drag, and the
gait should shorten. If it makes the robot unable to move at all, `DRAG_K` is
too high; if the gait is unchanged from rigid ground, it is too low.
"""
def __init__(self, cfg, env) -> None:
names = cfg.params.get("foot_bodies")
if not names:
raise ValueError("soft ground drag requires explicit foot_bodies in event params")
self._ids = [env.scene["robot"].body_names.index(f) for f in names]
def __call__(self, env, env_ids, drag_k: float = 0.0,
max_force: float = 0.0, foot_bodies=()) -> None:
import torch
del env_ids # a step-mode term is called for every environment
robot = env.scene["robot"]
# **Depth from the foot's own z, not from `foot_height_scan`.** That
# sensor raycasts down to the terrain and cannot report a foot *below* the
# surface: written against it first, `heights` read 0 for a foot sitting
# 10.6 mm under, so `depth` was identically zero and the whole term was
# inert -- measured, 3 N of shove moved the robot 14.9 mm with the drag on
# against 14.5 mm with it off.
#
# This assumes the ground is a plane at z = 0, which is what this scene
# sets (`terrain_type="plane"`). On generated terrain it would be wrong,
# and silently: every foot below z=0 would be charged drag regardless of
# where the ground actually is.
depth = (-robot.data.body_link_pos_w[:, self._ids, 2]).clamp(min=0.0)
v_xy = robot.data.body_link_lin_vel_w[:, self._ids, :2]
f_xy = (-drag_k * depth.unsqueeze(-1) * v_xy).clamp(-max_force, max_force)
# **Written to the foot rows only, via `body_ids`.** Writing a whole
# zeroed buffer would erase every other external wrench on the robot each
# step -- mjlab's own `push_robot` among them -- and do it silently, since
# nothing reports a force that was applied and then overwritten before the
# solver saw it. Caught in the harness rather than in a run: with the
# full-buffer write, a 3 N shove and the drag were cancelling each other.
forces = torch.zeros(
(env.num_envs, len(self._ids), 3), device=f_xy.device, dtype=f_xy.dtype
)
forces[:, :, :2] = f_xy
robot.write_external_wrench_to_sim(
forces=forces, torques=torch.zeros_like(forces), body_ids=self._ids
)
#: Drag coefficient, in N per (metre of depth) per (m/s). **Not measured against
#: anything real** -- see `GranularDrag`.
#:
#: Measured effect: a steady 3 N shove on the base for 1 s, and how far the robot
#: slid.
#:
#: rigid (no scene) 6.0 mm
#: soft, DRAG_K = 0 14.5 mm
#: soft, DRAG_K = 300 13.7 mm <- this
#: soft, DRAG_K = 1500 10.0 mm
#:
#: **Two things in that table matter more than the coefficient.**
#:
#: First, soft ground with the drag off is *more* slippery than rigid, not less --
#: 14.5 mm against 6.0. A compliant contact yields sideways as well as downwards,
#: so making the ground soft makes it slide easier. That is the opposite of sand
#: and it is what this term exists to correct.
#:
#: Second, 300 barely corrects it (14.5 -> 13.7, 6%) and even 1500 leaves the
#: ground slipperier than concrete. **If the goal is for sand to feel harder to
#: push through than a hard floor, `DRAG_K` needs to be well above 1500**, and
#: nothing above 1500 has been tried -- neither for its effect nor for whether the
#: solver stays stable. 300 is a deliberately timid starting point, not a tuned
#: value.
DRAG_K = 300.0
#: Per-foot saturation, in newtons. A hard stop rather than a soft one: the drag
#: is only a model, and an unbounded model term is a numerical instability
#: waiting for the first time a foot moves fast while buried.
DRAG_MAX_FORCE = 5.0
def scene(*, foot_bodies=()) -> Scene:
"""The `default` scene's look, with soft ground under it.
**The visual block is copied from `scenes/default.py` verbatim** -- same
headlight, same checker texture and material, same single directional light,
and no skybox override, so the sky is mjlab's own exactly as it is there.
That is the point rather than laziness. This scene exists to change one
variable, and if it also changed the lighting then "the robot walks
differently here" would have two candidate explanations and no way to
separate them by looking. Sharing the look makes `--scene soft` against no
scene at all a clean one-variable comparison.
The consequence to keep in mind: **the two are indistinguishable in a
screenshot.** A frame from this scene and a frame from `default` differ only
in how deep the feet are, which at 10.6 mm is visible if you look at the feet
and invisible if you look at anything else. Label recordings.
If the copy and `default.py` ever drift, this docstring is the only thing
that says they were meant to match; nothing enforces it, because importing
`default.scene()` and mutating it would couple the two files in a way that
makes a change to `default` silently change this scene's physics comparison
as well.
"""
from mjlab.utils import spec_config as sc
return Scene(
headlight=Headlight(ambient=(0.30, 0.30, 0.30), diffuse=(0.60, 0.60, 0.60)),
textures=(
sc.TextureCfg(
name="groundplane", type="2d", builtin="checker", mark="edge",
rgb1=(0.2, 0.3, 0.4), rgb2=(0.1, 0.2, 0.3),
markrgb=(0.8, 0.8, 0.8), width=300, height=300,
),
),
materials=(
sc.MaterialCfg(
name="groundplane", texture="groundplane", texuniform=True,
texrepeat=(4.0, 4.0), reflectance=0.2, geom_names_expr=GROUND,
),
),
lights=(sc.LightCfg(name="sun", pos=(0.0, 0.0, 1.5), type="directional"),),
# `terrain_type` is set even though "plane" is usually already the task's
# choice: it is what makes `scripts/_cli.py` warn that this scene changes
# the ground. A scene that alters contact physics silently is exactly what
# that warning exists for, and `decorate` alone does not trigger it.
terrain_type="plane",
decorate=_soften,
events={"soft_ground_drag": _drag_event(foot_bodies)} if foot_bodies else {},
)