#!/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)