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.
299 lines
17 KiB
Python
299 lines
17 KiB
Python
#!/usr/bin/env python3
|
|
"""Non-learning PhysX integration: sampled loads, first-episode recorder, 32D timeout rollout.
|
|
|
|
Uses a privileged analytic controller and an UNTRAINED network for API checks.
|
|
No optimizer, learner checkpoint selection, training grant or independent test.
|
|
"""
|
|
|
|
import argparse
|
|
import hashlib
|
|
import json
|
|
import traceback
|
|
from pathlib import Path
|
|
|
|
from runtime_common import finish_run, write_json
|
|
|
|
|
|
def run(env, output):
|
|
import torch
|
|
import warp as wp
|
|
from SkillFactory.legs.contact_cycle.curriculum import EpisodeSampler, assert_sample_domain
|
|
from SkillFactory.legs.contact_cycle.mechanics import inverse_rotate
|
|
from SkillFactory.legs.contact_cycle.ppo import CycleActorCritic, collect_rollout, features
|
|
from SkillFactory.legs.tokens import LegKinematics
|
|
from SkillFactory.tasks.manager_based.contact_cycle.mdp import state
|
|
from SkillFactory.tasks.manager_based.contact_cycle.recording import CycleRecorder
|
|
|
|
s = state(env)
|
|
sampler = EpisodeSampler(s.cc, env.num_envs, 90211, update=1500)
|
|
s.set_sampler(sampler)
|
|
env.reset()
|
|
sample = {"scenario": s.scenarios.cpu().clone(), "stand": s.stand_only.cpu().clone(),
|
|
"stage": s.sampled_stage.cpu().clone(), "commands": s.schedule.commands.cpu().clone(),
|
|
"points": s.sampled_points.cpu().clone(), "delays": s.schedule.delays.cpu().clone()}
|
|
assert_sample_domain(s.cc, sample)
|
|
torch.save(sample, output/"episode_inputs.pt")
|
|
fk = LegKinematics(s.bundle, env.device)
|
|
nominal = s.nominal.expand(env.num_envs, -1)
|
|
q_ik = nominal.clone()
|
|
stiffness = torch.tensor([j["kp"] for j in s.bundle["spec"]["joints"]], device=env.device)
|
|
|
|
def foot(q):
|
|
transform = fk.forward(q)[s.bundle["foot"]["body"]]
|
|
return transform[:, :3, 3] + (transform[:, :3, :3] @ s.offset[:, :, None]).squeeze(-1)
|
|
|
|
def jacobian(q):
|
|
columns = []
|
|
for j in range(3):
|
|
delta = torch.zeros_like(q)
|
|
delta[:, j] = 0.0005
|
|
columns.append((foot(q+delta)-foot(q-delta))/0.001)
|
|
return torch.stack(columns, -1)
|
|
|
|
def analytic_action(early_env=None):
|
|
nonlocal q_ik
|
|
_, _, target, fn, _ = s.reference()
|
|
if early_env is not None and s.time[early_env] >= 3.8:
|
|
target[early_env, 2] += 0.03 # physical negative control, not edited sensor data
|
|
pos, quat, _, _ = s.root()
|
|
local = inverse_rotate(quat, target-pos)
|
|
for _ in range(3):
|
|
j = jacobian(q_ik)
|
|
dq = torch.linalg.solve(j @ j.transpose(-1, -2) + torch.eye(3, device=env.device)*1e-5,
|
|
(local-foot(q_ik))[:, :, None])
|
|
q_ik += (j.transpose(-1, -2) @ dq).squeeze(-1).clamp(-0.08, 0.08)
|
|
q_ik.clamp_(min=nominal-0.5, max=nominal+0.5)
|
|
force = torch.zeros_like(local)
|
|
force[:, 2] = fn
|
|
torque = -(jacobian(q_ik).transpose(-1, -2) @ inverse_rotate(quat, force)[:, :, None]).squeeze(-1)
|
|
return ((q_ik + torque/stiffness - nominal)/0.5).clamp(-1, 1).float()
|
|
|
|
steps = round(sum(s.cc["phase_durations_s"])/env.step_dt)
|
|
with CycleRecorder(env) as recorder:
|
|
initial_snapshot = recorder.initial_state
|
|
for k in range(steps):
|
|
action = analytic_action()
|
|
recorder.before_control(None, action)
|
|
_, _, terminated, truncated, _ = env.step(action)
|
|
torch.testing.assert_close(recorder.frames[-1]["requested_action"], action.cpu(), rtol=0, atol=0)
|
|
recorder.end_control(terminated, truncated, k == steps-1)
|
|
if terminated.any() or truncated.any():
|
|
raise RuntimeError("Development sampled trace must not silently auto-reset")
|
|
thresholds = json.loads(Path("configs/contact_cycle/acceptance_v1.json").read_text())["gates"]
|
|
rows, trace = recorder.summarize(thresholds)
|
|
torch.save(trace, output/"sampled_physics_trace.pt")
|
|
torch.save(initial_snapshot, output/"sampled_initial_state.pt")
|
|
initialization_checks = {
|
|
"configured_on_plane": env.cfg.scene.robot.init_state.pos[2] == env.cfg.anchor_height,
|
|
"measured_reset_gap": bool((initial_snapshot["gap"].abs() <= 1e-5).all()),
|
|
"all_acquired_load": all(r["contact_acquired"] for r in rows),
|
|
"no_initialization_failure": not bool(trace["acquisition_failed"].any()),
|
|
"initial_snapshot_separate": bool((initial_snapshot["time"] == 0).all()) and len(trace["time"]) == 4500,
|
|
}
|
|
expected = torch.tensor([[e["scenario"] == int(sc) for e in s.cc["events"]] for sc in sample["scenario"]])
|
|
event_completed = torch.equal(s.schedule.completed.cpu(), expected)
|
|
integral = torch.tensor([e["duration_s"]-e["ramp_s"] for e in s.cc["events"]])
|
|
delivered = bool(((s.schedule.integral.cpu() - expected*integral).abs() <= 0.002).all())
|
|
# Selective reset must not resample another environment, its RNG counters or backend load.
|
|
old_commands, old_points = s.schedule.commands.clone(), s.sampled_points.clone()
|
|
old_counts = sampler.episodes.clone()
|
|
composer = s.robot.permanent_wrench_composer
|
|
old_force = composer.out_force_b.torch.clone()
|
|
buffer_names = ("global_force_w", "global_force_at_com_w", "global_torque_w", "local_force_b", "local_torque_b")
|
|
before_buffers = {name: wp.to_torch(getattr(composer, name)).clone() for name in buffer_names}
|
|
ids = torch.tensor([1, 3], dtype=torch.int32, device=env.device)
|
|
others = torch.tensor([i for i in range(env.num_envs) if i not in (1, 3)], device=env.device)
|
|
env.reset(env_ids=ids)
|
|
reset_checks = {
|
|
"other_commands_unchanged": torch.equal(old_commands[others], s.schedule.commands[others]),
|
|
"other_points_unchanged": torch.equal(old_points[others], s.sampled_points[others]),
|
|
"other_counters_unchanged": torch.equal(old_counts[others.cpu()], sampler.episodes[others.cpu()]),
|
|
"selected_counters_advance_once": bool((sampler.episodes[ids.cpu()] == old_counts[ids.cpu()] + 1).all()),
|
|
"selected_points_resampled": not torch.equal(old_points[ids], s.sampled_points[ids]),
|
|
}
|
|
for name in buffer_names:
|
|
after = wp.to_torch(getattr(composer, name))
|
|
reset_checks[f"{name}_isolated"] = torch.equal(before_buffers[name][others], after[others])
|
|
reset_checks[f"{name}_selected_zero"] = bool((after[ids] == 0).all())
|
|
# Do not compare stale cached body-frame output to recomposition at the newest body pose.
|
|
# The world/local INPUT buffers are the persistent applied-command source of truth.
|
|
cache_difference = float((old_force[others] - composer.out_force_b.torch[others]).abs().max())
|
|
isolated = all(reset_checks.values())
|
|
# Zero normalized actions must not inherit a 3mm airborne reset and fail the 10ms monitor.
|
|
s.set_sampler(None)
|
|
env.reset()
|
|
zero_failed = False
|
|
with CycleRecorder(env) as zero_recorder:
|
|
for _ in range(25):
|
|
action = torch.zeros(env.num_envs, 3, device=env.device)
|
|
zero_recorder.before_control(None, action)
|
|
_, _, terminated, truncated, _ = env.step(action)
|
|
zero_failed |= bool(terminated.any() or truncated.any())
|
|
initialization_checks["zero_action_startup"] = not zero_failed and bool(s.acquisition.acquired.all())
|
|
torch.save({k: torch.stack([f[k] for f in zero_recorder.frames]) for k in zero_recorder.frames[0]},
|
|
output/"zero_action_startup.pt")
|
|
# Invalid physical reset probe: lift only env0's root 30mm, with a separately declared 20ms deadline.
|
|
# This shortened diagnostic is not a production-policy episode or a relaxed acceptance gate.
|
|
from SkillFactory.legs.contact_cycle.task import FAILURE_NAMES
|
|
|
|
env.reset()
|
|
pose = s.robot.data.root_state_w.torch[:1, :7].clone()
|
|
pose[:, 2] += .03
|
|
s.robot.write_root_pose_to_sim(pose, env_ids=torch.tensor([0], device=env.device))
|
|
original_deadline = s.acquisition.deadline
|
|
s.acquisition.deadline = .02
|
|
try:
|
|
with CycleRecorder(env) as invalid_recorder:
|
|
for _ in range(2):
|
|
action = torch.zeros(env.num_envs, 3, device=env.device)
|
|
invalid_recorder.before_control(None, action)
|
|
_, _, terminated, _, info = env.step(action)
|
|
flags = info["cycle_step"]["failure_flags"][0]
|
|
initialization_checks["invalid_start_times_out"] = bool(
|
|
terminated[0] and flags[FAILURE_NAMES.index("contact_acquisition_timeout")])
|
|
initialization_checks["invalid_start_not_fake_departure"] = not bool(
|
|
flags[FAILURE_NAMES.index("premature_separation")])
|
|
initialization_checks["autoreset_clears_acquisition"] = not bool(s.acquisition.failed[0]) and s.time[0] == 0
|
|
torch.save({k: torch.stack([f[k] for f in invalid_recorder.frames]) for k in invalid_recorder.frames[0]},
|
|
output/"invalid_start_negative.pt")
|
|
finally:
|
|
s.acquisition.deadline = original_deadline
|
|
initialization_checks = {k: bool(v) for k, v in initialization_checks.items()}
|
|
# Separate physical negative control: lift env0 early while unloading.
|
|
# Keep actual termination enabled: capture before autoreset, then require that reset clears only env0.
|
|
s.set_sampler(None)
|
|
env.reset()
|
|
s.scenarios[:] = 2
|
|
q_ik = nominal.clone()
|
|
with CycleRecorder(env) as early_recorder:
|
|
for k in range(round(4.3/env.step_dt)):
|
|
action = analytic_action(early_env=0)
|
|
early_recorder.before_control(None, action)
|
|
_, _, terminated, truncated, _ = env.step(action)
|
|
early_recorder.end_control(terminated, truncated, False)
|
|
if terminated.any() or truncated.any():
|
|
break
|
|
early_trace = {key: torch.stack([f[key] for f in early_recorder.frames]) for key in early_recorder.frames[0]}
|
|
early_checks = {
|
|
"premature_detected_at_physics_rate": bool(early_trace["premature_separation"][:, 0].any()),
|
|
"actual_termination": bool(terminated[0]) and not bool(terminated[1:].any() or truncated.any()),
|
|
"no_synthetic_delayed_pulse": not bool(early_trace["event_fired"][:, 0].any()),
|
|
"other_envs_not_premature": not bool(early_trace["premature_separation"][:, 1:].any()),
|
|
"autoreset_clears_failure": not bool(s.separation.premature[0]) and s.time[0] == 0,
|
|
"other_episode_clocks_not_reset": bool((s.time[1:] > 3.8).all()),
|
|
"terminal_snapshot": (env.extras.get("terminal_ids") is not None
|
|
and env.extras["terminal_ids"].cpu().tolist() == [0]),
|
|
}
|
|
early_checks = {key: bool(value) for key, value in early_checks.items()}
|
|
early_time = float(early_trace["time"][-1, 0])
|
|
torch.save({key: value[:, :2].clone() for key, value in early_trace.items()}, output/"early_lift_negative_trace.pt")
|
|
# API test only: force a short TIMEOUT to check terminal snapshots/32D GAE inputs.
|
|
# This shortened episode is NOT included in task metrics or counted as skill acceptance.
|
|
s.set_sampler(None)
|
|
env.cfg.episode_length_s = 2*env.step_dt
|
|
x = features(env.reset()[0]["policy"])
|
|
torch.manual_seed(90212)
|
|
model = CycleActorCritic(hidden=16).to(env.device)
|
|
before = {k: v.clone() for k, v in model.state_dict().items()}
|
|
# Isolate timeout/bootstrap API from random untrained early-lift failures. ONLY this 0.02s API fixture
|
|
# disables the failed term; the full 9s positive run and early-lift negative above use the real term.
|
|
from isaaclab.managers import TerminationTermCfg
|
|
|
|
original_failed = env.termination_manager.get_term_cfg("failed")
|
|
env.termination_manager.set_term_cfg("failed", TerminationTermCfg(
|
|
func=lambda e: torch.zeros(e.num_envs, dtype=torch.bool, device=e.device)))
|
|
try:
|
|
data, _, failures, timeouts = collect_rollout(env, model, x, {"horizon": 4, "failure_penalty": 10.})
|
|
finally:
|
|
env.termination_manager.set_term_cfg("failed", original_failed)
|
|
canonical_terminal = env.extras["terminal_ids"].dtype == torch.int64
|
|
no_learning = all(torch.equal(before[k], v) for k, v in model.state_dict().items())
|
|
x = features(env.reset()[0]["policy"])
|
|
with CycleRecorder(env) as actor_recorder:
|
|
with torch.no_grad():
|
|
action, mean, _, _ = model.act(x, deterministic=True)
|
|
actor_recorder.before_control(x, action, mean, model.log_std.exp())
|
|
env.step(action)
|
|
actor_frame = actor_recorder.frames[0]
|
|
actor_logging = (bool(actor_frame["actor_metadata_available"].all())
|
|
and torch.equal(actor_frame["actor_input"], x.cpu())
|
|
and torch.equal(actor_frame["requested_action"], action.cpu())
|
|
and torch.equal(actor_frame["actor_mean"], mean.cpu()))
|
|
torch.save({"initial": actor_recorder.initial_state, "first_tick": actor_frame}, output/"actor_diagnostic_api.pt")
|
|
gates = {"domain_audit": True, "scenario_coverage": set(sample["scenario"].tolist()) == {0, 1, 2, 3},
|
|
"full_sampled_duration": len(trace["time"]) == round(9/env.physics_dt),
|
|
"all_sampled_analytic_metrics": all(r["success"] for r in rows),
|
|
"event_delivery": event_completed and delivered, "indexed_sampler_reset": isolated,
|
|
"initialization_contract": all(initialization_checks.values()),
|
|
"early_lift_negative_control": all(early_checks.values()),
|
|
"timeout_rollout": failures == 0 and timeouts == 2*env.num_envs and bool(data["done"].any()),
|
|
"canonical_terminal_ids": canonical_terminal,
|
|
"actor_diagnostic_logging": actor_logging,
|
|
"terminal_metadata_not_stale": (env.extras["terminal_ids"] is None
|
|
and env.extras["terminal_failure_flags"] is None),
|
|
"analytic_actions_logged": (bool(trace["control_action_available"].all())
|
|
and not bool(trace["actor_metadata_available"].any())),
|
|
"rollout_diagnostic_coverage": bool((data["phase"] >= 0).all()),
|
|
"finite_rollout": all(torch.isfinite(v).all().item() for v in data.values()),
|
|
"no_parameter_update": no_learning}
|
|
torch.save(data, output/"untrained_timeout_api_rollout.pt")
|
|
return {"passed": all(gates.values()), "gates": gates, "rows": rows, "accepted_skill": False,
|
|
"trained_skill": False, "optimizer_steps": 0, "independent_test_executed": False,
|
|
"sampled_development_seed": 90211, "timeout_api_test_episode_s": env.cfg.episode_length_s,
|
|
"timeout_api_failed_term_disabled": True,
|
|
"timeout_api_failures": failures, "timeout_api_timeouts": timeouts,
|
|
"reset_checks": reset_checks, "cached_vs_recomposed_body_force_difference_N": cache_difference,
|
|
"initialization_checks": initialization_checks,
|
|
"invalid_start_probe": {"root_raise_m": .03, "deadline_s": .02, "production_deadline_s": original_deadline},
|
|
"early_lift_checks": early_checks, "early_lift_probe_end_s": early_time,
|
|
"early_lift_max_unload_gap_m": float(early_trace["gap"][:, 0][early_trace["phase"][:, 0] == 1].max()),
|
|
"note": "Analytic development, early-lift negative control and short timeout API. Not skill acceptance."}
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--output", type=Path, required=True)
|
|
from isaaclab.app import AppLauncher
|
|
|
|
AppLauncher.add_app_launcher_args(parser)
|
|
args = parser.parse_args()
|
|
args.output.mkdir(parents=True, exist_ok=False)
|
|
root = Path(__file__).resolve().parents[1]
|
|
files = list((root/"source/SkillFactory").rglob("*.py")) + [Path(__file__)]
|
|
files += list((root/"configs/contact_cycle").glob("*.json"))
|
|
identity = {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in files}
|
|
write_json(args.output/"identity.json", identity)
|
|
launcher = AppLauncher(args)
|
|
env = None
|
|
result = {"passed": False, "accepted_skill": False, "trained_skill": False}
|
|
code = 1
|
|
try:
|
|
from SkillFactory.tasks.manager_based.contact_cycle.cfg import ContactCycleEnvCfg
|
|
from SkillFactory.tasks.manager_based.contact_cycle.env import ContactCycleEnv
|
|
|
|
cfg = ContactCycleEnvCfg()
|
|
cfg.scene.num_envs = 16
|
|
cfg.sim.device = args.device
|
|
cfg.seed = 90211
|
|
cfg.episode_length_s = sum(cfg.cycle_parameters["phase_durations_s"]) + 1
|
|
env = ContactCycleEnv(cfg=cfg)
|
|
from SkillFactory.tasks.manager_based.contact_cycle.recording import PhysicalSafetyAudit
|
|
|
|
thresholds = json.loads((root/"configs/contact_cycle/acceptance_v1.json").read_text())["gates"]
|
|
with PhysicalSafetyAudit(env, thresholds) as safety:
|
|
result = run(env, args.output)
|
|
safety.check()
|
|
result["physics_safety"] = safety.report()
|
|
if any(hashlib.sha256(Path(p).read_bytes()).hexdigest() != h for p, h in identity.items()):
|
|
raise RuntimeError("Source changed during runtime test")
|
|
code = 0 if result["passed"] else 2
|
|
except BaseException:
|
|
result["error"] = traceback.format_exc()
|
|
traceback.print_exc()
|
|
finally:
|
|
finish_run(launcher.app, env, args.output/"result.json", result, code)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|