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.
263 lines
15 KiB
Python
263 lines
15 KiB
Python
#!/usr/bin/env python3
|
|
"""Development-only physical preflight. No optimizer, checkpoint selection or holdout access."""
|
|
|
|
import argparse
|
|
import hashlib
|
|
import json
|
|
import shutil
|
|
import sys
|
|
import traceback
|
|
from pathlib import Path
|
|
|
|
from runtime_common import finish_run, write_json
|
|
|
|
|
|
def evaluate(env, args, thresholds):
|
|
import torch
|
|
from SkillFactory.legs.contact_cycle.mechanics import inverse_rotate
|
|
from SkillFactory.legs.contact_cycle.metrics import summarize
|
|
from SkillFactory.legs.tokens import LegKinematics
|
|
from SkillFactory.tasks.manager_based.contact_cycle.mdp import failed, state
|
|
|
|
s = state(env)
|
|
fk = LegKinematics(s.bundle, env.device)
|
|
nominal = s.nominal.expand(env.num_envs, -1)
|
|
q_ik = nominal.clone()
|
|
limits = torch.tensor([[j["limits"]["lower"], j["limits"]["upper"]] for j in s.bundle["joints"]], device=env.device)
|
|
speeds = torch.tensor([j["limits"]["velocity"] for j in s.bundle["joints"]], device=env.device)
|
|
stiffness = torch.tensor([j["kp"] for j in s.bundle["spec"]["joints"]], device=env.device)
|
|
trace = {}
|
|
|
|
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)
|
|
|
|
original = env.scene.update
|
|
|
|
def observe(dt):
|
|
original(dt)
|
|
if dt <= 0:
|
|
return
|
|
phase, progress, target, desired, _ = s.reference()
|
|
pos = s.root()[0]
|
|
excess = torch.maximum(limits[:, 0]-s.q(), s.q()-limits[:, 1]).clamp(min=0).amax(-1)
|
|
ratio = torch.maximum((s.robot.data.applied_torque.torch[:, s.joints]/s.effort).abs().amax(-1),
|
|
(s.robot.data.joint_vel.torch[:, s.joints]/speeds).abs().amax(-1))
|
|
mass = s.robot.data.body_mass.torch
|
|
gravity = torch.zeros(env.num_envs, s.robot.num_bodies, 3, device=env.device)
|
|
gravity[..., 2] = -9.81 * mass
|
|
com = s.robot.data.body_com_pos_w.torch
|
|
payload_force = (s.external + s.guide + s.auxiliary)[:, s.payload, :3] + gravity[:, s.payload]
|
|
payload_torque = (s.external + s.guide + s.auxiliary)[:, s.payload, 3:]
|
|
expected_mount = torch.cat((payload_force, payload_torque + torch.cross(
|
|
com[:, s.payload] - s.robot.data.body_link_pos_w.torch[:, s.anchor], payload_force, dim=-1)), -1)
|
|
values = {
|
|
"time": s.time, "phase": phase, "progress": progress, "scenario": s.scenarios,
|
|
"foot": s.foot_center(), "target": target, "desired_force": desired, "weight": s.weight,
|
|
"normal": s.contact(), "friction": s.friction(), "gap": s.foot_center()[:, 2]-s.bundle["foot"]["radius"],
|
|
"slip_speed": s.slip_velocity(), "height_error": (pos[:, 2]-s.port_reference()[0][:, 2]).abs(),
|
|
"joint_excess": excess, "limit_ratio": ratio, "joint_pos": s.q(),
|
|
"joint_vel": s.robot.data.joint_vel.torch[:, s.joints],
|
|
"applied_torque": s.robot.data.applied_torque.torch[:, s.joints],
|
|
"base_state": torch.cat(s.root(), -1), "mount_wrench": s.mount_wrench(),
|
|
"external": s.external, "guide": s.guide, "auxiliary": s.auxiliary,
|
|
"contact_latch": s.latch.contact, "event_fired": s.schedule.fired,
|
|
"event_envelope": s.schedule.envelope, "event_integral": s.schedule.integral,
|
|
"event_completed": s.schedule.completed, "would_fail": failed(env), "tilt": s.tilt(),
|
|
"linear_momentum": (mass[..., None] * s.robot.data.body_com_lin_vel_w.torch).sum(1),
|
|
"total_external_force": (s.external[..., :3] + s.guide[..., :3] + s.auxiliary[..., :3] + gravity).sum(1)
|
|
+ s.contact() + s.friction(),
|
|
"mount_world": s.mount_world(), "expected_static_mount_world": expected_mount,
|
|
}
|
|
for k, v in values.items():
|
|
trace.setdefault(k, []).append(v.detach().cpu().clone())
|
|
|
|
env.scene.update = observe
|
|
steps = round(sum(s.cc["phase_durations_s"]) / env.step_dt)
|
|
observations, actions = [], []
|
|
try:
|
|
for _ in range(steps):
|
|
# Analytic engineering controller with privileged root pose/FK. Not a deployable learned policy.
|
|
_, _, target, fn, _ = s.reference()
|
|
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)
|
|
f = torch.zeros_like(local)
|
|
f[:, 2] = fn
|
|
torque = -(jacobian(q_ik).transpose(-1, -2) @ inverse_rotate(quat, f)[:, :, None]).squeeze(-1)
|
|
command = q_ik + torque / stiffness
|
|
action = ((command-nominal)/0.5).clamp(-1, 1)
|
|
if args.controller == "disabled":
|
|
action.zero_()
|
|
obs, _, terminated, truncated, _ = env.step(action.float())
|
|
if terminated.any() or truncated.any():
|
|
raise RuntimeError("Physical validation must retain failures without automatic reset")
|
|
observations.append(obs["policy"].cpu().clone())
|
|
actions.append(action.cpu().clone())
|
|
finally:
|
|
env.scene.update = original
|
|
data = {k: torch.stack(v) for k, v in trace.items()}
|
|
torch.save(data, args.output / "physics_trace.pt")
|
|
torch.save({"policy": torch.stack(observations) if observations else [],
|
|
"action": torch.stack(actions) if actions else []}, args.output / "control_trace.pt")
|
|
rows = summarize(data, env.physics_dt, thresholds, s.cc["scenarios"])
|
|
momentum_residual = ((data["linear_momentum"][1:] - data["linear_momentum"][:-1]) / env.physics_dt
|
|
- data["total_external_force"][1:])
|
|
momentum_rms = momentum_residual.square().mean(0).sqrt().norm(dim=-1)
|
|
static = (data["time"][:, 0] >= 2.0) & (data["time"][:, 0] < 2.8)
|
|
sensor_residual = (data["mount_world"][static].mean(0)
|
|
- data["expected_static_mount_world"][static].mean(0)).abs()
|
|
expected_events = torch.tensor([[i == e["scenario"] for e in s.cc["events"]]
|
|
for i in s.scenarios.tolist()], device="cpu")
|
|
# Reset probes are outside the physics trial. Live nonzero wrench + selective reset tests persistence.
|
|
before = {"root": s.robot.data.root_state_w.torch.clone(), "q": s.q().clone(),
|
|
"time": s.time.clone(), "fired": s.schedule.fired.clone(), "sensor": s.sensed.clone()}
|
|
ids = torch.arange(0, env.num_envs, 2, device=env.device)
|
|
other = torch.arange(1, env.num_envs, 2, device=env.device)
|
|
composer = s.robot.permanent_wrench_composer
|
|
seeded = torch.arange(1, env.num_envs + 1, device=env.device).float()[:, None, None]
|
|
seeded = seeded.expand(-1, s.robot.num_bodies, 3).contiguous()
|
|
composer.set_forces_and_torques_index(forces=seeded, torques=seeded*0.1, positions=None, is_global=True)
|
|
before_force, before_torque = composer.out_force_b.torch.clone(), composer.out_torque_b.torch.clone()
|
|
env.reset(env_ids=ids)
|
|
composer_reset = bool((composer.out_force_b.torch[ids] == 0).all()
|
|
and (composer.out_torque_b.torch[ids] == 0).all()
|
|
and torch.equal(before_force[other], composer.out_force_b.torch[other])
|
|
and torch.equal(before_torque[other], composer.out_torque_b.torch[other]))
|
|
isolated = all(torch.equal(a[other], b[other]) for a, b in (
|
|
(before["root"], s.robot.data.root_state_w.torch), (before["q"], s.q()),
|
|
(before["time"], s.time), (before["fired"], s.schedule.fired), (before["sensor"], s.sensed)))
|
|
cleared = bool((s.external[ids] == 0).all() and (s.auxiliary[ids] == 0).all()
|
|
and (s.time[ids] == 0).all() and not s.schedule.fired[ids].any()
|
|
and (s.action_delay.data[:, ids] == 0).all() and not s.latch.contact[ids].any()
|
|
and (s.schedule.integral[ids] == 0).all()
|
|
and (s.sensor_delay.data[:, ids] == s.raw_sensors()[ids]).all()
|
|
and (s.sensed[ids] == s.raw_sensors()[ids]).all())
|
|
snapshot = s.policy().clone()
|
|
env.reset(env_ids=[])
|
|
empty = torch.equal(snapshot, s.policy())
|
|
env.cfg.episode_length_s = 3 * env.step_dt
|
|
env.reset()
|
|
for _ in range(3):
|
|
obs, _, terminated, truncated, info = env.step(torch.zeros(env.num_envs, 3, device=env.device))
|
|
terminal = bool(truncated.all() and not terminated.any() and info["terminal_observation"] is not None
|
|
and info["terminal_observation"]["policy"].shape == (env.num_envs, 32)
|
|
and (s.time == 0).all() and not s.schedule.fired.any())
|
|
before = s.time.clone()
|
|
rejected = False
|
|
try:
|
|
env.step(torch.full((env.num_envs, 3), float("nan"), device=env.device))
|
|
except ValueError:
|
|
rejected = torch.equal(before, s.time)
|
|
expected_integral = torch.tensor([e["duration_s"] - e["ramp_s"] for e in s.cc["events"]])
|
|
delivered = (data["event_integral"][-1] - expected_events * expected_integral).abs() <= 0.002
|
|
gates = {"floating_root": not s.robot.is_fixed_base, "finite_trace": all(r["finite"] for r in rows),
|
|
"full_phases": all(r["complete"] for r in rows), "indexed_reset": isolated and cleared,
|
|
"empty_reset": empty, "composer_reset": composer_reset,
|
|
"timeout_terminal": terminal, "bad_action_rejected": rejected,
|
|
"guide_vertical_zero": bool((data["guide"][..., 2] == 0).all()),
|
|
"all_scenario_metrics": all(r["success"] for r in rows),
|
|
"event_coverage": torch.equal(data["event_fired"][-1], expected_events)
|
|
and torch.equal(data["event_completed"][-1], expected_events) and bool(delivered.all()),
|
|
"scenario_coverage": set(data["scenario"][0].tolist()) == set(range(len(s.cc["scenarios"]))),
|
|
"linear_momentum_balance": bool((momentum_rms <= 5.0).all()),
|
|
"static_mount_force": bool((sensor_residual[:, :3] <= 1.5).all()),
|
|
"static_mount_torque": bool((sensor_residual[:, 3:] <= 0.3).all())}
|
|
return {"passed": all(gates.values()), "accepted_skill": False, "trained_skill": False,
|
|
"kind": "development_physics_preflight", "controller": args.controller,
|
|
"controller_uses_privileged_state": True, "seed": args.seed,
|
|
"gates": gates, "rows": rows, "physics_dt": env.physics_dt, "control_dt": env.step_dt,
|
|
"physics_steps": len(data["time"]), "support_enabled": env.cfg.auxiliary_enabled,
|
|
"effective_mass_kg": dict(zip(s.robot.body_names, s.robot.data.body_mass.torch[0].tolist())),
|
|
"event_triggered_per_env": data["event_fired"][-1].tolist(),
|
|
"event_integral_s_per_env": data["event_integral"][-1].tolist(),
|
|
"event_completed_per_env": data["event_completed"][-1].tolist(),
|
|
"linear_momentum_balance_rms_N_per_env": momentum_rms.tolist(),
|
|
"static_mount_residual_N_Nm_per_env": sensor_residual.tolist(),
|
|
"note": "Preflight is not independent acceptance, multi-seed training, or hardware generalization."}
|
|
|
|
|
|
def main():
|
|
from isaaclab.app import AppLauncher
|
|
|
|
from isaaclab_tasks.utils import add_launcher_args, launch_simulation
|
|
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--config", type=Path, default=Path("configs/contact_cycle/go2_fl_v1.json"))
|
|
parser.add_argument("--thresholds", type=Path, default=Path("configs/contact_cycle/acceptance_v1.json"))
|
|
parser.add_argument("--output", type=Path, required=True)
|
|
parser.add_argument("--num_envs", type=int, default=4)
|
|
parser.add_argument("--seed", type=int, default=90101)
|
|
parser.add_argument("--no-auxiliary", action="store_true")
|
|
parser.add_argument("--controller", choices=("ik", "disabled"), default="ik")
|
|
add_launcher_args(parser)
|
|
args = parser.parse_args()
|
|
if args.num_envs < 4:
|
|
parser.error("At least four environments required for development scenarios")
|
|
args.output.mkdir(parents=True, exist_ok=False)
|
|
root = Path(__file__).resolve().parents[1]
|
|
from SkillFactory.legs.contact_cycle.config import load_config, require_scenario_coverage
|
|
|
|
require_scenario_coverage(load_config(args.config), args.num_envs)
|
|
sources = [Path(__file__), args.config, args.thresholds, root/"scripts/runtime_common.py",
|
|
root/"scripts/supervise_sim.py", root/"configs/standing/go2_fl_support_v0.json",
|
|
root/"assets/generated/support/go2_fl_3kg_v0/support.json",
|
|
root/"assets/generated/legs/go2_fl/manifest.json"]
|
|
sources += list((root/"source/SkillFactory/SkillFactory").rglob("*.py"))
|
|
identity = {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in sources}
|
|
for p in sources:
|
|
relative = (p.resolve().relative_to(root) if p.resolve().is_relative_to(root)
|
|
else Path("external")/hashlib.sha256(p.read_bytes()).hexdigest()/p.name)
|
|
target = args.output/"source_snapshot"/relative
|
|
target.parent.mkdir(parents=True, exist_ok=True)
|
|
shutil.copyfile(p, target)
|
|
write_json(args.output/"identity.json", identity)
|
|
write_json(args.output/"invocation.json", {"argv": sys.argv, "thresholds": json.loads(args.thresholds.read_text())})
|
|
write_json(args.output/"result.json", {"passed": False, "status": "running", "accepted_skill": False})
|
|
app = AppLauncher(args).app
|
|
env, result, code = None, {"passed": False, "accepted_skill": False}, 1
|
|
try:
|
|
import gymnasium as gym
|
|
import SkillFactory.tasks # noqa: F401
|
|
from SkillFactory.tasks.manager_based.contact_cycle.cfg import ContactCycleEnvCfg
|
|
|
|
cfg = ContactCycleEnvCfg(cycle_config=str(args.config))
|
|
cfg.scene.num_envs = args.num_envs
|
|
cfg.seed = args.seed
|
|
cfg.auxiliary_enabled = not args.no_auxiliary
|
|
cfg.episode_length_s += 1
|
|
cfg.terminations.failed = None # Full failed trajectory retained for audit, never censored by reset.
|
|
if args.controller == "disabled":
|
|
cfg.scene.robot.actuators["leg"].stiffness = 0
|
|
cfg.scene.robot.actuators["leg"].damping = 0
|
|
with launch_simulation(cfg, args):
|
|
env = gym.make("SkillFactory-Go2FL-ContactCycle-v1", cfg=cfg).unwrapped
|
|
env.reset(seed=args.seed)
|
|
result = evaluate(env, args, json.loads(args.thresholds.read_text())["gates"])
|
|
write_json(args.output/"config.json", cfg.cycle_parameters)
|
|
if any(hashlib.sha256(Path(p).read_bytes()).hexdigest() != digest for p, digest in identity.items()):
|
|
raise RuntimeError("Bound source/config changed during physical trial")
|
|
code = 0 if result["passed"] else 2
|
|
except BaseException:
|
|
result.update(passed=False, error=traceback.format_exc())
|
|
print(result["error"], file=sys.stderr)
|
|
finally:
|
|
finish_run(app, env, args.output/"result.json", result, code)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|