Source code for wbc_mjlab.rl.wm.model

"""World model: robot dynamics, termination and the reward terms imagination cannot compute.

The dynamics predict the change of the state over one control step from the state, the PD
targets and the domain-randomization context. Inputs and the change are expressed in the
robot's heading frame (and body frame for rotations), so the model is invariant to where the
robot stands and which way it faces, as the physics on flat ground is.

Algorithm: SuperMimic (Liao et al., 2026), Eq. 1 / Table A.4,
https://arxiv.org/abs/2610.28480. Blink velocity blending and loss scaling:
https://github.com/fuyichen0725/blinkmimic/blob/d264658d6c9ce61fb52f9f0f8985a369f3ccc409/src/blinkmimic/networks.py
Optional integrated residuals and fixed-scale
diagnostics draw on CuboiLeo/SuperMimic-Reproduced at ba61053 (links below).
"""

from __future__ import annotations

import torch
from mjlab.utils.lab_api.math import (
  matrix_from_quat,
  quat_apply,
  quat_apply_inverse,
  quat_conjugate,
  quat_mul,
  yaw_quat,
)
from torch import nn

from wbc_mjlab.rl.wm.distributed import sum_
from wbc_mjlab.rl.wm.kinematics import RobotState


@torch.no_grad()
def update_running_stats(
  mean: torch.Tensor,
  var: torch.Tensor,
  std: torch.Tensor,
  count: torch.Tensor,
  x: torch.Tensor,
  weight: torch.Tensor | None = None,
) -> None:
  """rsl_rl's running mean / variance update, in place (the tensors keep their storage, as
  CUDA graphs need), over the rows of ``x`` with nonzero ``weight``."""
  w = torch.ones(x.shape[0], device=x.device) if weight is None else weight.float()
  # Excluded tail rows must not poison moments (0 * inf is NaN).
  x = torch.where(w[:, None] > 0, x, torch.zeros_like(x))
  n = sum_(w.sum())
  mean_x = sum_((w[:, None] * x).sum(0)) / n.clamp(min=1e-8)
  var_x = sum_((w[:, None] * (x - mean_x).square()).sum(0)) / n.clamp(min=1e-8)
  count.add_(n.to(count.dtype))
  rate = n / count.clamp(min=1e-8)
  delta = mean_x - mean
  mean.add_(rate * delta)
  var.add_(rate * (var_x - var + delta * (mean_x - mean)))
  std.copy_(var.sqrt())


class RunningNorm(nn.Module):
  """Feature normalization by a running mean / std, updated in place (see
  :func:`update_running_stats`)."""

  def __init__(self, dim: int, eps: float):
    super().__init__()
    self.eps = eps
    self.register_buffer("mean", torch.zeros(dim))
    self.register_buffer("var", torch.ones(dim))
    self.register_buffer("std", torch.ones(dim))
    self.register_buffer("count", torch.zeros(()))

  def forward(self, x: torch.Tensor) -> torch.Tensor:
    return (x - self.mean) / (self.std + self.eps)

  def inverse(self, y: torch.Tensor) -> torch.Tensor:
    return y * (self.std + self.eps) + self.mean

  def update(self, x: torch.Tensor, weight: torch.Tensor | None = None) -> None:
    if self.training and x.shape[-1]:
      update_running_stats(self.mean, self.var, self.std, self.count, x, weight)


def mlp(in_dim: int, out_dim: int, hidden: tuple[int, ...]) -> nn.Sequential:
  """Linear → LayerNorm → SiLU blocks and a linear output."""
  layers: list[nn.Module] = []
  for h in hidden:
    layers += [nn.Linear(in_dim, h), nn.LayerNorm(h), nn.SiLU()]
    in_dim = h
  layers.append(nn.Linear(in_dim, out_dim))
  return nn.Sequential(*layers)


def state_features(s: RobotState) -> torch.Tensor:
  """Height, tilt (6D, heading removed), body-frame velocities, joint state."""
  heading = yaw_quat(s.root_quat)
  tilt = matrix_from_quat(quat_mul(quat_conjugate(heading), s.root_quat))[..., :2]
  return torch.cat(
    [
      s.root_pos[:, 2:3],
      tilt.reshape(-1, 6),
      quat_apply_inverse(s.root_quat, s.root_lin_vel),
      quat_apply_inverse(s.root_quat, s.root_ang_vel),
      s.joint_pos,
      s.joint_vel,
    ],
    dim=-1,
  )


def feature_dim(num_joints: int) -> int:
  return 13 + 2 * num_joints


def change_dim(num_joints: int) -> int:
  return 12 + 2 * num_joints


def _quat_exp(v: torch.Tensor) -> torch.Tensor:
  """Rotation vector → unit quaternion (wxyz); smooth (and its gradient finite) at zero."""
  half = (v.square().sum(-1, keepdim=True) + 1e-12).sqrt() / 2
  return torch.cat([half.cos(), v * 0.5 * torch.sinc(half / torch.pi)], dim=-1)


def _quat_log(q: torch.Tensor) -> torch.Tensor:
  """Unit quaternion (wxyz) → rotation vector of angle ≤ π; gradient finite at zero."""
  q = q * torch.where(q[..., :1] < 0, -1.0, 1.0)
  sin_half = (q[..., 1:].square().sum(-1, keepdim=True) + 1e-12).sqrt()
  return q[..., 1:] * (2 * torch.atan2(sin_half, q[..., :1]) / sin_half)


def apply_change(s: RobotState, d: torch.Tensor) -> RobotState:
  """The state after a change ``d`` (:func:`state_change`)."""
  j = s.joint_pos.shape[-1]
  heading = yaw_quat(s.root_quat)
  quat = quat_mul(s.root_quat, _quat_exp(d[:, 3:6]))
  quat = quat / quat.norm(dim=-1, keepdim=True)
  ang_b = quat_apply_inverse(s.root_quat, s.root_ang_vel) + d[:, 9:12]
  return RobotState(
    s.root_pos + quat_apply(heading, d[:, 0:3]),
    quat,
    s.root_lin_vel + quat_apply(heading, d[:, 6:9]),
    quat_apply(quat, ang_b),
    s.joint_pos + d[:, 12 : 12 + j],
    s.joint_vel + d[:, 12 + j :],
  )


def state_change(s0: RobotState, s1: RobotState) -> torch.Tensor:
  """Change from ``s0`` to ``s1``: root displacement and velocity change in ``s0``'s heading
  frame, rotation (vector) and angular-velocity change in the body frame, joint changes."""
  heading = yaw_quat(s0.root_quat)
  return torch.cat(
    [
      quat_apply_inverse(heading, s1.root_pos - s0.root_pos),
      _quat_log(quat_mul(quat_conjugate(s0.root_quat), s1.root_quat)),
      quat_apply_inverse(heading, s1.root_lin_vel - s0.root_lin_vel),
      quat_apply_inverse(s1.root_quat, s1.root_ang_vel)
      - quat_apply_inverse(s0.root_quat, s0.root_ang_vel),
      s1.joint_pos - s0.joint_pos,
      s1.joint_vel - s0.joint_vel,
    ],
    dim=-1,
  )


def integrate_velocity(s: RobotState, dt: float) -> RobotState:
  """Constant-velocity prior, with body-frame exponential-map orientation integration.

  Inspired by SuperMimic-Reproduced (ba61053), world_model/state_spec.py:
  https://github.com/CuboiLeo/SuperMimic-Reproduced/blob/ba6105349069bf040f1fdad60873d13138d505a2/src/supermimic/world_model/state_spec.py
  Residuals retain this repo's state ordering, frames and running normalization.
  """
  quat = quat_mul(
    s.root_quat, _quat_exp(dt * quat_apply_inverse(s.root_quat, s.root_ang_vel))
  )
  return RobotState(
    s.root_pos + dt * s.root_lin_vel,
    quat / quat.norm(dim=-1, keepdim=True),
    s.root_lin_vel,
    s.root_ang_vel,
    s.joint_pos + dt * s.joint_vel,
    s.joint_vel,
  )


def apply_integrated_change(
  s: RobotState, d: torch.Tensor, dt: float, velocity_blend: float = 1.0
) -> RobotState:
  """Semi-implicit velocity integration plus pose corrections (reproduction D2).

  Unlike full increments, predicted velocity changes also move the pose through
  the known dt factor. Tensor ordering remains the repo's [dp, dtheta, dv, dw, dq, dqd].
  See the SuperMimic-Reproduced state_spec.py link in integrate_velocity.
  """
  j = s.joint_pos.shape[-1]
  heading = yaw_quat(s.root_quat)
  velocity = s.root_lin_vel + quat_apply(heading, d[:, 6:9])
  omega = quat_apply_inverse(s.root_quat, s.root_ang_vel) + d[:, 9:12]
  joint_velocity = s.joint_vel + d[:, 12 + j :]
  # BlinkMimic d264658 networks.py: (n+1)/(2n) for n semi-implicit substeps.
  # Blend angular velocities in the old body frame; retain this repo's state frames.
  drift_v = torch.lerp(s.root_lin_vel, velocity, velocity_blend)
  drift_w = torch.lerp(
    quat_apply_inverse(s.root_quat, s.root_ang_vel), omega, velocity_blend
  )
  drift_qd = torch.lerp(s.joint_vel, joint_velocity, velocity_blend)
  quat = quat_mul(s.root_quat, _quat_exp(dt * drift_w + d[:, 3:6]))
  quat = quat / quat.norm(dim=-1, keepdim=True)
  return RobotState(
    s.root_pos + dt * drift_v + quat_apply(heading, d[:, :3]),
    quat,
    velocity,
    quat_apply(quat, omega),
    s.joint_pos + dt * drift_qd + d[:, 12 : 12 + j],
    joint_velocity,
  )


def apply_velocity_change(
  s: RobotState, delta: torch.Tensor, dt: float, blend: float
) -> RobotState:
  """Velocity-only integration; no independently predicted pose corrections.

  Adapted from official SuperMimic GenCoordRepresentation (Apache-2.0),
  abc124c1bd5976341034668af3d12c087f2f1f55, representations.py; Liao et al.,
  https://arxiv.org/abs/2610.28480. Both root deltas use the CURRENT body frame.
  Store the resulting twist in world coordinates, without rotating it a second
  time with the successor orientation.
  """
  dv, dw, dq = delta[:, :3], delta[:, 3:6], delta[:, 6:]
  dv_w, dw_w = quat_apply(s.root_quat, dv), quat_apply(s.root_quat, dw)
  omega = quat_apply_inverse(s.root_quat, s.root_ang_vel) + blend * dw
  quat = quat_mul(s.root_quat, _quat_exp(dt * omega))
  return RobotState(
    s.root_pos + dt * (s.root_lin_vel + blend * dv_w),
    quat / quat.norm(dim=-1, keepdim=True),
    s.root_lin_vel + dv_w,
    s.root_ang_vel + dw_w,
    s.joint_pos + dt * (s.joint_vel + blend * dq),
    s.joint_vel + dq,
  )


def nominal_error(pred: RobotState, true: RobotState) -> torch.Tensor:
  """Fixed-scale squared errors, independent of the training normalizer.

  Nominal scales follow SuperMimic-Reproduced's StateSpecCfg (ba61053), not
  unpublished paper constants: pos .05 m, rot .1 rad, v .5 m/s, omega 1 rad/s,
  joint pos .1 rad, joint vel 2 rad/s. These scores are not paper-equivalent.
  """
  error = state_change(true, pred)
  j = true.joint_pos.shape[-1]
  scales = error.new_tensor(
    [0.05] * 3 + [0.1] * 3 + [0.5] * 3 + [1.0] * 3 + [0.1] * j + [2.0] * j
  )
  return (error / scales).square()


[docs] class WorldModel(nn.Module): """Dynamics ``f(s, u, ξ) → s'`` plus step heads ``h(s, u, g, ξ) → (termination, rewards)``. ``u`` are the PD targets relative to the joint positions, ``g`` the tracking features of the state against the reference, ``ξ`` the domain-randomization context. The step heads predict whether the step ends the episode and the reward terms that are not functions of the state (actuator forces, contacts, …). """ def __init__( self, num_joints: int, context_dim: int, tracking_dim: int, num_learned_rewards: int, dynamics_hidden: tuple[int, ...], heads_hidden: tuple[int, ...], dynamics_mode: str = "increment", step_dt: float = 0.02, velocity_blend: float = 1.0, loss_scale: str = "output", physical_error_scales: tuple[float, ...] = (0.01, 0.02, 0.5, 1.0, 0.1, 3.14), ): super().__init__() self.num_joints = num_joints if dynamics_mode not in ("increment", "integrated", "velocity"): raise ValueError(f"unknown dynamics mode: {dynamics_mode}") self.dynamics_mode, self.step_dt = dynamics_mode, step_dt if not 0 <= velocity_blend <= 1 or loss_scale not in ( "output", "increment", "physical", ): raise ValueError("invalid velocity blend or dynamics loss scale") import math if len(physical_error_scales) != 6 or any( not math.isfinite(v) or v <= 0 for v in physical_error_scales ): raise ValueError("physical_error_scales needs six finite positive tolerances") if dynamics_mode == "velocity" and loss_scale == "output": raise ValueError("velocity dynamics needs increment or physical loss scaling") self.physical_error_scales = tuple(physical_error_scales) self.velocity_blend, self.loss_scale = velocity_blend, loss_scale feat, change = feature_dim(num_joints), change_dim(num_joints) dyn_in = feat + num_joints + context_dim head_in = dyn_in + tracking_dim self.dynamics_in = RunningNorm(dyn_in, eps=1e-4) output = 6 + num_joints if dynamics_mode == "velocity" else change self.change_norm = RunningNorm(output, eps=1e-6) # BlinkMimic separates residual output statistics from physical error scales. # Optional so existing checkpoints retain their exact parameter/buffer schema. self.increment_norm = ( RunningNorm(change, eps=1e-4) if loss_scale == "increment" else None ) self.dynamics = mlp(dyn_in, output, dynamics_hidden) if dynamics_mode in ("integrated", "velocity"): nn.init.zeros_(self.dynamics[-1].weight) nn.init.zeros_(self.dynamics[-1].bias) self.heads_in = RunningNorm(head_in, eps=1e-4) self.reward_norm = RunningNorm(num_learned_rewards, eps=1e-4) self.heads = mlp(head_in, 1 + num_learned_rewards, heads_hidden) def _dynamics_input( self, s: RobotState, setpoint: torch.Tensor, context: torch.Tensor ): return torch.cat([state_features(s), setpoint - s.joint_pos, context], dim=-1)
[docs] def step( self, s: RobotState, setpoint: torch.Tensor, context: torch.Tensor ) -> RobotState: x = self.dynamics_in(self._dynamics_input(s, setpoint, context)) change = self.change_norm.inverse(self.dynamics(x)) if self.dynamics_mode == "velocity": return apply_velocity_change(s, change, self.step_dt, self.velocity_blend) if self.dynamics_mode == "integrated": return apply_integrated_change(s, change, self.step_dt, self.velocity_blend) return apply_change(s, change)
[docs] def target_change(self, s: RobotState, next_state: RobotState) -> torch.Tensor: if self.dynamics_mode == "velocity": return torch.cat( [ quat_apply_inverse(s.root_quat, next_state.root_lin_vel - s.root_lin_vel), quat_apply_inverse(s.root_quat, next_state.root_ang_vel - s.root_ang_vel), next_state.joint_vel - s.joint_vel, ], dim=-1, ) change = state_change(s, next_state) if self.dynamics_mode != "integrated": return change j, dt = self.num_joints, self.step_dt return torch.cat( [ change[:, :3] - dt * quat_apply_inverse( yaw_quat(s.root_quat), torch.lerp(s.root_lin_vel, next_state.root_lin_vel, self.velocity_blend), ), change[:, 3:6] - dt * torch.lerp( quat_apply_inverse(s.root_quat, s.root_ang_vel), quat_apply_inverse(next_state.root_quat, next_state.root_ang_vel), self.velocity_blend, ), change[:, 6:12], change[:, 12 : 12 + j] - dt * torch.lerp(s.joint_vel, next_state.joint_vel, self.velocity_blend), change[:, 12 + j :], ], dim=-1, )
[docs] def step_heads( self, s: RobotState, setpoint: torch.Tensor, tracking: torch.Tensor, context: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: """(termination logit, learned reward terms) of the step from ``s``.""" x = torch.cat([self._dynamics_input(s, setpoint, context), tracking], dim=-1) out = self.heads(self.heads_in(x)) return out[:, 0], self.reward_norm.inverse(out[:, 1:])
[docs] def update_dynamics_normalization( self, s: RobotState, setpoint: torch.Tensor, context: torch.Tensor, change: torch.Tensor, weight: torch.Tensor, ) -> None: """From logged steps (``weight``: those that are real transitions).""" self.dynamics_in.update(self._dynamics_input(s, setpoint, context), weight) self.change_norm.update(change, weight)
[docs] def update_heads_normalization( self, s: RobotState, setpoint: torch.Tensor, tracking: torch.Tensor, context: torch.Tensor, rewards: torch.Tensor, ) -> None: x = torch.cat([self._dynamics_input(s, setpoint, context), tracking], dim=-1) self.heads_in.update(x) self.reward_norm.update(rewards)
[docs] def reward_error(self, pred: torch.Tensor, target: torch.Tensor) -> torch.Tensor: """Squared error of the learned reward terms in units of their spread.""" return ((pred - target) / (self.reward_norm.std + 1e-4)).square()
[docs] def change_error(self, pred: RobotState, true: RobotState) -> torch.Tensor: """Squared error of ``pred`` vs ``true`` in units of the typical one-step change.""" if self.loss_scale == "physical": # Bryson-style fixed physical weighting, inspired by official wm_learner.py. # Retain our rotation-vector metric and supervise root velocities explicitly. p, r, v, w, q, qd = self.physical_error_scales j = self.num_joints scale = pred.root_pos.new_tensor( [p] * 3 + [r] * 3 + [v] * 3 + [w] * 3 + [q] * j + [qd] * j ) elif self.increment_norm is None: scale = self.change_norm.std + 1e-6 else: j = self.num_joints # Per-component floors from BlinkMimic train.py; physical SI/radian units. floor = pred.root_pos.new_tensor( [0.003] * 3 + [0.005] * 3 + [0.02] * 3 + [0.05] * 3 + [0.003] * j + [0.05] * j ) scale = self.increment_norm.std.clamp_min(floor) return (state_change(true, pred) / scale).square()