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:
2026-10-10 09:58:30 +08:00
parent 180f8f7f97
commit 7f39abb613
52 changed files with 4370 additions and 147 deletions
+92
View File
@@ -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()
+85
View File
@@ -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()
+260
View File
@@ -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()
+24
View File
@@ -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()
+71
View File
@@ -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()