7f39abb613
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
56 lines
2.0 KiB
Python
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)
|