"""Optional goal encoder while retaining rsl_rl's observation/export interface.
SuperMimic, Appendix C/Table A.4: https://arxiv.org/abs/2610.28480.
The bounded motion latent and separate decoder follow the design in
CuboiLeo/SuperMimic-Reproduced (ba61053), src/supermimic/policy/nets.py:
https://github.com/CuboiLeo/SuperMimic-Reproduced/blob/ba6105349069bf040f1fdad60873d13138d505a2/src/supermimic/policy/nets.py
Observation indices come from the preset, rather than a fixed 680/930-d layout.
"""
from dataclasses import dataclass, field, fields
from typing import Literal
import torch
from mjlab.rl import RslRlModelCfg, RslRlOnPolicyRunnerCfg
from rsl_rl.models import MLPModel
from rsl_rl.modules import MLP
from torch import nn
[docs]
@dataclass
class PolicyCfg(RslRlModelCfg):
"""Policy architecture, independent of the optimizer used to train it."""
activation: Literal[
"elu",
"selu",
"relu",
"crelu",
"lrelu",
"tanh",
"sigmoid",
"softplus",
"gelu",
"swish",
"silu",
"mish",
"identity",
] = "elu"
reference_residual: bool = field(
default=False,
metadata={
"setting": True,
"checkpoint": True,
"help": "add the next reference joint target inside the policy; requires unscaled lookahead joint observations and default-relative actions",
},
)
keypoint_skip: bool = field(
default=False,
metadata={
"setting": True,
"checkpoint": True,
"help": "add a joint target estimated from the keypoint reference (ref_keypoints) inside the policy; fit to the motions before training",
},
)
keypoint_skip_hidden: tuple[int, ...] = field(
default=(1024, 1024, 512),
metadata={
"setting": True,
"checkpoint": True,
"visible_if": {"actor.keypoint_skip": True},
},
)
keypoint_skip_steps: int = field(
default=20000,
metadata={
"setting": True,
"visible_if": {"actor.keypoint_skip": True},
"help": "Adam steps (batch 8192) of the keypoint skip's fit",
},
)
architecture: Literal["mlp", "motion_encoder", "xattn"] = field(
default="mlp",
metadata={
"setting": True,
"checkpoint": True,
"help": "flat MLP, a bounded motion latent with a proprioceptive decoder, or the latent of the observation history attending to the reference frames (xattn)",
},
)
xattn_dim: int = field(
default=128,
metadata={
"setting": True,
"checkpoint": True,
"visible_if": {"actor.architecture": "xattn"},
"help": "width of the cross-attention encoder's tokens and latent",
},
)
xattn_heads: int = field(
default=4,
metadata={
"setting": True,
"checkpoint": True,
"visible_if": {"actor.architecture": "xattn"},
},
)
encoder_hidden_dims: tuple[int, ...] = field(
default=(2048, 1024, 512, 256),
metadata={
"setting": True,
"checkpoint": True,
"visible_if": {"actor.architecture": "motion_encoder"},
},
)
latent_dim: int = field(
default=32,
metadata={
"setting": True,
"checkpoint": True,
"visible_if": {"actor.architecture": "motion_encoder"},
},
)
latent_aux: bool = field(
default=False,
metadata={
"setting": True,
"checkpoint": True,
"visible_if": {"actor.architecture": ["motion_encoder", "xattn"]},
"help": "a linear head predicting the goal frames' joint positions from the reference latent (trained with algorithm.latent_aux)",
},
)
goal_terms: tuple[str, ...] = field(
default=(),
metadata={
"setting": True,
"checkpoint": True,
"visible_if": {"actor.architecture": ["motion_encoder", "xattn"]},
"help": "actor term names to encode; empty selects reference/lookahead terms",
},
)
@dataclass
class TrackingRunnerCfg(RslRlOnPolicyRunnerCfg):
clip_actions: float | None = field(
default=None, metadata={"nullable": True, "kind": "number"}
)
light_checkpoints: bool = False
"""Periodic actor-only checkpoints; the final checkpoint always includes training state."""
init_checkpoint: str | None = None
"""Warm start (fine-tuning, e.g. on a few hard clips): networks from this full checkpoint,
with fresh optimizers, schedules, iteration count and RSI statistics (PartyMimic ``--init``)."""
actor: PolicyCfg = field(default_factory=PolicyCfg)
def tracking_runner_cfg(cfg):
"""Extend the upstream runner's actor settings without changing their defaults."""
values = {f.name: getattr(cfg, f.name) for f in fields(RslRlOnPolicyRunnerCfg)}
actor = values["actor"]
values["actor"] = PolicyCfg(**{f.name: getattr(actor, f.name) for f in fields(actor)})
return TrackingRunnerCfg(**values)
# Read old run configs/recipes without maintaining a second set of public knobs.
OLD_ACTOR_KEYS = {
"policy_architecture": "architecture",
"encoder_hidden_dims": "encoder_hidden_dims",
"latent_dim": "latent_dim",
"goal_terms": "goal_terms",
}
def migrate_actor_settings(values):
"""Return run/recipe settings with pre-refactor algorithm-owned policy keys moved."""
import copy
values = copy.deepcopy(values)
actor = values.setdefault("actor", {})
algorithm = values.get("algorithm", {})
for old, new in OLD_ACTOR_KEYS.items():
if old in algorithm:
actor.setdefault(new, algorithm.pop(old))
return values
class MotionDecoder(nn.Module):
"""An MLPModel's head: normalized flat observation → encoded goal + proprioception."""
def __init__(
self,
obs_dim,
goal_indices,
output_dim,
encoder_hidden,
decoder_hidden,
latent_dim=32,
activation="silu",
aux_dim=0,
):
super().__init__()
goal = set(goal_indices)
if not goal or len(goal) >= obs_dim:
raise ValueError(
"motion encoder needs goal terms and non-goal actor observations"
)
self.aux = nn.Linear(latent_dim, aux_dim) if aux_dim else None
self.register_buffer("goal_indices", torch.tensor(goal_indices, dtype=torch.long))
self.register_buffer(
"proprio_indices", torch.tensor([i for i in range(obs_dim) if i not in goal])
)
self.encoder = MLP(len(goal_indices), latent_dim, encoder_hidden, activation)
self.decoder = MLP(
obs_dim - len(goal_indices) + latent_dim, output_dim, decoder_hidden, activation
)
# Small initial actions, as in the reproduced policy implementation.
nn.init.orthogonal_(self.decoder[-1].weight, gain=0.01)
nn.init.zeros_(self.decoder[-1].bias)
def latent(self, obs: torch.Tensor) -> torch.Tensor:
return torch.tanh(self.encoder(obs[..., self.goal_indices]))
def forward(self, obs: torch.Tensor) -> torch.Tensor:
z = self.latent(obs)
return self.decoder(torch.cat([z, obs[..., self.proprio_indices]], dim=-1))
def _silu_mlp(inp, hidden, out):
layers = []
for width in hidden:
layers += [nn.Linear(inp, width), nn.SiLU()]
inp = width
return nn.Sequential(*layers, nn.Linear(inp, out))
def _sinusoid(n, d):
pos = torch.arange(n, dtype=torch.float32).unsqueeze(1)
angle = pos / 10000 ** (torch.arange(0, d, 2, dtype=torch.float32) / d)
return torch.stack((angle.sin(), angle.cos()), -1).flatten(1)
class _Attention(nn.Module):
"""Multi-head attention of Linear projections (matrices Muon can take)."""
def __init__(self, d, heads):
super().__init__()
self.heads = heads
self.q, self.k, self.v, self.o = (nn.Linear(d, d) for _ in range(4))
def forward(self, x, y, causal=False):
b, n, d = x.shape
def split(t):
return t.reshape(b, -1, self.heads, d // self.heads).transpose(1, 2)
out = nn.functional.scaled_dot_product_attention(
split(self.q(x)), split(self.k(y)), split(self.v(y)), is_causal=causal
)
return self.o(out.transpose(1, 2).reshape(b, n, d))
class CrossAttentionEncoder(nn.Module):
"""RGMT's dynamics-conditioned command encoder, as in PartyMimic (Roboparty, 2026)
networks.py ``XAttnEncoder``, MIT; see THIRD_PARTY_NOTICES.
A causal pre-LN Transformer block over the observation history tokens gives the
dynamics embedding (its newest token); a query from it attends to the reference frame
tokens (MLP + sinusoidal position) in one pre-LN cross-attention block.
"""
def __init__(self, history_dim, frame_dim, frames, history, d=128, heads=4):
super().__init__()
if d % heads or d % 2:
raise ValueError("xattn_dim must be even and divisible by xattn_heads")
self.history_in = _silu_mlp(history_dim, [d], d)
self.register_buffer("history_pos", _sinusoid(history, d))
self.ln_h1, self.self_attention = nn.LayerNorm(d), _Attention(d, heads)
self.ln_h2, self.ff_h = nn.LayerNorm(d), _silu_mlp(d, [2 * d], d)
self.query = _silu_mlp(d, [d], d)
self.frame_in = _silu_mlp(frame_dim, [d], d)
self.register_buffer("frame_pos", _sinusoid(frames, d))
self.ln_q, self.cross_attention = nn.LayerNorm(d), _Attention(d, heads)
self.ln_s, self.ff = nn.LayerNorm(d), _silu_mlp(d, [2 * d], d)
self.ln_out = nn.LayerNorm(d)
def forward(self, history, frames):
x = self.history_in(history) + self.history_pos
h = self.ln_h1(x)
x = x + self.self_attention(h, h, causal=True)
x = x + self.ff_h(self.ln_h2(x))
q = self.query(x[:, -1:])
z = self.frame_in(frames) + self.frame_pos
s = q + self.cross_attention(self.ln_q(q), z)
s = s + self.ff(self.ln_s(s))
return self.ln_out(s).squeeze(1)
class CrossAttentionDecoder(nn.Module):
"""An MLPModel's head: normalized flat observation → cross-attention latent of its
history and reference tokens + proprioception → action."""
def __init__(
self,
obs_dim,
history_index,
frame_index,
goal_indices,
output_dim,
decoder_hidden,
activation,
d=128,
heads=4,
aux_dim=0,
):
super().__init__()
goal = set(goal_indices)
self.aux = nn.Linear(d, aux_dim) if aux_dim else None
self.register_buffer("history_index", torch.tensor(history_index))
self.register_buffer("frame_index", torch.tensor(frame_index))
self.register_buffer(
"proprio_indices", torch.tensor([i for i in range(obs_dim) if i not in goal])
)
self.encoder = CrossAttentionEncoder(
self.history_index.shape[1],
self.frame_index.shape[1],
self.frame_index.shape[0],
self.history_index.shape[0],
d,
heads,
)
self.decoder = MLP(obs_dim - len(goal) + d, output_dim, decoder_hidden, activation)
nn.init.orthogonal_(self.decoder[-1].weight, gain=0.01)
nn.init.zeros_(self.decoder[-1].bias)
def latent(self, obs: torch.Tensor) -> torch.Tensor:
flat = obs.reshape(-1, obs.shape[-1])
u = self.encoder(flat[:, self.history_index], flat[:, self.frame_index])
return u.reshape(*obs.shape[:-1], -1)
def forward(self, obs: torch.Tensor) -> torch.Tensor:
flat = obs.reshape(-1, obs.shape[-1])
u = self.latent(flat)
out = self.decoder(torch.cat([u, flat[:, self.proprio_indices]], dim=-1))
return out.reshape(*obs.shape[:-1], -1)
def cross_attention_indices(env, names=()):
"""Observation indices of the history tokens ``[history, d_h]`` (non-goal terms, oldest
first) and reference frame tokens ``[frames, d_g]`` (goal terms, current frame first),
and the goal indices. Terms without history (frames) repeat in every token."""
from wbc_mjlab.env.mdp.observations import takes_frames
om = env.observation_manager
term_names = om._group_obs_term_names["actor"]
configs = om._group_obs_term_cfgs["actor"]
goal = set(goal_term_names(dict(zip(term_names, configs, strict=True)), names))
blocks, offset = [], 0
for name, cfg, shape in zip(
term_names, configs, om._group_obs_term_dim["actor"], strict=True
):
width = int(torch.tensor(shape).prod())
if cfg.history_length > 1 and not cfg.flatten_history_dim:
raise ValueError("xattn needs flattened observation histories")
steps = max(cfg.history_length, 1)
frames = int(cfg.params.get("frames", 1)) if takes_frames(cfg) else 1
if name in goal and steps > 1 and frames > 1:
raise ValueError(f"xattn goal term {name} has both history and lookahead")
parts = (frames if steps == 1 else 1) if name in goal else steps
blocks.append((name in goal, offset, width, parts))
offset += width
def tokens(of_goal):
entries = [(o, w, p) for g, o, w, p in blocks if g == of_goal]
count = max(p for _, _, p in entries)
rows = [[] for _ in range(count)]
for start, width, parts in entries:
if parts not in (1, count):
kind = "lookahead frames" if of_goal else "history lengths"
raise ValueError(f"xattn needs equal {kind} (or none) across terms")
for i, row in enumerate(rows):
if parts == 1:
row.extend(range(start, start + width))
else:
size = width // count
row.extend(range(start + i * size, start + (i + 1) * size))
return rows
goal_indices = [i for g, o, w, _ in blocks if g for i in range(o, o + w)]
return tokens(False), tokens(True), goal_indices
def goal_term_names(terms, names=()) -> list[str]:
"""Resolve goal names from configured terms, for launch validation and the model."""
active = {name: cfg for name, cfg in terms.items() if cfg is not None}
if set(names) - set(active):
raise ValueError(f"unknown encoder goal terms: {set(names) - set(active)}")
selected = [
name
for name, cfg in active.items()
if name in names
or (not names and (name.startswith("ref_") or cfg.params.get("frames", 1) > 1))
]
if not selected or len(selected) == len(active):
raise ValueError("motion encoder needs goal terms and non-goal actor observations")
return selected
def goal_frames(env, names=()) -> int:
"""Reference frames the goal terms look ahead (the most of any)."""
from wbc_mjlab.env.mdp.observations import takes_frames
om = env.observation_manager
terms = dict(
zip(
om._group_obs_term_names["actor"], om._group_obs_term_cfgs["actor"], strict=True
)
)
return max(
int(terms[n].params.get("frames", 1)) if takes_frames(terms[n]) else 1
for n in goal_term_names(terms, names)
)
def goal_observation_indices(env, names=()) -> list[int]:
"""Resolve complete term blocks, including their configured history, in actor order."""
om = env.observation_manager
term_names = om._group_obs_term_names["actor"]
configs = om._group_obs_term_cfgs["actor"]
selected = goal_term_names(dict(zip(term_names, configs, strict=True)), names)
print(f"[INFO] Motion encoder goal terms: {', '.join(selected)}")
indices, offset = [], 0
for name, shape in zip(term_names, om._group_obs_term_dim["actor"], strict=True):
width = int(torch.tensor(shape).prod())
if name in selected:
indices.extend(range(offset, offset + width))
offset += width
return indices
def actor_options(env, cfg):
"""Resolve the actor once for the upstream PPO runner, WM and actor-only inference."""
options = dict(cfg["actor"])
if options.get("activation") == "silu":
options["activation"] = "swish" # rsl_rl names torch.nn.SiLU swish
for key in ("cnn_cfg", "distribution_cfg"):
if options.get(key) is None:
options.pop(key, None)
if options.get("rnn_type") is None:
for key in ("rnn_type", "rnn_hidden_dim", "rnn_num_layers"):
options.pop(key, None)
if isinstance(options.get("distribution_cfg"), dict):
options["distribution_cfg"] = dict(options["distribution_cfg"])
algorithm = cfg["algorithm"]
# PPO distribution config is retained by the evaluator; wm uses fixed exploration.
if algorithm.get("class_name") == "wbc_mjlab.rl.wm.algorithm:WorldModelRL":
options["distribution_cfg"] = {
"class_name": "GaussianDistribution",
"init_std": algorithm.get("action_noise", 0.1),
"std_type": "scalar",
"learn_std": False,
}
if any(
k in options for k in ("goal_indices", "reference_skip", "history_index")
) or isinstance(options.get("keypoint_skip"), dict):
return options # already resolved by the common runner
residual = options.pop("reference_residual", False)
keypoint = options.pop("keypoint_skip", False)
keypoint_hidden = options.pop("keypoint_skip_hidden", (1024, 1024, 512))
options.pop("keypoint_skip_steps", None)
if residual and keypoint:
raise ValueError("choose the reference residual or the keypoint skip, not both")
if residual:
options["reference_skip"] = reference_skip_options(env)
if keypoint:
options["keypoint_skip"] = keypoint_skip_options(env, keypoint_hidden)
residual = residual or keypoint
architecture = options.pop("architecture", "mlp")
encoder_hidden = options.pop("encoder_hidden_dims", (2048, 1024, 512, 256))
latent = options.pop("latent_dim", 32)
names = options.pop("goal_terms", ())
xattn_dim = options.pop("xattn_dim", 128)
xattn_heads = options.pop("xattn_heads", 4)
latent_aux = options.pop("latent_aux", False)
if latent_aux and architecture == "mlp":
raise ValueError("latent_aux needs an encoder architecture (motion_encoder, xattn)")
if latent_aux:
options["aux_dim"] = goal_frames(env, names) * env.action_manager.total_action_dim
if architecture == "xattn":
history_index, frame_index, goal_indices = cross_attention_indices(env, names)
print(
f"[INFO] Cross-attention tokens: {len(history_index)} history × "
f"{len(history_index[0])}, {len(frame_index)} frames × {len(frame_index[0])}"
)
options.update(
class_name="wbc_mjlab.rl.policy:CrossAttentionModel",
history_index=history_index,
frame_index=frame_index,
goal_indices=goal_indices,
xattn_dim=xattn_dim,
xattn_heads=xattn_heads,
)
elif architecture == "motion_encoder":
options.update(
class_name="wbc_mjlab.rl.policy:MotionEncoderModel",
goal_indices=goal_observation_indices(env, names),
encoder_hidden_dims=encoder_hidden,
latent_dim=latent,
)
elif architecture == "mlp" and residual:
options["class_name"] = "wbc_mjlab.rl.policy:ReferenceResidualModel"
elif architecture != "mlp":
raise ValueError(f"unknown policy architecture: {architecture}")
return options
class MotionEncoderModel(MLPModel):
"""A standard rsl_rl actor with an encoded goal head; export remains inherited."""
def __init__(
self,
obs,
obs_groups,
obs_set,
output_dim,
*,
goal_indices,
encoder_hidden_dims=(2048, 1024, 512, 256),
latent_dim=32,
reference_skip=None,
keypoint_skip=None,
aux_dim=0,
**options,
):
super().__init__(obs, obs_groups, obs_set, output_dim, **options)
self.mlp = MotionDecoder(
self.obs_dim,
goal_indices,
output_dim,
encoder_hidden_dims,
options.get("hidden_dims", (256, 256, 256)),
latent_dim,
options.get("activation", "elu"),
aux_dim,
)
_add_skips(self, output_dim, reference_skip, keypoint_skip)
class CrossAttentionModel(MLPModel):
"""A standard rsl_rl actor with the cross-attention head; export remains inherited."""
def __init__(
self,
obs,
obs_groups,
obs_set,
output_dim,
*,
history_index,
frame_index,
goal_indices,
xattn_dim=128,
xattn_heads=4,
reference_skip=None,
keypoint_skip=None,
aux_dim=0,
**options,
):
super().__init__(obs, obs_groups, obs_set, output_dim, **options)
self.mlp = CrossAttentionDecoder(
self.obs_dim,
history_index,
frame_index,
goal_indices,
output_dim,
options.get("hidden_dims", (256, 256, 256)),
options.get("activation", "elu"),
xattn_dim,
xattn_heads,
aux_dim,
)
_add_skips(self, output_dim, reference_skip, keypoint_skip)
def _skip_action_contract(env):
"""Tracked joint ids and the action's per-joint offset and scale, for skips that add a
reference joint target to the policy's action."""
action = env.action_manager.get_term("joint_pos")
command = env.command_manager.get_term("motion")
if getattr(action, "reference_relative", False) or action.cfg.clip is not None:
raise ValueError("reference skip needs unclipped default-relative joint actions")
ids = action._target_ids.tolist()
if ids != (
command._tracked_joint_ids.tolist()
if command._tracked_joint_ids is not None
else list(range(len(command.robot.joint_names)))
):
raise ValueError("reference skip needs tracked joints in action order")
def vector(value):
value = torch.as_tensor(value, device=env.device).expand(env.num_envs, len(ids))
if not torch.allclose(value, value[:1].expand_as(value)):
raise ValueError(
"reference skip needs the same action scale/offset in every environment"
)
return value[0].tolist()
scale = vector(action._scale)
if any(v == 0 for v in scale):
raise ValueError("reference skip action scale must be nonzero")
return ids, vector(action._offset), scale
def reference_skip_options(env):
"""Resolve Blink's next-frame skip from the actual observation/action contract.
BlinkMimic d264658, networks.Policy; SuperMimic arXiv:2610.28480.
https://github.com/fuyichen0725/blinkmimic/blob/d264658d6c9ce61fb52f9f0f8985a369f3ccc409/src/blinkmimic/networks.py
"""
from wbc_mjlab.env.mdp.observations import ref_joint_pos
ids, offset, scale = _skip_action_contract(env)
om = env.observation_manager
start, indices = 0, None
for cfg, shape in zip(
om._group_obs_term_cfgs["actor"], om._group_obs_term_dim["actor"], strict=True
):
width = int(torch.tensor(shape).prod())
if cfg.func is ref_joint_pos:
if (
cfg.params.get("frames", 1) < 2
or cfg.params.get("stride", 1) != 1
or cfg.noise is not None
or cfg.scale is not None
or cfg.clip is not None
or cfg.history_length > 1
):
raise ValueError(
"reference skip needs clean, unscaled joint lookahead including frame +1"
)
indices = list(range(start + len(ids), start + 2 * len(ids)))
start += width
if indices is None:
raise ValueError("reference skip requires the ref_joint_pos lookahead observation")
return dict(indices=indices, offset=offset, scale=scale)
def _raw(obs, normalizer, indices):
"""``obs[..., indices]`` with the observation normalization undone."""
x = obs[..., indices]
if hasattr(normalizer, "_mean"):
x = (
x * (normalizer._std[..., indices] + normalizer.eps)
+ normalizer._mean[..., indices]
)
return x
class ReferenceResidualHead(nn.Module):
"""Exportable skip around any shared policy head; undo observation normalization."""
def __init__(self, head, normalizer, indices, offset, scale):
super().__init__()
self.head, self.normalizer = head, normalizer
self.register_buffer("indices", torch.tensor(indices, dtype=torch.long))
self.register_buffer("offset", torch.tensor(offset))
self.register_buffer("scale", torch.tensor(scale))
def forward(self, obs):
ref = _raw(obs, self.normalizer, self.indices)
return self.head(obs) + (ref - self.offset) / self.scale
class FrozenMLP(nn.Module):
"""An ELU MLP with its input standardization, held as buffers: no optimizer, EMA or
Muon grouping trains it, and it is saved and exported with the module owning it.
PartyMimic (Roboparty, 2026) networks.py ``FrozenMLP``, MIT; see THIRD_PARTY_NOTICES.
"""
def __init__(self, inp, hidden, out):
super().__init__()
dims = [inp, *hidden, out]
self.layers = len(dims) - 1
for i, (a, b) in enumerate(zip(dims[:-1], dims[1:], strict=True)):
self.register_buffer(f"w{i}", torch.zeros(b, a))
self.register_buffer(f"b{i}", torch.zeros(b))
self.register_buffer("mu", torch.zeros(inp))
self.register_buffer("sd", torch.ones(inp))
self.register_buffer("fitted", torch.zeros((), dtype=torch.bool))
def forward(self, x):
x = (x - self.mu) / self.sd
for i in range(self.layers):
x = nn.functional.linear(x, getattr(self, f"w{i}"), getattr(self, f"b{i}"))
if i < self.layers - 1:
x = nn.functional.elu(x)
return x
@torch.no_grad()
def load_from(self, net: nn.Sequential, mu, sd):
linears = [m for m in net if isinstance(m, nn.Linear)]
assert len(linears) == self.layers
for i, m in enumerate(linears):
getattr(self, f"w{i}").copy_(m.weight)
getattr(self, f"b{i}").copy_(m.bias)
self.mu.copy_(mu)
self.sd.copy_(sd)
self.fitted.fill_(True)
class KeypointSkipHead(nn.Module):
"""Blink's reference skip with an estimated reference: ``a = head(obs) + kp2q(goal)``,
kp2q a frozen MLP from the raw keypoint goal to the next frame's reference joint target
(in action units), fit to the motion library before training (:func:`fit_keypoint_skips`).
PartyMimic (Roboparty, 2026) ``--kp_skip``, networks.py/train.py, MIT; see
THIRD_PARTY_NOTICES.
"""
def __init__(
self,
head,
normalizer,
output_dim,
indices,
hidden,
body_indexes,
anchor_index,
frames,
stride,
joint_ids,
offset,
scale,
):
super().__init__()
self.head, self.normalizer = head, normalizer
self.register_buffer("indices", torch.tensor(indices, dtype=torch.long))
self.kp2q = FrozenMLP(len(indices), hidden, output_dim)
self.hidden, self.output_dim = tuple(hidden), int(output_dim)
# How the goal is computed from reference frames: for the fit only.
self.body_indexes, self.anchor_index = list(body_indexes), int(anchor_index)
self.frames, self.stride = int(frames), int(stride)
self.joint_ids = None if joint_ids is None else list(joint_ids)
self.offset, self.scale = list(offset), list(scale)
def forward(self, obs):
return self.head(obs) + self.kp2q(_raw(obs, self.normalizer, self.indices))
def keypoint_skip_options(env, hidden):
"""Resolve the keypoint skip from the ``ref_keypoints`` observation and the action."""
from wbc_mjlab.env.mdp.observations import ref_keypoints
ids, offset, scale = _skip_action_contract(env)
command = env.command_manager.get_term("motion")
om = env.observation_manager
offset_obs, found = 0, None
for cfg, shape in zip(
om._group_obs_term_cfgs["actor"], om._group_obs_term_dim["actor"], strict=True
):
width = int(torch.tensor(shape).prod())
if cfg.func is ref_keypoints:
if cfg.noise is not None or cfg.scale is not None or cfg.history_length > 1:
raise ValueError("keypoint skip needs a clean ref_keypoints without history")
found = (list(range(offset_obs, offset_obs + width)), cfg.params)
offset_obs += width
if found is None:
raise ValueError("keypoint skip requires the ref_keypoints observation")
indices, params = found
names = list(command.cfg.body_names)
tracked = command._tracked_joint_ids
return dict(
indices=indices,
hidden=tuple(hidden),
body_indexes=[names.index(n) for n in params["body_names"]],
anchor_index=int(command.motion_anchor_body_index),
frames=int(params.get("frames", 1)),
stride=int(params.get("stride", 1)),
joint_ids=None if tracked is None else tracked.tolist(),
offset=offset,
scale=scale,
)
@torch.no_grad()
def keypoint_skip_table(skip, motion, chunk=65536):
"""Keypoint and anchor poses, the reference joint target in action units and the clip
end of every reference step (interpolating the motions once, not per batch)."""
total = int(motion.segment_end_idx[-1])
device = motion.segment_end_idx.device
offset = torch.tensor(skip.offset, device=device)
scale = torch.tensor(skip.scale, device=device)
parts = []
for steps in torch.arange(total, device=device).split(chunk):
frame = motion.frame(steps)
q = (
frame.joint_pos if skip.joint_ids is None else frame.joint_pos[:, skip.joint_ids]
)
parts.append(
(
frame.body_pos_w[:, skip.body_indexes],
frame.body_quat_w[:, skip.body_indexes],
frame.body_pos_w[:, skip.anchor_index],
frame.body_quat_w[:, skip.anchor_index],
(q - offset) / scale,
)
)
pos, quat, anchor_pos, anchor_quat, action = (
torch.cat(p) for p in zip(*parts, strict=True)
)
end = motion.segment_end_idx[
torch.searchsorted(
motion.segment_end_idx, torch.arange(total, device=device), right=True
)
]
return dict(
pos=pos,
quat=quat,
anchor_pos=anchor_pos,
anchor_quat=anchor_quat,
action=action,
end=end,
)
def _keypoint_skip_data(skip, table, steps):
"""The keypoint goal at reference steps ``steps`` and the next frame's reference joint
target in action units (frames past a clip's end repeat its last one)."""
from wbc_mjlab.env.mdp.observations import keypoint_features
last = table["end"][steps] - 1
ahead = torch.arange(skip.frames, device=steps.device) * skip.stride
window = torch.minimum(steps[:, None] + ahead, last[:, None])
k = skip.frames
x = keypoint_features(
table["pos"][window],
table["quat"][window],
table["anchor_pos"][steps, None].expand(-1, k, 3),
table["anchor_quat"][steps, None].expand(-1, k, 4),
).flatten(1)
return x, table["action"][torch.minimum(steps + 1, last)]
def fit_keypoint_skip(skip, motion, steps=20000, batch=8192, seed=0):
"""Fit ``skip.kp2q`` on every reference step but a held-out 5 % (PartyMimic train.py
``fit_kp2q``): Adam with a cosine learning rate, then freeze into the buffers."""
import math
import time
device = skip.kp2q.mu.device
start = time.time()
table = keypoint_skip_table(skip, motion)
total = table["end"].shape[0]
g = torch.Generator(device=device).manual_seed(seed)
order = torch.randperm(total, device=device, generator=g)
held, train = order[: max(1, total // 20)], order[max(1, total // 20) :]
def sample(n):
return train[torch.randint(len(train), (n,), device=device, generator=g)]
with torch.no_grad():
x0, _ = _keypoint_skip_data(skip, table, sample(min(65536, 8 * batch)))
mu, sd = x0.mean(0), x0.std(0).clamp_min(1e-3)
dims = [x0.shape[1], *skip.hidden]
layers = []
for a, b in zip(dims[:-1], dims[1:], strict=True):
layers += [nn.Linear(a, b), nn.ELU()]
net = nn.Sequential(*layers, nn.Linear(dims[-1], skip.output_dim)).to(device)
optimizer = torch.optim.Adam(net.parameters(), lr=1e-3)
with torch.inference_mode(False), torch.enable_grad():
for step in range(steps):
for group in optimizer.param_groups:
group["lr"] = 1e-3 * 0.5 * (1 + math.cos(math.pi * step / steps))
with torch.no_grad():
x, y = _keypoint_skip_data(skip, table, sample(batch))
loss = (net((x - mu) / sd) - y).square().mean()
optimizer.zero_grad(set_to_none=True)
loss.backward()
optimizer.step()
with torch.no_grad():
scale = torch.tensor(skip.scale, device=device)
error, base = 0.0, 0.0
for chunk in held.split(65536):
x, y = _keypoint_skip_data(skip, table, chunk)
error = error + ((net((x - mu) / sd) - y) * scale).square().sum(0)
base = base + (y * scale).square().sum(0)
rms = (error / len(held)).sqrt()
default = (base / len(held)).sqrt()
print(
f"[INFO] Keypoint skip: kp2q fit in {time.time() - start:.0f} s, held-out joint RMS "
f"{rms.mean():.4f} rad (max {rms.max():.4f}; default pose {default.mean():.4f})"
)
skip.kp2q.load_from(net, mu, sd)
def fit_keypoint_skips(modules, motion, steps=20000):
"""Fit the keypoint skip of the first module that has an unfitted one, and copy it into
every other (EMA copies, live actors). Fitted skips (loaded checkpoints) are kept."""
skips = [
m for module in modules for m in module.modules() if isinstance(m, KeypointSkipHead)
]
if not skips or all(bool(s.kp2q.fitted) for s in skips):
return
source = next((s for s in skips if bool(s.kp2q.fitted)), None)
if source is None:
source = skips[0]
fit_keypoint_skip(source, motion, steps)
for s in skips:
if s is not source:
s.kp2q.load_state_dict(source.kp2q.state_dict())
class ReferenceResidualModel(MLPModel):
"""The flat MLP with a reference skip (joint lookahead) or keypoint skip."""
def __init__(
self,
obs,
obs_groups,
obs_set,
output_dim,
*,
reference_skip=None,
keypoint_skip=None,
**options,
):
super().__init__(obs, obs_groups, obs_set, output_dim, **options)
_add_skips(self, output_dim, reference_skip, keypoint_skip)
def _add_skips(model, output_dim, reference_skip, keypoint_skip):
if reference_skip is not None:
model.mlp = ReferenceResidualHead(model.mlp, model.obs_normalizer, **reference_skip)
if keypoint_skip is not None:
model.mlp = KeypointSkipHead(
model.mlp, model.obs_normalizer, output_dim, **keypoint_skip
)
def policy_head(actor):
"""The actor's head inside its skips (an encoder head has ``latent`` and ``aux``)."""
head = actor.mlp
while isinstance(head, (ReferenceResidualHead, KeypointSkipHead)):
head = head.head
return head
def policy_mlps(actor):
"""Independent encoder/decoder MLP boundaries for optimizer parameter grouping."""
head = policy_head(actor)
if isinstance(head, (MotionDecoder, CrossAttentionDecoder)):
return [head.encoder, head.decoder]
return [head]
def build_actor(obs, env, cfg):
from rsl_rl.utils import resolve_callable
options = actor_options(env, cfg)
cls = resolve_callable(options.pop("class_name"))
return cls(
obs, cfg["obs_groups"], "actor", env.action_manager.total_action_dim, **options
)
def load_actor(env, cfg, checkpoint, device):
"""Load only the actor; evaluation neither allocates a replay nor a world model."""
from rsl_rl.utils import resolve_obs_groups
from tensordict import TensorDict
obs = TensorDict(env.observation_manager.compute(), batch_size=[env.num_envs])
cfg["obs_groups"] = resolve_obs_groups(obs, cfg["obs_groups"], ["actor", "critic"])
actor = build_actor(obs, env, cfg).to(device)
from wbc_mjlab.rl.checkpoints import read_checkpoint
state = read_checkpoint(checkpoint, device)
actor.load_state_dict(state["actor_state_dict"])
return actor.eval()