9fe9d27865
Consolidate next-stage docs, archive superseded narratives, and preserve frozen document inputs. Retain invalid P2 pairs, partial RootXY training, failed attempts and nonselectable checkpoints as explicit limitations. Include supervised evaluation/training, desktop GPU admission, bounded evidence and atomic-publication monitor repairs with CPU regressions. Ignore local dotenv configuration. Validation: 1403 passed, 6 skipped for contact-cycle and related contracts/CLI tests. Full-suite legacy ABI and registration tests require Python 3.12/Torch 2.10/pxr; no runtime checks relaxed. No new physical run or accepted skill.
739 lines
34 KiB
Python
739 lines
34 KiB
Python
"""CPU spies of the actual evaluation state/step, never Kit or physics launches.
|
|
|
|
Only Isaac imports/backend operations and IK solve are mocked. Frozen latch,
|
|
command, action composition, delays, 46D policy and runtime code are exercised.
|
|
"""
|
|
import importlib
|
|
import json
|
|
import sys
|
|
import types
|
|
from pathlib import Path
|
|
from types import SimpleNamespace as NS
|
|
|
|
import pytest
|
|
import torch
|
|
from SkillFactory.legs.contact_cycle.mechanics import inverse_rotate, rotate
|
|
from SkillFactory.legs.contact_cycle_sim.evaluation_protocol import load_protocol
|
|
from SkillFactory.legs.spec import load_bundle
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
TASK = 'SkillFactory.tasks.manager_based.'
|
|
|
|
|
|
@pytest.fixture
|
|
def runtime(monkeypatch): # noqa: C901 - local spy classes keep Isaac mocks scoped to each test
|
|
# Test-local namespace imports only; never passed to frozen verify_bundle.
|
|
for relative in ('tasks', 'tasks.manager_based', 'tasks.manager_based.contact_cycle',
|
|
'tasks.manager_based.contact_cycle_sim', 'tasks.manager_based.loaded_leg',
|
|
'tasks.manager_based.single_leg_anchor', 'tasks.manager_based.single_leg_anchor.mdp'):
|
|
name = 'SkillFactory.' + relative
|
|
module = types.ModuleType(name)
|
|
module.__path__ = [str(ROOT / 'source/SkillFactory/SkillFactory' / relative.replace('.', '/'))]
|
|
monkeypatch.setitem(sys.modules, name, module)
|
|
names = [TASK + p for p in ('loaded_leg.mdp', 'contact_cycle.mdp', 'contact_cycle_sim.mdp',
|
|
'contact_cycle_sim.evaluation_mdp', 'contact_cycle_sim.evaluation_env')]
|
|
for name in names:
|
|
monkeypatch.delitem(sys.modules, name, raising=False)
|
|
|
|
class Sim:
|
|
is_rendering = False
|
|
|
|
def __init__(self):
|
|
self.calls = 0
|
|
|
|
def step(self, render=False):
|
|
self.calls += 1
|
|
|
|
def forward(self):
|
|
pass
|
|
|
|
def reset(self):
|
|
pass
|
|
|
|
class Composer:
|
|
def __init__(self):
|
|
self.set_calls = self.reset_calls = 0
|
|
self.fail = False
|
|
self.last = None
|
|
|
|
def reset(self, **kwargs):
|
|
self.reset_calls += 1
|
|
|
|
def set_forces_and_torques_index(self, **kwargs):
|
|
self.set_calls += 1
|
|
self.reset(env_ids=None) # installed setter's legitimate internal replacement
|
|
self.last = kwargs
|
|
if self.fail:
|
|
raise RuntimeError('composer setter failure')
|
|
|
|
class Robot:
|
|
num_bodies = 4
|
|
body_names = ['virtual_payload', 'FL_thigh', 'FL_calf', 'anchor']
|
|
actuators = {}
|
|
|
|
def __init__(self):
|
|
self.data = NS(body_mass=NS(torch=torch.tensor([[4., 0., 0., 0.]])),
|
|
applied_torque=NS(torch=torch.zeros(1, 3)),
|
|
joint_vel=NS(torch=torch.zeros(1, 3)), root_state_w=NS(torch=torch.zeros(1, 13)))
|
|
self.permanent_wrench_composer = Composer()
|
|
self.data.body_com_pos_w = NS(torch=torch.zeros(1, 4, 3))
|
|
self.data.body_link_pos_w = NS(torch=torch.zeros(1, 4, 3))
|
|
self.data.body_link_quat_w = NS(torch=torch.tensor([[[0., 0., 0., 1.]]]).expand(1, 4, 4))
|
|
|
|
def write_root_pose_to_sim(self, *args):
|
|
pass
|
|
|
|
def reset(self):
|
|
pass
|
|
|
|
class Scene(dict):
|
|
sensors = {}
|
|
|
|
def __init__(self):
|
|
super().__init__(robot=Robot())
|
|
self.env_origins = torch.zeros(1, 3)
|
|
self.updates = 0
|
|
|
|
def update(self, dt):
|
|
self.updates += 1
|
|
|
|
def write_data_to_sim(self):
|
|
pass
|
|
|
|
def reset(self, *args):
|
|
self['robot'].reset()
|
|
self['robot'].write_root_pose_to_sim(None)
|
|
|
|
class BoundedAction:
|
|
def __init__(self, cfg, env):
|
|
self._env = env
|
|
self.raw_actions = torch.zeros(1, 3)
|
|
self.processed_actions = torch.zeros(1, 3)
|
|
|
|
def process_actions(self, action):
|
|
self.raw_actions.copy_(action)
|
|
self.processed_actions.copy_(action)
|
|
|
|
def apply_actions(self):
|
|
pass
|
|
|
|
class Manager:
|
|
active_terms = ()
|
|
available_modes = ()
|
|
|
|
def reset(self, *a, **kw):
|
|
return {}
|
|
|
|
def compute(self, **kw):
|
|
return torch.zeros(1)
|
|
|
|
class BaseEnv:
|
|
def __init__(self, cfg):
|
|
self.cfg, self.device, self.num_envs = cfg, 'cpu', 1
|
|
self.physics_dt, self.step_dt = .002, .01
|
|
self.sim, self.scene = Sim(), Scene()
|
|
self.sim.step() # constructor step MUST be counted
|
|
self._physics_handles_decimation = False
|
|
self._sim_step_counter = self.common_step_counter = 0
|
|
self.episode_length_buf = torch.zeros(1, dtype=torch.long)
|
|
self.extras = {}
|
|
self.render_enabled = False
|
|
for name in ('recorder', 'event', 'reward', 'termination', 'curriculum', 'command'):
|
|
setattr(self, name + '_manager', Manager())
|
|
mdp = importlib.import_module(TASK + 'contact_cycle_sim.evaluation_mdp')
|
|
self.term = mdp.EvaluationResidualAction(None, self)
|
|
self.action_manager = Manager()
|
|
self.action_manager.action = torch.zeros(1, 3)
|
|
self.action_manager.prev_action = torch.zeros(1, 3)
|
|
|
|
def process(action):
|
|
self.action_manager.prev_action.copy_(self.action_manager.action)
|
|
self.action_manager.action.copy_(action)
|
|
self.term.process_actions(action)
|
|
|
|
self.action_manager.process_action = process
|
|
self.action_manager.apply_action = self.term.apply_actions
|
|
self.observation_manager = Manager()
|
|
self.observation_manager.compute = lambda **kw: {'policy': self._cycle_state.policy()}
|
|
|
|
def reset(self, seed=None):
|
|
torch.manual_seed(seed)
|
|
self._reset_idx(torch.tensor([0]))
|
|
self.sim.step() # mocked reset warmup; not a rollout sample
|
|
return {}, {}
|
|
|
|
def _reset_idx(self, ids):
|
|
self.scene.reset(ids)
|
|
self.action_manager.reset(ids)
|
|
self.episode_length_buf.zero_()
|
|
|
|
def step(self, action):
|
|
raise AssertionError('Inherited autoreset step MUST NEVER RUN')
|
|
|
|
monkeypatch.setitem(sys.modules, 'isaaclab.envs', NS(ManagerBasedRLEnv=BaseEnv))
|
|
monkeypatch.setitem(sys.modules, 'isaaclab.utils.math', NS(
|
|
axis_angle_from_quat=lambda q: q[:, :3], quat_apply=rotate, quat_apply_inverse=inverse_rotate))
|
|
monkeypatch.setitem(sys.modules, TASK + 'single_leg_anchor.mdp.actions', NS(
|
|
BoundedJointPositionAction=BoundedAction))
|
|
loaded = importlib.import_module(TASK + 'loaded_leg.mdp')
|
|
|
|
def loaded_init(self, env):
|
|
self.env, self.bundle, self.robot = env, env.cfg.leg_bundle, env.scene['robot']
|
|
self.nominal = torch.tensor([j['q_default'] for j in self.bundle['spec']['joints']])
|
|
self.joints, self.payload, self.anchor = [0, 1, 2], 0, 3
|
|
self.effort = torch.tensor([j['limits']['effort'] for j in self.bundle['joints']])
|
|
self.stiffness = self.damping = torch.zeros(6)
|
|
self.wrench_limit = torch.ones(6) * 100
|
|
|
|
monkeypatch.setattr(loaded.LoadedState, '__init__', loaded_init)
|
|
monkeypatch.setattr(loaded.LoadedState, 'foot_center', lambda s: torch.tensor([[.1, -.1, .022]]))
|
|
monkeypatch.setattr(loaded.LoadedState, 'root', lambda s: (
|
|
torch.tensor([[0., 0., .3]]), torch.tensor([[0., 0., 0., 1.]]), torch.zeros(1, 3), torch.zeros(1, 3)))
|
|
monkeypatch.setattr(loaded.LoadedState, 'contact', lambda s: torch.tensor([[0., 0., 39.24]]))
|
|
monkeypatch.setattr(loaded.LoadedState, 'q', lambda s: s.nominal[None].clone())
|
|
monkeypatch.setattr(loaded.LoadedState, 'tilt', lambda s: torch.zeros(1))
|
|
base = importlib.import_module(TASK + 'contact_cycle.mdp')
|
|
monkeypatch.setattr(base.CycleState, 'raw_sensors', lambda s: torch.zeros(1, 18))
|
|
real_apply_loads = base.CycleState.apply_loads
|
|
monkeypatch.setattr(base.CycleState, 'apply_loads', lambda s:
|
|
s.robot.permanent_wrench_composer.set_forces_and_torques_index())
|
|
frozen = importlib.import_module(TASK + 'contact_cycle_sim.mdp')
|
|
|
|
class Controller:
|
|
def __init__(self, s, cfg):
|
|
self.s = s
|
|
self.q_ik = torch.ones(1, 3)
|
|
self.action = torch.zeros(1, 3)
|
|
self.refreshes = torch.zeros(1, dtype=torch.long)
|
|
|
|
def reset(self, ids):
|
|
self.q_ik[ids] = 1
|
|
self.refreshes[ids] = 0
|
|
self.refresh(ids)
|
|
|
|
def refresh(self, ids):
|
|
self.q_ik[ids] += .001
|
|
self.refreshes[ids] += 1
|
|
|
|
monkeypatch.setattr(frozen, 'ReferenceController', Controller)
|
|
mdp = importlib.import_module(TASK + 'contact_cycle_sim.evaluation_mdp')
|
|
env_module = importlib.import_module(TASK + 'contact_cycle_sim.evaluation_env')
|
|
def config():
|
|
return NS(
|
|
scene=NS(num_envs=1), sim=NS(dt=.002, render_interval=5), decimation=5, anchor_height=.3,
|
|
guide_enabled=True, auxiliary_enabled=True,
|
|
leg_bundle=load_bundle(ROOT / 'assets/generated/legs/go2_fl'),
|
|
cycle_parameters=json.loads((ROOT / 'configs/contact_cycle/go2_fl_v1.json').read_text()),
|
|
residual_parameters=json.loads((ROOT / 'configs/contact_cycle/simulation_residual_v1.json').read_text()),
|
|
support_parameters={'action_scale_rad': .5})
|
|
|
|
yield NS(env=env_module, mdp=mdp, base=base, frozen=frozen, config=config, Sim=Sim, Robot=Robot, Scene=Scene,
|
|
real_apply_loads=real_apply_loads)
|
|
for name in names:
|
|
sys.modules.pop(name, None)
|
|
|
|
|
|
def protocol(stage='P1'):
|
|
path = 'continuous_eval_p1_v1.json' if stage == 'P1' else 'bounded_diagnostic_p2_v1.json'
|
|
return load_protocol(ROOT / 'configs/contact_cycle' / path)
|
|
|
|
|
|
@pytest.fixture
|
|
def make_env(runtime):
|
|
opened = []
|
|
|
|
def make(stage='P1', trial=0, evidence=None, seam=None, safety=False):
|
|
hooks = runtime.env.RuntimeHooks(runtime.Sim, [(runtime.Robot, 'reset'),
|
|
(runtime.Robot, 'write_root_pose_to_sim'), (runtime.Scene, 'reset')])
|
|
hooks.__enter__()
|
|
opened.append(hooks)
|
|
events = []
|
|
|
|
def record(event, s):
|
|
events.append(event)
|
|
return () if evidence is None else evidence(event, s)
|
|
|
|
env = runtime.env.EvaluationContactCycleEnv(runtime.config(), protocol=protocol(stage), trial_index=trial,
|
|
runtime_hooks=hooks, evidence_callback=record, seam_callback=seam)
|
|
if not safety:
|
|
env._cycle_state.safety_causes = lambda: ()
|
|
env.reset()
|
|
return env, hooks, events
|
|
|
|
yield make
|
|
for hooks in reversed(opened):
|
|
hooks.__exit__()
|
|
|
|
|
|
def advance(env, controls):
|
|
result = None
|
|
for _ in range(controls):
|
|
result = env.step(torch.zeros(1, 3))
|
|
if result.stop:
|
|
break
|
|
return result
|
|
|
|
|
|
def test_evaluation_runtime_initial_reset_instrumentation_and_pose_guards(make_env, runtime):
|
|
env, hooks, events = make_env()
|
|
assert hooks.initialization_steps == hooks.completed_steps == 2
|
|
assert hooks.rollout_steps == 0 and hooks.reset_calls > 0 and hooks.pose_write_calls > 0
|
|
assert [e.kind for e in events] == ['initial'] and events[0].clock.global_tick == 0
|
|
assert events[0].held_control_index is None and env._cycle_state.policy().shape == (1, 46)
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.scene['robot'].write_root_pose_to_sim(None)
|
|
result = advance(env, 1)
|
|
assert result.stop.fatal and result.ticks_advanced == 0 and hooks.rollout_steps == 0
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.reset()
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env._cycle_state.controller.reset(torch.tensor([0]))
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env._cycle_state.reset(torch.tensor([0]))
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.sim.step()
|
|
|
|
|
|
def test_evaluation_runtime_substep_two_failure_collects_all_causes_no_three(make_env):
|
|
def evidence(event, s):
|
|
return ('gate_overlap',) if event.kind == 'tick' and event.clock.global_tick == 2 else ()
|
|
|
|
env, hooks, events = make_env(evidence=evidence)
|
|
env._cycle_state.safety_causes = lambda: ('swing_clearance', 'premature_separation') if int(
|
|
env._cycle_state.substeps[0]) == 2 else ()
|
|
result = advance(env, 1)
|
|
assert result.ticks_advanced == hooks.rollout_steps == 2
|
|
assert result.stop.causes == ('swing_clearance', 'premature_separation', 'gate_overlap')
|
|
assert result.stop.completed_controls == 0 and result.stop.partial_control_ticks == 2
|
|
assert [e.clock.global_tick for e in events if e.kind == 'tick'] == [1, 2]
|
|
assert env._cycle_state.controller.refreshes.item() == 1
|
|
|
|
|
|
def test_evaluation_runtime_complete_clock_seam_order_carry_and_no_double_refresh(make_env, runtime):
|
|
seam_records = []
|
|
|
|
def seam(event, s):
|
|
seam_records.append((event, s.controller.q_ik.clone(), s.sensor_delay.data.clone(),
|
|
s.action_delay.data.clone(), torch.get_rng_state().clone(),
|
|
s.acquisition.timer.clone(), s.schedule.integral.clone()))
|
|
return runtime.env.SeamDecision(True)
|
|
|
|
env, hooks, events = make_env(seam=seam)
|
|
s = env._cycle_state
|
|
anchors = s.anchors
|
|
advance(env, 899)
|
|
s.raw_sensors = lambda: torch.full((1, 18), .123)
|
|
s.schedule.integral[0, 0] = .4 # arbitrary carried scheduler memory, not event evidence
|
|
request = torch.tensor([[.4, .2, -.1]])
|
|
first = env.step(request)
|
|
assert first.stop is None
|
|
assert torch.equal(s.residual_request, request)
|
|
assert torch.equal(env.action_manager.action, request)
|
|
assert s.action_delay.data.abs().sum() > 0 and s.sensed.abs().sum() > 0
|
|
assert s.acquisition.acquired.item() and s.separation.armed.item()
|
|
assert events[-2].kind == 'tick' and events[-2].clock.local_tick == 4500
|
|
assert events[-2].command.phase == 5 and events[-2].command.progress == 1
|
|
assert events[-1].kind == 'next_reference' and events[-1].clock.local_tick == 0
|
|
assert events[-1].command.phase == 0 and events[-1].clock.cycle_index == 1
|
|
assert events[-2].reference_used_clock.global_tick == 4495
|
|
assert events[-1].reference_next_clock.global_tick == 4500
|
|
_, ik, sensor, action, rng, acquisition, integral = seam_records[0]
|
|
assert torch.equal(s.controller.q_ik, ik + .001)
|
|
assert torch.equal(sensor, s.sensor_delay.data) and torch.equal(action, s.action_delay.data)
|
|
assert torch.equal(rng, torch.get_rng_state())
|
|
assert torch.equal(acquisition, s.acquisition.timer) and torch.equal(integral, s.schedule.integral)
|
|
assert s.anchors is anchors
|
|
snapshots = (s.controller.q_ik.clone(), s.controller.refreshes.clone(), torch.get_rng_state())
|
|
assert torch.equal(s.policy(), s.policy())
|
|
assert torch.equal(snapshots[0], s.controller.q_ik) and torch.equal(snapshots[2], torch.get_rng_state())
|
|
reset_calls = hooks.reset_calls
|
|
last = advance(env, 1800)
|
|
assert last.stop.reason == 'protocol_complete' and not last.stop.fatal
|
|
assert last.stop.physics_ticks == hooks.rollout_steps == 13500
|
|
assert last.stop.completed_controls == 2700 and last.stop.completed_clock_cycles == 3
|
|
assert last.stop.partial_control_ticks == 0 and hooks.reset_calls == reset_calls
|
|
assert s.controller.refreshes.item() == 2700 # initial + 2699 next references, no final refresh
|
|
assert events[-1].kind == 'tick' and events[-1].clock.local_tick == 4500
|
|
ticks = [e for e in events if e.kind == 'tick']
|
|
assert [e.clock.global_tick for e in ticks] == list(range(1, 13501))
|
|
assert ticks[4500].clock.cycle_index == 1 and ticks[4500].clock.local_tick == 1
|
|
assert ticks[-1].held_control_index == 2699 and ticks[-1].action_origin_tick == 13495
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.step(torch.zeros(1, 3))
|
|
|
|
|
|
@pytest.mark.parametrize('decision', [None, False, 'denied'])
|
|
def test_evaluation_runtime_seam_admission_fails_closed(make_env, runtime, decision):
|
|
callback = None if decision is None else lambda e, s: (
|
|
False if decision is False else runtime.env.SeamDecision(False, ('reload_dwell',)))
|
|
env, hooks, events = make_env(seam=callback)
|
|
result = advance(env, 901)
|
|
assert result.stop.physics_ticks == 4500 and hooks.rollout_steps == 4500
|
|
assert result.stop.reason == 'failure' and events[-1].clock.local_tick == 4500
|
|
assert env._cycle_state.controller.refreshes.item() == 900
|
|
assert hooks.composer_buffer_replacements == 4500 # no extra setter/reset at seam
|
|
|
|
|
|
def test_evaluation_runtime_callback_failure_stops_even_at_partial_control(make_env):
|
|
def evidence(event, s):
|
|
if event.kind == 'tick' and event.clock.global_tick == 2:
|
|
raise OSError('storage failure')
|
|
return ()
|
|
|
|
env, hooks, _ = make_env(evidence=evidence)
|
|
result = advance(env, 1)
|
|
assert result.stop.fatal and 'recording_callback' in result.stop.causes[0]
|
|
assert hooks.rollout_steps == 2
|
|
|
|
|
|
def test_evaluation_runtime_frozen_coordinate_layout_and_anchor_immutability(make_env):
|
|
env, _, _ = make_env()
|
|
s = env._cycle_state
|
|
anchors = s.anchors
|
|
s.origin.add_(100)
|
|
s.base_origin.add_(100)
|
|
s.command_clock = s.protocol.evidence_clock(0, 2500)
|
|
c = s.protocol_command()
|
|
policy = s.policy()
|
|
assert torch.allclose(policy[0, 28:31], torch.tensor(c.actor_target_displacement_world_m))
|
|
assert tuple(c.root_target_world_m) == anchors.root_origin_world
|
|
assert policy.shape == (1, 46)
|
|
assert s.controller.refreshes.item() == 1
|
|
with pytest.raises(AttributeError):
|
|
s.anchors = anchors
|
|
|
|
|
|
def test_evaluation_runtime_original_and_physical_overlapping_safety(make_env):
|
|
env, _, _ = make_env(safety=True)
|
|
s = env._cycle_state
|
|
s.command_clock = s.protocol.evidence_clock(0, 2400)
|
|
s.robot.data.applied_torque.torch.fill_(100000)
|
|
s.guide[..., 2] = 1
|
|
s.separation.premature[:] = True
|
|
causes = s.safety_causes()
|
|
assert {'swing_clearance', 'premature_separation', 'hardware_effort_velocity', 'guide_vertical'} <= set(causes)
|
|
s.robot.data.root_state_w.torch[0, 0] = float('nan')
|
|
assert 'nonfinite' in s.safety_causes()
|
|
|
|
|
|
def test_p2_rows_restore_first_constructor_command_not_previous_terminal_foot(make_env, runtime, monkeypatch):
|
|
env, hooks, _ = make_env(stage='P2')
|
|
original = env._cycle_state.origin.clone()
|
|
root = env._cycle_state.base_origin.clone()
|
|
for row in range(19):
|
|
s = env._cycle_state
|
|
s.safety_causes = lambda: ('functional_failure',)
|
|
stopped = advance(env, 1).stop
|
|
env.acknowledge_finalized(stopped)
|
|
# Emulate the preceding physical terminal foot used by the new constructor.
|
|
dirty = original + torch.tensor([[.1 + row * .01, -.07, .02]])
|
|
monkeypatch.setattr(runtime.base.LoadedState, 'foot_center', lambda _: dirty.clone())
|
|
s.origin.add_(1.) # aliasing an old state must not change canonical commands
|
|
env.begin_trial(env.evaluation_protocol, row + 1)
|
|
new = env._cycle_state
|
|
assert torch.equal(new.origin, original)
|
|
assert torch.equal(new.base_origin, root)
|
|
assert new.anchors.anchor_a_world == tuple(original[0].tolist())
|
|
assert not torch.equal(new.origin, dirty)
|
|
assert hooks.initialization_steps == 21 # no added physical reset/step
|
|
assert hooks.rollout_steps == 19
|
|
|
|
|
|
def test_evaluation_runtime_p2_lifecycle_order_finalization_and_descriptor_install(make_env, runtime):
|
|
env, hooks, _ = make_env(stage='P2')
|
|
p = env.evaluation_protocol
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.begin_trial(p, 1)
|
|
for row in range(20):
|
|
s = env._cycle_state
|
|
s.safety_causes = lambda: ('functional_failure',)
|
|
assert int(s.scenarios[0]) == p.trial(row).scenario_id
|
|
assert s.trial.event_indices == p.trial(row).event_indices
|
|
assert s.sampler is None and s.schedule.commands is None
|
|
with pytest.raises(RuntimeError):
|
|
s.set_sampler(None)
|
|
result = advance(env, 1)
|
|
assert not result.stop.fatal and result.stop.physics_ticks == 1
|
|
if row < 19:
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.begin_trial(p, row + 1) # must preserve evidence first
|
|
env.acknowledge_finalized(result.stop)
|
|
for wrong in (row, row + 2):
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.begin_trial(p, wrong)
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.begin_trial(protocol('P2'), row + 1) # even equal config is not same bound object
|
|
env.begin_trial(p, row + 1)
|
|
assert hooks.initialization_steps == 21 # constructor + one reset per row
|
|
assert hooks.rollout_steps == 20 and len(env._consumed_rows) == 20
|
|
|
|
|
|
@pytest.mark.parametrize('cause', ['nonfinite', 'hardware_joint_limit', 'identity', 'illegal_reset_or_pose_write',
|
|
'recording', 'storage', 'budget_expired', 'infrastructure'])
|
|
def test_evaluation_runtime_fatal_p2_row_cannot_restart(make_env, runtime, cause):
|
|
env, _, _ = make_env(stage='P2')
|
|
env._cycle_state.safety_causes = lambda: (cause,)
|
|
result = advance(env, 1)
|
|
env.acknowledge_finalized(result.stop)
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.begin_trial(env.evaluation_protocol, 1)
|
|
|
|
|
|
def test_evaluation_runtime_p1_cannot_begin_second_controller_in_launch(make_env, runtime):
|
|
env, _, _ = make_env()
|
|
env._cycle_state.safety_causes = lambda: ('functional_failure',)
|
|
result = advance(env, 1)
|
|
env.acknowledge_finalized(result.stop)
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.begin_trial(env.evaluation_protocol, 1)
|
|
|
|
|
|
def test_evaluation_runtime_bound_event_uses_existing_scheduler_and_onset_phase(make_env):
|
|
env, _, _ = make_env(stage='P2')
|
|
# Advance rows explicitly with preserved failure evidence, no ad-hoc descriptor overwrite.
|
|
for row in range(8):
|
|
env._cycle_state.safety_causes = lambda: ('functional_failure',)
|
|
result = advance(env, 1)
|
|
env.acknowledge_finalized(result.stop)
|
|
env.begin_trial(env.evaluation_protocol, row + 1)
|
|
s = env._cycle_state
|
|
t, phase = torch.tensor([4.02]), torch.tensor([2])
|
|
edge = torch.tensor([True])
|
|
s.schedule.step(t, phase, edge, ~edge, s.scenarios, .002, liftoff_phase=torch.tensor([1]))
|
|
assert not s.schedule.fired.any() # unload-onset is not a valid lift trigger
|
|
s.schedule.step(t, phase, edge, ~edge, s.scenarios, .002, liftoff_phase=torch.tensor([2]))
|
|
assert s.schedule.fired[0].tolist() == [False, True, False, False]
|
|
assert s.schedule.start[0, 1].item() == pytest.approx(4.04)
|
|
assert type(s).apply_loads is type(s).__mro__[2].apply_loads # inherited unique composer
|
|
|
|
|
|
def test_evaluation_runtime_installed_stepping_source_pin(runtime, tmp_path):
|
|
path = Path('/home/cen/IsaacLab/source/isaaclab/isaaclab/envs')
|
|
if not path.is_dir():
|
|
pytest.skip('Installed Isaac sources not present')
|
|
runtime.env.verify_installed_stepping(path) # reads source only; no Isaac import/launch
|
|
for name in runtime.env.INSTALLED_ISAAC_SOURCES:
|
|
(tmp_path / name).write_bytes((path / name).read_bytes())
|
|
(tmp_path / 'manager_based_rl_env.py').write_text('tampered')
|
|
with pytest.raises(RuntimeError, match='dependency changed'):
|
|
runtime.env.verify_installed_stepping(tmp_path)
|
|
|
|
|
|
def test_evaluation_runtime_unique_composer_internal_reset_permission_is_exception_safe(make_env, runtime):
|
|
env, hooks, _ = make_env()
|
|
composer = env._cycle_state.robot.permanent_wrench_composer
|
|
resets = hooks.reset_calls
|
|
result = advance(env, 1)
|
|
assert result.stop is None and hooks.composer_buffer_replacements == 5
|
|
assert hooks.reset_calls == resets
|
|
composer.fail = True
|
|
result = advance(env, 1)
|
|
assert result.stop.fatal and result.ticks_advanced == 0
|
|
assert hooks._load_owner is hooks._replacement_owner is None
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
composer.reset()
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
composer.set_forces_and_torques_index()
|
|
|
|
|
|
def test_evaluation_runtime_event_force_reaches_only_frozen_unique_composer(make_env, runtime):
|
|
env, hooks, _ = make_env(stage='P2')
|
|
for row in range(4):
|
|
env._cycle_state.safety_causes = lambda: ('functional_failure',)
|
|
result = advance(env, 1)
|
|
env.acknowledge_finalized(result.stop)
|
|
env.begin_trial(env.evaluation_protocol, row + 1)
|
|
s = env._cycle_state
|
|
s.time[:] = 1.5
|
|
s.command_clock = s.protocol.evidence_clock(s.trial.index, 750)
|
|
composer = s.robot.permanent_wrench_composer
|
|
before = composer.set_calls
|
|
with hooks.wrench_update(composer):
|
|
runtime.real_apply_loads(s)
|
|
assert composer.set_calls == before + 1
|
|
assert composer.last['is_global'] is True and composer.last['positions'] is None
|
|
assert torch.equal(composer.last['forces'][0, 0], torch.tensor([3., -4., -2.]))
|
|
assert s.schedule.fired.tolist() == [[True, False, False, False]]
|
|
assert s.scenarios.item() == 1
|
|
|
|
|
|
def test_evaluation_runtime_budget_callback_before_any_additional_sim_step(runtime):
|
|
def before_step(hooks):
|
|
if hooks.attempted_steps > 2:
|
|
raise RuntimeError('budget expired')
|
|
|
|
with runtime.env.RuntimeHooks(runtime.Sim, [], before_step=before_step) as hooks:
|
|
sim = runtime.Sim()
|
|
sim.step()
|
|
sim.step()
|
|
with pytest.raises(RuntimeError, match='budget expired'):
|
|
sim.step()
|
|
assert hooks.attempted_steps == 3 and hooks.completed_steps == 2 and sim.calls == 2
|
|
|
|
|
|
def test_evaluation_runtime_wrong_state_cannot_be_silently_reused(runtime):
|
|
with pytest.raises(RuntimeError, match='Wrong evaluation state'):
|
|
runtime.mdp.state(NS(_cycle_state=object()))
|
|
|
|
|
|
def test_evaluation_runtime_p2_final_endpoint_then_new_initial_snapshot(make_env):
|
|
env, hooks, events = make_env(stage='P2')
|
|
original = env._cycle_state
|
|
resets = hooks.reset_calls
|
|
result = advance(env, 900)
|
|
assert result.stop.reason == 'protocol_complete'
|
|
assert result.stop.physics_ticks == result.stop.rollout_steps == 4500
|
|
assert result.stop.completed_clock_cycles == 1 and result.stop.initialization_steps == 2
|
|
assert hooks.reset_calls == resets and events[-1].kind == 'tick'
|
|
env.acknowledge_finalized(result.stop)
|
|
env.begin_trial(env.evaluation_protocol, 1)
|
|
assert env._cycle_state is not original
|
|
assert env._cycle_state.controller.refreshes.item() == 1
|
|
assert events[-1].kind == 'initial' and events[-1].trial_index == 1
|
|
assert events[-1].clock.global_tick == 0 and hooks.initialization_steps == 3
|
|
invalid = env.step(torch.ones(1, 3)) # bound analytic-reference row cannot accept a residual
|
|
assert invalid.stop.fatal and invalid.ticks_advanced == 0
|
|
assert invalid.stop.causes == ('identity_nonzero_reference_residual',)
|
|
|
|
|
|
def test_evaluation_runtime_illegal_reset_does_not_erase_earliest_failure(make_env, runtime):
|
|
env, _, _ = make_env()
|
|
env._cycle_state.safety_causes = lambda: ('swing_clearance',)
|
|
first = advance(env, 1).stop
|
|
with pytest.raises(runtime.env.RuntimeViolation):
|
|
env.reset()
|
|
assert env.stop_result.clock == first.clock
|
|
assert env.stop_result.causes == ('swing_clearance', 'illegal_reset_or_pose_write')
|
|
assert env.stop_result.fatal
|
|
|
|
|
|
@pytest.mark.parametrize('component,value', [(0, .01), (2, -.01), (5, .01)])
|
|
def test_evaluation_runtime_original_fixture_direction_and_couple_floors(make_env, component, value):
|
|
env, _, _ = make_env(safety=True)
|
|
s = env._cycle_state
|
|
s.auxiliary[0, 0, component] = value
|
|
assert 'auxiliary_geometry' in s.safety_causes()
|
|
assert 'auxiliary_support_phase' in s.safety_causes()
|
|
|
|
|
|
def test_evaluation_runtime_next_reference_does_not_relabel_endpoint_physics(make_env, runtime):
|
|
env, _, events = make_env(seam=lambda e, s: runtime.env.SeamDecision(True))
|
|
s = env._cycle_state
|
|
checks = []
|
|
|
|
def safety():
|
|
checks.append(s.command_clock)
|
|
return ()
|
|
|
|
s.safety_causes = safety
|
|
advance(env, 900)
|
|
assert len(checks) == 4500
|
|
assert checks[-1].local_tick == 4500 and checks[-1].cycle_index == 0
|
|
assert events[-1].kind == 'next_reference' and events[-1].clock.local_tick == 0
|
|
|
|
|
|
def test_initialize_trial_preserves_constructor_command_not_forwarded_foot(runtime, monkeypatch):
|
|
"""Recorded P0/P1 tick-zero replay through real state/reference/model, CPU only."""
|
|
import gzip
|
|
import hashlib
|
|
|
|
from SkillFactory.legs.contact_cycle_sim import replay
|
|
from SkillFactory.legs.contact_cycle_sim.reference import ReferenceController
|
|
|
|
directory = ROOT / 'logs/contact_cycle/frozen_replay_p0_01/run'
|
|
trace = Path('/home/cen/sf-runs/p1-01/launch-0/trial-00/trace.v1.jsonl.gz')
|
|
if not (directory / 'initial_state.pt').exists() or not trace.exists():
|
|
pytest.skip('Recorded P0/P1 artifacts not present')
|
|
for path, expected in (
|
|
(directory / 'initial_state.pt', '2e2b506b9d98d72cf64e32fcabfcc66561a694bb44a203408e95b960f29fa168'),
|
|
(directory / 'first_episodes.pt', 'd416095c66245f4c3359e619a04b0b092a2aca551cf0c23ca990b6b592d72149'),
|
|
(trace, 'c943489ad035ca94a9f3edb0307113ed67af0d7bfc90543963f0eac699bfc6ee'),
|
|
):
|
|
assert hashlib.sha256(path.read_bytes()).hexdigest() == expected
|
|
initial = torch.load(directory / 'initial_state.pt', map_location='cpu', weights_only=False)
|
|
episode = torch.load(directory / 'first_episodes.pt', map_location='cpu', weights_only=False)['0']
|
|
with gzip.open(trace, 'rt') as stream:
|
|
p1 = json.loads(next(stream))['state']
|
|
assert (initial['policy_snapshot'][0] != torch.tensor(p1['policy'])).nonzero().flatten().tolist() == [
|
|
28, 29, 40, 41, 42]
|
|
physical = NS(foot=initial['target'].clone(), root=initial['root_state'].clone())
|
|
loaded = runtime.base.LoadedState
|
|
monkeypatch.setattr(loaded, 'foot_center', lambda _: physical.foot.clone())
|
|
monkeypatch.setattr(loaded, 'root', lambda _: (
|
|
physical.root[:, :3], physical.root[:, 3:7], physical.root[:, 7:10], physical.root[:, 10:13]))
|
|
monkeypatch.setattr(loaded, 'contact', lambda _: initial['normal'].clone())
|
|
monkeypatch.setattr(runtime.base.CycleState, 'raw_sensors', lambda _: initial['policy_snapshot'][:, :18].clone())
|
|
monkeypatch.setattr(runtime.frozen, 'ReferenceController', ReferenceController)
|
|
robot_init = runtime.Robot.__init__
|
|
|
|
def initialize_robot(robot):
|
|
robot_init(robot)
|
|
robot.data.body_mass.torch[:] = 0
|
|
robot.data.body_mass.torch[:, 0] = initial['weight'] / 9.81
|
|
|
|
monkeypatch.setattr(runtime.Robot, '__init__', initialize_robot)
|
|
cfg = runtime.config()
|
|
cfg.anchor_height = initial['root_state'][0, 2].item()
|
|
with runtime.env.RuntimeHooks(runtime.Sim, []) as hooks:
|
|
env = runtime.env.EvaluationContactCycleEnv(cfg, protocol=protocol(), trial_index=0,
|
|
runtime_hooks=hooks, evidence_callback=lambda *_: ())
|
|
s = env._cycle_state
|
|
assert torch.equal(s.origin, initial['target'])
|
|
physical.foot = initial['foot'].clone() # reset/forward differs by 55.56 mm XY
|
|
env.reset()
|
|
assert s.anchors.anchor_a_world == tuple(initial['target'][0].tolist())
|
|
assert s.anchors.root_origin_world == tuple(initial['root_state'][0, :3].tolist())
|
|
assert s.anchors.initial_root_world == tuple(physical.root[0, :3].tolist())
|
|
torch.testing.assert_close(s.controller.q_ik, initial['reference_ik_rad'], atol=2e-5, rtol=0)
|
|
torch.testing.assert_close(s.controller.action, initial['reference_next_normalized'], atol=1.5e-4, rtol=0)
|
|
actor = s.policy()
|
|
torch.testing.assert_close(actor, initial['policy_snapshot'], atol=1.5e-4, rtol=0)
|
|
# All non-IK features match exactly; only CPU/GPU finite-difference IK is approximate.
|
|
assert torch.equal(actor[:, :40], initial['policy_snapshot'][:, :40])
|
|
assert torch.equal(actor[:, 43:], initial['policy_snapshot'][:, 43:])
|
|
before = (s.controller.q_ik.clone(), s.controller.refreshes.clone(), torch.get_rng_state())
|
|
assert torch.equal(actor, s.policy())
|
|
assert all(torch.equal(a, b) for a, b in zip(before, (
|
|
s.controller.q_ik, s.controller.refreshes, torch.get_rng_state()), strict=True))
|
|
manifest = json.loads((ROOT / replay.CANDIDATE).with_name('manifest.json').read_bytes())
|
|
model = replay.load_frozen_policy(ROOT / replay.CANDIDATE, manifest)
|
|
action = replay.inference_action(actor, 'learned', model)[0]
|
|
recorded_action = episode['requested_action'][0]
|
|
exact = replay.inference_action(initial['policy_snapshot'], 'learned', model)[0]
|
|
torch.testing.assert_close(exact, recorded_action, atol=1e-7, rtol=0)
|
|
torch.testing.assert_close(action, recorded_action, atol=3e-5, rtol=0)
|
|
env.term.process_actions(action)
|
|
processed = s.nominal + .5 * s.executed # bounded actuator's unchanged target formula
|
|
torch.testing.assert_close(processed, episode['processed_target'][0], atol=8e-5, rtol=0)
|
|
# Phase-zero old/new commands and held/next refresh clocks, without simulated motion.
|
|
for tick in (0, 5, 10, 205, 210):
|
|
s.time[:] = tick * .002
|
|
s.command_clock = s.protocol.next_command_clock(0, tick)
|
|
for old, new in zip(runtime.base.CycleState.reference(s), s.reference(), strict=True):
|
|
torch.testing.assert_close(old, new, atol=3e-8, rtol=0)
|
|
if tick:
|
|
assert s.command_clock.global_tick == tick
|
|
assert s.protocol.evidence_clock(0, tick).global_tick == tick
|
|
assert s.controller.refreshes.item() == 1 # command/observation reads never refresh
|
|
|
|
|
|
def test_initialize_trial_commanded_root_is_not_measured_initial_root(runtime):
|
|
s = object.__new__(runtime.mdp.EvaluationCycleState)
|
|
s._anchors = None
|
|
s.origin = torch.tensor([[.0384319425, .0940605775, .022]])
|
|
s.base_origin = torch.tensor([[0., 0., .3345983624]])
|
|
measured = torch.tensor([[.003, -.004, .335]])
|
|
s.root = lambda: (measured, None, None, None)
|
|
s.foot_center = lambda: torch.tensor([[-.0171082187, .0955000445, .0220000464]])
|
|
s.env = NS(num_envs=1, device='cpu')
|
|
s.command_clock = protocol().next_command_clock(0, 0)
|
|
resets = []
|
|
s.reset = lambda ids: resets.append(ids.tolist())
|
|
s.initialize_trial()
|
|
assert s.anchors.root_origin_world == tuple(s.base_origin[0].tolist())
|
|
assert s.anchors.initial_root_world == tuple(measured[0].tolist())
|
|
assert s.anchors.anchor_a_world == tuple(s.origin[0].tolist())
|
|
assert resets == [[0]] and s.reference_clock == s.command_clock
|
|
with pytest.raises(RuntimeError, match='already captured'):
|
|
s.initialize_trial()
|