diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 7ae36271fc..309704bb55 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -8,6 +8,14 @@ Upcoming version (not yet released) Added ^^^^^ +- Added ``ManagerBasedRlEnvCfg.capture_terminal_observations``. When enabled + (with ``auto_reset=True``), ``step()`` stores the true terminal observation + (the real next state, before reset) for done environments in + ``extras["terminal"]``, so algorithms that need the real next state at episode + boundaries (value bootstrapping at truncation, AMP-style style rewards) can + access it without disabling auto-reset. The snapshot is read-only: it applies + delay and history but not observation noise, and enabling the flag does not + change the training trajectory (:issue:`1026`, :issue:`900`). - Added ``BuiltinDcMotorActuator``, a native MuJoCo ```` wrapper. Supports voltage / position / velocity input modes with back-EMF, configurable motor constants, and optional integral, slew, inductance, diff --git a/src/mjlab/envs/manager_based_rl_env.py b/src/mjlab/envs/manager_based_rl_env.py index b123ef872c..1720867ac5 100644 --- a/src/mjlab/envs/manager_based_rl_env.py +++ b/src/mjlab/envs/manager_based_rl_env.py @@ -154,6 +154,40 @@ class ManagerBasedRlEnvCfg: their own training loop (or a wrapper that handles the reset between steps). """ + capture_terminal_observations: bool = False + """Whether to expose the true terminal observation for done environments while + keeping auto-reset enabled. + + When True (and ``auto_reset=True``), on any step where one or more environments + terminate or time out, ``step()`` stores their true terminal observation (the + real ``s_{t+1}``, before reset) in ``extras["terminal"]``:: + + extras["terminal"] = { + "env_ids": LongTensor, # the done environment indices, shape [k] + "observations": {group: Tensor, ...}, # mirrors the obs dict, rows = [k] + } + + The key is present only on steps where at least one environment resets, and only + when this flag is enabled. It is intended for algorithms that need the real next + state at episode boundaries (e.g. value bootstrapping at truncation, or AMP-style + style rewards) without giving up the standard auto-reset vectorized API. To build + a full ``[num_envs]`` next observation, scatter the terminal rows into the + returned observation:: + + next_obs = obs[group].clone() + if (term := extras.get("terminal")) is not None: + next_obs[term["env_ids"]] = term["observations"][group] + + The terminal observation is a read-only snapshot: delay and history are applied, + but observation noise is *not* (it would draw from the RNG and perturb training), + and command resampling / step events are not re-run. For typical proprioceptive + observations this is exactly the terminal state; observations that depend on + commands reflect their value as of the previous control step. Enabling this flag + does not change the training trajectory; it only adds one ``forward``/``sense`` + and an extra observation pass on steps where environments reset. This flag has no + effect when ``auto_reset=False`` (the returned observation is already terminal). + """ + scale_rewards_by_dt: bool = True """Whether to multiply rewards by the environment step duration (dt). @@ -366,6 +400,7 @@ def reset( if seed is not None: self.seed(seed) self.extras["log"] = dict() + self.extras.pop("terminal", None) self._reset_idx(env_ids) self.scene.write_data_to_sim() self.sim.forward() @@ -416,6 +451,7 @@ def step(self, action: torch.Tensor) -> types.VecEnvStepReturn: ) self.extras["log"] = dict() + self.extras.pop("terminal", None) self.action_manager.process_action(action.to(self.device)) for _ in range(self.cfg.decimation): @@ -442,6 +478,14 @@ def step(self, action: torch.Tensor) -> types.VecEnvStepReturn: # Reset envs that terminated/timed-out and log the episode info. reset_env_ids = self.reset_buf.nonzero(as_tuple=False).squeeze(-1) + if ( + self.cfg.capture_terminal_observations + and self.cfg.auto_reset + and len(reset_env_ids) > 0 + ): + # Snapshot the true terminal observation (real s_{t+1}) for done envs + # before they are reset below. Must run before _reset_idx. + self._store_terminal_observations(reset_env_ids) if self.cfg.auto_reset and len(reset_env_ids) > 0: self.recorder_manager.record_pre_reset(reset_env_ids) self._reset_idx(reset_env_ids) @@ -589,3 +633,50 @@ def _reset_idx(self, env_ids: torch.Tensor | None = None) -> None: # reset the episode length buffer. self.episode_length_buf[env_ids] = 0 self._manual_reset_pending[env_ids] = False + + def _store_terminal_observations(self, env_ids: torch.Tensor) -> None: + """Snapshot the true terminal observation for done envs into extras. + + Called from ``step()`` before resetting ``env_ids``. Refreshes derived + quantities and sensors at the terminal (pre-reset) state so the snapshot + matches what ``step()`` would return with ``auto_reset=False``. Both + ``forward()`` and ``sense()`` are deterministic, draw no RNG, and do not + modify ``qpos``/``qvel``, and the peek observation pass mutates no buffers, + so this does not perturb the training trajectory. + + This extra ``forward()`` precedes the unconditional ``forward()`` later in + ``step()``. That is safe because ``forward()`` is a fixed point: it writes + ``qacc`` into ``qacc_warmstart``, and re-solving from that warm start + reproduces the same ``qacc`` (verified to bit-equality on contact-rich + envs). So the extra call cannot change the result of the subsequent + ``forward()``; it is redundant compute on reset steps, not a perturbation. + (A deliberately under-converged solver would weaken this, at the cost of a + one-substep difference for done envs only.) See + :attr:`ManagerBasedRlEnvCfg.capture_terminal_observations`. + """ + self.sim.forward() + self.sim.sense() + terminal_obs = self.observation_manager.compute(peek=True) + self.extras["terminal"] = { + "env_ids": env_ids.clone(), + "observations": _select_env_observations(terminal_obs, env_ids), + } + + +def _select_env_observations( + obs: dict[str, torch.Tensor | dict[str, torch.Tensor]], + env_ids: torch.Tensor, +) -> dict[str, torch.Tensor | dict[str, torch.Tensor]]: + """Extract and clone the rows for ``env_ids`` from an observation dict. + + Mirrors the structure of ``obs`` (concatenated groups are tensors; non- + concatenated groups are term dicts), returning ``[len(env_ids), ...]`` rows. + Clones so the result does not alias the live observation buffers. + """ + selected: dict[str, torch.Tensor | dict[str, torch.Tensor]] = {} + for group, value in obs.items(): + if isinstance(value, torch.Tensor): + selected[group] = value[env_ids].clone() + else: + selected[group] = {k: v[env_ids].clone() for k, v in value.items()} + return selected diff --git a/src/mjlab/managers/observation_manager.py b/src/mjlab/managers/observation_manager.py index a47fdb8d3e..7058c5a459 100644 --- a/src/mjlab/managers/observation_manager.py +++ b/src/mjlab/managers/observation_manager.py @@ -303,8 +303,16 @@ def _check_and_handle_nans( return torch.nan_to_num(tensor, nan=0.0, posinf=0.0, neginf=0.0) def compute( - self, update_history: bool = False + self, update_history: bool = False, peek: bool = False ) -> dict[str, torch.Tensor | dict[str, torch.Tensor]]: + # Peek is a read-only snapshot (see compute_group): it never reads or writes + # the cache, never mutates delay/history buffers, and never draws RNG. + if peek: + return { + group_name: self.compute_group(group_name, peek=True) + for group_name in self._group_obs_term_names + } + # Return cached observations if not updating and cache exists. # This prevents double-pushing to delay buffers when compute() is called # multiple times per control step (e.g., in get_observations() after step()). @@ -318,8 +326,17 @@ def compute( return obs_buffer def compute_group( - self, group_name: str, update_history: bool = False + self, group_name: str, update_history: bool = False, peek: bool = False ) -> torch.Tensor | dict[str, torch.Tensor]: + """Compute observations for a single group. + + When ``peek=True``, computes a read-only snapshot of the current observation + *without mutating any state*: delay and history buffers are queried via their + ``peek_append`` methods (no pointer advance, no lag resampling), and noise is + skipped (it would draw from the global RNG and perturb the training + trajectory). This is used to capture true terminal observations for done + environments under auto-reset. Shapes are identical to the non-peek result. + """ group_cfg = self.cfg[group_name] group_term_names = self._group_obs_term_names[group_name] group_obs: dict[str, torch.Tensor] = {} @@ -328,10 +345,14 @@ def compute_group( ) for term_name, term_cfg in obs_terms: obs: torch.Tensor = term_cfg.func(self._env, **term_cfg.params).clone() - if isinstance(term_cfg.noise, noise_cfg.NoiseCfg): - obs = term_cfg.noise.apply(obs) - elif isinstance(term_cfg.noise, noise_cfg.NoiseModelCfg): - obs = self._group_obs_class_instances[group_name][term_name](obs) + # Noise is skipped on peek: NoiseCfg draws from the global RNG (which would + # perturb the training trajectory) and NoiseModelCfg carries per-step state + # (which would be advanced twice). Terminal observations are noise-free. + if not peek: + if isinstance(term_cfg.noise, noise_cfg.NoiseCfg): + obs = term_cfg.noise.apply(obs) + elif isinstance(term_cfg.noise, noise_cfg.NoiseModelCfg): + obs = self._group_obs_class_instances[group_name][term_name](obs) if term_cfg.clip: obs = obs.clip_(min=term_cfg.clip[0], max=term_cfg.clip[1]) if term_cfg.scale is not None: @@ -347,17 +368,24 @@ def compute_group( if term_cfg.delay_max_lag > 0: delay_buffer = self._group_obs_term_delay_buffer[group_name][term_name] - delay_buffer.append(obs) - obs = delay_buffer.compute() + if peek: + obs = delay_buffer.peek_append(obs) + else: + delay_buffer.append(obs) + obs = delay_buffer.compute() if term_cfg.history_length > 0: circular_buffer = self._group_obs_term_history_buffer[group_name][term_name] - if update_history or not circular_buffer.is_initialized: - circular_buffer.append(obs) + if peek: + history = circular_buffer.peek_append(obs) + else: + if update_history or not circular_buffer.is_initialized: + circular_buffer.append(obs) + history = circular_buffer.buffer if term_cfg.flatten_history_dim: - group_obs[term_name] = circular_buffer.buffer.reshape(self._env.num_envs, -1) + group_obs[term_name] = history.reshape(self._env.num_envs, -1) else: - group_obs[term_name] = circular_buffer.buffer + group_obs[term_name] = history else: group_obs[term_name] = obs diff --git a/src/mjlab/utils/buffers/circular_buffer.py b/src/mjlab/utils/buffers/circular_buffer.py index 1031944031..f1f9b40118 100644 --- a/src/mjlab/utils/buffers/circular_buffer.py +++ b/src/mjlab/utils/buffers/circular_buffer.py @@ -214,6 +214,49 @@ def append(self, data: torch.Tensor) -> None: self._num_pushes += 1 + def peek_append(self, data: torch.Tensor) -> torch.Tensor: + """Return the window :meth:`buffer` would yield after ``append(data)``. + + This is a pure, read-only counterpart to :meth:`append`: it computes the + chronological history window that *would* result from appending ``data``, + but mutates no state (pointer, push counts, and stored frames are all left + untouched). + + The buffer's single pointer is shared across the batch, so a real partial + append cannot update a subset of rows without shifting every other row's + window. ``peek_append`` sidesteps that entirely by never advancing the + pointer, which makes it safe for computing terminal observations on a + subset of (done) environments without disturbing the live history that the + rest of the batch continues to accumulate. + + Args: + data: Tensor of shape (batch_size, ...) to hypothetically append. + + Returns: + Chronological window (oldest to newest) of shape (batch_size, max_len, + ...), where the newest frame is ``data``. + """ + if data.shape[0] != self._batch_size: + raise ValueError(f"Expected batch size {self._batch_size}, got {data.shape[0]}") + data = data.to(self._device) + + # Never appended: a real append would backfill every slot with this first + # frame, so the window is `data` repeated across the time axis. Materialize + # (repeat, not expand) so the result never aliases `data`. + if self._buffer is None: + return data.unsqueeze(1).repeat(1, self._max_len, *([1] * (data.ndim - 1))) + + # Normal case: drop the oldest frame and append `data` as the newest. + old_window = self.buffer # (batch, max_len, ...), chronological. + window = torch.cat([old_window[:, 1:], data.unsqueeze(1)], dim=1) + + # Rows not yet pushed since a reset get backfilled with `data`, matching the + # first-push behavior in append(). + is_first_push = self._num_pushes == 0 + if torch.any(is_first_push): + window[is_first_push] = data[is_first_push].unsqueeze(1) + return window + def __getitem__(self, key: torch.Tensor | int) -> torch.Tensor: """Retrieve lagged frames per batch (LIFO). diff --git a/src/mjlab/utils/buffers/delay_buffer.py b/src/mjlab/utils/buffers/delay_buffer.py index 642be6f8bd..708c9ae691 100644 --- a/src/mjlab/utils/buffers/delay_buffer.py +++ b/src/mjlab/utils/buffers/delay_buffer.py @@ -226,6 +226,33 @@ def append(self, data: torch.Tensor) -> None: """ self._buffer.append(data) + def peek_append(self, data: torch.Tensor) -> torch.Tensor: + """Return the delayed obs :meth:`compute` would yield after ``append(data)``. + + Pure, read-only counterpart to ``append`` followed by ``compute``: it + mutates no state and, crucially, does *not* resample lags (so it draws no + RNG and leaves the step counter untouched). The most recently sampled + per-env lags are reused. This makes it safe for computing terminal + observations on a subset of environments without perturbing the live delay + process shared by the rest of the batch. See + :meth:`CircularBuffer.peek_append`. + + Args: + data: Observation tensor of shape (batch_size, ...). + + Returns: + Delayed observation of shape (batch_size, ...). + """ + window = self._buffer.peek_append(data) # (batch, buf_len, ...), chronological. + buf_len = window.shape[1] + # Valid history length after the hypothetical append, capped at capacity. + length_post = (self._buffer.current_length + 1).clamp_max(buf_len) + valid_lags = torch.minimum(self._current_lags, length_post - 1).clamp_min(0) + # Newest frame sits at window index buf_len - 1; lag L is L frames before it. + gather_idx = (buf_len - 1) - valid_lags + batch_idx = torch.arange(self.batch_size, device=self.device) + return window[batch_idx, gather_idx] + def compute(self) -> torch.Tensor: """Compute delayed observation for current step. diff --git a/tests/test_circular_buffer.py b/tests/test_circular_buffer.py index f571e9a112..5b5eeb5cc2 100644 --- a/tests/test_circular_buffer.py +++ b/tests/test_circular_buffer.py @@ -192,3 +192,104 @@ def test_dtype_and_device_preserved(device): buffer.append(x) assert buffer.buffer.dtype == torch.float32 assert buffer.buffer.device.type == torch.device(device).type + + +# peek_append: read-only counterpart to append. + + +def _buffer_state(buffer: CircularBuffer): + """Snapshot the mutable state of a buffer for no-mutation assertions.""" + return ( + buffer._pointer, + buffer._num_pushes.clone(), + None if buffer._buffer is None else buffer._buffer.clone(), + ) + + +def _assert_state_unchanged(buffer: CircularBuffer, before) -> None: + pointer, num_pushes, storage = before + assert buffer._pointer == pointer + assert torch.equal(buffer._num_pushes, num_pushes) + if storage is None: + assert buffer._buffer is None + else: + assert buffer._buffer is not None + assert torch.equal(buffer._buffer, storage) + + +@pytest.mark.parametrize("max_len", [1, 3, 4]) +@pytest.mark.parametrize("num_appends", [0, 1, 2, 5, 7]) +def test_peek_append_matches_real_append(device, max_len, num_appends): + """peek_append yields exactly the window a real append would, mutating nothing. + + This is the core correctness guarantee: the terminal-observation snapshot uses + peek_append instead of append so the shared pointer is never advanced. We prove + it by comparing against a clone that performs the real append. + """ + torch.manual_seed(0) + batch_size = 3 + peeked = CircularBuffer(max_len=max_len, batch_size=batch_size, device=device) + for _ in range(num_appends): + peeked.append(torch.randn(batch_size, 2, device=device)) + + new_frame = torch.randn(batch_size, 2, device=device) + + # Reference: a clone that actually appends. + reference = CircularBuffer(max_len=max_len, batch_size=batch_size, device=device) + if peeked._buffer is not None: + reference._buffer = peeked._buffer.clone() + reference._pointer = peeked._pointer + reference._num_pushes = peeked._num_pushes.clone() + reference.append(new_frame) + + before = _buffer_state(peeked) + window = peeked.peek_append(new_frame) + + assert window.shape == (batch_size, max_len, 2) + assert torch.allclose(window, reference.buffer) + # peek_append must not mutate the buffer it was called on. + _assert_state_unchanged(peeked, before) + + +def test_peek_append_uninitialized_backfills(device): + """peek_append on a fresh buffer returns the frame repeated across time.""" + buffer = CircularBuffer(max_len=3, batch_size=2, device=device) + frame = torch.tensor([[5.0], [10.0]], device=device) + + window = buffer.peek_append(frame) + + assert window.shape == (2, 3, 1) + assert torch.allclose(window[0], torch.tensor([[5.0], [5.0], [5.0]], device=device)) + assert torch.allclose( + window[1], torch.tensor([[10.0], [10.0], [10.0]], device=device) + ) + # Still uninitialized: no allocation happened. + assert buffer._buffer is None + assert not buffer.is_initialized + + +def test_peek_append_backfills_reset_rows(device): + """A row reset since its last push is backfilled with the peeked frame.""" + buffer = CircularBuffer(max_len=3, batch_size=2, device=device) + buffer.append(torch.tensor([[1.0], [10.0]], device=device)) + buffer.append(torch.tensor([[2.0], [20.0]], device=device)) + buffer.reset(batch_ids=torch.tensor([1], device=device)) + + before = _buffer_state(buffer) + window = buffer.peek_append(torch.tensor([[3.0], [50.0]], device=device)) + + # Row 0 (not reset): oldest dropped, newest appended -> [1, 2, 3]. + assert torch.allclose(window[0], torch.tensor([[1.0], [2.0], [3.0]], device=device)) + # Row 1 (reset, num_pushes==0): backfilled with 50. + assert torch.allclose( + window[1], torch.tensor([[50.0], [50.0], [50.0]], device=device) + ) + _assert_state_unchanged(buffer, before) + + +def test_peek_append_batch_size_validation(device): + """peek_append rejects a mismatched batch size.""" + buffer = CircularBuffer(max_len=2, batch_size=2, device=device) + buffer.append(torch.tensor([[1.0], [2.0]], device=device)) + with pytest.raises(ValueError, match="batch size"): + buffer.peek_append(torch.tensor([[1.0]], device=device)) diff --git a/tests/test_delay_buffer.py b/tests/test_delay_buffer.py index 53e0b14677..7c894c53df 100644 --- a/tests/test_delay_buffer.py +++ b/tests/test_delay_buffer.py @@ -298,3 +298,63 @@ def test_delay_buffer_validation(device): with pytest.raises(ValueError, match="update_period must be >= 0"): DelayBuffer(min_lag=0, max_lag=3, batch_size=1, update_period=-1, device=device) + + +# peek_append: read-only counterpart to append + compute. + + +def test_peek_append_matches_real_compute_constant_lag(device): + """peek_append equals a real append+compute (constant lag), mutating nothing. + + With min_lag == max_lag, the per-step resampling in compute() is + deterministic, so peek_append (which reuses the current lags and resamples + nothing) must produce identical values to a real append+compute. + """ + torch.manual_seed(0) + batch_size = 3 + peeked = DelayBuffer(min_lag=2, max_lag=2, batch_size=batch_size, device=device) + # Warm the buffer through several real steps so lags/state have settled. + for i in range(5): + peeked.append(torch.full((batch_size, 2), float(i), device=device)) + peeked.compute() + + new_frame = torch.randn(batch_size, 2, device=device) + + assert peeked._buffer._buffer is not None + + # Reference: clone the internal state and perform the real append+compute. + reference = DelayBuffer(min_lag=2, max_lag=2, batch_size=batch_size, device=device) + reference._buffer._buffer = peeked._buffer._buffer.clone() + reference._buffer._pointer = peeked._buffer._pointer + reference._buffer._num_pushes = peeked._buffer._num_pushes.clone() + reference._current_lags = peeked._current_lags.clone() + reference._step_count = peeked._step_count.clone() + reference.append(new_frame) + expected = reference.compute() + + # Snapshot mutable state to assert peek_append leaves it untouched. + pointer = peeked._buffer._pointer + pushes = peeked._buffer._num_pushes.clone() + storage = peeked._buffer._buffer.clone() + lags = peeked._current_lags.clone() + step_count = peeked._step_count.clone() + + result = peeked.peek_append(new_frame) + + assert torch.allclose(result, expected) + assert peeked._buffer._pointer == pointer + assert torch.equal(peeked._buffer._num_pushes, pushes) + assert torch.equal(peeked._buffer._buffer, storage) + assert torch.equal(peeked._current_lags, lags) + assert torch.equal(peeked._step_count, step_count) + + +def test_peek_append_clamps_to_available_history(device): + """peek_append clamps the lag to available history early in an episode.""" + buffer = DelayBuffer(min_lag=3, max_lag=3, batch_size=1, device=device) + buffer.append(torch.tensor([[1.0]], device=device)) + buffer.compute() # current_lags becomes 3, but only 1 frame exists. + + # One real frame in history; peeking a second can return at most the oldest. + result = buffer.peek_append(torch.tensor([[2.0]], device=device)) + assert torch.allclose(result, torch.tensor([[1.0]], device=device)) diff --git a/tests/test_terminal_observation.py b/tests/test_terminal_observation.py new file mode 100644 index 0000000000..1d7c4d33a0 --- /dev/null +++ b/tests/test_terminal_observation.py @@ -0,0 +1,339 @@ +"""Tests for capture_terminal_observations (true terminal obs under auto-reset). + +See ManagerBasedRlEnvCfg.capture_terminal_observations. The feature stores the +real terminal observation (s_{t+1} before reset) for done environments in +extras["terminal"] while keeping auto-reset enabled. +""" + +import pytest +import torch +from conftest import get_test_device + +from mjlab.envs import ManagerBasedRlEnv +from mjlab.managers.termination_manager import TerminationTermCfg +from mjlab.tasks.cartpole.cartpole_env_cfg import cartpole_balance_env_cfg +from mjlab.tasks.velocity.config.go1.env_cfgs import unitree_go1_flat_env_cfg + + +@pytest.fixture(scope="module") +def device(): + return get_test_device() + + +def _t(value) -> torch.Tensor: + """Narrow an obs-group value to a Tensor (cartpole groups are concatenated).""" + assert isinstance(value, torch.Tensor) + return value + + +def _staggered_timeout(env: ManagerBasedRlEnv) -> torch.Tensor: + """Time out env i one step earlier than env i+1, producing partial resets. + + Depends only on episode_length_buf, the env index, and max_episode_length, so + it is valid at the init-time shape-validation call (where the counter is 0 and + this returns all-False). + """ + idx = torch.arange(env.num_envs, device=env.device) + return env.episode_length_buf + idx >= env.max_episode_length + + +def _make_cfg( + *, + auto_reset: bool = True, + capture: bool = False, + num_envs: int = 4, + corruption: bool = False, + history: int = 0, + staggered: bool = False, +): + cfg = cartpole_balance_env_cfg() + cfg.episode_length_s = 0.5 # 10 steps at dt=0.05. + cfg.scene.num_envs = num_envs + cfg.auto_reset = auto_reset + cfg.capture_terminal_observations = capture + cfg.observations["actor"].enable_corruption = corruption + if history > 0: + for group in cfg.observations.values(): + group.history_length = history + if staggered: + cfg.terminations = { + "staggered": TerminationTermCfg(func=_staggered_timeout, time_out=True) + } + return cfg + + +def _step_until_done(env): + action = torch.zeros((env.num_envs, 1), device=env.device) + for _ in range(env.max_episode_length + 5): + result = env.step(action) + terminated, truncated = result[2], result[3] + if (terminated | truncated).any(): + return result + pytest.fail("No env terminated within max_episode_length steps") + + +# Presence / contract. + + +def test_disabled_by_default_no_terminal_key(device): + """Without the flag, extras never gains a 'terminal' key, even on reset steps.""" + env = ManagerBasedRlEnv(cfg=_make_cfg(capture=False), device=device) + env.reset() + _, _, terminated, truncated, extras = _step_until_done(env) + assert (terminated | truncated).any() # we did hit a reset step + assert "terminal" not in extras + env.close() + + +def test_terminal_present_only_on_reset_steps(device): + """extras['terminal'] appears exactly on steps where >=1 env resets.""" + env = ManagerBasedRlEnv(cfg=_make_cfg(capture=True, staggered=True), device=device) + env.reset() + action = torch.zeros((env.num_envs, 1), device=env.device) + saw_reset_step = saw_non_reset_step = False + for _ in range(env.max_episode_length + 5): + _, _, terminated, truncated, extras = env.step(action) + done = terminated | truncated + if done.any(): + saw_reset_step = True + assert "terminal" in extras + else: + saw_non_reset_step = True + assert "terminal" not in extras + assert saw_reset_step and saw_non_reset_step + env.close() + + +def test_terminal_env_ids_and_shapes(device): + """env_ids match the done mask; observation rows mirror groups with [k] rows.""" + env = ManagerBasedRlEnv( + cfg=_make_cfg(capture=True, history=3, staggered=True), device=device + ) + env.reset() + obs, _, terminated, truncated, extras = _step_until_done(env) + done = terminated | truncated + done_ids = done.nonzero(as_tuple=False).squeeze(-1) + + terminal = extras["terminal"] + assert torch.equal(terminal["env_ids"], done_ids) + + num_done = int(done.sum().item()) + assert set(terminal["observations"].keys()) == set(obs.keys()) + for group, term_obs in terminal["observations"].items(): + returned = obs[group] + assert isinstance(term_obs, torch.Tensor) and isinstance(returned, torch.Tensor) + # Same trailing shape as the returned obs (e.g. history is flattened in). + assert term_obs.shape == (num_done, *returned.shape[1:]) + env.close() + + +def test_terminal_obs_mirrors_non_concatenated_group(device): + """A non-concatenated group yields a term dict (not a tensor) in extras. + + Exercises the dict branch of both the peek observation pass and the env-id + selector, which the concatenated cartpole groups otherwise leave untested. + """ + cfg = _make_cfg(capture=True, history=3, staggered=True) + cfg.observations["critic"].concatenate_terms = False + env = ManagerBasedRlEnv(cfg=cfg, device=device) + env.reset() + obs, _, terminated, truncated, extras = _step_until_done(env) + num_done = int((terminated | truncated).sum().item()) + + term_obs = extras["terminal"]["observations"] + # actor stays concatenated (a tensor); critic is now a dict of per-term rows. + assert isinstance(term_obs["actor"], torch.Tensor) + returned_critic = obs["critic"] + assert isinstance(term_obs["critic"], dict) and isinstance(returned_critic, dict) + assert set(term_obs["critic"].keys()) == set(returned_critic.keys()) + for name, term in term_obs["critic"].items(): + assert term.shape == (num_done, *returned_critic[name].shape[1:]) + env.close() + + +def test_no_effect_when_auto_reset_false(device): + """The flag is a no-op under auto_reset=False (returned obs is already terminal).""" + env = ManagerBasedRlEnv(cfg=_make_cfg(auto_reset=False, capture=True), device=device) + env.reset() + _, _, terminated, truncated, extras = _step_until_done(env) + assert (terminated | truncated).any() + assert "terminal" not in extras + env.close() + + +def test_terminal_differs_from_post_reset_obs(device): + """Terminal obs (pre-reset) differs from the returned post-reset obs.""" + env = ManagerBasedRlEnv(cfg=_make_cfg(capture=True, staggered=True), device=device) + env.reset(seed=0) + obs, _, terminated, truncated, extras = _step_until_done(env) + done_ids = (terminated | truncated).nonzero(as_tuple=False).squeeze(-1) + for group, terminal in extras["terminal"]["observations"].items(): + returned = _t(obs[group])[done_ids] + # Post-reset obs come from a freshly randomized initial state, so they must + # differ from the terminal state that triggered the reset. + assert not torch.equal(_t(terminal), returned) + env.close() + + +# Correctness: equivalence to auto_reset=False. + + +def _terminal_at_first_reset(cfg, seed, device): + """Run a capture env alone to its first reset; return (ids, terminal obs dict). + + Run in isolation so the (global) RNG stream is identical to any other run up + to the first reset: with corruption off, no RNG is consumed between the + initial reset and the first episode boundary. + """ + env = ManagerBasedRlEnv(cfg=cfg, device=device) + env.reset(seed=seed) + action = torch.zeros((env.num_envs, 1), device=env.device) + for _ in range(env.max_episode_length + 5): + _, _, terminated, truncated, extras = env.step(action) + done = terminated | truncated + if done.any(): + ids = done.nonzero(as_tuple=False).squeeze(-1) + out = ( + ids.clone(), + {g: _t(v).clone() for g, v in extras["terminal"]["observations"].items()}, + ) + env.close() + return out + env.close() + pytest.fail("No env terminated") + + +def _returned_at_first_reset(cfg, seed, device): + """Run an auto_reset=False env alone to its first reset; return (ids, obs[ids]).""" + env = ManagerBasedRlEnv(cfg=cfg, device=device) + env.reset(seed=seed) + action = torch.zeros((env.num_envs, 1), device=env.device) + for _ in range(env.max_episode_length + 5): + obs, _, terminated, truncated, _ = env.step(action) + done = terminated | truncated + if done.any(): + ids = done.nonzero(as_tuple=False).squeeze(-1) + out = (ids.clone(), {g: _t(v)[ids].clone() for g, v in obs.items()}) + env.close() + return out + env.close() + pytest.fail("No env terminated") + + +@pytest.mark.parametrize("history", [0, 2]) +def test_terminal_matches_auto_reset_false(device, history): + """The captured terminal obs equals what auto_reset=False would return. + + This is the gold-standard correctness check, including the history path. With + observation noise disabled the whole pipeline is deterministic, so equivalence + is exact. Compared at the first reset to keep the shared global RNG in sync. + """ + seed = 123 + capture_ids, terminal = _terminal_at_first_reset( + _make_cfg(auto_reset=True, capture=True, corruption=False, history=history), + seed, + device, + ) + ref_ids, returned = _returned_at_first_reset( + _make_cfg(auto_reset=False, corruption=False, history=history), + seed, + device, + ) + + assert torch.equal(capture_ids, ref_ids) + assert set(terminal.keys()) == set(returned.keys()) + for group in returned: + assert torch.allclose(terminal[group], returned[group], atol=1e-6), ( + f"terminal obs mismatch for group '{group}'" + ) + + +# Correctness: enabling the feature must not change the trajectory. + + +def test_capture_does_not_perturb_trajectory(device): + """Returned obs and rewards are identical with the flag on vs off. + + Stresses every state channel the WIP corrupted: observation noise (global + RNG), history buffers, and partial (staggered) resets. If peek drew RNG or + mutated a buffer, the two runs would diverge. + """ + steps = 30 + seed = 7 + + def run(capture: bool): + env = ManagerBasedRlEnv( + cfg=_make_cfg( + capture=capture, + num_envs=4, + corruption=True, + history=3, + staggered=True, + ), + device=device, + ) + env.reset(seed=seed) + action = torch.zeros((env.num_envs, 1), device=env.device) + obs_log, rew_log = [], [] + for _ in range(steps): + obs, rew, _, _, _ = env.step(action) + obs_log.append({g: _t(v).clone() for g, v in obs.items()}) + rew_log.append(rew.clone()) + env.close() + return obs_log, rew_log + + obs_off, rew_off = run(capture=False) + obs_on, rew_on = run(capture=True) + + for t in range(steps): + assert torch.equal(rew_off[t], rew_on[t]), f"reward diverged at step {t}" + for group in obs_off[t]: + assert torch.equal(obs_off[t][group], obs_on[t][group]), ( + f"obs group '{group}' diverged at step {t}" + ) + + +# Correctness: the safety invariant the extra forward() relies on. + + +@pytest.mark.slow +def test_forward_is_a_fixed_point_under_contact(): + """Guard the invariant that makes capture's extra forward() non-perturbing. + + When capturing, step() runs an extra forward()+sense() at the terminal state + before the unconditional post-reset forward(). That is safe only because + forward() is a fixed point: it copies qacc into qacc_warmstart, and re-solving + from that warm start reproduces the same qacc, so the extra call cannot change + the result of the following forward(). If a solver change ever under-converged + this, enabling the flag would silently perturb training. Assert it directly on + a contact-rich env, in dynamic high-force states. + + Pinned to CPU: warp's parallel contact kernels use nondeterministic reductions + on GPU, so bit-identity of two forwards only holds on CPU (and on GPU the + feature's effect is already within the sim's inherent nondeterminism). + """ + device = "cpu" + cfg = unitree_go1_flat_env_cfg() + cfg.scene.num_envs = 2 + env = ManagerBasedRlEnv(cfg=cfg, device=device) + env.reset(seed=1) + gen = torch.Generator(device=device) + gen.manual_seed(0) + dim = env.action_manager.total_action_dim + for _ in range(15): + # Strong random actions drive dynamic, high-force ground contacts. + action = (torch.rand((env.num_envs, dim), generator=gen, device=device) * 2 - 1) * 3 + env.step(action) + + env.sim.forward() + env.sim.sense() + sensordata = env.sim.data.sensordata.clone() + qacc = env.sim.data.qacc.clone() + + env.sim.forward() + env.sim.sense() + assert torch.equal(sensordata, env.sim.data.sensordata), ( + "forward() moved sensordata" + ) + assert torch.equal(qacc, env.sim.data.qacc), "forward() moved qacc" + env.close()