release: SkillFactory v0.0.1
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
This commit is contained in:
@@ -0,0 +1,132 @@
|
||||
"""Shared finite benchmark launcher; preserve failures through Kit fast shutdown."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
|
||||
from runtime_common import finish_run, write_json
|
||||
|
||||
TASK = "SkillFactory-SingleLegAnchor-Go2-v0"
|
||||
|
||||
|
||||
def launch(description, configure_parser, run):
|
||||
if not __debug__:
|
||||
raise RuntimeError("Benchmark assertions require Python without -O/PYTHONOPTIMIZE")
|
||||
if sys.version_info[:2] != (3, 12):
|
||||
raise RuntimeError("Use /home/cen/IsaacLab/isaaclab.sh -p with Python 3.12, not the old conda environment")
|
||||
for key in ("CONDA_PREFIX", "VIRTUAL_ENV"):
|
||||
if os.environ.get(key):
|
||||
print(f"[WARNING] Active {key}={os.environ[key]}; actual interpreter: {sys.executable}")
|
||||
from isaaclab_tasks.utils import add_launcher_args, launch_simulation, resolve_task_config, setup_preset_cli
|
||||
|
||||
parser = argparse.ArgumentParser(description=description)
|
||||
parser.add_argument("--task", default=TASK)
|
||||
parser.add_argument("--num_envs", type=int, default=64)
|
||||
parser.add_argument("--seed", type=int, default=42)
|
||||
parser.add_argument("--anchor_height", type=float, default=None)
|
||||
parser.add_argument("--output", type=Path, default=Path("logs/week1_anchor/smoke"))
|
||||
configure_parser(parser)
|
||||
add_launcher_args(parser)
|
||||
args, hydra_args = setup_preset_cli(parser)
|
||||
sys.argv = [sys.argv[0]] + hydra_args
|
||||
# Our selected backend is known to require Kit. Start it before resolving
|
||||
# configs: action classes can import pxr and preload an incompatible USD.
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
app = AppLauncher(args).app
|
||||
code = 0
|
||||
env = None
|
||||
args.output.mkdir(parents=True, exist_ok=True)
|
||||
result = {"passed": False, "status": "running"}
|
||||
write_json(args.output / "result.json", result)
|
||||
try:
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
|
||||
cfg, _ = resolve_task_config(args.task, "")
|
||||
cfg.scene.num_envs = args.num_envs
|
||||
cfg.seed = args.seed
|
||||
cfg.terminations.time_out = None # no automatic reset hiding verification failures
|
||||
if getattr(args, "suite", None) == "stability" and args.anchor_height is None:
|
||||
args.anchor_height = 0.6
|
||||
if args.anchor_height is not None:
|
||||
cfg.scene.robot.init_state.pos = (0.0, 0.0, args.anchor_height)
|
||||
if getattr(args, "suite", None) == "calibration":
|
||||
import isaaclab.sim as sim_utils
|
||||
from isaaclab.assets import RigidObjectCfg
|
||||
from isaaclab.sensors import ContactSensorCfg
|
||||
|
||||
cfg.scene.calibration_probe = RigidObjectCfg(
|
||||
prim_path="{ENV_REGEX_NS}/Probe",
|
||||
spawn=sim_utils.SphereCfg(
|
||||
radius=0.05,
|
||||
mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
|
||||
rigid_props=sim_utils.RigidBodyPropertiesCfg(),
|
||||
collision_props=sim_utils.CollisionPropertiesCfg(),
|
||||
activate_contact_sensors=True,
|
||||
),
|
||||
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.7, 0.0, 0.3)),
|
||||
)
|
||||
cfg.scene.probe_contact = ContactSensorCfg(
|
||||
prim_path="{ENV_REGEX_NS}/Probe",
|
||||
update_period=0.0,
|
||||
history_length=5,
|
||||
filter_prim_paths_expr=cfg.scene.foot_contact.filter_prim_paths_expr,
|
||||
)
|
||||
# Kit is already launched, so this context performs backend setup but
|
||||
# does not own app.close(). We retain the failure exit code ourselves.
|
||||
with launch_simulation(cfg, args):
|
||||
import gymnasium as gym
|
||||
import torch
|
||||
|
||||
torch.manual_seed(args.seed)
|
||||
env = gym.make(args.task, cfg=cfg)
|
||||
u = env.unwrapped
|
||||
from pxr import UsdPhysics
|
||||
|
||||
from isaaclab.sim import get_current_stage
|
||||
|
||||
stage = get_current_stage()
|
||||
grounds = [
|
||||
str(p.GetPath())
|
||||
for p in stage.Traverse()
|
||||
if str(p.GetPath()).startswith("/World/ground/") and p.HasAPI(UsdPhysics.CollisionAPI)
|
||||
]
|
||||
expected = cfg.scene.foot_contact.filter_prim_paths_expr
|
||||
if grounds != expected:
|
||||
raise RuntimeError(f"Ground collision mapping mismatch: actual={grounds}, expected={expected}")
|
||||
assert u.scene["foot_contact"].num_sensors == 1
|
||||
assert u.scene["foot_contact"].data.force_matrix_w.torch.shape == (args.num_envs, 1, 1, 3)
|
||||
import importlib.metadata
|
||||
|
||||
from SkillFactory.assets.go2_leg import CONVERSION, MANIFEST
|
||||
|
||||
body_masses = u.scene["robot"].data.body_mass.torch[0].tolist()
|
||||
asset_report = {
|
||||
"source": MANIFEST["source"],
|
||||
"derived_urdf_sha256": MANIFEST["derived_urdf_sha256"],
|
||||
"conversion": CONVERSION,
|
||||
"backend": "Kit PhysxCfg",
|
||||
"python": sys.version,
|
||||
"packages": {n: importlib.metadata.version(n) for n in ("isaaclab", "isaaclab-physx", "torch")},
|
||||
"effective_body_mass_kg": dict(zip(u.scene["robot"].body_names, body_masses)),
|
||||
"missing_inertial_fixed_links": MANIFEST["missing_inertial_fixed_links"],
|
||||
"inertial_handling": (
|
||||
"Fixed auxiliary colliders preserved; PhysX mass inferred from geometry/default density"
|
||||
),
|
||||
"pd": MANIFEST["control"],
|
||||
"ground_colliders": grounds,
|
||||
}
|
||||
write_json(args.output / "physical_asset.json", asset_report)
|
||||
result = run(env, args)
|
||||
print(json.dumps(result, indent=2), flush=True)
|
||||
except BaseException:
|
||||
traceback.print_exc()
|
||||
code = 1
|
||||
error = traceback.format_exc()
|
||||
(args.output / "failure.txt").write_text(error)
|
||||
result = {"passed": False, "error": error}
|
||||
finally:
|
||||
finish_run(app, env, args.output / "result.json", result, code)
|
||||
@@ -0,0 +1,55 @@
|
||||
#!/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)
|
||||
@@ -0,0 +1,92 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Convert the extracted Go2 leg with Kit and verify independent physics bodies."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
|
||||
|
||||
def main():
|
||||
if not __debug__:
|
||||
raise RuntimeError("Asset checks require Python without -O/PYTHONOPTIMIZE")
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--asset_dir", type=Path, default=ROOT / "assets/generated/go2_fl")
|
||||
AppLauncher.add_app_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
app = AppLauncher(args).app
|
||||
exit_code = 0
|
||||
try:
|
||||
from pxr import PhysxSchema, Usd, UsdPhysics
|
||||
|
||||
from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg
|
||||
|
||||
directory = args.asset_dir.resolve()
|
||||
cfg = UrdfConverterCfg(
|
||||
asset_path=str(directory / "go2_fl.urdf"),
|
||||
usd_dir=str(directory / "usd"),
|
||||
fix_base=True,
|
||||
merge_fixed_joints=False,
|
||||
force_usd_conversion=True,
|
||||
joint_drive=UrdfConverterCfg.JointDriveCfg(target_type="none"),
|
||||
run_multi_physics_conversion=False,
|
||||
)
|
||||
converter = UrdfConverter(cfg)
|
||||
stage = Usd.Stage.Open(converter.usd_path)
|
||||
if stage is None:
|
||||
raise RuntimeError("Converter produced no readable stage")
|
||||
root = stage.GetDefaultPrim()
|
||||
assert root, "No default prim"
|
||||
bodies = {p.GetName(): str(p.GetPath()) for p in stage.Traverse() if p.HasAPI(UsdPhysics.RigidBodyAPI)}
|
||||
revolute = [p for p in stage.Traverse() if p.IsA(UsdPhysics.RevoluteJoint)]
|
||||
fixed = [p for p in stage.Traverse() if p.IsA(UsdPhysics.FixedJoint)]
|
||||
expected = ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint"]
|
||||
assert sorted(p.GetName() for p in revolute) == sorted(expected)
|
||||
assert "FL_foot" in bodies and "anchor_base" in bodies
|
||||
|
||||
def is_world_fixed(p):
|
||||
targets = UsdPhysics.Joint(p).GetBody0Rel().GetTargets() + UsdPhysics.Joint(p).GetBody1Rel().GetTargets()
|
||||
return len(targets) < 2 or any(not stage.GetPrimAtPath(t).HasAPI(UsdPhysics.RigidBodyAPI) for t in targets)
|
||||
|
||||
assert any(is_world_fixed(p) for p in fixed), "No fixed-to-world joint"
|
||||
foot = stage.GetPrimAtPath(bodies["FL_foot"])
|
||||
assert any(p.HasAPI(UsdPhysics.CollisionAPI) for p in Usd.PrimRange(foot, Usd.TraverseInstanceProxies())), (
|
||||
"No foot collision"
|
||||
)
|
||||
# Importer 3.0 produces nested rigid bodies. The generic spawner stops
|
||||
# at the first rigid body (anchor), so explicitly enable the foot here.
|
||||
PhysxSchema.PhysxContactReportAPI.Apply(foot).CreateThresholdAttr(0.0)
|
||||
stage.GetRootLayer().Save()
|
||||
assert foot.HasAPI(PhysxSchema.PhysxContactReportAPI)
|
||||
used_layers = stage.GetUsedLayers()
|
||||
missing = [
|
||||
layer.identifier for layer in used_layers if not layer.anonymous and not Path(layer.realPath).is_file()
|
||||
]
|
||||
assert not missing, missing
|
||||
mapping = {
|
||||
"usd_path": str(Path(converter.usd_path).relative_to(directory)),
|
||||
"default_prim": str(root.GetPath()),
|
||||
"body_paths": {k: v.removeprefix(str(root.GetPath())) for k, v in bodies.items()},
|
||||
"joint_paths": {p.GetName(): str(p.GetPath()).removeprefix(str(root.GetPath())) for p in revolute},
|
||||
"used_layers": [
|
||||
str(Path(layer.realPath).relative_to(directory)) for layer in used_layers if layer.realPath
|
||||
],
|
||||
"fixed_to_world": True,
|
||||
"merge_fixed_joints": False,
|
||||
}
|
||||
(directory / "conversion.json").write_text(json.dumps(mapping, indent=2) + "\n")
|
||||
print(json.dumps(mapping, indent=2))
|
||||
except BaseException:
|
||||
import traceback
|
||||
|
||||
traceback.print_exc()
|
||||
exit_code = 1
|
||||
finally:
|
||||
app.close(exit_code=exit_code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Convert a PartSpec-derived single-leg bundle and validate its USD mapping."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def main():
|
||||
if not __debug__:
|
||||
raise RuntimeError("Conversion assertions require Python without -O")
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--bundle", type=Path, required=True)
|
||||
AppLauncher.add_app_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
app = AppLauncher(args).app
|
||||
code = 0
|
||||
try:
|
||||
from SkillFactory.legs.spec import load_bundle
|
||||
from SkillFactory.legs.urdf import digest
|
||||
|
||||
from pxr import PhysxSchema, Usd, UsdPhysics
|
||||
|
||||
from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg
|
||||
|
||||
directory = args.bundle.resolve()
|
||||
bundle = load_bundle(directory)
|
||||
converter = UrdfConverter(
|
||||
UrdfConverterCfg(
|
||||
asset_path=str(directory / "leg.urdf"),
|
||||
usd_dir=str(directory / "usd"),
|
||||
fix_base=True,
|
||||
merge_fixed_joints=False,
|
||||
force_usd_conversion=True,
|
||||
joint_drive=UrdfConverterCfg.JointDriveCfg(target_type="none"),
|
||||
run_multi_physics_conversion=False,
|
||||
)
|
||||
)
|
||||
stage = Usd.Stage.Open(converter.usd_path)
|
||||
assert stage and stage.GetDefaultPrim()
|
||||
root = stage.GetDefaultPrim()
|
||||
bodies = {
|
||||
prim.GetName(): str(prim.GetPath()) for prim in stage.Traverse() if prim.HasAPI(UsdPhysics.RigidBodyAPI)
|
||||
}
|
||||
revolute = [prim for prim in stage.Traverse() if prim.IsA(UsdPhysics.RevoluteJoint)]
|
||||
assert sorted(prim.GetName() for prim in revolute) == sorted(bundle["joint_order"])
|
||||
assert set(bundle["links"]) | {bundle["anchor_body"]} == set(bodies)
|
||||
fixed_world = False
|
||||
for prim in stage.Traverse():
|
||||
if prim.IsA(UsdPhysics.FixedJoint):
|
||||
joint = UsdPhysics.Joint(prim)
|
||||
targets = joint.GetBody0Rel().GetTargets() + joint.GetBody1Rel().GetTargets()
|
||||
fixed_world |= len(targets) < 2 or any(
|
||||
not stage.GetPrimAtPath(path).HasAPI(UsdPhysics.RigidBodyAPI) for path in targets
|
||||
)
|
||||
assert fixed_world, "Missing fixed-to-world support"
|
||||
foot = stage.GetPrimAtPath(bodies[bundle["foot"]["body"]])
|
||||
assert any(prim.HasAPI(UsdPhysics.CollisionAPI) for prim in Usd.PrimRange(foot, Usd.TraverseInstanceProxies()))
|
||||
PhysxSchema.PhysxContactReportAPI.Apply(foot).CreateThresholdAttr(0.0)
|
||||
stage.GetRootLayer().Save()
|
||||
layers = [Path(layer.realPath).resolve() for layer in stage.GetUsedLayers() if layer.realPath]
|
||||
assert layers and all(layer.is_relative_to(directory) and layer.is_file() for layer in layers)
|
||||
conversion = {
|
||||
"usd_path": str(Path(converter.usd_path).relative_to(directory)),
|
||||
"manifest_sha256": digest(directory / "manifest.json"),
|
||||
"fixed_to_world": True,
|
||||
"body_paths": {name: path.removeprefix(str(root.GetPath())) for name, path in bodies.items()},
|
||||
"joint_paths": {prim.GetName(): str(prim.GetPath()).removeprefix(str(root.GetPath())) for prim in revolute},
|
||||
"used_layers": sorted(str(layer.relative_to(directory)) for layer in layers),
|
||||
}
|
||||
(directory / "conversion.json").write_text(json.dumps(conversion, indent=2) + "\n")
|
||||
print(json.dumps(conversion, indent=2))
|
||||
except BaseException:
|
||||
import traceback
|
||||
|
||||
traceback.print_exc()
|
||||
code = 1
|
||||
finally:
|
||||
app.close(exit_code=code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,260 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Extract the real Go2 FL subtree without importing Isaac Lab (SI units)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import shutil
|
||||
import subprocess
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
DEFAULT_SOURCE = Path("/home/cen/Embodied_Workspace/unitree_ros/robots/go2_description")
|
||||
JOINTS = ("FL_hip_joint", "FL_thigh_joint", "FL_calf_joint")
|
||||
Q_DEFAULT = (0.0, 0.8, -1.5)
|
||||
|
||||
|
||||
def digest(path: Path) -> str:
|
||||
return hashlib.sha256(path.read_bytes()).hexdigest()
|
||||
|
||||
|
||||
def numbers(text: str) -> list[float]:
|
||||
result = [float(x) for x in text.split()]
|
||||
if not all(math.isfinite(x) for x in result):
|
||||
raise ValueError(f"Non-finite numeric field: {text}")
|
||||
return result
|
||||
|
||||
|
||||
def origin(element: ET.Element) -> dict:
|
||||
value = element.find("origin")
|
||||
return {key: numbers(value.get(key, "0 0 0") if value is not None else "0 0 0") for key in ("xyz", "rpy")}
|
||||
|
||||
|
||||
def inertia_record(link: ET.Element) -> dict | None:
|
||||
inertial = link.find("inertial")
|
||||
if inertial is None:
|
||||
return None
|
||||
mass = float(inertial.find("mass").get("value"))
|
||||
values = {k: float(inertial.find("inertia").get(k, "0")) for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")}
|
||||
a, b, c, d, e, f = (values[k] for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz"))
|
||||
# Sylvester's criterion for the complete symmetric inertia, not just its diagonal.
|
||||
determinant = a * (d * f - e * e) - b * (b * f - c * e) + c * (b * e - c * d)
|
||||
if (
|
||||
not all(math.isfinite(x) for x in (mass, a, b, c, d, e, f))
|
||||
or mass <= 0
|
||||
or min(a, a * d - b * b, determinant) <= 0
|
||||
):
|
||||
raise ValueError(f"Invalid inertia on {link.get('name')}")
|
||||
return {"mass": mass, "inertia": values, "origin": origin(inertial)}
|
||||
|
||||
|
||||
def matmul(a, b):
|
||||
return [[sum(a[i][k] * b[k][j] for k in range(len(b))) for j in range(len(b[0]))] for i in range(len(a))]
|
||||
|
||||
|
||||
def transform(xyz, rpy):
|
||||
r, p, y = rpy
|
||||
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
|
||||
return [
|
||||
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, xyz[0]],
|
||||
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, xyz[1]],
|
||||
[-sp, cp * sr, cp * cr, xyz[2]],
|
||||
[0, 0, 0, 1],
|
||||
]
|
||||
|
||||
|
||||
def foot_center(robot: ET.Element, q=Q_DEFAULT) -> list[float]:
|
||||
"""Collision sphere center in the anchor frame at the specified joint angles [m]."""
|
||||
t = transform([0, 0, 0], [0, 0, 0])
|
||||
by_name = {j.get("name"): j for j in robot.findall("joint")}
|
||||
for name in (*JOINTS, "FL_foot_joint"):
|
||||
j = by_name[name]
|
||||
o = origin(j)
|
||||
t = matmul(t, transform(o["xyz"], o["rpy"]))
|
||||
if name in JOINTS:
|
||||
angle = q[JOINTS.index(name)]
|
||||
axis = numbers(j.find("axis").get("xyz"))
|
||||
# Go2 uses the canonical x and y axes. Refuse unhandled topologies.
|
||||
if axis not in ([1.0, 0.0, 0.0], [0.0, 1.0, 0.0]):
|
||||
raise ValueError(f"Unsupported axis {axis}")
|
||||
t = matmul(t, transform([0, 0, 0], [angle * axis[0], angle * axis[1], 0]))
|
||||
foot = next(link for link in robot.findall("link") if link.get("name") == "FL_foot")
|
||||
o = origin(foot.find("collision"))
|
||||
t = matmul(t, transform(o["xyz"], o["rpy"]))
|
||||
return [t[i][3] for i in range(3)]
|
||||
|
||||
|
||||
def extract(source: Path, output: Path) -> dict:
|
||||
source = source.resolve()
|
||||
source_urdf = source / "urdf/go2_description.urdf"
|
||||
original = ET.parse(source_urdf).getroot()
|
||||
links = {link.get("name"): link for link in original.findall("link")}
|
||||
joints = {j.get("name"): j for j in original.findall("joint")}
|
||||
for name in (*JOINTS, "FL_foot_joint", "FL_hip_rotor_joint"):
|
||||
if name not in joints:
|
||||
raise ValueError(f"Missing required joint: {name}")
|
||||
keep = {"FL_hip", "FL_hip_rotor"}
|
||||
while True:
|
||||
expanded = keep | {j.find("child").get("link") for j in joints.values() if j.find("parent").get("link") in keep}
|
||||
if expanded == keep:
|
||||
break
|
||||
keep = expanded
|
||||
if "FL_foot" not in keep or any(not n.startswith("FL_") for n in keep):
|
||||
raise ValueError("FL branch must contain only the FL leg and its independent foot")
|
||||
robot = ET.Element("robot", name="go2_fl_anchor")
|
||||
anchor = ET.SubElement(robot, "link", name="anchor_base")
|
||||
anchor.append(
|
||||
ET.fromstring(
|
||||
'<inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="1"/>'
|
||||
'<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/></inertial>'
|
||||
)
|
||||
)
|
||||
shift = origin(joints[JOINTS[0]])["xyz"]
|
||||
for link in original.findall("link"):
|
||||
if link.get("name") in keep:
|
||||
inertia_record(link)
|
||||
robot.append(copy.deepcopy(link))
|
||||
for j in original.findall("joint"):
|
||||
if j.find("child").get("link") in keep:
|
||||
j = copy.deepcopy(j)
|
||||
if j.find("parent").get("link") == "base":
|
||||
j.find("parent").set("link", "anchor_base")
|
||||
old_xyz = origin(j)["xyz"]
|
||||
j.find("origin").set("xyz", " ".join(f"{x - s:.12g}" for x, s in zip(old_xyz, shift)))
|
||||
robot.append(j)
|
||||
revolute = [j.get("name") for j in robot.findall("joint") if j.get("type") != "fixed"]
|
||||
if tuple(revolute) != JOINTS:
|
||||
raise ValueError(f"Expected exactly the three ordered joints, got {revolute}")
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
resources = {}
|
||||
for mesh in robot.findall(".//mesh"):
|
||||
filename = mesh.get("filename")
|
||||
prefix = "package://go2_description/"
|
||||
if not filename.startswith(prefix):
|
||||
raise ValueError(f"Unexpected mesh URI: {filename}")
|
||||
relative = Path(filename[len(prefix) :])
|
||||
if relative.is_absolute() or ".." in relative.parts:
|
||||
raise ValueError(f"Unsafe mesh path: {relative}")
|
||||
src, dst = source / relative, output / relative
|
||||
dst.parent.mkdir(parents=True, exist_ok=True)
|
||||
shutil.copy2(src, dst)
|
||||
mesh.set("filename", relative.as_posix())
|
||||
resources[relative.as_posix()] = digest(src)
|
||||
if src.suffix.lower() == ".dae":
|
||||
for image in ET.parse(src).getroot().findall(".//{*}image/{*}init_from"):
|
||||
image_relative = Path(image.text.strip())
|
||||
if image_relative.is_absolute() or ".." in image_relative.parts:
|
||||
raise ValueError("Unsafe DAE image reference")
|
||||
image_src, image_dst = src.parent / image_relative, dst.parent / image_relative
|
||||
image_dst.parent.mkdir(parents=True, exist_ok=True)
|
||||
shutil.copy2(image_src, image_dst)
|
||||
resources[image_dst.relative_to(output).as_posix()] = digest(image_src)
|
||||
license_path = source.parents[1] / "LICENSE"
|
||||
shutil.copy2(license_path, output / "LICENSE")
|
||||
records = []
|
||||
for name in JOINTS:
|
||||
j = joints[name]
|
||||
limits = {k: float(j.find("limit").get(k)) for k in ("lower", "upper", "effort", "velocity")}
|
||||
if (
|
||||
not all(math.isfinite(x) for x in limits.values())
|
||||
or limits["lower"] >= limits["upper"]
|
||||
or min(limits["effort"], limits["velocity"]) <= 0
|
||||
):
|
||||
raise ValueError(f"Invalid limits: {name}")
|
||||
midpoint = (limits["lower"] + limits["upper"]) / 2
|
||||
records.append(
|
||||
{
|
||||
"name": name,
|
||||
"axis": numbers(j.find("axis").get("xyz")),
|
||||
"limits": limits,
|
||||
"soft_limits": [midpoint + 0.9 * (limits[k] - midpoint) for k in ("lower", "upper")],
|
||||
}
|
||||
)
|
||||
radius = float(links["FL_foot"].find("collision/geometry/sphere").get("radius"))
|
||||
center = foot_center(robot)
|
||||
manifest = {
|
||||
"schema_version": 1,
|
||||
"asset": "Go2 FL fixed anchor",
|
||||
"source": {
|
||||
"urdf": str(source_urdf),
|
||||
"sha256": digest(source_urdf),
|
||||
"commit": subprocess.check_output(["git", "-C", str(source), "rev-parse", "HEAD"], text=True).strip(),
|
||||
},
|
||||
"resources": dict(sorted(resources.items())),
|
||||
"license": "BSD-3-Clause",
|
||||
"joint_order": list(JOINTS),
|
||||
"joints": records,
|
||||
"links": {link.get("name"): inertia_record(link) for link in robot.findall("link")},
|
||||
"transforms": {
|
||||
j.get("name"): {"parent": j.find("parent").get("link"), "child": j.find("child").get("link"), **origin(j)}
|
||||
for j in robot.findall("joint")
|
||||
},
|
||||
"missing_inertial_fixed_links": [
|
||||
link.get("name") for link in robot.findall("link") if link.find("inertial") is None
|
||||
],
|
||||
"anchor": {
|
||||
"fixed": True,
|
||||
"original_mount_translation": shift,
|
||||
"synthetic_root_mass": 1.0,
|
||||
"default_height": radius - center[2] + 0.01,
|
||||
},
|
||||
"foot": {"frame": "FL_foot", "radius": radius, "collision_origin": origin(links["FL_foot"].find("collision"))},
|
||||
"control": {
|
||||
"q_default": list(Q_DEFAULT),
|
||||
"action_scale": 0.25,
|
||||
"kp": 25.0,
|
||||
"kd": 0.5,
|
||||
"physics_dt": 0.002,
|
||||
"decimation": 5,
|
||||
},
|
||||
"interface": {
|
||||
"action_shape": ["B", 3],
|
||||
"policy_shape": ["B", 19],
|
||||
"device": "cuda",
|
||||
"dtype": "float32",
|
||||
"quaternion_order": "xyzw",
|
||||
"policy_order": [
|
||||
"q_rel",
|
||||
"dq",
|
||||
"foot_pos_anchor",
|
||||
"foot_vel_anchor",
|
||||
"foot_normal_force_anchor",
|
||||
"contact",
|
||||
"last_action",
|
||||
],
|
||||
"force_semantics": "normal contact force, not full friction wrench",
|
||||
},
|
||||
"skill": {"weights": None, "latent": None, "reaction_wrench": None, "valid": False},
|
||||
"modifications": [
|
||||
"Replace whole body with fixed synthetic anchor root",
|
||||
"Translate mount origin to anchor origin",
|
||||
"Preserve FL fixed subtrees and root-side rotor",
|
||||
"Relative mesh URIs; no ROS runtime dependency",
|
||||
],
|
||||
}
|
||||
ET.indent(robot)
|
||||
ET.ElementTree(robot).write(output / "go2_fl.urdf", encoding="utf-8", xml_declaration=True)
|
||||
manifest["derived_urdf_sha256"] = digest(output / "go2_fl.urdf")
|
||||
(output / "manifest.json").write_text(json.dumps(manifest, indent=2, sort_keys=True) + "\n")
|
||||
return manifest
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--source", type=Path, default=DEFAULT_SOURCE)
|
||||
parser.add_argument("--output", type=Path, default=ROOT / "assets/generated/go2_fl")
|
||||
args = parser.parse_args()
|
||||
manifest = extract(args.source, args.output)
|
||||
print(
|
||||
f"Extracted {manifest['joint_order']} to {args.output}; "
|
||||
f"anchor height={manifest['anchor']['default_height']:.6f} m"
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,24 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Extract an explicitly described real single leg; no simulator startup."""
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from SkillFactory.legs import LegSpec
|
||||
from SkillFactory.legs.urdf import extract_leg
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--spec", type=Path, required=True)
|
||||
parser.add_argument("--source-root", type=Path)
|
||||
parser.add_argument("--output", type=Path)
|
||||
args = parser.parse_args()
|
||||
spec = LegSpec.load(args.spec, args.source_root)
|
||||
output = args.output or Path("assets/generated/legs") / spec.part_id
|
||||
bundle = extract_leg(spec, output)
|
||||
print(f"Extracted {bundle['joint_order']} to {output}; source {bundle['source']['sha256']}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,71 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Finite single-leg GUI physics preview before registering the RL environment."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def main():
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--steps", type=int, default=1500)
|
||||
AppLauncher.add_app_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
app = AppLauncher(args).app
|
||||
code = 0
|
||||
try:
|
||||
import torch
|
||||
from isaaclab_physx.physics import PhysxCfg
|
||||
from SkillFactory.assets.go2_leg import GO2_LEG_CFG, JOINT_NAMES
|
||||
|
||||
import isaaclab.sim as sim_utils
|
||||
from isaaclab.assets import Articulation
|
||||
|
||||
sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.002, device="cuda:0", physics=PhysxCfg()))
|
||||
sim.set_camera_view((1.0, 1.0, 0.8), (0.0, 0.0, 0.35))
|
||||
ground = sim_utils.GroundPlaneCfg()
|
||||
ground.func("/World/ground", ground)
|
||||
light = sim_utils.DomeLightCfg(intensity=2000)
|
||||
light.func("/World/light", light)
|
||||
cfg = GO2_LEG_CFG.replace(prim_path="/World/Robot")
|
||||
cfg.init_state.pos = (0.0, 0.0, 0.60)
|
||||
robot = Articulation(cfg)
|
||||
sim.reset()
|
||||
robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch)
|
||||
robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch)
|
||||
ids, names = robot.find_joints(JOINT_NAMES, preserve_order=True)
|
||||
assert names == JOINT_NAMES and robot.num_joints == 3
|
||||
start = robot.data.root_pos_w.torch.clone()
|
||||
for step in range(args.steps):
|
||||
target = robot.data.default_joint_pos.torch.clone()
|
||||
target[:, ids] += 0.05 * math.sin(2 * math.pi * 0.5 * step * 0.002)
|
||||
robot.set_joint_position_target_index(target=target)
|
||||
robot.write_data_to_sim()
|
||||
sim.step()
|
||||
robot.update(0.002)
|
||||
assert torch.isfinite(robot.data.joint_pos.torch).all()
|
||||
result = {
|
||||
"joint_names": names,
|
||||
"num_joints": robot.num_joints,
|
||||
"body_names": robot.body_names,
|
||||
"q": robot.data.joint_pos.torch.tolist(),
|
||||
"root_drift_m": (robot.data.root_pos_w.torch - start).abs().max().item(),
|
||||
"steps": args.steps,
|
||||
"gui_requested": not args.headless,
|
||||
}
|
||||
assert result["root_drift_m"] <= 1e-4
|
||||
Path("logs/week1_anchor/preview.json").write_text(json.dumps(result, indent=2))
|
||||
print(json.dumps(result, indent=2))
|
||||
except BaseException:
|
||||
traceback.print_exc()
|
||||
code = 1
|
||||
finally:
|
||||
app.close(exit_code=code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,359 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Finite single-leg verification or on-policy rollout in a fresh Kit worker."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
|
||||
from runtime_common import finish_run, write_json
|
||||
|
||||
|
||||
def clone_batch(batch):
|
||||
return {name: value.detach().clone() for name, value in batch.items()}
|
||||
|
||||
|
||||
def verify(env, args):
|
||||
import torch
|
||||
from SkillFactory.tasks.manager_based.leg_skill.mdp import state
|
||||
|
||||
from isaaclab.sim import get_current_stage
|
||||
|
||||
adapter = state(env)
|
||||
stage = get_current_stage()
|
||||
count = sum(1 for _ in stage.Traverse())
|
||||
initial_anchor = adapter.robot.data.body_link_pose_w.torch[:, adapter.anchor].clone()
|
||||
worst_fk, max_torque = 0.0, 0.0
|
||||
timeouts = 0
|
||||
for step in range(args.steps):
|
||||
action = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device)
|
||||
phase = torch.arange(env.num_envs, device=env.device) * 0.1 + step * env.step_dt * 3.141592653589793
|
||||
joint_phase = torch.arange(len(adapter.joint_ids), device=env.device) * 0.35
|
||||
action[:, : len(adapter.joint_ids)] = 0.4 * (phase[:, None] + joint_phase).sin()
|
||||
expected = (adapter.nominal + action[:, : len(adapter.joint_ids)] * adapter.scale).clamp(
|
||||
min=adapter.soft[:, 0], max=adapter.soft[:, 1]
|
||||
)
|
||||
obs, _, terminated, truncated, info = env.step(adapter.actions(action))
|
||||
if terminated.any():
|
||||
raise RuntimeError("Unexpected invalid-state termination")
|
||||
snapshot = adapter.snapshot()
|
||||
for name, value in obs["policy"].items():
|
||||
assert torch.isfinite(value).all()
|
||||
assert torch.equal(value, snapshot[name]), ("Observation term/snapshot mismatch", name)
|
||||
assert (
|
||||
(adapter.robot.data.joint_pos_target.torch[:, adapter.joint_ids][~truncated] - expected[~truncated])
|
||||
.abs()
|
||||
.max()
|
||||
.item()
|
||||
< 1e-6
|
||||
if (~truncated).any()
|
||||
else True
|
||||
)
|
||||
assert torch.equal(
|
||||
obs["policy"]["joint_observation"][:, : len(adapter.joint_ids), 2][~truncated],
|
||||
action[:, : len(adapter.joint_ids)][~truncated],
|
||||
)
|
||||
fk_error = (adapter.fk.foot_position(adapter.q()) - adapter.foot_position()).abs().max().item()
|
||||
worst_fk = max(worst_fk, fk_error)
|
||||
assert fk_error < 1e-4, ("URDF/PhysX frame mismatch", fk_error)
|
||||
torque = (adapter.robot.data.applied_torque.torch[:, adapter.joint_ids] / adapter.effort).abs().max().item()
|
||||
max_torque = max(max_torque, torque)
|
||||
assert torque <= 1.00001
|
||||
if truncated.any():
|
||||
assert info["terminal_tokens"] is not None
|
||||
assert len(info["terminal_ids"]) == truncated.sum().item()
|
||||
terminal = info["terminal_tokens"]
|
||||
terminal_q = adapter.nominal + terminal["joint_observation"][:, : len(adapter.joint_ids), 0] * adapter.scale
|
||||
assert (
|
||||
adapter.fk.foot_position(terminal_q) / adapter.length - terminal["effector_observation"][:, 0, :3]
|
||||
).abs().max().item() < 1e-4
|
||||
assert torch.equal(
|
||||
terminal["joint_observation"][:, : len(adapter.joint_ids), 2],
|
||||
action[info["terminal_ids"], : len(adapter.joint_ids)],
|
||||
)
|
||||
assert obs["policy"]["joint_observation"][truncated].abs().max().item() == 0.0
|
||||
timeouts += truncated.sum().item()
|
||||
robot = adapter.robot
|
||||
q_before, dq_before, action_before = (
|
||||
adapter.q().clone(),
|
||||
robot.data.joint_vel.torch[:, adapter.joint_ids].clone(),
|
||||
env.action_manager.action.clone(),
|
||||
)
|
||||
ids = torch.arange(0, env.num_envs, 2, device=env.device)
|
||||
untouched = torch.arange(1, env.num_envs, 2, device=env.device)
|
||||
old_goal = env.command_manager.get_command("reach").clone()
|
||||
old_time = env._sim_step_counter
|
||||
env.reset(env_ids=ids)
|
||||
assert (adapter.q()[ids] - adapter.nominal).abs().max().item() < 1e-5
|
||||
assert robot.data.joint_vel.torch[ids].abs().max().item() < 1e-5
|
||||
assert (robot.data.joint_pos_target.torch[ids][:, adapter.joint_ids] - adapter.nominal).abs().max().item() < 1e-5
|
||||
assert env.action_manager.action[ids].abs().max().item() == 0.0
|
||||
assert env.scene["foot_contact"].data.net_forces_w_history.torch[ids].abs().max().item() == 0.0
|
||||
assert env.episode_length_buf[ids].max().item() == 0
|
||||
if len(untouched):
|
||||
assert (adapter.q()[untouched] - q_before[untouched]).abs().max().item() <= 1e-6
|
||||
assert (
|
||||
robot.data.joint_vel.torch[untouched][:, adapter.joint_ids] - dq_before[untouched]
|
||||
).abs().max().item() <= 1e-6
|
||||
assert torch.equal(env.action_manager.action[untouched], action_before[untouched])
|
||||
assert torch.equal(env.command_manager.get_command("reach")[untouched], old_goal[untouched])
|
||||
before_empty = clone_batch(adapter.snapshot())
|
||||
env.reset(env_ids=[])
|
||||
assert all(torch.equal(v, adapter.snapshot()[k]) for k, v in before_empty.items())
|
||||
for invalid in ([-1], [env.num_envs], [0, 0], [0.5]):
|
||||
try:
|
||||
env.reset(env_ids=invalid)
|
||||
except ValueError:
|
||||
pass
|
||||
else:
|
||||
raise AssertionError("Invalid reset indices accepted")
|
||||
native_bad = env.action_manager.action.clone()
|
||||
native_bad[:, 0] = torch.nan
|
||||
manager_before = (env.action_manager.action.clone(), env.action_manager.prev_action.clone())
|
||||
try:
|
||||
env.step(native_bad)
|
||||
except ValueError:
|
||||
pass
|
||||
else:
|
||||
raise AssertionError("Non-finite native action accepted")
|
||||
assert torch.equal(env.action_manager.action, manager_before[0])
|
||||
assert torch.equal(env.action_manager.prev_action, manager_before[1])
|
||||
assert env.single_observation_space["policy"]["joint_mask"].dtype.kind == "b"
|
||||
bad = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device)
|
||||
bad[:, 0] = torch.nan
|
||||
try:
|
||||
adapter.actions(bad)
|
||||
except ValueError:
|
||||
pass
|
||||
else:
|
||||
raise AssertionError("Non-finite active action accepted")
|
||||
padded = torch.zeros_like(bad)
|
||||
padded[:, len(adapter.joint_ids) :] = torch.nan
|
||||
assert torch.isfinite(adapter.actions(padded)).all()
|
||||
assert env._sim_step_counter == old_time
|
||||
assert sum(1 for _ in stage.Traverse()) == count
|
||||
drift = (robot.data.body_link_pose_w.torch[:, adapter.anchor] - initial_anchor).abs().max().item()
|
||||
assert drift <= 1e-4
|
||||
return {
|
||||
"passed": True,
|
||||
"mode": "verify",
|
||||
"part_id": adapter.bundle["spec"]["part_id"],
|
||||
"num_envs": env.num_envs,
|
||||
"steps": args.steps,
|
||||
"physics_dt": env.physics_dt,
|
||||
"control_dt": env.step_dt,
|
||||
"joint_order": adapter.bundle["joint_order"],
|
||||
"max_fk_error_m": worst_fk,
|
||||
"max_normalized_applied_torque": max_torque,
|
||||
"fixed_anchor_pose_error": drift,
|
||||
"timeouts_checked": timeouts,
|
||||
"indexed_reset": True,
|
||||
"empty_reset_noop": True,
|
||||
"stage_prim_count": count,
|
||||
"policy_shapes": {k: list(v.shape) for k, v in adapter.snapshot().items()},
|
||||
"trained_skill": False,
|
||||
}
|
||||
|
||||
|
||||
def rollout(env, args):
|
||||
import torch
|
||||
from SkillFactory.legs.policy import load_policy
|
||||
from SkillFactory.tasks.manager_based.leg_skill.mdp import state
|
||||
|
||||
if args.weights is None:
|
||||
raise ValueError("rollout requires --weights")
|
||||
model = load_policy(args.weights, env.device).eval()
|
||||
adapter = state(env)
|
||||
observation, _ = env.reset(seed=args.seed)
|
||||
snapshots, actions, logs, values, rewards, next_values, done = [], [], [], [], [], [], []
|
||||
with torch.no_grad():
|
||||
for _ in range(args.horizon):
|
||||
batch = clone_batch(observation["policy"])
|
||||
action, log_prob, value = model.act(batch)
|
||||
observation, reward, terminated, truncated, info = env.step(adapter.actions(action))
|
||||
if terminated.any():
|
||||
raise RuntimeError("Invalid physical state during rollout")
|
||||
nxt = model.value(observation["policy"])
|
||||
if truncated.any():
|
||||
terminal_value = model.value(info["terminal_tokens"])
|
||||
ids = info["terminal_ids"]
|
||||
nxt[ids] = torch.where(truncated[ids], terminal_value, nxt[ids])
|
||||
nxt[terminated] = 0.0
|
||||
assert torch.isfinite(reward).all() and torch.isfinite(nxt).all()
|
||||
assert all(torch.isfinite(v).all() for v in observation["policy"].values())
|
||||
snapshots.append(batch)
|
||||
actions.append(action)
|
||||
logs.append(log_prob)
|
||||
values.append(value)
|
||||
rewards.append(reward.clone())
|
||||
next_values.append(nxt)
|
||||
done.append((terminated | truncated).clone())
|
||||
data = {"observation": {name: torch.stack([batch[name] for batch in snapshots]).cpu() for name in snapshots[0]}}
|
||||
for name, items in {
|
||||
"actions": actions,
|
||||
"log_prob": logs,
|
||||
"values": values,
|
||||
"rewards": rewards,
|
||||
"next_values": next_values,
|
||||
"done": done,
|
||||
}.items():
|
||||
data[name] = torch.stack(items).cpu()
|
||||
torch.save(data, args.output / "rollout.pt")
|
||||
return {
|
||||
"passed": True,
|
||||
"mode": "rollout",
|
||||
"part_id": adapter.bundle["spec"]["part_id"],
|
||||
"horizon": args.horizon,
|
||||
"num_envs": env.num_envs,
|
||||
"mean_reward_per_control_step": data["rewards"].mean().item(),
|
||||
"episode_end_count": data["done"].sum().item(),
|
||||
"source": adapter.bundle["source"],
|
||||
"trained_skill": False,
|
||||
}
|
||||
|
||||
|
||||
def contact_trial(env, args):
|
||||
import torch
|
||||
from SkillFactory.tasks.manager_based.leg_skill.mdp import state
|
||||
|
||||
adapter = state(env)
|
||||
press = [float(v) for v in args.contact_action.split(",")]
|
||||
if len(press) != len(adapter.joint_ids):
|
||||
raise ValueError("contact-action must have one value per declared actuator")
|
||||
action = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device)
|
||||
action[:, : len(press)] = torch.tensor(press, device=env.device)
|
||||
rows = {}
|
||||
for phase, sign in (("air", -1.0), ("press", 1.0)):
|
||||
forces, gaps = [], []
|
||||
for step in range(100):
|
||||
env.step(adapter.actions(action * sign))
|
||||
if step >= 70:
|
||||
forces.append(env.scene["foot_contact"].data.force_matrix_w.torch[:, 0, 0].clone())
|
||||
gaps.append(adapter.gap().clone())
|
||||
force, gap = torch.stack(forces), torch.stack(gaps)
|
||||
rows[phase] = {
|
||||
"mean_fz_N": force[:, :, 2].mean(dim=0).min().item(),
|
||||
"max_force_N": force.norm(dim=-1).max().item(),
|
||||
"min_gap_m": gap.min().item(),
|
||||
}
|
||||
if phase == "air":
|
||||
assert gap.min().item() >= 0.01 and force.norm(dim=-1).max().item() < 0.1, rows
|
||||
else:
|
||||
assert force[:, :, 2].mean(dim=0).min().item() > 1.0 and gap.min().item() >= -0.005, rows
|
||||
ids = torch.arange(0, env.num_envs, 2, device=env.device)
|
||||
env.reset(env_ids=ids)
|
||||
assert env.scene["foot_contact"].data.net_forces_w_history.torch[ids].abs().max().item() == 0.0
|
||||
return rows
|
||||
|
||||
|
||||
def main():
|
||||
if not __debug__:
|
||||
raise RuntimeError("Worker checks require Python without -O")
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
from isaaclab_tasks.utils import add_launcher_args, launch_simulation
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--bundle", type=Path, required=True)
|
||||
parser.add_argument("--mode", choices=("verify", "rollout"), default="verify")
|
||||
parser.add_argument("--num_envs", type=int, default=16)
|
||||
parser.add_argument("--max-joints", type=int, default=8)
|
||||
parser.add_argument("--steps", type=int, default=300)
|
||||
parser.add_argument("--horizon", type=int, default=32)
|
||||
parser.add_argument("--episode-length", type=float, default=0.2)
|
||||
parser.add_argument("--weights", type=Path)
|
||||
parser.add_argument("--contact-action", help="Optional configured press action, e.g. 0,0,1; verify only")
|
||||
parser.add_argument("--seed", type=int, default=42)
|
||||
parser.add_argument("--output", type=Path, default=Path("logs/leg_interface/verify"))
|
||||
add_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
if (
|
||||
args.num_envs < 1
|
||||
or args.max_joints < 1
|
||||
or args.steps < 10
|
||||
or args.horizon < 1
|
||||
or not math.isfinite(args.episode_length)
|
||||
or args.episode_length <= 0
|
||||
):
|
||||
parser.error("Require positive finite counts/duration and steps>=10")
|
||||
if args.mode == "rollout" and args.weights is None:
|
||||
parser.error("rollout requires --weights")
|
||||
if args.contact_action is not None:
|
||||
if args.mode != "verify":
|
||||
parser.error("contact-action is for verification only")
|
||||
try:
|
||||
press = [float(value) for value in args.contact_action.split(",")]
|
||||
except ValueError:
|
||||
parser.error("contact-action requires comma-separated finite numbers")
|
||||
if not press or not all(math.isfinite(value) for value in press):
|
||||
parser.error("contact-action requires comma-separated finite numbers")
|
||||
args.output.mkdir(parents=True, exist_ok=True)
|
||||
result = {"passed": False, "status": "running"}
|
||||
write_json(args.output / "result.json", result)
|
||||
app = AppLauncher(args).app
|
||||
code, env = 0, None
|
||||
try:
|
||||
import gymnasium as gym
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
import torch
|
||||
from SkillFactory.tasks.manager_based.leg_skill.leg_env_cfg import LegEnvCfg
|
||||
|
||||
from pxr import UsdPhysics
|
||||
|
||||
from isaaclab.sim import get_current_stage
|
||||
|
||||
cfg = LegEnvCfg(bundle_dir=str(args.bundle.resolve()), max_joints=args.max_joints)
|
||||
cfg.scene.num_envs = args.num_envs
|
||||
cfg.seed = args.seed
|
||||
cfg.episode_length_s = args.episode_length
|
||||
if args.contact_action is not None:
|
||||
from SkillFactory.legs.urdf import forward, multiply, pose
|
||||
|
||||
bundle = cfg.leg_bundle
|
||||
nominal = {j["name"]: j["q_default"] for j in bundle["spec"]["joints"]}
|
||||
poses, _ = forward(bundle["tree"], nominal)
|
||||
foot = bundle["foot"]
|
||||
collision = multiply(poses[foot["body"]], pose(foot["xyz"], foot["rpy"]))
|
||||
extent = (
|
||||
foot["radius"]
|
||||
if foot["type"] == "sphere"
|
||||
else sum(abs(collision[2][i]) * foot["size"][i] / 2 for i in range(3))
|
||||
)
|
||||
cfg.scene.robot.init_state.pos = (0.0, 0.0, extent - collision[2][3] + 0.01)
|
||||
cfg.terminations.time_out = None
|
||||
with launch_simulation(cfg, args):
|
||||
torch.manual_seed(args.seed)
|
||||
torch.set_float32_matmul_precision("highest")
|
||||
env = gym.make("SkillFactory-LegSkill-v0", cfg=cfg).unwrapped
|
||||
env.reset(seed=args.seed)
|
||||
stage = get_current_stage()
|
||||
grounds = [
|
||||
str(p.GetPath())
|
||||
for p in stage.Traverse()
|
||||
if str(p.GetPath()).startswith("/World/ground/") and p.HasAPI(UsdPhysics.CollisionAPI)
|
||||
]
|
||||
assert grounds == cfg.scene.foot_contact.filter_prim_paths_expr
|
||||
assert env.scene["foot_contact"].num_sensors == 1
|
||||
assert env.scene["foot_contact"].data.force_matrix_w.torch.shape == (args.num_envs, 1, 1, 3)
|
||||
result = verify(env, args) if args.mode == "verify" else rollout(env, args)
|
||||
if args.contact_action is not None:
|
||||
result["contact_trial"] = contact_trial(env, args)
|
||||
masses = dict(zip(env.scene["robot"].body_names, env.scene["robot"].data.body_mass.torch[0].tolist()))
|
||||
result["effective_body_mass_kg"] = masses
|
||||
result["manifest_sha256"] = cfg.leg_bundle["conversion"]["manifest_sha256"]
|
||||
result["seed"] = args.seed
|
||||
result["device"] = env.device
|
||||
print(json.dumps(result, indent=2))
|
||||
except BaseException:
|
||||
error = traceback.format_exc()
|
||||
print(error, file=sys.stderr, flush=True)
|
||||
result = {"passed": False, "error": error}
|
||||
code = 1
|
||||
finally:
|
||||
finish_run(app, env, args.output / "result.json", result, code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+15
-13
@@ -9,8 +9,8 @@ Script to print all the available environments in Isaac Lab.
|
||||
The script iterates over all registered environments and stores the details in a table.
|
||||
It prints the name of the environment, the entry point and the config file.
|
||||
|
||||
All the environments are registered in the `SkillFactory` extension. They start
|
||||
with `Isaac` in their name.
|
||||
List only environments whose config entry points belong to SkillFactory.
|
||||
Both the original Cartpole template and the single-leg anchor are retained.
|
||||
"""
|
||||
|
||||
"""Launch Isaac Sim Simulator first."""
|
||||
@@ -44,9 +44,8 @@ simulation_app = app_launcher.app
|
||||
"""Rest everything follows."""
|
||||
|
||||
import gymnasium as gym
|
||||
from prettytable import PrettyTable
|
||||
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
from prettytable import PrettyTable
|
||||
|
||||
# PLACEHOLDER: Extension template (do not remove this comment)
|
||||
with contextlib.suppress(ImportError):
|
||||
@@ -57,7 +56,7 @@ def _format_presets(preset_map: dict | None) -> str:
|
||||
"""Format a preset map returned by :func:`enumerate_task_presets` into a human-readable string.
|
||||
|
||||
Args:
|
||||
preset_map: Mapping of :class:`~SkillFactory.utils.preset_target.PresetTarget`
|
||||
preset_map: Mapping of :class:`~isaaclab_tasks.utils.preset_target.PresetTarget`
|
||||
to sorted preset name lists, or ``None`` when the env cfg could not be loaded.
|
||||
|
||||
Returns:
|
||||
@@ -66,7 +65,7 @@ def _format_presets(preset_map: dict | None) -> str:
|
||||
"""
|
||||
if preset_map is None:
|
||||
return "(unavailable)"
|
||||
from SkillFactory.utils.preset_target import PresetTarget
|
||||
from isaaclab_tasks.utils.preset_target import PresetTarget
|
||||
|
||||
lines = []
|
||||
labels = {
|
||||
@@ -87,11 +86,12 @@ def main():
|
||||
task_specs = [
|
||||
spec
|
||||
for spec in gym.registry.values()
|
||||
if "Template-" in spec.id and (args_cli.keyword is None or args_cli.keyword in spec.id)
|
||||
if str(spec.kwargs.get("env_cfg_entry_point", "")).startswith("SkillFactory.")
|
||||
and (args_cli.keyword is None or args_cli.keyword in spec.id)
|
||||
]
|
||||
|
||||
if args_cli.show_presets:
|
||||
from SkillFactory.utils.preset_cli import enumerate_task_presets
|
||||
from isaaclab_tasks.utils.preset_cli import enumerate_task_presets
|
||||
|
||||
table = PrettyTable(["S. No.", "Task Name", "Entry Point", "Config", "Presets"])
|
||||
table.title = "Available Environments in Isaac Lab"
|
||||
@@ -125,11 +125,13 @@ def main():
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
exit_code = 0
|
||||
try:
|
||||
# run the main function
|
||||
main()
|
||||
except Exception as e:
|
||||
raise e
|
||||
except BaseException:
|
||||
import traceback
|
||||
|
||||
traceback.print_exc()
|
||||
exit_code = 1
|
||||
finally:
|
||||
# close the app
|
||||
simulation_app.close()
|
||||
simulation_app.close(exit_code=exit_code)
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -55,6 +55,7 @@ from isaaclab_tasks.utils.hydra import hydra_task_config
|
||||
import cli_args # isort: skip
|
||||
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
|
||||
with contextlib.suppress(ImportError):
|
||||
import isaaclab_tasks_experimental # noqa: F401
|
||||
|
||||
|
||||
@@ -44,6 +44,7 @@ from isaaclab_tasks.utils.hydra import hydra_task_config
|
||||
import cli_args # isort: skip
|
||||
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
|
||||
with contextlib.suppress(ImportError):
|
||||
import isaaclab_tasks_experimental # noqa: F401
|
||||
|
||||
|
||||
@@ -53,6 +53,7 @@ import cli_args # isort: skip
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
|
||||
with contextlib.suppress(ImportError):
|
||||
import isaaclab_tasks_experimental # noqa: F401
|
||||
|
||||
|
||||
@@ -41,6 +41,7 @@ RL_ROOT = Path(__file__).resolve().parents[1]
|
||||
CLI_ARGS = import_local_module("isaaclab_rsl_rl_cli_args", RL_ROOT / "rsl_rl" / "cli_args.py")
|
||||
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
|
||||
with contextlib.suppress(ImportError):
|
||||
import isaaclab_tasks_experimental # noqa: F401
|
||||
|
||||
|
||||
@@ -0,0 +1,56 @@
|
||||
"""Stdlib-only reporting and shutdown helpers; safe to import before Kit."""
|
||||
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import tempfile
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
|
||||
CRITICAL_LOG = re.compile(r"\[(?:Error|Fatal)\]|buffer\s+overflow", re.IGNORECASE)
|
||||
|
||||
|
||||
def write_json(path, data):
|
||||
"""Replace complete reports atomically; never serialize NaN/Inf as valid JSON."""
|
||||
path = Path(path)
|
||||
content = json.dumps(data, indent=2, allow_nan=False) + "\n"
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary = None
|
||||
try:
|
||||
with tempfile.NamedTemporaryFile(mode="w", encoding="utf-8", dir=path.parent, delete=False) as stream:
|
||||
temporary = Path(stream.name)
|
||||
stream.write(content)
|
||||
os.replace(temporary, path)
|
||||
finally:
|
||||
if temporary is not None:
|
||||
temporary.unlink(missing_ok=True)
|
||||
|
||||
|
||||
def critical_log_lines(path):
|
||||
with Path(path).open(errors="replace") as stream:
|
||||
return [line.rstrip() for line in stream if CRITICAL_LOG.search(line)]
|
||||
|
||||
|
||||
def finish_run(app, env, path, result, exit_code=0):
|
||||
"""Close the environment before publishing success and Kit's fast exit."""
|
||||
if env is not None:
|
||||
try:
|
||||
env.close()
|
||||
except BaseException:
|
||||
error = traceback.format_exc()
|
||||
traceback.print_exc()
|
||||
result.update(passed=False, shutdown_error=error)
|
||||
if "milestone_complete" in result:
|
||||
result["milestone_complete"] = False
|
||||
exit_code = 1
|
||||
if not result.get("passed"):
|
||||
exit_code = exit_code or 1
|
||||
if exit_code:
|
||||
result["passed"] = False
|
||||
result["status"] = "completed" if not exit_code else "failed"
|
||||
try:
|
||||
write_json(path, result)
|
||||
except BaseException:
|
||||
traceback.print_exc()
|
||||
exit_code = 1
|
||||
app.close(exit_code=exit_code)
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Finite shared-policy PPO prototype across asset-homogeneous Kit workers.
|
||||
|
||||
Workers restart for each rollout in this initial integration runner. Use small
|
||||
update counts; this is not yet a throughput-optimized long-training service.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
|
||||
from runtime_common import critical_log_lines, write_json
|
||||
|
||||
|
||||
def main():
|
||||
import torch
|
||||
from SkillFactory.legs.policy import LegActorCritic, load_policy, ppo_update, save_policy
|
||||
from SkillFactory.legs.spec import load_bundle
|
||||
from SkillFactory.legs.tokens import FEATURE_SCHEMA
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--bundles", type=Path, nargs="+", required=True)
|
||||
parser.add_argument("--num_envs", type=int, default=16, help="Per-asset environment count")
|
||||
parser.add_argument("--max-joints", type=int, default=8)
|
||||
parser.add_argument("--horizon", type=int, default=32)
|
||||
parser.add_argument("--updates", type=int, default=1)
|
||||
parser.add_argument("--episode-length", type=float, default=0.2)
|
||||
parser.add_argument("--seed", type=int, default=42)
|
||||
parser.add_argument("--device", default="cuda:0")
|
||||
parser.add_argument("--output", type=Path, default=Path("logs/leg_interface/train_smoke"))
|
||||
args = parser.parse_args()
|
||||
if (
|
||||
min(args.num_envs, args.max_joints, args.horizon, args.updates) < 1
|
||||
or not math.isfinite(args.episode_length)
|
||||
or args.episode_length <= 0
|
||||
):
|
||||
parser.error("Counts and episode length must be positive")
|
||||
args.output.mkdir(parents=True, exist_ok=True)
|
||||
report = {
|
||||
"passed": False,
|
||||
"status": "running",
|
||||
"trained_skill": False,
|
||||
"seed": args.seed,
|
||||
"feature_schema": FEATURE_SCHEMA,
|
||||
}
|
||||
write_json(args.output / "result.json", report)
|
||||
start = time.perf_counter()
|
||||
try:
|
||||
bundles = [load_bundle(path, require_usd=True) for path in args.bundles]
|
||||
ids = [bundle["spec"]["part_id"] for bundle in bundles]
|
||||
if len(set(ids)) != len(ids) or any(len(bundle["joints"]) > args.max_joints for bundle in bundles):
|
||||
raise ValueError("Require unique assets fitting max-joints")
|
||||
timing = {(bundle["spec"]["physics_dt"], bundle["spec"]["decimation"]) for bundle in bundles}
|
||||
if len(timing) != 1:
|
||||
raise ValueError("Initial mixed trainer requires a common physics/control timestep")
|
||||
torch.manual_seed(args.seed)
|
||||
torch.set_float32_matmul_precision("highest")
|
||||
model = LegActorCritic().to(args.device)
|
||||
args.device = str(next(model.parameters()).device)
|
||||
optimizer = torch.optim.Adam(model.parameters(), lr=3e-4, eps=1e-5)
|
||||
initial = {k: v.detach().clone() for k, v in model.state_dict().items()}
|
||||
rows = []
|
||||
for update in range(args.updates):
|
||||
weights = args.output / "sampling_policy.pt"
|
||||
save_policy(model, weights, {"update": update})
|
||||
all_data, workers = [], []
|
||||
for index, (bundle_path, part_id) in enumerate(zip(args.bundles, ids)):
|
||||
directory = args.output / f"update_{update:04d}" / part_id
|
||||
directory.mkdir(parents=True, exist_ok=True)
|
||||
log_path = directory / "worker.log"
|
||||
command = [
|
||||
sys.executable,
|
||||
str(Path(__file__).with_name("leg_worker.py")),
|
||||
"--bundle",
|
||||
str(bundle_path.resolve()),
|
||||
"--mode",
|
||||
"rollout",
|
||||
"--weights",
|
||||
str(weights.resolve()),
|
||||
"--num_envs",
|
||||
str(args.num_envs),
|
||||
"--max-joints",
|
||||
str(args.max_joints),
|
||||
"--horizon",
|
||||
str(args.horizon),
|
||||
"--episode-length",
|
||||
str(args.episode_length),
|
||||
"--seed",
|
||||
str(args.seed + index + update * 100),
|
||||
"--device",
|
||||
args.device,
|
||||
"--output",
|
||||
str(directory.resolve()),
|
||||
]
|
||||
with log_path.open("w") as log:
|
||||
child = subprocess.run(command, stdout=log, stderr=subprocess.STDOUT)
|
||||
critical = critical_log_lines(log_path)
|
||||
if child.returncode or critical:
|
||||
raise RuntimeError(f"Worker failed ({child.returncode}): {log_path}; {critical[-3:]}")
|
||||
worker = json.loads((directory / "result.json").read_text())
|
||||
if (
|
||||
not worker["passed"]
|
||||
or worker["device"] != str(torch.device(args.device))
|
||||
or worker["manifest_sha256"] != bundles[index]["conversion"]["manifest_sha256"]
|
||||
):
|
||||
raise RuntimeError(f"Worker report/device/provenance mismatch: {directory}")
|
||||
workers.append(worker)
|
||||
all_data.append(torch.load(directory / "rollout.pt", weights_only=True, map_location=args.device))
|
||||
# Equal per-asset environment counts yield balanced morphology sampling.
|
||||
data = {
|
||||
"observation": {
|
||||
key: torch.cat([item["observation"][key] for item in all_data], dim=1)
|
||||
for key in all_data[0]["observation"]
|
||||
}
|
||||
}
|
||||
for key in ("actions", "log_prob", "values", "rewards", "next_values", "done"):
|
||||
data[key] = torch.cat([item[key] for item in all_data], dim=1)
|
||||
with torch.no_grad():
|
||||
sampled = {key: value.flatten(0, 1) for key, value in data["observation"].items()}
|
||||
current_log, _, _ = model.evaluate(sampled, data["actions"].flatten(0, 1))
|
||||
mismatch = (current_log - data["log_prob"].flatten()).abs().max().item()
|
||||
if not math.isfinite(mismatch) or mismatch > 1e-5:
|
||||
raise RuntimeError(f"Sampling policy/log-prob mismatch: {mismatch}")
|
||||
metrics = ppo_update(model, optimizer, data)
|
||||
metrics["sampling_log_prob_error"] = mismatch
|
||||
metrics.update(update=update, workers=workers, mean_reward=data["rewards"].mean().item())
|
||||
rows.append(metrics)
|
||||
write_json(args.output / "progress.json", rows)
|
||||
print(
|
||||
f"PPO update {update}: reward={metrics['mean_reward']:.6f}, value_loss={metrics['value_loss']:.6f}",
|
||||
flush=True,
|
||||
)
|
||||
difference = max((model.state_dict()[key] - value).abs().max().item() for key, value in initial.items())
|
||||
if not difference > 0 or not all(torch.isfinite(value).all() for value in model.state_dict().values()):
|
||||
raise RuntimeError("PPO did not produce a finite parameter update")
|
||||
checkpoint = args.output / "policy.pt"
|
||||
save_policy(model, checkpoint, {"training_assets": ids, "seed": args.seed, "updates": args.updates})
|
||||
loaded = load_policy(checkpoint, args.device).eval()
|
||||
sample = {key: value[0] for key, value in data["observation"].items()}
|
||||
with torch.no_grad():
|
||||
before = model.act(sample, deterministic=True)
|
||||
after = loaded.act(sample, deterministic=True)
|
||||
if not all(torch.equal(a, b) for a, b in zip(before, after)):
|
||||
raise RuntimeError("Checkpoint reload mismatch")
|
||||
report.update(
|
||||
passed=True,
|
||||
status="completed",
|
||||
training_assets=ids,
|
||||
updates=rows,
|
||||
environment_control_steps=args.updates * args.horizon * args.num_envs * len(ids),
|
||||
max_parameter_change=difference,
|
||||
checkpoint_reload_exact=True,
|
||||
wall_time_s=time.perf_counter() - start,
|
||||
device=args.device,
|
||||
checkpoint=str(checkpoint),
|
||||
capability="URMA-inspired goal-conditioned shared PPO smoke; not a converged/modular Skill",
|
||||
)
|
||||
except BaseException:
|
||||
error = traceback.format_exc()
|
||||
print(error, file=sys.stderr)
|
||||
report.update(status="failed", error=error)
|
||||
write_json(args.output / "result.json", report)
|
||||
raise SystemExit(1) from None
|
||||
write_json(args.output / "result.json", report)
|
||||
print(f"Shared PPO integration passed: {args.output}/result.json; trained_skill=False", flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,494 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Deterministic exit-criteria tests for the real Go2 single-leg anchor."""
|
||||
|
||||
import math
|
||||
import time
|
||||
|
||||
from anchor_common import launch
|
||||
from runtime_common import critical_log_lines, write_json
|
||||
|
||||
|
||||
def configure(parser):
|
||||
parser.add_argument(
|
||||
"--suite",
|
||||
choices=["contract", "stability", "calibration", "contact", "reset", "long", "core", "full"],
|
||||
default="full",
|
||||
)
|
||||
parser.add_argument("--duration", type=float, default=600.0)
|
||||
parser.add_argument("--reset_count", type=int, default=1334)
|
||||
|
||||
|
||||
def check_contract(env):
|
||||
import torch
|
||||
from SkillFactory.assets.go2_leg import JOINT_NAMES
|
||||
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
|
||||
|
||||
u = env.unwrapped
|
||||
obs, _ = env.reset()
|
||||
robot = u.scene["robot"]
|
||||
assert robot.joint_names == JOINT_NAMES
|
||||
assert robot.num_joints == 3
|
||||
assert obs["policy"].shape == (u.num_envs, 19)
|
||||
assert obs["policy"].dtype == torch.float32 and obs["policy"].is_cuda
|
||||
assert torch.isfinite(obs["policy"]).all()
|
||||
d = diagnostics(u)
|
||||
assert d["joint_mask"].shape == (u.num_envs, 3) and d["joint_mask"].dtype == torch.bool
|
||||
assert d["force_history_w"].shape == (u.num_envs, 5, 1, 3)
|
||||
actions = torch.zeros((u.num_envs, 3), device=u.device)
|
||||
u.action_manager.process_action(actions)
|
||||
term = u.action_manager.get_term("joint_position")
|
||||
assert torch.allclose(term.processed_actions, robot.data.default_joint_pos.torch, atol=1e-7)
|
||||
u.action_manager.process_action(actions + 10.0)
|
||||
assert torch.allclose(term.processed_actions, robot.data.default_joint_pos.torch + 0.25, atol=1e-7)
|
||||
assert u.action_manager.action.max() == 1.0
|
||||
limits = robot.data.soft_joint_pos_limits.torch
|
||||
assert ((term.processed_actions >= limits[:, :, 0]) & (term.processed_actions <= limits[:, :, 1])).all()
|
||||
counter = u._sim_step_counter
|
||||
try:
|
||||
env.step(torch.full_like(actions, float("nan")))
|
||||
except ValueError:
|
||||
pass
|
||||
else:
|
||||
raise AssertionError("NaN input was accepted")
|
||||
assert u._sim_step_counter == counter
|
||||
env.reset()
|
||||
obs, _, done, timeout, _ = env.step(actions)
|
||||
assert torch.isfinite(obs["policy"]).all() and not done.any() and not timeout.any()
|
||||
return {
|
||||
"passed": True,
|
||||
"policy_shape": list(obs["policy"].shape),
|
||||
"device": str(obs["policy"].device),
|
||||
"joint_order": robot.joint_names,
|
||||
"force_history_shape": list(d["force_history_w"].shape),
|
||||
"nan_rejected_without_physics_step": True,
|
||||
}
|
||||
|
||||
|
||||
def _step(env, actions):
|
||||
import torch
|
||||
|
||||
obs, _, done, timeout, _ = env.step(actions)
|
||||
assert not done.any() and not timeout.any(), "Unexpected automatic reset"
|
||||
assert torch.isfinite(obs["policy"]).all(), "Non-finite observation"
|
||||
return obs
|
||||
|
||||
|
||||
def check_stability(env):
|
||||
import torch
|
||||
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
|
||||
|
||||
u = env.unwrapped
|
||||
env.reset()
|
||||
robot = u.scene["robot"]
|
||||
initial = diagnostics(u)["anchor_pose_w"].clone()
|
||||
actions = torch.zeros((u.num_envs, 3), device=u.device)
|
||||
errors = torch.zeros((u.num_envs, 3), device=u.device)
|
||||
max_drift = max_angle = 0.0
|
||||
for step in range(6200):
|
||||
actions[:] = 0.4 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
|
||||
_step(env, actions)
|
||||
d = diagnostics(u)
|
||||
assert (d["gap"] >= 0.01).all(), "Stability test must be in air"
|
||||
limits = robot.data.joint_pos_limits.torch
|
||||
assert ((d["q"] >= limits[:, :, 0] - 0.01) & (d["q"] <= limits[:, :, 1] + 0.01)).all()
|
||||
assert (d["applied_torque"].abs() <= robot.data.joint_effort_limits.torch + 1e-4).all()
|
||||
max_drift = max(max_drift, (d["anchor_pose_w"][:, :3] - initial[:, :3]).abs().max().item())
|
||||
dot = (d["anchor_pose_w"][:, 3:] * initial[:, 3:]).sum(dim=-1).abs().clamp(max=1.0)
|
||||
max_angle = max(max_angle, (2 * torch.acos(dot)).max().item())
|
||||
if step >= 200:
|
||||
errors += (d["q"] - d["q_target"]).square()
|
||||
rms = (errors / 6000).sqrt().max(dim=0).values
|
||||
assert (rms <= 0.10).all(), f"Tracking RMS: {rms.tolist()}"
|
||||
assert max_drift <= 1e-4 and max_angle <= 1e-3
|
||||
return {
|
||||
"passed": True,
|
||||
"warmup_s": 2.0,
|
||||
"duration_s": 60.0,
|
||||
"amplitude_rad": 0.1,
|
||||
"frequency_hz": 0.5,
|
||||
"worst_env_rms_rad": rms.tolist(),
|
||||
"root_drift_m": max_drift,
|
||||
"root_angle_rad": max_angle,
|
||||
}
|
||||
|
||||
|
||||
def check_calibration(env):
|
||||
import torch
|
||||
|
||||
u = env.unwrapped
|
||||
env.reset()
|
||||
actions = torch.zeros((u.num_envs, 3), device=u.device)
|
||||
force = torch.zeros(u.num_envs, device=u.device)
|
||||
for step in range(500):
|
||||
_step(env, actions)
|
||||
if step >= 300:
|
||||
force += u.scene["probe_contact"].data.force_matrix_w.torch[:, 0, 0, 2]
|
||||
force /= 200
|
||||
assert ((force - 9.81).abs() / 9.81 <= 0.2).all(), force.tolist()
|
||||
return {
|
||||
"passed": True,
|
||||
"probe_mass_kg": 1.0,
|
||||
"expected_N": 9.81,
|
||||
"mean_fz_min_N": force.min().item(),
|
||||
"mean_fz_max_N": force.max().item(),
|
||||
}
|
||||
|
||||
|
||||
def check_contact(env):
|
||||
import torch
|
||||
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
|
||||
|
||||
u = env.unwrapped
|
||||
env.reset()
|
||||
actions = torch.zeros((u.num_envs, 3), device=u.device)
|
||||
air_max, press_min, penetration = 0.0, float("inf"), 0.0
|
||||
latency = 0
|
||||
transitions = []
|
||||
for cycle in range(20):
|
||||
# Alternate assignments across environments: verify contact channels
|
||||
# and independent commands without changing the fixed anchor poses.
|
||||
signs = torch.where(torch.arange(u.num_envs, device=u.device) % 2 == 0, 1.0, -1.0)
|
||||
for phase in ("air", "press"):
|
||||
actions[:, 2] = -signs if phase == "air" else signs
|
||||
air_ids = actions[:, 2] < 0
|
||||
press_ids = ~air_ids
|
||||
gap_crossed = torch.zeros(u.num_envs, dtype=torch.bool, device=u.device)
|
||||
latency_steps = torch.zeros(u.num_envs, dtype=torch.int32, device=u.device)
|
||||
air_window, press_window, gap_window = [], [], []
|
||||
for step in range(100):
|
||||
_step(env, actions)
|
||||
d = diagnostics(u)
|
||||
f = d["normal_force_w"]
|
||||
contact = f.norm(dim=-1) > 1.0
|
||||
crossed = (d["gap"] <= 0.0001) & press_ids
|
||||
gap_crossed |= crossed
|
||||
pending = gap_crossed & ~contact & press_ids
|
||||
latency_steps[pending] += 1
|
||||
if step >= 70:
|
||||
if air_ids.any():
|
||||
assert (d["gap"][air_ids] >= 0.01).all(), ("air gap", d["gap"].tolist())
|
||||
air_window.append(f[air_ids].norm(dim=-1))
|
||||
if press_ids.any():
|
||||
press_window.append(f[press_ids, 2])
|
||||
gap_window.append(d["gap"][press_ids])
|
||||
latency = max(latency, latency_steps.max().item())
|
||||
assert latency_steps.max() <= 1, f"Contact response >1 control cycle: {latency_steps.tolist()}"
|
||||
if air_window:
|
||||
current = torch.stack(air_window).max().item()
|
||||
air_max = max(air_max, current)
|
||||
assert current < 0.1, f"Air force: {current}"
|
||||
if press_window:
|
||||
mean = torch.stack(press_window).mean(dim=0).min().item()
|
||||
press_min = min(press_min, mean)
|
||||
assert mean > 1.0, f"Pressed force: {mean}"
|
||||
penetration = max(penetration, max(0.0, -torch.stack(gap_window).min().item()))
|
||||
assert penetration <= 0.005, f"Penetration: {penetration}"
|
||||
transitions.append([cycle, phase, d["gap"][0].item(), d["normal_force_w"][0, 2].item()])
|
||||
return {
|
||||
"passed": True,
|
||||
"cycles": 20,
|
||||
"air_force_max_N": air_max,
|
||||
"press_fz_min_N": press_min,
|
||||
"penetration_max_m": penetration,
|
||||
"latency_control_steps_max": latency,
|
||||
"trace": transitions,
|
||||
}
|
||||
|
||||
|
||||
def check_resets(env, count):
|
||||
import torch
|
||||
|
||||
from isaaclab.sim import get_current_stage
|
||||
|
||||
u = env.unwrapped
|
||||
robot = u.scene["robot"]
|
||||
actions = torch.zeros((u.num_envs, 3), device=u.device)
|
||||
all_ids = torch.arange(u.num_envs, device=u.device, dtype=torch.int32)
|
||||
env.reset()
|
||||
for _ in range(50):
|
||||
actions[:, 2] = torch.where(all_ids % 2 == 0, 1.0, -1.0)
|
||||
_step(env, actions)
|
||||
stage = get_current_stage()
|
||||
prim_count = sum(1 for _ in stage.Traverse())
|
||||
sensor_id, robot_id = id(u.scene["foot_contact"]), id(robot)
|
||||
torch.cuda.synchronize()
|
||||
mem0 = torch.cuda.memory_allocated()
|
||||
memory = []
|
||||
max_q_error = max_vel = max_unselected = max_history = 0.0
|
||||
latencies = []
|
||||
for iteration in range(count):
|
||||
mode = iteration % 4
|
||||
ids = (
|
||||
all_ids
|
||||
if mode == 0
|
||||
else (
|
||||
all_ids[iteration % u.num_envs : iteration % u.num_envs + 1]
|
||||
if mode == 1
|
||||
else all_ids[iteration % 2 :: 2]
|
||||
)
|
||||
)
|
||||
if mode == 3:
|
||||
ids = all_ids[:0] # documented no-op edge handling at caller
|
||||
selected = torch.zeros(u.num_envs, dtype=torch.bool, device=u.device)
|
||||
selected[ids.long()] = True
|
||||
q0, dq0 = robot.data.joint_pos.torch.clone(), robot.data.joint_vel.torch.clone()
|
||||
action0 = u.action_manager.action.clone()
|
||||
sim_count = u._sim_step_counter
|
||||
start = time.perf_counter()
|
||||
if ids.numel():
|
||||
u.reset(env_ids=ids)
|
||||
torch.cuda.synchronize()
|
||||
latencies.append(time.perf_counter() - start)
|
||||
assert u._sim_step_counter == sim_count, "Reset advanced physical time"
|
||||
if ids.numel():
|
||||
max_q_error = max(
|
||||
max_q_error,
|
||||
(robot.data.joint_pos.torch[ids] - robot.data.default_joint_pos.torch[ids]).abs().max().item(),
|
||||
)
|
||||
max_vel = max(max_vel, robot.data.joint_vel.torch[ids].abs().max().item())
|
||||
assert max_q_error <= 1e-5 and max_vel <= 1e-5
|
||||
assert u.episode_length_buf[ids].max() == 0
|
||||
assert u.action_manager.action[ids].abs().max() == 0
|
||||
assert u.action_manager.prev_action[ids].abs().max() == 0
|
||||
assert torch.allclose(
|
||||
robot.data.joint_pos_target.torch[ids], robot.data.default_joint_pos.torch[ids], atol=1e-5
|
||||
)
|
||||
h = u.scene["foot_contact"].data.net_forces_w_history.torch[ids]
|
||||
max_history = max(max_history, h.abs().max().item())
|
||||
assert max_history == 0.0, f"Stale contact history after reset: {max_history}"
|
||||
other = ~selected
|
||||
if other.any():
|
||||
change = max(
|
||||
(robot.data.joint_pos.torch[other] - q0[other]).abs().max().item(),
|
||||
(robot.data.joint_vel.torch[other] - dq0[other]).abs().max().item(),
|
||||
(u.action_manager.action[other] - action0[other]).abs().max().item(),
|
||||
)
|
||||
max_unselected = max(max_unselected, change)
|
||||
assert change <= 1e-6, f"Unselected state changed during reset: {change}"
|
||||
_step(env, actions)
|
||||
assert id(u.scene["foot_contact"]) == sensor_id and id(u.scene["robot"]) == robot_id
|
||||
if iteration == min(7, count - 1):
|
||||
# Include verification scratch tensors and warm-up all subset shapes.
|
||||
# The baseline must not precede the test's own snapshot allocations.
|
||||
torch.cuda.synchronize()
|
||||
mem0 = torch.cuda.memory_allocated()
|
||||
free, total = torch.cuda.mem_get_info()
|
||||
gpu0 = total - free
|
||||
if iteration % 100 == 0:
|
||||
torch.cuda.synchronize()
|
||||
memory.append(torch.cuda.memory_allocated())
|
||||
torch.cuda.synchronize()
|
||||
mem1 = torch.cuda.memory_allocated()
|
||||
free, total = torch.cuda.mem_get_info()
|
||||
gpu1 = total - free
|
||||
assert gpu1 <= gpu0 * 1.05, f"Device memory growth (including Warp/PhysX): {gpu0}->{gpu1}"
|
||||
assert get_current_stage() == stage and sum(1 for _ in stage.Traverse()) == prim_count
|
||||
assert mem1 <= mem0 * 1.05, f"CUDA memory growth: {mem0}->{mem1}"
|
||||
return {
|
||||
"passed": True,
|
||||
"reset_operations": count,
|
||||
"nonempty_reset_operations": count - count // 4,
|
||||
"q_error_max_rad": max_q_error,
|
||||
"dq_max_rad_s": max_vel,
|
||||
"unselected_change_max": max_unselected,
|
||||
"stale_history_max_N": max_history,
|
||||
"cuda_before_bytes": mem0,
|
||||
"cuda_after_bytes": mem1,
|
||||
"device_used_before_bytes": gpu0,
|
||||
"device_used_after_bytes": gpu1,
|
||||
"cuda_samples_bytes": memory,
|
||||
"reset_latency_mean_ms": 1000 * sum(latencies) / len(latencies),
|
||||
"reset_latency_max_ms": 1000 * max(latencies),
|
||||
"stage_prim_count": prim_count,
|
||||
}
|
||||
|
||||
|
||||
def check_long(env, duration):
|
||||
import torch
|
||||
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
|
||||
|
||||
u = env.unwrapped
|
||||
env.reset()
|
||||
steps = round(duration / u.step_dt)
|
||||
assert steps > 0
|
||||
actions = torch.zeros((u.num_envs, 3), device=u.device)
|
||||
initial = diagnostics(u)["anchor_pose_w"].clone()
|
||||
drift = 0.0
|
||||
forces = 0.0
|
||||
counter = u._sim_step_counter
|
||||
for _ in range(200):
|
||||
_step(env, actions)
|
||||
d = diagnostics(u) # warm the same diagnostic/scratch allocations as rollout
|
||||
counter = u._sim_step_counter
|
||||
torch.cuda.synchronize()
|
||||
start = time.perf_counter()
|
||||
mem0 = torch.cuda.memory_allocated()
|
||||
free, total = torch.cuda.mem_get_info()
|
||||
gpu0 = total - free
|
||||
rows = []
|
||||
for step in range(steps):
|
||||
actions[:, 0] = 0.2 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
|
||||
actions[:, 2] = torch.where(
|
||||
torch.arange(u.num_envs, device=u.device) % 2 == 0,
|
||||
0.8 * math.sin(2 * math.pi * 0.25 * step * u.step_dt),
|
||||
-0.8 * math.sin(2 * math.pi * 0.25 * step * u.step_dt),
|
||||
)
|
||||
_step(env, actions)
|
||||
d = diagnostics(u)
|
||||
assert all(torch.isfinite(d[k]).all() for k in ("q", "dq", "computed_torque", "normal_force_w"))
|
||||
limits = u.scene["robot"].data.joint_pos_limits.torch
|
||||
assert ((d["q"] >= limits[:, :, 0] - 0.01) & (d["q"] <= limits[:, :, 1] + 0.01)).all()
|
||||
assert (d["applied_torque"].abs() <= u.scene["robot"].data.joint_effort_limits.torch + 1e-4).all()
|
||||
drift = max(drift, (d["anchor_pose_w"][:, :3] - initial[:, :3]).abs().max().item())
|
||||
forces = max(forces, d["normal_force_w"][:, 2].max().item())
|
||||
if step % 1000 == 0:
|
||||
rows.append(
|
||||
[
|
||||
step * u.step_dt,
|
||||
*d["q"][0].tolist(),
|
||||
d["gap"][0].item(),
|
||||
d["normal_force_w"][0, 2].item(),
|
||||
torch.cuda.memory_allocated(),
|
||||
]
|
||||
)
|
||||
if step % 10000 == 0:
|
||||
print(f"[VERIFY] long rollout {step}/{steps}", flush=True)
|
||||
torch.cuda.synchronize()
|
||||
elapsed = time.perf_counter() - start
|
||||
assert u._sim_step_counter - counter == steps * 5
|
||||
assert (u.episode_length_buf == steps + 200).all()
|
||||
assert drift <= 1e-4
|
||||
mem1 = torch.cuda.memory_allocated()
|
||||
free, total = torch.cuda.mem_get_info()
|
||||
gpu1 = total - free
|
||||
assert mem1 <= mem0 * 1.05
|
||||
assert gpu1 <= gpu0 * 1.05, f"Device memory growth (including Warp/PhysX): {gpu0}->{gpu1}"
|
||||
return {
|
||||
"passed": True,
|
||||
"duration_s": steps * u.step_dt,
|
||||
"control_steps": steps,
|
||||
"physics_steps": steps * 5,
|
||||
"physics_dt": u.physics_dt,
|
||||
"control_dt": u.step_dt,
|
||||
"wall_seconds": elapsed,
|
||||
"env_steps_per_second": steps * u.num_envs / elapsed,
|
||||
"root_drift_m": drift,
|
||||
"max_normal_fz_N": forces,
|
||||
"cuda_before_bytes": mem0,
|
||||
"cuda_after_bytes": mem1,
|
||||
"cuda_peak_bytes": torch.cuda.max_memory_allocated(),
|
||||
"device_used_before_bytes": gpu0,
|
||||
"device_used_after_bytes": gpu1,
|
||||
"device_total_bytes": total,
|
||||
"trace": rows,
|
||||
}
|
||||
|
||||
|
||||
def run(env, args):
|
||||
import json
|
||||
|
||||
if args.num_envs < 1 or args.reset_count < 8 or args.duration <= 0 or not math.isfinite(args.duration):
|
||||
raise ValueError("Require num_envs >=1, reset_count >=8, finite duration >0")
|
||||
result = {
|
||||
"suite": args.suite,
|
||||
"num_envs": args.num_envs,
|
||||
"seed": args.seed,
|
||||
"backend": "Kit PhysxCfg",
|
||||
"contract": check_contract(env),
|
||||
}
|
||||
checks = {
|
||||
"stability": lambda: check_stability(env),
|
||||
"calibration": lambda: check_calibration(env),
|
||||
"contact": lambda: check_contact(env),
|
||||
"reset": lambda: check_resets(env, args.reset_count),
|
||||
"long": lambda: check_long(env, args.duration),
|
||||
}
|
||||
names = ["contact", "reset", "long"] if args.suite == "core" else ([args.suite] if args.suite in checks else [])
|
||||
for name in names:
|
||||
print(f"[VERIFY] Starting {name}", flush=True)
|
||||
result[name] = checks[name]()
|
||||
if "trace" in result[name]:
|
||||
import csv
|
||||
|
||||
headers = (
|
||||
["cycle", "phase", "gap_m", "normal_fz_N"]
|
||||
if name == "contact"
|
||||
else [
|
||||
"time_s",
|
||||
"q_hip_rad",
|
||||
"q_thigh_rad",
|
||||
"q_calf_rad",
|
||||
"gap_m",
|
||||
"normal_fz_N",
|
||||
"torch_allocated_bytes",
|
||||
]
|
||||
)
|
||||
with (args.output / f"{name}.csv").open("w", newline="") as stream:
|
||||
writer = csv.writer(stream)
|
||||
writer.writerow(headers)
|
||||
writer.writerows(result[name]["trace"])
|
||||
(args.output / "checkpoint.json").write_text(json.dumps(result, indent=2) + "\n")
|
||||
result["passed"] = True
|
||||
result["milestone_complete"] = False # individual suites are not full acceptance
|
||||
return result
|
||||
|
||||
|
||||
def main():
|
||||
# Full acceptance runs fresh Kit processes so the air anchor and probe are
|
||||
# independent physical scenes, not reset-time mutations of a fixed joint.
|
||||
import argparse
|
||||
import json
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
parser = argparse.ArgumentParser(add_help=False)
|
||||
parser.add_argument("--suite", default="full")
|
||||
parser.add_argument("--output", type=Path, default=Path("logs/week1_anchor/acceptance"))
|
||||
args, argv = parser.parse_known_args()
|
||||
if args.suite != "full" or "--help" in sys.argv or "-h" in sys.argv:
|
||||
launch(__doc__, configure, run)
|
||||
return
|
||||
args.output.mkdir(parents=True, exist_ok=True)
|
||||
write_json(args.output / "result.json", {"passed": False, "status": "running"})
|
||||
results = {}
|
||||
for suite in ("stability", "calibration", "core"):
|
||||
output = args.output / suite
|
||||
print(f"[VERIFY] Fresh process: {suite}", flush=True)
|
||||
with (args.output / f"{suite}.log").open("w") as log:
|
||||
child = subprocess.run(
|
||||
[sys.executable, str(Path(__file__).resolve()), *argv, "--suite", suite, "--output", str(output)],
|
||||
stdout=log,
|
||||
stderr=subprocess.STDOUT,
|
||||
)
|
||||
critical = critical_log_lines(args.output / f"{suite}.log")
|
||||
if child.returncode or critical:
|
||||
write_json(
|
||||
args.output / "result.json",
|
||||
{
|
||||
"passed": False,
|
||||
"failed_suite": suite,
|
||||
"returncode": child.returncode,
|
||||
"critical_log_lines": critical[-20:],
|
||||
"completed": results,
|
||||
},
|
||||
)
|
||||
raise SystemExit(child.returncode or 1)
|
||||
report = json.loads((output / "result.json").read_text())
|
||||
if not report.get("passed"):
|
||||
write_json(args.output / "result.json", {"passed": False, "failed_suite": suite, "report": report})
|
||||
raise SystemExit(1)
|
||||
results[suite] = report
|
||||
core = results["core"]
|
||||
complete = (
|
||||
core["num_envs"] == 64
|
||||
and core["long"]["duration_s"] >= 600.0
|
||||
and core["reset"]["nonempty_reset_operations"] >= 1000
|
||||
)
|
||||
result = {"passed": True, "milestone_complete": complete, "results": results}
|
||||
write_json(args.output / "result.json", result)
|
||||
print(f"[VERIFY] Passed; complete acceptance={complete}; {args.output}/result.json", flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -38,6 +38,7 @@ args_cli, hydra_args = setup_preset_cli(parser)
|
||||
sys.argv = [sys.argv[0]] + hydra_args
|
||||
|
||||
import SkillFactory.tasks # noqa: F401
|
||||
|
||||
MAX_STEPS = 100
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user