Files
chenlin 7f39abb613 release: SkillFactory v0.0.1
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
2026-10-10 09:58:30 +08:00

56 lines
2.0 KiB
Python

#!/usr/bin/env python3
"""Finite Go2 anchor demonstrations; no training and no unbounded random agent."""
import csv
import math
from anchor_common import launch
def configure(parser):
parser.add_argument("--steps", type=int, default=600)
parser.add_argument("--mode", choices=["hold", "sine", "contact"], default="sine")
def run(env, args):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
obs, _ = env.reset()
assert obs["policy"].shape == (args.num_envs, 19)
actions = torch.zeros((args.num_envs, 3), device=u.device)
rows = []
for step in range(args.steps):
if args.mode == "sine":
actions[:] = 0.4 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
elif args.mode == "contact":
actions.zero_()
actions[:, 2] = -1.0 if int(step * u.step_dt) % 2 == 0 else 1.0
obs, _, terminated, timedout, _ = env.step(actions)
assert not terminated.any() and not timedout.any()
assert torch.isfinite(obs["policy"]).all()
d = diagnostics(u)
rows.append([step * u.step_dt, *d["q"][0].tolist(), d["gap"][0].item(), *d["normal_force_w"][0].tolist()])
with (args.output / "trace.csv").open("w", newline="") as f:
writer = csv.writer(f)
writer.writerow(["time_s", "q_hip", "q_thigh", "q_calf", "gap_m", "normal_fx_N", "normal_fy_N", "normal_fz_N"])
writer.writerows(rows)
return {
"passed": True,
"mode": args.mode,
"num_envs": args.num_envs,
"steps": args.steps,
"physics_dt": u.physics_dt,
"control_dt": u.step_dt,
"policy_shape": list(obs["policy"].shape),
"joint_names": u.scene["robot"].joint_names,
"final_q": d["q"][0].tolist(),
"min_gap_m": min(r[4] for r in rows),
"max_normal_fz_N": max(r[7] for r in rows),
}
if __name__ == "__main__":
launch(__doc__, configure, run)