chore(release): v0.1.1 USD文件初步校验
原因：记录 L20 USD 初步校验、严格浮动覆盖层、受限动力学回放及 HDF5 交付契约，包版本更新为 0.1.1。 验证：68 项 CPU/USD 回归测试通过，Ruff/format 与暂存 diff 检查通过；已核验单环境 small 2x480、合成 HDF5 2x960 步明确 PASS。独立暂存审查未发现问题。完整 pre-commit 因模块缺失未执行，干净环境安装未验证。 兼容性：Cartpole 及任务 ID 不变，原始 USD/URDF 未改；旧 prepared 覆盖层需重新生成。仅合成轨迹初步校验，不代表真实专家回放、训练或硬件验收。
This commit is contained in:
@@ -1,7 +1,7 @@
|
||||
[package]
|
||||
|
||||
# Semantic Versioning is used: https://semver.org/
|
||||
version = "0.1.0"
|
||||
version = "0.1.1"
|
||||
|
||||
# Description
|
||||
category = "isaaclab"
|
||||
|
||||
@@ -0,0 +1,338 @@
|
||||
"""Composed USD inspection and non-destructive, topology-only floating overlay.
|
||||
|
||||
Requires pxr, not a running Kit application. No physical coupling is synthesized.
|
||||
"""
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
|
||||
from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics, UsdUtils
|
||||
|
||||
|
||||
def _require(condition, message):
|
||||
if not condition:
|
||||
raise ValueError(message)
|
||||
|
||||
|
||||
def _sha(path):
|
||||
return hashlib.sha256(Path(path).read_bytes()).hexdigest()
|
||||
|
||||
|
||||
def _plain(value):
|
||||
if value is None or isinstance(value, (str, bool, int)):
|
||||
return value
|
||||
if isinstance(value, float):
|
||||
return value if math.isfinite(value) else str(value)
|
||||
try:
|
||||
return [_plain(item) for item in value]
|
||||
except TypeError:
|
||||
return str(value)
|
||||
|
||||
|
||||
def dependencies(source):
|
||||
"""Hash the local complete dependency bundle, including names relative to the entry.
|
||||
|
||||
Sorted UTF-8 records are ``relative_posix_path + NUL + sha256 + LF``.
|
||||
Renaming dependency files changes identity. Moving the entire bundle does not.
|
||||
"""
|
||||
source = Path(source).resolve(strict=True)
|
||||
layers, assets, unresolved = UsdUtils.ComputeAllDependencies(str(source))
|
||||
_require(not unresolved, f"Unresolved dependencies: {unresolved}")
|
||||
paths = {Path(layer.realPath).resolve(strict=True) for layer in layers}
|
||||
paths.update(Path(asset).resolve(strict=True) for asset in assets)
|
||||
paths.add(source)
|
||||
entries = sorted((os.path.relpath(path, source.parent).replace(os.sep, "/"), _sha(path)) for path in paths)
|
||||
digest = hashlib.sha256()
|
||||
for name, sha in entries:
|
||||
digest.update(f"{name}\0{sha}\n".encode())
|
||||
return [{"path": name, "sha256": sha} for name, sha in entries], digest.hexdigest()
|
||||
|
||||
|
||||
def inspect(source, source_urdf=None):
|
||||
source = Path(source).resolve(strict=True)
|
||||
deps, bundle_sha = dependencies(source)
|
||||
stage = Usd.Stage.Open(str(source))
|
||||
_require(stage and stage.GetDefaultPrim(), "USD must have a default prim")
|
||||
_require(not stage.GetCompositionErrors(), f"Composition errors: {stage.GetCompositionErrors()}")
|
||||
_require(
|
||||
UsdGeom.GetStageUpAxis(stage) == "Z"
|
||||
and UsdGeom.GetStageMetersPerUnit(stage) == 1
|
||||
and UsdPhysics.GetStageKilogramsPerUnit(stage) == 1,
|
||||
"Expected Z-up, meter, kilogram asset",
|
||||
)
|
||||
default = stage.GetDefaultPrim()
|
||||
prims = list(Usd.PrimRange(default))
|
||||
bodies = [prim for prim in prims if prim.HasAPI(UsdPhysics.RigidBodyAPI)]
|
||||
body_paths = {str(prim.GetPath()) for prim in bodies}
|
||||
joints, world_joints, child_paths = [], [], set()
|
||||
coupling_evidence = []
|
||||
for prim in prims:
|
||||
for prop in prim.GetProperties():
|
||||
if any(term in prop.GetName().lower() for term in ("mimic", "tendon", "gearing", "coupling")):
|
||||
coupling_evidence.append(str(prop.GetPath()))
|
||||
# Raw schemas retain evidence even if a PhysX schema plugin is not loaded.
|
||||
schemas = str(prim.GetMetadata("apiSchemas"))
|
||||
if any(term in schemas.lower() for term in ("mimic", "tendon", "gearing", "coupling")):
|
||||
coupling_evidence.append(f"{prim.GetPath()}: {schemas}")
|
||||
if not prim.IsA(UsdPhysics.Joint):
|
||||
continue
|
||||
joint = UsdPhysics.Joint(prim)
|
||||
if joint.GetJointEnabledAttr().Get() is False:
|
||||
continue
|
||||
b0 = [str(p) for p in joint.GetBody0Rel().GetTargets()]
|
||||
b1 = [str(p) for p in joint.GetBody1Rel().GetTargets()]
|
||||
_require(len(b1) == 1 and b1[0] in body_paths, f"Unresolved/non-body body1: {prim.GetPath()}")
|
||||
if not b0:
|
||||
_require(prim.IsA(UsdPhysics.FixedJoint), "Unsupported non-fixed world joint")
|
||||
world_joints.append({"path": str(prim.GetPath()), "body1": b1[0]})
|
||||
continue
|
||||
_require(len(b0) == 1 and b0[0] in body_paths, f"Unresolved/non-body body0: {prim.GetPath()}")
|
||||
_require(prim.IsA(UsdPhysics.RevoluteJoint), f"Unsupported joint type {prim.GetTypeName()}")
|
||||
child_paths.add(b1[0])
|
||||
revolute = UsdPhysics.RevoluteJoint(prim)
|
||||
lower, upper = revolute.GetLowerLimitAttr().Get(), revolute.GetUpperLimitAttr().Get()
|
||||
_require(
|
||||
lower is not None
|
||||
and upper is not None
|
||||
and math.isfinite(lower)
|
||||
and math.isfinite(upper)
|
||||
and lower <= upper,
|
||||
f"Missing/nonfinite limits: {prim.GetPath()}",
|
||||
)
|
||||
joints.append(
|
||||
{
|
||||
"name": prim.GetName(),
|
||||
"path": str(prim.GetPath()),
|
||||
"body0": b0[0],
|
||||
"body1": b1[0],
|
||||
"axis": revolute.GetAxisAttr().Get(),
|
||||
"lower_rad": math.radians(lower),
|
||||
"upper_rad": math.radians(upper),
|
||||
"authored_drive_properties_usd_units": {
|
||||
attr.GetName(): _plain(attr.Get())
|
||||
for attr in prim.GetAttributes()
|
||||
if attr.GetName().startswith(("drive:", "physxJoint:")) and attr.HasAuthoredValue()
|
||||
},
|
||||
}
|
||||
)
|
||||
_require(bodies and joints, "No articulated hand found")
|
||||
roots = sorted(body_paths - child_paths)
|
||||
_require(len(roots) == 1, f"Expected one body-tree root, got {roots}")
|
||||
_require(len(joints) == len(bodies) - 1 and len(child_paths) == len(joints), "Not a simple articulated tree")
|
||||
reachable = {roots[0]}
|
||||
while True:
|
||||
expanded = reachable | {joint["body1"] for joint in joints if joint["body0"] in reachable}
|
||||
if expanded == reachable:
|
||||
break
|
||||
reachable = expanded
|
||||
_require(reachable == body_paths, "Disconnected/cyclic body graph")
|
||||
joints.sort(key=lambda joint: joint["name"])
|
||||
names = [joint["name"] for joint in joints]
|
||||
_require(len(set(names)) == len(names), "Duplicate joint names")
|
||||
articulation_roots = [str(p.GetPath()) for p in prims if p.HasAPI(UsdPhysics.ArticulationRootAPI)]
|
||||
_require(len(articulation_roots) == 1, f"Expected one articulation root, got {articulation_roots}")
|
||||
_require(len(world_joints) <= 1 and all(j["body1"] == roots[0] for j in world_joints), "Ambiguous world anchor")
|
||||
source_info = {"status": "NOT_PROVIDED", "mimic": []}
|
||||
if source_urdf is not None:
|
||||
path = Path(source_urdf).resolve(strict=True)
|
||||
xml = ET.parse(path).getroot()
|
||||
urdf_joints = {j.attrib["name"]: j for j in xml.findall("joint") if j.attrib["type"] != "fixed"}
|
||||
_require(set(urdf_joints) == set(names), "Source URDF/USD joint-name mismatch")
|
||||
mimic = []
|
||||
for joint in joints:
|
||||
node = urdf_joints[joint["name"]]
|
||||
limit = node.find("limit")
|
||||
_require(node.attrib["type"] == "revolute" and limit is not None, "Unsupported source URDF joint")
|
||||
for key in ("lower", "upper"):
|
||||
_require(
|
||||
abs(float(limit.attrib[key]) - joint[f"{key}_rad"]) < 1e-5,
|
||||
f"Source URDF/USD limit mismatch: {joint['name']}",
|
||||
)
|
||||
parent, child = node.find("parent"), node.find("child")
|
||||
_require(
|
||||
parent is not None
|
||||
and child is not None
|
||||
and parent.attrib["link"] == joint["body0"].split("/")[-1]
|
||||
and child.attrib["link"] == joint["body1"].split("/")[-1],
|
||||
"Source URDF/USD link mismatch",
|
||||
)
|
||||
equation = node.find("mimic")
|
||||
if equation is not None:
|
||||
_require(equation.attrib["joint"] in names, "Unresolved source mimic reference")
|
||||
multiplier = float(equation.attrib.get("multiplier", "1"))
|
||||
offset = float(equation.attrib.get("offset", "0"))
|
||||
_require(math.isfinite(multiplier) and math.isfinite(offset), "Nonfinite mimic equation")
|
||||
mimic.append(
|
||||
{
|
||||
"joint": joint["name"],
|
||||
"reference": equation.attrib["joint"],
|
||||
"multiplier": multiplier,
|
||||
"offset_rad": offset,
|
||||
}
|
||||
)
|
||||
source_info = {"status": "STRUCTURAL_MATCH_ONLY", "file_name": path.name, "sha256": _sha(path), "mimic": mimic}
|
||||
coupling_status = "UNVERIFIED"
|
||||
physx_coupling = [
|
||||
item
|
||||
for item in coupling_evidence
|
||||
if "physxmimic" in item.lower() or "physxtendon" in item.lower() or ".physxmimic" in item.lower()
|
||||
]
|
||||
newton_coupling = [item for item in coupling_evidence if "newtonmimic" in item.lower()]
|
||||
if newton_coupling or physx_coupling:
|
||||
# PhysX 110.1.13 parses NewtonMimicAPI natively. Prefix is not a backend gate.
|
||||
coupling_status = "MIMIC_SCHEMA_PRESENT_RUNTIME_UNVERIFIED"
|
||||
elif source_info["mimic"]:
|
||||
coupling_status = "MISSING_MIMIC_SCHEMA"
|
||||
return {
|
||||
"manifest_version": "l20_asset_manifest_v1",
|
||||
"asset_sha256": bundle_sha,
|
||||
"entry_file": source.name,
|
||||
"dependencies": deps,
|
||||
"composition_errors": [],
|
||||
"unresolved_dependencies": [],
|
||||
"default_prim": str(default.GetPath()),
|
||||
"root_link": roots[0].split("/")[-1],
|
||||
"root_body_path": roots[0],
|
||||
"units": {"length": "m", "mass": "kg", "up_axis": "Z"},
|
||||
"physics_variant": default.GetVariantSet("Physics").GetVariantSelection(),
|
||||
"articulation_roots": articulation_roots,
|
||||
"world_fixed_joints": world_joints,
|
||||
"bodies": [
|
||||
{
|
||||
"path": str(p.GetPath()),
|
||||
"reset_xform_stack": UsdGeom.Xformable(p).GetResetXformStack(),
|
||||
"mass_properties": {
|
||||
a.GetName(): _plain(a.Get())
|
||||
for a in p.GetAttributes()
|
||||
if a.GetName()
|
||||
in ("physics:mass", "physics:centerOfMass", "physics:diagonalInertia", "physics:principalAxes")
|
||||
},
|
||||
}
|
||||
for p in bodies
|
||||
],
|
||||
"joints": joints,
|
||||
"source_urdf": source_info,
|
||||
"coupling_evidence": coupling_evidence,
|
||||
"physx_coupling_evidence": physx_coupling,
|
||||
"coupling_status": coupling_status,
|
||||
"dynamic_replay_ready": False,
|
||||
"blockers": [
|
||||
"Runtime mimic response is unverified; original follower drives need suppression for diagnostics.",
|
||||
"No calibrated wrist support controller or whole-hand dynamic replay validation.",
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def prepare_floating_overlay(source, output):
|
||||
"""Release only the world anchor; preserve transforms and source layer bytes.
|
||||
|
||||
This is NOT a usable control asset: missing coupling remains missing. Output
|
||||
is explicitly marked diagnostic and must never overwrite an existing file.
|
||||
"""
|
||||
source, output = Path(source).resolve(strict=True), Path(output).resolve()
|
||||
_require(not output.exists(), "Refusing to overwrite output")
|
||||
before = inspect(source)
|
||||
_require(before["physics_variant"] == "physx", "Only the current PhysX variant is supported")
|
||||
anchors = before["world_fixed_joints"]
|
||||
_require(
|
||||
len(anchors) == 1 and before["articulation_roots"] == [anchors[0]["path"]],
|
||||
"Expected a fixed-world articulation root; will not guess topology",
|
||||
)
|
||||
original = Usd.Stage.Open(str(source))
|
||||
stage = Usd.Stage.CreateInMemory()
|
||||
stage.GetRootLayer().subLayerPaths = [str(source)]
|
||||
stage.SetDefaultPrim(stage.GetPrimAtPath(before["default_prim"]))
|
||||
UsdGeom.SetStageUpAxis(stage, "Z")
|
||||
UsdGeom.SetStageMetersPerUnit(stage, 1)
|
||||
UsdPhysics.SetStageKilogramsPerUnit(stage, 1)
|
||||
anchor = stage.GetPrimAtPath(anchors[0]["path"])
|
||||
# Snapshot resolved schema fallbacks before removing the API. Preserve them
|
||||
# explicitly on the inert anchor and the new root, not just authored values.
|
||||
articulation_values = []
|
||||
for attr in anchor.GetAttributes():
|
||||
if attr.GetName().startswith("physxArticulation:") and attr.Get() is not None:
|
||||
_require(not attr.GetTimeSamples(), f"Time samples prohibited: {attr.GetPath()}")
|
||||
articulation_values.append((attr.GetName(), attr.GetTypeName(), attr.Get()))
|
||||
anchor.RemoveAPI(UsdPhysics.ArticulationRootAPI)
|
||||
anchor.RemoveAppliedSchema("PhysxArticulationAPI")
|
||||
UsdPhysics.Joint(anchor).GetJointEnabledAttr().Set(False)
|
||||
root = stage.GetPrimAtPath(before["root_body_path"])
|
||||
UsdPhysics.ArticulationRootAPI.Apply(root)
|
||||
root.AddAppliedSchema("PhysxArticulationAPI")
|
||||
for name, dtype, value in articulation_values:
|
||||
anchor.CreateAttribute(name, dtype).Set(value)
|
||||
root.CreateAttribute(name, dtype).Set(value)
|
||||
stage.GetRootLayer().customLayerData = {"purpose": "L20 topology-only diagnostic; dynamic replay BLOCKED"}
|
||||
cache0, cache1 = UsdGeom.XformCache(), UsdGeom.XformCache()
|
||||
for prim in original.Traverse():
|
||||
after = stage.GetPrimAtPath(prim.GetPath())
|
||||
for attr in prim.GetAttributes():
|
||||
if str(prim.GetPath()) == anchors[0]["path"] and attr.GetName() == "physics:jointEnabled":
|
||||
continue
|
||||
_require(
|
||||
attr.Get() == after.GetAttribute(attr.GetName()).Get(), f"Unexpected attribute edit: {attr.GetPath()}"
|
||||
)
|
||||
for rel in prim.GetRelationships():
|
||||
_require(
|
||||
rel.GetTargets() == after.GetRelationship(rel.GetName()).GetTargets(), "Unexpected relationship edit"
|
||||
)
|
||||
if prim.IsA(UsdGeom.Xformable):
|
||||
_require(
|
||||
Gf.IsClose(cache0.GetLocalToWorldTransform(prim), cache1.GetLocalToWorldTransform(after), 1e-12),
|
||||
f"Unexpected transform edit: {prim.GetPath()}",
|
||||
)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
layer = Sdf.Layer.CreateAnonymous()
|
||||
layer.TransferContent(stage.GetRootLayer())
|
||||
layer.subLayerPaths = [os.path.relpath(source, output.parent).replace(os.sep, "/")]
|
||||
_require(layer.Export(str(output)), "USD export failed")
|
||||
after = inspect(output)
|
||||
_require(
|
||||
not after["world_fixed_joints"] and after["articulation_roots"] == [before["root_body_path"]],
|
||||
"Floating root topology check failed",
|
||||
)
|
||||
_require(dependencies(source)[1] == before["asset_sha256"], "Source bundle unexpectedly changed")
|
||||
return after
|
||||
|
||||
|
||||
def main():
|
||||
import argparse
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("asset", type=Path)
|
||||
parser.add_argument("--source-urdf", type=Path, help="Optional local original; inspected, never modified")
|
||||
parser.add_argument("--output", type=Path, required=True, help="New JSON manifest; no overwrite")
|
||||
parser.add_argument("--floating-overlay", type=Path, help="New topology-only USD layer; no overwrite")
|
||||
args = parser.parse_args()
|
||||
try:
|
||||
_require(not args.output.exists(), "Refusing to overwrite manifest")
|
||||
manifest = inspect(args.asset, args.source_urdf)
|
||||
if args.floating_overlay:
|
||||
prepare_floating_overlay(args.asset, args.floating_overlay)
|
||||
manifest = inspect(args.floating_overlay, args.source_urdf)
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
with args.output.open("x", encoding="utf-8") as stream:
|
||||
json.dump(manifest, stream, indent=2, allow_nan=False)
|
||||
stream.write("\n")
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"status": "PASS",
|
||||
"check": "static_asset_inspection_only",
|
||||
"manifest": str(args.output),
|
||||
"joints": len(manifest["joints"]),
|
||||
"coupling_status": manifest["coupling_status"],
|
||||
"dynamic_replay": "BLOCKED",
|
||||
}
|
||||
)
|
||||
)
|
||||
except (ValueError, OSError, RuntimeError, ET.ParseError) as error:
|
||||
parser.exit(1, f"FAIL: {error}\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,181 @@
|
||||
"""CPU preparation CLI: validate, synthesize test references, or resample HDF5."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
import h5py
|
||||
import numpy as np
|
||||
|
||||
from .trajectory import (
|
||||
ContractError,
|
||||
Demonstrations,
|
||||
Episode,
|
||||
load,
|
||||
require,
|
||||
require_dynamic_replay_ready,
|
||||
sample,
|
||||
validate_against_manifest,
|
||||
)
|
||||
|
||||
|
||||
def write(path, data):
|
||||
"""Never overwrite inputs, original demonstrations, or an earlier generated file."""
|
||||
path = Path(path)
|
||||
with h5py.File(path, "x") as file:
|
||||
for name, value in data.metadata.items():
|
||||
file.attrs[name] = value
|
||||
meta = file.create_group("metadata")
|
||||
meta.create_dataset("joint_names", data=data.joint_names, dtype=h5py.string_dtype("utf-8"))
|
||||
meta.create_dataset("world_from_source", data=data.world_from_source, dtype="float64")
|
||||
episodes = file.create_group("episodes")
|
||||
for name, episode in data.episodes.items():
|
||||
group = episodes.create_group(name)
|
||||
for field in ("time", "wrist_position", "wrist_quaternion", "joint_position", "valid"):
|
||||
group.create_dataset(field, data=getattr(episode, field))
|
||||
|
||||
|
||||
def publish_validated(path, data, manifest):
|
||||
"""Validate privately, then publish without replacing any existing destination."""
|
||||
path = Path(path)
|
||||
with tempfile.TemporaryDirectory(prefix=".l20-tracking-", dir=path.parent) as directory:
|
||||
temporary = Path(directory) / "validated.hdf5"
|
||||
write(temporary, data)
|
||||
load(temporary, manifest)
|
||||
# Same-filesystem hard link publishes atomically and refuses overwrites.
|
||||
os.link(temporary, path)
|
||||
|
||||
|
||||
def synthetic(manifest, profile="small"):
|
||||
"""Named analytic fixtures, not experts. ``range_4s`` has smooth out-and-back endpoints."""
|
||||
require(profile in ("small", "range_4s"), "Unknown synthetic profile")
|
||||
larger = profile == "range_4s"
|
||||
duration = 4 if larger else 2
|
||||
translation, yaw = (0.01, 0.1) if larger else (0.002, 0.01)
|
||||
joints = manifest["joints"]
|
||||
time = np.linspace(0, duration, duration * 60 + 1, dtype=np.float64)
|
||||
phase = np.sin(np.pi * time / duration) ** 2
|
||||
position = np.zeros((len(time), 3), dtype=np.float32)
|
||||
position[:, 0] = translation * phase
|
||||
position[:, 2] = 0.4
|
||||
quaternion = np.zeros((len(time), 4), dtype=np.float32)
|
||||
quaternion[:, 0] = np.cos(yaw / 2 * phase)
|
||||
quaternion[:, 3] = np.sin(yaw / 2 * phase)
|
||||
names = tuple(joint["name"] for joint in joints)
|
||||
q = np.zeros((len(time), len(names)), dtype=np.float32)
|
||||
for index, joint in enumerate(joints):
|
||||
require(joint["lower_rad"] <= 0 <= joint["upper_rad"], "Synthetic fixture requires zero within limits")
|
||||
amplitude = min(0.1, joint["upper_rad"] * 0.25) if larger else min(0.01, joint["upper_rad"] * 0.01)
|
||||
q[:, index] = phase * amplitude
|
||||
equations = {entry["joint"]: entry for entry in manifest.get("source_urdf", {}).get("mimic", [])}
|
||||
resolved = set(names) - equations.keys()
|
||||
while equations:
|
||||
ready = [name for name, eq in equations.items() if eq["reference"] in resolved]
|
||||
require(ready, "Cyclic/unresolved mimic graph")
|
||||
for name in ready:
|
||||
eq = equations.pop(name)
|
||||
q[:, names.index(name)] = eq["multiplier"] * q[:, names.index(eq["reference"])] + eq["offset_rad"]
|
||||
resolved.add(name)
|
||||
metadata = {
|
||||
"schema_version": "l20_tracking_v1",
|
||||
"embodiment": "L20",
|
||||
"hand_side": "left",
|
||||
"asset_sha256": manifest["asset_sha256"],
|
||||
"root_link": manifest["root_link"],
|
||||
"metric_scale_provenance": "Analytic SI fixture, not video reconstruction or measurement",
|
||||
"scale_to_meters": 1.0,
|
||||
"provenance": "synthetic",
|
||||
"source_description": (
|
||||
"Synthetic range_4s: 4 s sin^2 out-and-back, 10 mm translation, 0.1 rad yaw; "
|
||||
"independent positive joint amplitude min(0.1 rad, 25% upper limit), followers obey source mimic; "
|
||||
"60 Hz, analytic zero endpoint velocities; NOT expert data"
|
||||
if larger
|
||||
else "Deterministic analytic reference: 2 mm translation, 0.01 rad yaw, small joint motion; NOT expert data"
|
||||
),
|
||||
}
|
||||
data = Demonstrations(
|
||||
metadata,
|
||||
names,
|
||||
np.eye(4, dtype=np.float64),
|
||||
{
|
||||
"demo_000000": Episode(time, position, quaternion, q, np.ones(len(time), dtype=bool)),
|
||||
},
|
||||
)
|
||||
validate_against_manifest(data, manifest)
|
||||
return data
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
commands = parser.add_subparsers(dest="command", required=True)
|
||||
validate = commands.add_parser("validate")
|
||||
validate.add_argument("input", type=Path)
|
||||
validate.add_argument(
|
||||
"--manifest", type=Path, help="Required to check asset identity/order/limits; otherwise schema only"
|
||||
)
|
||||
generate = commands.add_parser("synthetic")
|
||||
generate.add_argument("--manifest", required=True, type=Path)
|
||||
generate.add_argument("--output", required=True, type=Path)
|
||||
generate.add_argument("--profile", choices=("small", "range_4s"), default="small")
|
||||
resample = commands.add_parser("resample")
|
||||
resample.add_argument("input", type=Path)
|
||||
resample.add_argument("--manifest", required=True, type=Path)
|
||||
resample.add_argument("--hz", required=True, type=float)
|
||||
resample.add_argument("--output", required=True, type=Path)
|
||||
gate = commands.add_parser("replay-check", help="Fails closed until a physical tracking Adapter is implemented")
|
||||
gate.add_argument("--manifest", required=True, type=Path)
|
||||
args = parser.parse_args()
|
||||
try:
|
||||
manifest = json.loads(args.manifest.read_text(encoding="utf-8")) if args.manifest else None
|
||||
if args.command == "replay-check":
|
||||
try:
|
||||
require_dynamic_replay_ready(manifest)
|
||||
except ContractError as error:
|
||||
parser.exit(2, f"{error}\n")
|
||||
if args.command == "synthetic":
|
||||
data = synthetic(manifest, args.profile)
|
||||
publish_validated(args.output, data, manifest)
|
||||
else:
|
||||
data = load(args.input, manifest)
|
||||
if args.command == "resample":
|
||||
require(np.isfinite(args.hz) and 0 < args.hz <= 1000, "hz must be finite and in (0, 1000]")
|
||||
episodes = {}
|
||||
for name, episode in data.episodes.items():
|
||||
require(
|
||||
episode.valid.all(),
|
||||
f"{name}: CLI resampling requires a fully valid episode; segment explicitly first",
|
||||
)
|
||||
count = int(np.floor(episode.time[-1] * args.hz)) + 1
|
||||
require(2 <= count <= 10_000_000, "Resampled frame count must be between 2 and 10 million")
|
||||
query = np.arange(count, dtype=np.float64) / args.hz
|
||||
# Endpoint is included only when on-grid; never append a shortened final interval.
|
||||
query = query[query <= episode.time[-1]]
|
||||
episodes[name] = sample(episode, query)
|
||||
data = Demonstrations(dict(data.metadata), data.joint_names, data.world_from_source, episodes)
|
||||
data.metadata["source_description"] += f"; resampled at {args.hz:g} Hz (off-grid end omitted)"
|
||||
publish_validated(args.output, data, manifest)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"status": "PASS",
|
||||
"command": args.command,
|
||||
"schema": data.metadata["schema_version"],
|
||||
"provenance": data.metadata["provenance"],
|
||||
"asset_compatibility": "PASS" if manifest else "NOT_CHECKED",
|
||||
"episodes": {
|
||||
name: {"frames": len(ep.time), "valid_frames": int(ep.valid.sum())}
|
||||
for name, ep in data.episodes.items()
|
||||
},
|
||||
"joint_count": len(data.joint_names),
|
||||
"dynamic_replay": "BLOCKED",
|
||||
}
|
||||
)
|
||||
)
|
||||
except (ContractError, OSError, KeyError, TypeError, ValueError) as error:
|
||||
parser.exit(1, f"FAIL: {error}\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,112 @@
|
||||
"""Single-hand experimental world-frame wrench PD. SI units; parameters UNCALIBRATED.
|
||||
|
||||
The controller tracks a root *link* pose, applies one bounded wrench at root COM,
|
||||
and compensates gravity for all links. No state writing, dynamics or hardware claims.
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .trajectory import require
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Limits:
|
||||
position_gain: float = 100.0 # N/m
|
||||
velocity_gain: float = 10.0 # N s/m
|
||||
rotation_gain: float = 0.2 # Nm/rad
|
||||
angular_velocity_gain: float = 0.02 # Nm s/rad
|
||||
force: float = 20.0 # norm N, including gravity
|
||||
torque: float = 1.0 # norm Nm, including gravity moment
|
||||
reference_speed: float = 0.05 # m/s
|
||||
reference_angular_speed: float = 0.5 # rad/s
|
||||
reference_joint_speed: float = 0.5 # rad/s
|
||||
workspace_radius: float = 0.1 # m from initial root link position
|
||||
finger_stiffness: float = 3.0 # Nm/rad; model-independent joints only
|
||||
finger_damping: float = 0.1 # Nm s/rad
|
||||
finger_effort: float = 0.2 # Nm
|
||||
finger_velocity: float = 0.5 # rad/s
|
||||
|
||||
def __post_init__(self):
|
||||
require(all(np.isfinite(v) and v > 0 for v in vars(self).values()), "Controller limits must be finite positive")
|
||||
|
||||
|
||||
def reference_pose_to_xyzw(position, quaternion):
|
||||
"""HDF5 world link position + wxyz -> installed Isaac Lab world link pose."""
|
||||
position, quaternion = np.asarray(position), np.asarray(quaternion)
|
||||
require(position.shape == (3,) and quaternion.shape == (4,), "Expected position[3], quaternion[4]")
|
||||
return np.r_[position, quaternion[1:], quaternion[0]]
|
||||
|
||||
|
||||
def xyzw_pose_to_reference(pose):
|
||||
"""Installed Isaac Lab world link pose -> controller position + wxyz."""
|
||||
pose = np.asarray(pose)
|
||||
require(pose.shape == (7,), "Expected runtime pose[7]")
|
||||
return np.r_[pose[:3], pose[6], pose[3:6]]
|
||||
|
||||
|
||||
def rotation_error(target, current):
|
||||
"""Shortest world-frame rotation vector taking current to target, both wxyz."""
|
||||
target, current = np.asarray(target, dtype=float), np.asarray(current, dtype=float)
|
||||
require(target.shape == current.shape == (4,), "quaternion[4] required")
|
||||
for q in (target, current):
|
||||
require(np.isfinite(q).all() and abs(np.linalg.norm(q) - 1) < 1e-4, "unit quaternion required")
|
||||
w0, v0 = target[0], target[1:]
|
||||
w1, v1 = current[0], -current[1:]
|
||||
error = np.r_[w0 * w1 - np.dot(v0, v1), w0 * v1 + w1 * v0 + np.cross(v0, v1)]
|
||||
if error[0] < 0:
|
||||
error = -error
|
||||
norm = np.linalg.norm(error[1:])
|
||||
return error[1:] * (2 * np.arctan2(norm, max(0, error[0])) / norm if norm > 1e-12 else 2)
|
||||
|
||||
|
||||
def bounded(vector, maximum):
|
||||
return vector * min(1.0, maximum / max(np.linalg.norm(vector), 1e-12))
|
||||
|
||||
|
||||
def wrench(target_position, target_quaternion, link_pose, link_velocity, root_com, body_com, masses, limits):
|
||||
"""All vectors are world-frame; poses use wxyz. Output acts at root COM.
|
||||
|
||||
A link-origin PD force F needs (p_link-p_rootCOM) x F when applied at COM.
|
||||
Gravity compensation is sum(-m*g) and sum((p_bodyCOM-p_rootCOM) x (-m*g)).
|
||||
Both complete wrench vectors are norm-clamped; no unclamped gravity feedforward.
|
||||
"""
|
||||
inputs = (target_position, target_quaternion, link_pose, link_velocity, root_com, body_com, masses)
|
||||
require(all(np.isfinite(v).all() for v in inputs), "Nonfinite controller state")
|
||||
require(np.asarray(link_pose).shape == (7,) and np.asarray(link_velocity).shape == (6,), "Invalid root state shape")
|
||||
require(np.asarray(body_com).shape == (len(masses), 3) and np.asarray(root_com).shape == (3,), "Invalid COM shape")
|
||||
require(np.asarray(target_position).shape == (3,) and (np.asarray(masses) > 0).all(), "Invalid target/mass")
|
||||
pd_force = limits.position_gain * (target_position - link_pose[:3]) - limits.velocity_gain * link_velocity[:3]
|
||||
pd_torque = limits.rotation_gain * rotation_error(target_quaternion, link_pose[3:])
|
||||
pd_torque -= limits.angular_velocity_gain * link_velocity[3:]
|
||||
compensation = np.asarray(masses)[:, None] * np.array([0.0, 0.0, 9.81])
|
||||
force = pd_force + compensation.sum(axis=0)
|
||||
torque = pd_torque + np.cross(link_pose[:3] - root_com, pd_force)
|
||||
torque += np.cross(body_com - root_com, compensation).sum(axis=0)
|
||||
return bounded(force, limits.force), bounded(torque, limits.torque)
|
||||
|
||||
|
||||
def validate_reference(episode, limits):
|
||||
"""Fail rather than silently clip reference speed/workspace or bridge invalid frames."""
|
||||
require(episode.valid.all(), "Dynamic diagnostic needs one fully valid episode")
|
||||
dt = np.diff(episode.time)
|
||||
require(len(dt) and (dt > 0).all(), "Increasing reference timestamps required")
|
||||
speed = np.linalg.norm(np.diff(episode.wrist_position, axis=0), axis=1) / dt
|
||||
omega = (
|
||||
np.array(
|
||||
[
|
||||
np.linalg.norm(rotation_error(b, a))
|
||||
for a, b in zip(episode.wrist_quaternion[:-1], episode.wrist_quaternion[1:], strict=True)
|
||||
]
|
||||
)
|
||||
/ dt
|
||||
)
|
||||
joint_speed = np.abs(np.diff(episode.joint_position, axis=0)) / dt[:, None]
|
||||
require((speed <= limits.reference_speed).all(), "Reference translation too fast")
|
||||
require((omega <= limits.reference_angular_speed).all(), "Reference rotation too fast")
|
||||
require((joint_speed <= limits.reference_joint_speed).all(), "Reference joints too fast")
|
||||
require(
|
||||
(np.linalg.norm(episode.wrist_position - episode.wrist_position[0], axis=1) <= limits.workspace_radius).all(),
|
||||
"Reference outside workspace",
|
||||
)
|
||||
@@ -0,0 +1,299 @@
|
||||
"""L20 model-derived floating control overlay for the inspected PhysX 110.1.13 backend.
|
||||
|
||||
NewtonMimicAPI is parsed by this PhysX version. Preserve it; never add a second
|
||||
legacy PhysxMimicJointAPI. Static eligibility is not runtime coupling verification.
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
from pxr import Gf, Plug, Sdf, Usd, UsdGeom, UsdPhysics
|
||||
|
||||
from .asset import _require, dependencies, inspect, prepare_floating_overlay
|
||||
|
||||
SOURCE_URDF_SHA = "b8ef22e436ab311fb61f87091b72ae717821092471d628d0ea9475b5088daa5d"
|
||||
BACKEND_VERSION = "110.1.13"
|
||||
MIMIC = [
|
||||
{"joint": f"{finger}_dip", "reference": f"{finger}_pip", "multiplier": 0.89, "offset_rad": 0.0}
|
||||
for finger in ("index", "middle", "pinky", "ring")
|
||||
] + [{"joint": "thumb_ip", "reference": "thumb_mcp", "multiplier": 1.02, "offset_rad": 0.0}]
|
||||
DRIVE_FIELDS = ("stiffness", "damping", "maxForce")
|
||||
|
||||
|
||||
def require_backend(version, newton_schema_registered):
|
||||
"""Conservative capability binding; this does not certify runtime constraint response."""
|
||||
_require(version == BACKEND_VERSION, f"Unsupported PhysX version {version}; revalidate backend first")
|
||||
_require(newton_schema_registered, "Newton mimic schema not registered")
|
||||
|
||||
|
||||
def schema_context():
|
||||
"""Resolved schema fallbacks are part of the authoring/inspection contract."""
|
||||
registry = Usd.SchemaRegistry()
|
||||
return {
|
||||
name: bool(registry.FindAppliedAPIPrimDefinition(name)) for name in ("PhysxArticulationAPI", "NewtonMimicAPI")
|
||||
}
|
||||
|
||||
|
||||
def register_schema_plugins(paths):
|
||||
"""Register caller-selected USD schema plugins; never discover vendor paths implicitly."""
|
||||
for path in paths:
|
||||
Plug.Registry().RegisterPlugins(str(Path(path).resolve(strict=True)))
|
||||
_require(all(schema_context().values()), "Schema paths must resolve PhysxArticulationAPI and NewtonMimicAPI")
|
||||
|
||||
|
||||
def schemas(prim):
|
||||
"""Composed schema list, including codeless schemas not registered in CPU Python."""
|
||||
value = prim.GetMetadata("apiSchemas")
|
||||
return list(value.GetAppliedItems()) if value else []
|
||||
|
||||
|
||||
def newton_mimic(prim):
|
||||
"""Read single-DOF Newton equation; fallbacks are from installed generatedSchema.
|
||||
|
||||
Coef0 is degrees for revolute joints (not NewtonActuator's radians).
|
||||
Coef1 is dimensionless; the USD joint axis implicitly selects the DOF.
|
||||
"""
|
||||
_require("NewtonMimicAPI" in schemas(prim), f"Missing NewtonMimicAPI: {prim.GetPath()}")
|
||||
|
||||
def value(name, fallback):
|
||||
attr = prim.GetAttribute("newton:" + name)
|
||||
result = attr.Get() if attr else None
|
||||
return fallback if result is None else result
|
||||
|
||||
targets = prim.GetRelationship("newton:mimicJoint").GetTargets()
|
||||
_require(len(targets) == 1, "Exactly one mimic leader required")
|
||||
_require(value("mimicEnabled", True) is True, "Disabled mimic constraint")
|
||||
return str(targets[0]), float(value("mimicCoef1", 1)), math.radians(float(value("mimicCoef0", 0)))
|
||||
|
||||
|
||||
def bind_source(source, manifest):
|
||||
"""Reject stale identity/structural metadata before authoring or accepting an overlay."""
|
||||
actual = inspect(source)
|
||||
for key in ("asset_sha256", "joints", "root_body_path", "root_link", "dependencies", "bodies"):
|
||||
_require(actual[key] == manifest[key], f"Stale/inconsistent source manifest: {key}")
|
||||
_require(manifest["source_urdf"]["sha256"] == SOURCE_URDF_SHA, "Unapproved URDF provenance")
|
||||
_require(manifest["source_urdf"]["mimic"] == MIMIC, "Unexpected L20 mimic equations")
|
||||
_require(actual["root_link"] == "hand_base_link" and len(actual["joints"]) == 21, "Wrong L20 model")
|
||||
_require(actual["physics_variant"] == "physx", "PhysX variant required")
|
||||
validate_mimic(Usd.Stage.Open(str(source)), actual)
|
||||
return actual
|
||||
|
||||
|
||||
def validate_mimic(stage, manifest, passive=False):
|
||||
joints = {j["name"]: j for j in manifest["joints"]}
|
||||
followers = {eq["joint"] for eq in MIMIC}
|
||||
for name, joint in joints.items():
|
||||
prim = stage.GetPrimAtPath(joint["path"])
|
||||
_require(not any("PhysxMimic" in s or "Tendon" in s for s in schemas(prim)), "Duplicate/unsupported coupling")
|
||||
_require(("NewtonMimicAPI" in schemas(prim)) == (name in followers), "Unexpected mimic set")
|
||||
_require(not UsdPhysics.Joint(prim).GetExcludeFromArticulationAttr().Get(), "Excluded joint")
|
||||
_require(joint["axis"] in ("X", "Y", "Z"), "Invalid revolute axis")
|
||||
if name not in followers:
|
||||
continue
|
||||
equation = next(eq for eq in MIMIC if eq["joint"] == name)
|
||||
leader, coefficient, offset = newton_mimic(prim)
|
||||
_require(leader == joints[equation["reference"]]["path"], "Wrong mimic leader")
|
||||
_require(abs(coefficient - equation["multiplier"]) < 1e-6, "Wrong mimic multiplier/sign")
|
||||
_require(abs(offset - equation["offset_rad"]) < 1e-8, "Wrong mimic offset/units")
|
||||
if passive:
|
||||
_require("PhysicsDriveAPI:angular" not in schemas(prim), "Follower drive API still enabled")
|
||||
for field in DRIVE_FIELDS:
|
||||
_require(prim.GetAttribute(f"drive:angular:physics:{field}").Get() == 0, "Follower drive not zero")
|
||||
return [name for name in joints if name not in followers]
|
||||
|
||||
|
||||
def _preserved(original, stage, before):
|
||||
"""Only leaf world-anchor deactivation, root migration and follower drive suppression are allowed."""
|
||||
followers = {j["path"] for j in before["joints"] if j["name"] in {eq["joint"] for eq in MIMIC}}
|
||||
anchor = before["world_fixed_joints"][0]["path"]
|
||||
root = before["root_body_path"]
|
||||
source_anchor = original.GetPrimAtPath(anchor)
|
||||
_require(
|
||||
source_anchor.IsA(UsdPhysics.FixedJoint) and not source_anchor.GetAllChildren(), "Expected leaf world anchor"
|
||||
)
|
||||
joint = UsdPhysics.Joint(source_anchor)
|
||||
_require(not joint.GetBody0Rel().GetTargets(), "Anchor must attach directly to world")
|
||||
_require(joint.GetBody1Rel().GetTargets() == [Sdf.Path(root)], "Anchor must attach only to root")
|
||||
c0, c1 = UsdGeom.XformCache(), UsdGeom.XformCache()
|
||||
# TraverseAll retains the deliberately inactive leaf: no other deletion or
|
||||
# deactivation may hide geometry, state joints or time-sampled attributes.
|
||||
_require(
|
||||
[str(p.GetPath()) for p in original.TraverseAll()] == [str(p.GetPath()) for p in stage.TraverseAll()],
|
||||
"Prim tree changed",
|
||||
)
|
||||
for prim in original.TraverseAll():
|
||||
path = str(prim.GetPath())
|
||||
after = stage.GetPrimAtPath(path)
|
||||
_require(after.IsActive() == (False if path == anchor else prim.IsActive()), f"Unexpected active state: {path}")
|
||||
allowed = set()
|
||||
expected = set(schemas(prim))
|
||||
if path == anchor:
|
||||
_require(after.GetAttribute("physics:jointEnabled").Get() is False, "Inactive anchor must remain disabled")
|
||||
allowed.add("physics:jointEnabled")
|
||||
expected -= {"PhysicsArticulationRootAPI", "PhysxArticulationAPI"}
|
||||
if path == root:
|
||||
expected |= {"PhysicsArticulationRootAPI", "PhysxArticulationAPI"}
|
||||
allowed |= {
|
||||
a.GetName()
|
||||
for a in stage.GetPrimAtPath(anchor).GetAttributes()
|
||||
if a.GetName().startswith("physxArticulation:")
|
||||
}
|
||||
for name in allowed:
|
||||
_require(
|
||||
after.GetAttribute(name).Get() == stage.GetPrimAtPath(anchor).GetAttribute(name).Get(),
|
||||
"Root solver property changed",
|
||||
)
|
||||
if path in followers:
|
||||
expected.discard("PhysicsDriveAPI:angular")
|
||||
allowed |= {f"drive:angular:physics:{field}" for field in DRIVE_FIELDS}
|
||||
_require(set(schemas(after)) == expected, f"Unexpected schema edit: {path}")
|
||||
attrs = {a.GetName() for a in prim.GetAttributes()} | {a.GetName() for a in after.GetAttributes()}
|
||||
# This diagnostic accepts static assets only, including fields whose
|
||||
# default values may be changed by the preparation allowlist.
|
||||
for name in attrs:
|
||||
for candidate in (prim.GetAttribute(name), after.GetAttribute(name)):
|
||||
if candidate:
|
||||
_require(not candidate.GetTimeSamples(), f"Time samples prohibited: {path}.{name}")
|
||||
for name in attrs - allowed:
|
||||
a, b = prim.GetAttribute(name), after.GetAttribute(name)
|
||||
_require((a.Get() if a else None) == (b.Get() if b else None), f"Unexpected attribute: {path}.{name}")
|
||||
rels = {r.GetName() for r in prim.GetRelationships()} | {r.GetName() for r in after.GetRelationships()}
|
||||
for name in rels:
|
||||
_require(
|
||||
prim.GetRelationship(name).GetTargets() == after.GetRelationship(name).GetTargets(),
|
||||
"Relationship changed",
|
||||
)
|
||||
if prim.IsA(UsdGeom.Xformable):
|
||||
_require(
|
||||
Gf.IsClose(c0.GetLocalToWorldTransform(prim), c1.GetLocalToWorldTransform(after), 1e-12),
|
||||
"Transform changed",
|
||||
)
|
||||
|
||||
|
||||
def inspect_prepared(output, source_manifest):
|
||||
"""Inspect actual composed USD + original dependency hashes, never a JSON ready flag."""
|
||||
output = Path(output).resolve(strict=True)
|
||||
layer = Sdf.Layer.FindOrOpen(str(output))
|
||||
_require(
|
||||
len(layer.subLayerPaths) == 1 and not os.path.isabs(layer.subLayerPaths[0]),
|
||||
"One relative source layer required",
|
||||
)
|
||||
source = (output.parent / layer.subLayerPaths[0]).resolve(strict=True)
|
||||
before = bind_source(source, source_manifest)
|
||||
_require(layer.customLayerData.get("source_bundle_sha256") == before["asset_sha256"], "Overlay provenance mismatch")
|
||||
_require(layer.customLayerData.get("source_urdf_sha256") == SOURCE_URDF_SHA, "Overlay URDF mismatch")
|
||||
_require(
|
||||
dict(layer.customLayerData.get("schema_context", {})) == schema_context(),
|
||||
"Authoring schema context mismatch; regenerate from source with --schema-plugin-path for both "
|
||||
"installed PhysX and Newton plugins, and use the same plugins when inspecting",
|
||||
)
|
||||
after = inspect(output)
|
||||
_require(
|
||||
not after["world_fixed_joints"] and after["articulation_roots"] == [before["root_body_path"]], "Not floating"
|
||||
)
|
||||
stage = Usd.Stage.Open(str(output))
|
||||
masters = validate_mimic(stage, after, passive=True)
|
||||
_preserved(Usd.Stage.Open(str(source)), stage, before)
|
||||
return {
|
||||
"status": (
|
||||
"STATIC_ELIGIBLE_RUNTIME_UNVERIFIED"
|
||||
if all(schema_context().values())
|
||||
else "STATIC_ONLY_SCHEMA_UNREGISTERED"
|
||||
),
|
||||
"schema_context": schema_context(),
|
||||
"preparation_note": "Runtime use requires matching registered PhysX/Newton schemas; regenerate if different",
|
||||
"prepared_asset_sha256": after["asset_sha256"],
|
||||
"source_asset_sha256": before["asset_sha256"],
|
||||
"independent_joint_names": masters,
|
||||
"mimic": MIMIC,
|
||||
"required_physx_version": BACKEND_VERSION,
|
||||
"root_body_path": before["root_body_path"],
|
||||
"runtime_verified": False,
|
||||
}
|
||||
|
||||
|
||||
def prepare(source, output, manifest):
|
||||
source, output = Path(source).resolve(strict=True), Path(output).resolve()
|
||||
_require(not output.exists(), "Refusing to overwrite output")
|
||||
before = bind_source(source, manifest)
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
with tempfile.TemporaryDirectory(prefix=".l20-prepared-", dir=output.parent) as directory:
|
||||
temporary = Path(directory) / "floating.usda"
|
||||
prepare_floating_overlay(source, temporary)
|
||||
stage = Usd.Stage.Open(str(temporary))
|
||||
for joint in before["joints"]:
|
||||
if joint["name"] not in {eq["joint"] for eq in MIMIC}:
|
||||
continue
|
||||
prim = stage.GetPrimAtPath(joint["path"])
|
||||
# Preserve resolved drive defaults as inert attributes when removing API.
|
||||
drive_values = [
|
||||
(a.GetName(), a.GetTypeName(), a.Get())
|
||||
for a in prim.GetAttributes()
|
||||
if a.GetName().startswith("drive:angular:") and a.Get() is not None
|
||||
]
|
||||
prim.RemoveAPI(UsdPhysics.DriveAPI, "angular")
|
||||
for name, dtype, value in drive_values:
|
||||
prim.CreateAttribute(name, dtype).Set(value)
|
||||
for field in DRIVE_FIELDS:
|
||||
prim.CreateAttribute(f"drive:angular:physics:{field}", Sdf.ValueTypeNames.Float).Set(0)
|
||||
# jointEnabled=false leaves a parsed joint with world/local frames that
|
||||
# diverge when the floating hand is spawned. Remove that obsolete joint
|
||||
# from active composition, not its data or any robot body/state joint.
|
||||
stage.GetPrimAtPath(before["world_fixed_joints"][0]["path"]).SetActive(False)
|
||||
stage.GetRootLayer().customLayerData = {
|
||||
"purpose": "L20 experimental floating tracking; runtime UNVERIFIED",
|
||||
"source_bundle_sha256": before["asset_sha256"],
|
||||
"source_urdf_sha256": SOURCE_URDF_SHA,
|
||||
"physx_evidence_version": BACKEND_VERSION,
|
||||
"schema_context": schema_context(),
|
||||
}
|
||||
stage.GetRootLayer().Save()
|
||||
inspect_prepared(temporary, manifest)
|
||||
# Rebase the relative source path for the final location before publication.
|
||||
final_layer = Sdf.Layer.CreateAnonymous()
|
||||
final_layer.TransferContent(stage.GetRootLayer())
|
||||
final_layer.subLayerPaths = [os.path.relpath(source, output.parent).replace(os.sep, "/")]
|
||||
# Validate with the final relative-path context before publishing.
|
||||
descriptor, name = tempfile.mkstemp(prefix=".l20-publication-", suffix=".usda", dir=output.parent)
|
||||
os.close(descriptor)
|
||||
publication = Path(name)
|
||||
try:
|
||||
_require(final_layer.Export(str(publication)), "Export failed")
|
||||
inspect_prepared(publication, manifest)
|
||||
os.link(publication, output)
|
||||
finally:
|
||||
publication.unlink()
|
||||
_require(dependencies(source)[1] == before["asset_sha256"], "Original changed")
|
||||
return inspect_prepared(output, manifest)
|
||||
|
||||
|
||||
def main():
|
||||
import argparse
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("asset", type=Path)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, help="Create new experimental floating overlay")
|
||||
parser.add_argument(
|
||||
"--schema-plugin-path",
|
||||
type=Path,
|
||||
action="append",
|
||||
default=[],
|
||||
help="Repeat for caller-selected PhysX and Newton USD plugInfo.json paths; required for runtime preparation",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
try:
|
||||
if args.schema_plugin_path:
|
||||
register_schema_plugins(args.schema_plugin_path)
|
||||
manifest = json.loads(args.manifest.read_text())
|
||||
result = prepare(args.asset, args.output, manifest) if args.output else inspect_prepared(args.asset, manifest)
|
||||
print(json.dumps(result, indent=2))
|
||||
except (ValueError, OSError, RuntimeError, KeyError) as error:
|
||||
parser.exit(1, f"FAIL: {error}\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,225 @@
|
||||
"""Strict l20_tracking_v1 reference-state ingestion, without simulator imports."""
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import h5py
|
||||
import numpy as np
|
||||
|
||||
|
||||
class ContractError(ValueError):
|
||||
"""An input cannot be safely interpreted as a reference trajectory."""
|
||||
|
||||
|
||||
def require(condition, message):
|
||||
if not condition:
|
||||
raise ContractError(message)
|
||||
|
||||
|
||||
def text(value, label):
|
||||
if isinstance(value, bytes):
|
||||
value = value.decode("utf-8", errors="strict")
|
||||
require(isinstance(value, str) and bool(value.strip()), f"{label}: nonempty UTF-8 text required")
|
||||
return value
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Episode:
|
||||
time: np.ndarray
|
||||
wrist_position: np.ndarray
|
||||
wrist_quaternion: np.ndarray
|
||||
joint_position: np.ndarray
|
||||
valid: np.ndarray
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Demonstrations:
|
||||
metadata: dict
|
||||
joint_names: tuple[str, ...]
|
||||
world_from_source: np.ndarray
|
||||
episodes: dict[str, Episode]
|
||||
|
||||
|
||||
def _dataset(group, name, dtype, shape):
|
||||
require(name in group and isinstance(group[name], h5py.Dataset), f"{group.name}/{name}: dataset required")
|
||||
data = group[name]
|
||||
require(
|
||||
data.dtype.kind == np.dtype(dtype).kind and data.dtype.itemsize == np.dtype(dtype).itemsize,
|
||||
f"{data.name}: dtype must be {dtype}",
|
||||
)
|
||||
require(data.shape == shape, f"{data.name}: expected shape {shape}, got {data.shape}")
|
||||
value = data[...]
|
||||
require(np.isfinite(value).all(), f"{data.name}: NaN/Inf prohibited, including invalid frames")
|
||||
return value
|
||||
|
||||
|
||||
def load(path: str | Path, manifest: dict | None = None) -> Demonstrations:
|
||||
"""Read and close an HDF5 file. Manifest checking never approves physical actuation."""
|
||||
with h5py.File(path, "r") as file:
|
||||
attributes = {}
|
||||
for name in (
|
||||
"schema_version",
|
||||
"embodiment",
|
||||
"hand_side",
|
||||
"asset_sha256",
|
||||
"root_link",
|
||||
"metric_scale_provenance",
|
||||
"provenance",
|
||||
"source_description",
|
||||
):
|
||||
require(name in file.attrs, f"missing root attribute {name}")
|
||||
attributes[name] = text(file.attrs[name], name)
|
||||
for name, expected in (("schema_version", "l20_tracking_v1"), ("embodiment", "L20"), ("hand_side", "left")):
|
||||
require(attributes[name] == expected, f"{name}: expected {expected}")
|
||||
require(
|
||||
re.fullmatch(r"[0-9a-f]{64}", attributes["asset_sha256"]) is not None,
|
||||
"asset_sha256: lowercase SHA-256 required",
|
||||
)
|
||||
require(attributes["provenance"] in ("expert_retargeted", "synthetic"), "unknown provenance")
|
||||
require("scale_to_meters" in file.attrs, "missing root attribute scale_to_meters")
|
||||
scale = np.asarray(file.attrs["scale_to_meters"])
|
||||
require(
|
||||
scale.shape == () and scale.dtype.kind == "f" and np.isfinite(scale) and scale > 0,
|
||||
"scale_to_meters: positive finite floating scalar required",
|
||||
)
|
||||
attributes["scale_to_meters"] = float(scale)
|
||||
require("metadata" in file and isinstance(file["metadata"], h5py.Group), "metadata group required")
|
||||
meta = file["metadata"]
|
||||
require(
|
||||
"joint_names" in meta and isinstance(meta["joint_names"], h5py.Dataset),
|
||||
"metadata/joint_names dataset required",
|
||||
)
|
||||
names = meta["joint_names"]
|
||||
info = h5py.check_string_dtype(names.dtype)
|
||||
require(
|
||||
info is not None and info.encoding == "utf-8" and names.ndim == 1 and len(names) > 0,
|
||||
"joint_names must be UTF-8[J], J > 0",
|
||||
)
|
||||
joint_names = tuple(text(n, "joint_names") for n in names.asstr()[...])
|
||||
require(len(set(joint_names)) == len(joint_names), "duplicate joint names")
|
||||
transform = _dataset(meta, "world_from_source", "float64", (4, 4))
|
||||
require(
|
||||
np.allclose(transform[3], [0, 0, 0, 1], rtol=0, atol=1e-8), "world_from_source: invalid homogeneous row"
|
||||
)
|
||||
rotation = transform[:3, :3]
|
||||
require(
|
||||
np.allclose(rotation.T @ rotation, np.eye(3), rtol=0, atol=1e-6)
|
||||
and np.isclose(np.linalg.det(rotation), 1, rtol=0, atol=1e-6),
|
||||
"world_from_source must be a proper rigid transform, not a scale/reflection",
|
||||
)
|
||||
require(
|
||||
"episodes" in file and isinstance(file["episodes"], h5py.Group) and len(file["episodes"]) > 0,
|
||||
"nonempty episodes group required",
|
||||
)
|
||||
episodes = {}
|
||||
for name, group in file["episodes"].items():
|
||||
require(
|
||||
re.fullmatch(r"demo_[0-9]{6}", name) is not None and isinstance(group, h5py.Group),
|
||||
f"invalid episode group {name}",
|
||||
)
|
||||
require(
|
||||
"time" in group and isinstance(group["time"], h5py.Dataset) and group["time"].ndim == 1,
|
||||
f"{name}: time[T] required",
|
||||
)
|
||||
count = len(group["time"])
|
||||
require(count >= 2, f"{name}: at least two frames required")
|
||||
time = _dataset(group, "time", "float64", (count,))
|
||||
require(time[0] == 0 and (np.diff(time) > 0).all(), f"{name}: time must start at 0 and increase strictly")
|
||||
position = _dataset(group, "wrist_position", "float32", (count, 3))
|
||||
quaternion = _dataset(group, "wrist_quaternion", "float32", (count, 4))
|
||||
joints = _dataset(group, "joint_position", "float32", (count, len(joint_names)))
|
||||
valid = _dataset(group, "valid", "bool", (count,))
|
||||
require(valid.any(), f"{name}: no valid frames")
|
||||
require(
|
||||
np.allclose(np.linalg.norm(quaternion[valid], axis=1), 1, rtol=0, atol=1e-4),
|
||||
f"{name}: valid quaternions must be unit length",
|
||||
)
|
||||
adjacent = valid[:-1] & valid[1:]
|
||||
require(
|
||||
(np.sum(quaternion[:-1] * quaternion[1:], axis=1)[adjacent] >= 0).all(),
|
||||
f"{name}: adjacent valid quaternion signs must be continuous",
|
||||
)
|
||||
episodes[name] = Episode(time, position, quaternion, joints, valid)
|
||||
result = Demonstrations(attributes, joint_names, transform, episodes)
|
||||
if manifest is not None:
|
||||
validate_against_manifest(result, manifest)
|
||||
return result
|
||||
|
||||
|
||||
def validate_against_manifest(data: Demonstrations, manifest: dict):
|
||||
"""Check identity/order/limits and known URDF mimic equations, not actuator validity."""
|
||||
require(manifest.get("manifest_version") == "l20_asset_manifest_v1", "unsupported manifest")
|
||||
require(data.metadata["asset_sha256"] == manifest["asset_sha256"], "asset bundle hash mismatch")
|
||||
require(data.metadata["root_link"] == manifest["root_link"], "root link mismatch")
|
||||
joints = manifest["joints"]
|
||||
expected = tuple(joint["name"] for joint in joints)
|
||||
require(data.joint_names == expected, "joint order/names must exactly match manifest (no implicit reorder)")
|
||||
lower = np.array([joint["lower_rad"] for joint in joints])
|
||||
upper = np.array([joint["upper_rad"] for joint in joints])
|
||||
require(
|
||||
np.isfinite(lower).all() and np.isfinite(upper).all() and (lower <= upper).all(),
|
||||
"manifest requires finite ordered revolute limits",
|
||||
)
|
||||
for name, episode in data.episodes.items():
|
||||
q = episode.joint_position[episode.valid]
|
||||
require(((q >= lower - 1e-6) & (q <= upper + 1e-6)).all(), f"{name}: reference joint limit violation")
|
||||
for mimic in manifest.get("source_urdf", {}).get("mimic", []):
|
||||
child = expected.index(mimic["joint"])
|
||||
parent = expected.index(mimic["reference"])
|
||||
error = q[:, child] - (mimic["multiplier"] * q[:, parent] + mimic["offset_rad"])
|
||||
require((np.abs(error) <= 1e-3).all(), f"{name}: inconsistent mimic reference {mimic['joint']}")
|
||||
|
||||
|
||||
def sample(episode: Episode, query_time) -> Episode:
|
||||
"""Linear position/joint interpolation and shortest-arc SLERP; never cross invalid gaps.
|
||||
|
||||
Inputs must come from ``load``. Query a separate valid segment at a time; this
|
||||
rejects even sparse queries spanning an invalid frame, and never extrapolates.
|
||||
"""
|
||||
query = np.asarray(query_time, dtype=np.float64)
|
||||
require(query.ndim == 1 and len(query) > 0 and np.isfinite(query).all(), "finite query_time[N] required")
|
||||
require((np.diff(query) > 0).all(), "query times must increase strictly")
|
||||
time = episode.time
|
||||
require(query[0] >= time[0] and query[-1] <= time[-1], "extrapolation prohibited")
|
||||
left = np.searchsorted(time, query, side="right") - 1
|
||||
right = np.searchsorted(time, query, side="left")
|
||||
require(episode.valid[left.min() : right.max() + 1].all(), "query crosses or touches an invalid frame")
|
||||
denominator = time[right] - time[left]
|
||||
alpha = np.divide(query - time[left], denominator, out=np.zeros_like(query), where=denominator > 0)[:, None]
|
||||
position = (1 - alpha) * episode.wrist_position[left] + alpha * episode.wrist_position[right]
|
||||
joints = (1 - alpha) * episode.joint_position[left] + alpha * episode.joint_position[right]
|
||||
q0 = episode.wrist_quaternion[left].astype(np.float64)
|
||||
q1 = episode.wrist_quaternion[right].astype(np.float64)
|
||||
q0 /= np.linalg.norm(q0, axis=1, keepdims=True)
|
||||
q1 /= np.linalg.norm(q1, axis=1, keepdims=True)
|
||||
dot = np.sum(q0 * q1, axis=1, keepdims=True)
|
||||
q1 = np.where(dot < 0, -q1, q1)
|
||||
dot = np.clip(np.abs(dot), 0, 1)
|
||||
theta = np.arccos(dot)
|
||||
denominator = np.sin(theta)
|
||||
curved = dot < 0.9995
|
||||
weight0 = np.divide(np.sin((1 - alpha) * theta), denominator, out=1 - alpha.copy(), where=curved)
|
||||
weight1 = np.divide(np.sin(alpha * theta), denominator, out=alpha.copy(), where=curved)
|
||||
quaternion = weight0 * q0 + weight1 * q1
|
||||
quaternion /= np.linalg.norm(quaternion, axis=1, keepdims=True)
|
||||
# Sparse output samples may span many source arcs. Preserve rotations while
|
||||
# choosing a continuous quaternion hemisphere on the output timeline too.
|
||||
flips = np.where(np.sum(quaternion[:-1] * quaternion[1:], axis=1) < 0, -1, 1)
|
||||
quaternion[1:] *= np.cumprod(flips)[:, None]
|
||||
return Episode(
|
||||
query,
|
||||
position.astype(np.float32),
|
||||
quaternion.astype(np.float32),
|
||||
joints.astype(np.float32),
|
||||
np.ones(len(query), dtype=bool),
|
||||
)
|
||||
|
||||
|
||||
def require_dynamic_replay_ready(manifest: dict):
|
||||
"""A data manifest alone never authorizes dynamic control."""
|
||||
raise ContractError(
|
||||
"BLOCKED: a data manifest cannot authorize actuation. Use the separate experimental track_l20.py "
|
||||
"path with actual prepared-USD inspection, backend checks and runtime assertions. "
|
||||
"Production replay and hardware control remain unvalidated."
|
||||
)
|
||||
@@ -4,9 +4,56 @@ Changelog
|
||||
Unreleased
|
||||
~~~~~~~~~~
|
||||
|
||||
0.1.1 (2026-09-11)
|
||||
~~~~~~~~~~~~~~~~~~
|
||||
|
||||
USD initial validation milestone (USD 文件初步校验). Synthetic single-hand
|
||||
replay only; not a production training or hardware release. Cartpole and its task
|
||||
ID are unchanged. Old prepared overlays require regeneration. Full pre-commit
|
||||
remains unavailable in the validation environment.
|
||||
|
||||
Added
|
||||
^^^^^
|
||||
|
||||
* Add the explicitly synthetic ``range_4s`` HDF5 fixture profile (4 seconds,
|
||||
10 mm translation, 0.1 rad yaw, bounded master joints). Real-file replay passes
|
||||
two 960-step repetitions with unchanged gains/thresholds; no training claim.
|
||||
|
||||
* Add CPU-only ``l20_tracking_v1`` HDF5 validation, reference resampling, synthetic
|
||||
fixtures, and regression tests, documented in ``L20_TRACKING.md``. Provide the
|
||||
standalone colleague-facing contract in ``HDF5_REQUIREMENTS.md``.
|
||||
* Add reproducible L20 composed-asset inspection and a non-destructive floating-root
|
||||
topology overlay plus an Isaac Lab scene-loading diagnostic. Dynamic wrist/finger
|
||||
production replay remains blocked pending runtime validation and hardware actuator mapping.
|
||||
Cartpole is unchanged.
|
||||
* Add an experimental floating tracking overlay that preserves native Newton mimic
|
||||
constraints, suppresses follower drives, and validates source identity/allowed edits.
|
||||
Add a bounded single-hand wrench-PD/master-position replay diagnostic with explicit
|
||||
COM/world-frame gravity accounting, reset and passive-coupling assertions. Runtime
|
||||
smoke passes on the corrected registered overlay: seed 42, one hand, two 480-step
|
||||
synthetic repetitions on PhysX 110.1.13. Real demonstrations, broad motion/contact,
|
||||
vectorized training and hardware remain unvalidated. Gains are uncalibrated.
|
||||
|
||||
Fixed
|
||||
^^^^^
|
||||
|
||||
* Deactivate only the obsolete disabled leaf world anchor in prepared control
|
||||
overlays while retaining its data and strict all-prim/time-sample checks.
|
||||
Both bounded runtime tests report a floating root and 21 active revolute joints,
|
||||
without the prior disjointed-frame warning; small-profile metrics match baseline.
|
||||
|
||||
* Launch Kit before USD-dependent diagnostic imports and preserve exceptions/nonzero
|
||||
exit status across fast shutdown; an exit without PASS metrics is not acceptance.
|
||||
* Preserve resolved articulation schema defaults on both the released anchor and
|
||||
floating root. Add explicit preparation CLI schema-plugin paths and authoring-context
|
||||
checks; old overlays require regeneration. CPU regressions and the subsequent
|
||||
authorized synthetic dynamic smoke pass; metrics and remaining runtime warnings
|
||||
are recorded in ``L20_TRACKING.md``.
|
||||
|
||||
* Correct the prior missing-PhysX-coupling inference: installed PhysX 110.1.13 parses
|
||||
NewtonMimicAPI natively. Schema presence is now reported as runtime-unverified,
|
||||
not missing physics; it does not establish actual asset dynamics.
|
||||
|
||||
* Configure the L20 PhysX preview with a world-fixed articulation root, ground,
|
||||
lighting, and 240 Hz physics to prevent the observed 60 Hz instability.
|
||||
Add a bounded editor smoke script; Isaac Lab task integration is not included.
|
||||
|
||||
@@ -24,7 +24,7 @@ INSTALL_REQUIRES = [
|
||||
# Installation operation
|
||||
setup(
|
||||
name="dex_workbench",
|
||||
packages=["dex_workbench"],
|
||||
packages=["dex_workbench", "dex_workbench_tracking"],
|
||||
author=EXTENSION_TOML_DATA["package"]["author"],
|
||||
maintainer=EXTENSION_TOML_DATA["package"]["maintainer"],
|
||||
url=EXTENSION_TOML_DATA["package"]["repository"],
|
||||
@@ -32,6 +32,7 @@ setup(
|
||||
description=EXTENSION_TOML_DATA["package"]["description"],
|
||||
keywords=EXTENSION_TOML_DATA["package"]["keywords"],
|
||||
install_requires=INSTALL_REQUIRES,
|
||||
extras_require={"tracking": ["numpy>=1.26", "h5py>=3.10"]},
|
||||
license="Apache-2.0",
|
||||
include_package_data=True,
|
||||
python_requires=">=3.12",
|
||||
@@ -41,4 +42,4 @@ setup(
|
||||
"Isaac Sim :: 6.0.0",
|
||||
],
|
||||
zip_safe=False,
|
||||
)
|
||||
)
|
||||
|
||||
@@ -0,0 +1,130 @@
|
||||
"""Static USD regression tests; run with Isaac Sim's pxr-capable Python, no GPU/Kit."""
|
||||
|
||||
import json
|
||||
import shutil
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
from dex_workbench_tracking.asset import dependencies, inspect, prepare_floating_overlay
|
||||
|
||||
from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics
|
||||
|
||||
|
||||
class TrackingAssetTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.directory = tempfile.TemporaryDirectory()
|
||||
self.addCleanup(self.directory.cleanup)
|
||||
self.path = Path(self.directory.name) / "robot.usda"
|
||||
stage = Usd.Stage.CreateNew(str(self.path))
|
||||
root = UsdGeom.Xform.Define(stage, "/Robot").GetPrim()
|
||||
stage.SetDefaultPrim(root)
|
||||
variant = root.GetVariantSets().AddVariantSet("Physics")
|
||||
variant.AddVariant("physx")
|
||||
variant.SetVariantSelection("physx")
|
||||
UsdGeom.SetStageMetersPerUnit(stage, 1)
|
||||
UsdGeom.SetStageUpAxis(stage, "Z")
|
||||
UsdPhysics.SetStageKilogramsPerUnit(stage, 1)
|
||||
for name in ("base", "tip"):
|
||||
prim = UsdGeom.Xform.Define(stage, f"/Robot/{name}").GetPrim()
|
||||
UsdPhysics.RigidBodyAPI.Apply(prim)
|
||||
UsdPhysics.MassAPI.Apply(prim).CreateMassAttr(0.1)
|
||||
hinge = UsdPhysics.RevoluteJoint.Define(stage, "/Robot/hinge")
|
||||
hinge.CreateBody0Rel().SetTargets(["/Robot/base"])
|
||||
hinge.CreateBody1Rel().SetTargets(["/Robot/tip"])
|
||||
hinge.CreateLowerLimitAttr(0)
|
||||
hinge.CreateUpperLimitAttr(90)
|
||||
fixed = UsdPhysics.FixedJoint.Define(stage, "/Robot/anchor")
|
||||
fixed.CreateBody1Rel().SetTargets(["/Robot/base"])
|
||||
UsdPhysics.ArticulationRootAPI.Apply(fixed.GetPrim())
|
||||
stage.GetRootLayer().Save()
|
||||
|
||||
def test_manifest_is_explicitly_not_actuator_map(self):
|
||||
result = inspect(self.path)
|
||||
self.assertEqual(result["root_link"], "base")
|
||||
self.assertEqual([j["name"] for j in result["joints"]], ["hinge"])
|
||||
self.assertAlmostEqual(result["joints"][0]["upper_rad"], 1.5707963267948966)
|
||||
self.assertFalse(result["dynamic_replay_ready"])
|
||||
self.assertEqual(result["coupling_status"], "UNVERIFIED")
|
||||
json.dumps(result, allow_nan=False)
|
||||
|
||||
def test_bundle_hash_is_portable_and_sensitive(self):
|
||||
target = Path(self.directory.name) / "moved"
|
||||
target.mkdir()
|
||||
copy = target / self.path.name
|
||||
shutil.copyfile(self.path, copy)
|
||||
self.assertEqual(dependencies(self.path)[1], dependencies(copy)[1])
|
||||
with copy.open("a") as stream:
|
||||
stream.write("\n# changed\n")
|
||||
self.assertNotEqual(dependencies(self.path)[1], dependencies(copy)[1])
|
||||
|
||||
def test_missing_dependency_rejected(self):
|
||||
stage = Usd.Stage.Open(str(self.path))
|
||||
stage.GetRootLayer().subLayerPaths = ["missing.usda"]
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "Unresolved"):
|
||||
inspect(self.path)
|
||||
|
||||
def test_floating_overlay_preserves_source_and_transforms(self):
|
||||
before = self.path.read_bytes()
|
||||
output = Path(self.directory.name) / "floating.usda"
|
||||
result = prepare_floating_overlay(self.path, output)
|
||||
self.assertEqual(result["world_fixed_joints"], [])
|
||||
self.assertEqual(result["articulation_roots"], ["/Robot/base"])
|
||||
self.assertEqual(self.path.read_bytes(), before)
|
||||
self.assertFalse(result["dynamic_replay_ready"])
|
||||
stage = Usd.Stage.Open(str(output))
|
||||
self.assertFalse(UsdPhysics.Joint(stage.GetPrimAtPath("/Robot/anchor")).GetJointEnabledAttr().Get())
|
||||
self.assertFalse(Sdf.Layer.FindOrOpen(str(output)).subLayerPaths[0].startswith("/"))
|
||||
self.assertTrue(
|
||||
Gf.IsClose(
|
||||
UsdGeom.XformCache().GetLocalToWorldTransform(stage.GetPrimAtPath("/Robot/base")), Gf.Matrix4d(1), 1e-12
|
||||
)
|
||||
)
|
||||
with self.assertRaisesRegex(ValueError, "overwrite"):
|
||||
prepare_floating_overlay(self.path, output)
|
||||
with self.assertRaisesRegex(ValueError, "fixed-world"):
|
||||
prepare_floating_overlay(output, Path(self.directory.name) / "again.usda")
|
||||
|
||||
def test_ambiguous_root_rejected(self):
|
||||
stage = Usd.Stage.Open(str(self.path))
|
||||
UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath("/Robot/base"))
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "one articulation"):
|
||||
inspect(self.path)
|
||||
|
||||
def test_urdf_matching_and_wrong_limits(self):
|
||||
urdf = Path(self.directory.name) / "robot.urdf"
|
||||
urdf.write_text(
|
||||
'<robot name="test"><joint name="hinge" type="revolute">'
|
||||
'<parent link="base"/><child link="tip"/>'
|
||||
'<limit lower="0" upper="1.5707963267948966"/></joint></robot>'
|
||||
)
|
||||
result = inspect(self.path, urdf)
|
||||
self.assertEqual(result["source_urdf"]["status"], "STRUCTURAL_MATCH_ONLY")
|
||||
urdf.write_text(urdf.read_text().replace('upper="1.5707963267948966"', 'upper="1"'))
|
||||
with self.assertRaisesRegex(ValueError, "limit mismatch"):
|
||||
inspect(self.path, urdf)
|
||||
|
||||
def test_newton_mimic_presence_is_not_runtime_verification(self):
|
||||
stage = Usd.Stage.Open(str(self.path))
|
||||
hinge = stage.GetPrimAtPath("/Robot/hinge")
|
||||
hinge.AddAppliedSchema("NewtonMimicAPI")
|
||||
hinge.CreateAttribute("newton:mimicCoef1", Sdf.ValueTypeNames.Float).Set(1)
|
||||
stage.GetRootLayer().Save()
|
||||
urdf = Path(self.directory.name) / "robot.urdf"
|
||||
# Deliberately self-referencing fixture only tests backend evidence, not valid transmission.
|
||||
urdf.write_text(
|
||||
'<robot name="test"><joint name="hinge" type="revolute">'
|
||||
'<parent link="base"/><child link="tip"/>'
|
||||
'<limit lower="0" upper="1.5707963267948966"/>'
|
||||
'<mimic joint="hinge" multiplier="1"/></joint></robot>'
|
||||
)
|
||||
result = inspect(self.path, urdf)
|
||||
self.assertTrue(result["coupling_evidence"])
|
||||
self.assertFalse(result["physx_coupling_evidence"])
|
||||
self.assertEqual(result["coupling_status"], "MIMIC_SCHEMA_PRESENT_RUNTIME_UNVERIFIED")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,103 @@
|
||||
"""Analytic CPU regressions for control.py; not simulated dynamics."""
|
||||
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from dex_workbench_tracking.cli import synthetic
|
||||
from dex_workbench_tracking.control import (
|
||||
Limits,
|
||||
reference_pose_to_xyzw,
|
||||
rotation_error,
|
||||
validate_reference,
|
||||
wrench,
|
||||
xyzw_pose_to_reference,
|
||||
)
|
||||
from dex_workbench_tracking.trajectory import ContractError
|
||||
|
||||
|
||||
class ControlTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.limits = Limits()
|
||||
self.pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=float)
|
||||
self.velocity = np.zeros(6)
|
||||
|
||||
def compute(self, target=None, root_com=None, body_com=None, masses=None):
|
||||
return wrench(
|
||||
np.zeros(3) if target is None else np.array(target),
|
||||
self.pose[3:],
|
||||
self.pose,
|
||||
self.velocity,
|
||||
np.zeros(3) if root_com is None else np.array(root_com),
|
||||
np.zeros((1, 3)) if body_com is None else np.array(body_com),
|
||||
np.array([0.1]) if masses is None else np.array(masses),
|
||||
self.limits,
|
||||
)
|
||||
|
||||
def test_hdf5_wxyz_runtime_xyzw_boundary(self):
|
||||
position = [0.1, 0.2, 0.3]
|
||||
quaternion = [0.8, 0.6, 0, 0]
|
||||
runtime = reference_pose_to_xyzw(position, quaternion)
|
||||
np.testing.assert_allclose(runtime, [0.1, 0.2, 0.3, 0.6, 0, 0, 0.8])
|
||||
np.testing.assert_allclose(xyzw_pose_to_reference(runtime), position + quaternion)
|
||||
|
||||
def test_gravity_all_links_and_moment_arm(self):
|
||||
force, torque = self.compute(body_com=[[0, 0, 0], [0.1, 0, 0]], masses=[0.1, 0.2])
|
||||
np.testing.assert_allclose(force, [0, 0, 2.943])
|
||||
np.testing.assert_allclose(torque, [0, -0.1962, 0])
|
||||
|
||||
def test_link_pd_force_shift_to_com(self):
|
||||
force, torque = self.compute(target=[0, 0.01, 0], root_com=[0.1, 0, 0], body_com=[[0.1, 0, 0]])
|
||||
np.testing.assert_allclose(force, [0, 1, 0.981])
|
||||
np.testing.assert_allclose(torque, [0, 0, -0.1])
|
||||
|
||||
def test_total_wrench_including_gravity_is_bounded(self):
|
||||
force, torque = self.compute(target=[100, 100, 100], body_com=[[1, 0, 0]], masses=[100])
|
||||
self.assertLessEqual(np.linalg.norm(force), self.limits.force + 1e-10)
|
||||
self.assertLessEqual(np.linalg.norm(torque), self.limits.torque + 1e-10)
|
||||
|
||||
def test_damping_opposes_world_velocity(self):
|
||||
self.velocity[:3] = [0.01, 0, 0]
|
||||
self.velocity[3:] = [0, 0, 0.1]
|
||||
force, torque = self.compute()
|
||||
self.assertAlmostEqual(force[0], -0.1)
|
||||
self.assertAlmostEqual(torque[2], -0.002)
|
||||
|
||||
def test_orientation_world_rotation_and_antipodes(self):
|
||||
angle = 0.2
|
||||
target = np.array([np.cos(angle / 2), 0, 0, np.sin(angle / 2)])
|
||||
np.testing.assert_allclose(rotation_error(target, self.pose[3:]), [0, 0, angle])
|
||||
np.testing.assert_allclose(rotation_error(-target, self.pose[3:]), [0, 0, angle])
|
||||
np.testing.assert_allclose(rotation_error(self.pose[3:], target), [0, 0, -angle])
|
||||
# Current local-X quarter-turn followed by world-Z quarter-turn.
|
||||
np.testing.assert_allclose(
|
||||
rotation_error([0.5, 0.5, 0.5, 0.5], [2**-0.5, 2**-0.5, 0, 0]), [0, 0, np.pi / 2], atol=1e-12
|
||||
)
|
||||
|
||||
def test_nonfinite_state_bad_mass_and_limits_rejected(self):
|
||||
for mass in (0, -1, float("nan")):
|
||||
with self.assertRaises(ContractError):
|
||||
self.compute(masses=[mass])
|
||||
for value in (0, -1, float("nan"), float("inf")):
|
||||
with self.assertRaises(ContractError):
|
||||
Limits(force=value)
|
||||
|
||||
def test_reference_envelope_and_invalid_gap(self):
|
||||
manifest = {
|
||||
"manifest_version": "l20_asset_manifest_v1",
|
||||
"asset_sha256": "a" * 64,
|
||||
"root_link": "test",
|
||||
"joints": [{"name": "a", "lower_rad": 0, "upper_rad": 1}],
|
||||
}
|
||||
episode = synthetic(manifest).episodes["demo_000000"]
|
||||
validate_reference(episode, self.limits)
|
||||
episode.valid[60] = False
|
||||
with self.assertRaisesRegex(ContractError, "fully valid"):
|
||||
validate_reference(episode, self.limits)
|
||||
episode.valid[:] = True
|
||||
episode.wrist_position[60, 0] = 1
|
||||
with self.assertRaisesRegex(ContractError, "too fast"):
|
||||
validate_reference(episode, self.limits)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,225 @@
|
||||
"""Source-named CPU/USD tests of prepared.py; actual L20 dependency copies, no Kit."""
|
||||
|
||||
import copy
|
||||
import json
|
||||
import shutil
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
from dex_workbench_tracking.asset import dependencies
|
||||
from dex_workbench_tracking.prepared import (
|
||||
BACKEND_VERSION,
|
||||
MIMIC,
|
||||
bind_source,
|
||||
inspect_prepared,
|
||||
newton_mimic,
|
||||
prepare,
|
||||
require_backend,
|
||||
schemas,
|
||||
)
|
||||
|
||||
from pxr import Sdf, Usd, UsdPhysics
|
||||
|
||||
REPO = Path(__file__).resolve().parents[3]
|
||||
ASSET_DIR = REPO / "assets/robots/dex_hand/linkerhand_g20_left"
|
||||
|
||||
|
||||
class PreparedTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.temp = tempfile.TemporaryDirectory()
|
||||
self.addCleanup(self.temp.cleanup)
|
||||
self.bundle = Path(self.temp.name) / "bundle"
|
||||
shutil.copytree(ASSET_DIR / "linkerhand_g20_left", self.bundle / "original")
|
||||
self.source = self.bundle / "original/linkerhand_g20_left.usda"
|
||||
self.output = self.bundle / "tracking.usda"
|
||||
self.manifest = json.loads((ASSET_DIR / "tracking_manifest.json").read_text())
|
||||
|
||||
def build(self):
|
||||
return prepare(self.source, self.output, self.manifest)
|
||||
|
||||
def test_unsupported_backend_or_unregistered_schema_rejected(self):
|
||||
require_backend(BACKEND_VERSION, True)
|
||||
for version, registered in (("110.1.12", True), ("future", True), (BACKEND_VERSION, False)):
|
||||
with self.assertRaises(ValueError):
|
||||
require_backend(version, registered)
|
||||
|
||||
def test_overlay_suppresses_only_followers_and_preserves_original(self):
|
||||
original = dependencies(self.source)
|
||||
result = self.build()
|
||||
self.assertEqual(dependencies(self.source), original)
|
||||
self.assertFalse(result["runtime_verified"])
|
||||
self.assertEqual(len(result["independent_joint_names"]), 16)
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
for eq in MIMIC:
|
||||
joint = next(j for j in self.manifest["joints"] if j["name"] == eq["joint"])
|
||||
prim = stage.GetPrimAtPath(joint["path"])
|
||||
self.assertIn("NewtonMimicAPI", schemas(prim))
|
||||
self.assertNotIn("PhysicsDriveAPI:angular", schemas(prim))
|
||||
leader, multiplier, offset = newton_mimic(prim)
|
||||
self.assertTrue(leader.endswith("/" + eq["reference"]))
|
||||
self.assertAlmostEqual(multiplier, eq["multiplier"], places=6)
|
||||
self.assertEqual(offset, 0)
|
||||
self.assertEqual(result, inspect_prepared(self.output, self.manifest))
|
||||
|
||||
def test_only_obsolete_leaf_world_anchor_is_inactive(self):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"])
|
||||
self.assertFalse(anchor.IsActive())
|
||||
self.assertFalse(anchor.GetAttribute("physics:jointEnabled").Get())
|
||||
self.assertFalse(anchor.GetAllChildren())
|
||||
self.assertIn(anchor, list(stage.TraverseAll()))
|
||||
self.assertNotIn(anchor, list(stage.Traverse()))
|
||||
joints = [p for p in stage.Traverse() if p.IsA(UsdPhysics.Joint)]
|
||||
self.assertEqual(len(joints), 21)
|
||||
self.assertTrue(all(p.IsA(UsdPhysics.RevoluteJoint) for p in joints))
|
||||
anchor.SetActive(True)
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "active state"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_unrelated_deactivation_rejected(self):
|
||||
self.edit_follower(lambda p: p.SetActive(False))
|
||||
|
||||
def test_unrelated_prim_addition_rejected(self):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
stage.DefinePrim(self.manifest["default_prim"] + "/Extra", "Scope")
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "Prim tree changed"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_inactive_anchor_attributes_still_checked(self):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"])
|
||||
anchor.GetAttribute("physics:jointEnabled").Set(True, Usd.TimeCode(1))
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "Time samples prohibited"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_inactive_anchor_must_remain_disabled(self):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"])
|
||||
anchor.GetAttribute("physics:jointEnabled").Set(True)
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "remain disabled"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_inactive_anchor_relationships_still_checked(self):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"])
|
||||
anchor.GetRelationship("physics:body1").SetTargets([])
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "Relationship changed"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_complete_bundle_relocation_preserves_identity(self):
|
||||
before = self.build()
|
||||
moved = Path(self.temp.name) / "moved"
|
||||
shutil.copytree(self.bundle, moved)
|
||||
self.assertEqual(before, inspect_prepared(moved / "tracking.usda", self.manifest))
|
||||
layer = Sdf.Layer.FindOrOpen(str(self.output))
|
||||
self.assertEqual(layer.subLayerPaths, ["original/linkerhand_g20_left.usda"])
|
||||
|
||||
def test_stale_manifest_or_original_bytes_rejected(self):
|
||||
wrong = copy.deepcopy(self.manifest)
|
||||
wrong["asset_sha256"] = "0" * 64
|
||||
with self.assertRaisesRegex(ValueError, "Stale"):
|
||||
prepare(self.source, self.output, wrong)
|
||||
self.assertFalse(self.output.exists())
|
||||
self.build()
|
||||
with self.source.open("a") as stream:
|
||||
stream.write("\n# change identity\n")
|
||||
with self.assertRaisesRegex(ValueError, "Stale"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_altered_model_mapping_rejected(self):
|
||||
wrong = copy.deepcopy(self.manifest)
|
||||
wrong["source_urdf"]["mimic"][0]["multiplier"] = -0.89
|
||||
with self.assertRaisesRegex(ValueError, "Unexpected L20 mimic"):
|
||||
bind_source(self.source, wrong)
|
||||
|
||||
def test_fixed_preview_and_json_flag_cannot_enable(self):
|
||||
self.manifest["dynamic_replay_ready"] = True
|
||||
with self.assertRaises(ValueError):
|
||||
inspect_prepared(self.source, self.manifest)
|
||||
|
||||
def test_no_overwrite(self):
|
||||
self.build()
|
||||
before = self.output.read_bytes()
|
||||
with self.assertRaisesRegex(ValueError, "overwrite"):
|
||||
self.build()
|
||||
self.assertEqual(self.output.read_bytes(), before)
|
||||
|
||||
def edit_follower(self, callback):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
path = next(j["path"] for j in self.manifest["joints"] if j["name"] == "thumb_ip")
|
||||
callback(stage.GetPrimAtPath(path))
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaises(ValueError):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_sampled_mimic_rejected(self):
|
||||
self.edit_follower(lambda p: p.GetAttribute("newton:mimicCoef1").Set(-1.02, Usd.TimeCode(1)))
|
||||
|
||||
def test_sampled_allowlisted_follower_gain_rejected(self):
|
||||
self.edit_follower(lambda p: p.GetAttribute("drive:angular:physics:stiffness").Set(99, Usd.TimeCode(1)))
|
||||
|
||||
def test_sampled_mass_rejected(self):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
root = stage.GetPrimAtPath(self.manifest["root_body_path"])
|
||||
root.GetAttribute("physics:mass").Set(99, Usd.TimeCode(1))
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "Time samples prohibited"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_sampled_transform_rejected(self):
|
||||
self.build()
|
||||
stage = Usd.Stage.Open(str(self.output))
|
||||
attr = next(
|
||||
a
|
||||
for p in stage.Traverse()
|
||||
for a in p.GetAttributes()
|
||||
if a.GetName().startswith("xformOp:") and a.Get() is not None
|
||||
)
|
||||
# Even a sample equal to the default is prohibited for this static asset.
|
||||
attr.Set(attr.Get(), Usd.TimeCode(1))
|
||||
stage.GetRootLayer().Save()
|
||||
with self.assertRaisesRegex(ValueError, "Time samples prohibited"):
|
||||
inspect_prepared(self.output, self.manifest)
|
||||
|
||||
def test_wrong_sign_rejected(self):
|
||||
self.edit_follower(lambda p: p.GetAttribute("newton:mimicCoef1").Set(-1.02))
|
||||
|
||||
def test_disabled_native_constraint_rejected(self):
|
||||
self.edit_follower(lambda p: p.CreateAttribute("newton:mimicEnabled", Sdf.ValueTypeNames.Bool).Set(False))
|
||||
|
||||
def test_duplicate_legacy_constraint_rejected(self):
|
||||
self.edit_follower(lambda p: p.AddAppliedSchema("PhysxMimicJointAPI:rotX"))
|
||||
|
||||
def test_follower_motor_rejected(self):
|
||||
self.edit_follower(lambda p: UsdPhysics.DriveAPI.Apply(p, "angular"))
|
||||
|
||||
def test_changed_mass_or_limits_rejected(self):
|
||||
self.edit_follower(lambda p: p.GetAttribute("physics:upperLimit").Set(180.0))
|
||||
|
||||
def test_newton_offset_degree_conversion(self):
|
||||
stage = Usd.Stage.CreateInMemory()
|
||||
prim = UsdPhysics.RevoluteJoint.Define(stage, "/follower").GetPrim()
|
||||
prim.AddAppliedSchema("NewtonMimicAPI")
|
||||
prim.CreateRelationship("newton:mimicJoint").SetTargets(["/leader"])
|
||||
prim.CreateAttribute("newton:mimicCoef0", Sdf.ValueTypeNames.Float).Set(90)
|
||||
prim.CreateAttribute("newton:mimicCoef1", Sdf.ValueTypeNames.Float).Set(-2)
|
||||
_, coefficient, offset = newton_mimic(prim)
|
||||
self.assertEqual(coefficient, -2)
|
||||
self.assertAlmostEqual(offset, 1.5707963267948966)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,89 @@
|
||||
"""Regression of CLI's named range_4s HDF5 reference against the actual L20 manifest."""
|
||||
|
||||
import json
|
||||
import subprocess
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from dex_workbench_tracking.cli import synthetic
|
||||
from dex_workbench_tracking.control import Limits, validate_reference
|
||||
from dex_workbench_tracking.trajectory import ContractError, load, sample
|
||||
|
||||
REPO = Path(__file__).resolve().parents[3]
|
||||
MANIFEST = REPO / "assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json"
|
||||
|
||||
|
||||
class TrackingRangeProfileTests(unittest.TestCase):
|
||||
def test_default_small_profile_is_unchanged(self):
|
||||
manifest = json.loads(MANIFEST.read_text())
|
||||
implicit, explicit = synthetic(manifest), synthetic(manifest, "small")
|
||||
self.assertEqual(implicit.metadata, explicit.metadata)
|
||||
a, b = implicit.episodes["demo_000000"], explicit.episodes["demo_000000"]
|
||||
for field in ("time", "wrist_position", "wrist_quaternion", "joint_position", "valid"):
|
||||
np.testing.assert_array_equal(getattr(a, field), getattr(b, field))
|
||||
self.assertEqual(len(a.time), 121)
|
||||
self.assertEqual(a.time[-1], 2)
|
||||
self.assertAlmostEqual(a.wrist_position[:, 0].max(), 0.002)
|
||||
with self.assertRaises(ContractError):
|
||||
synthetic(manifest, "unknown")
|
||||
|
||||
def test_range_cli_publication_mapping_and_reference_limits(self):
|
||||
manifest = json.loads(MANIFEST.read_text())
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
output = Path(directory) / "range.hdf5"
|
||||
command = [
|
||||
sys.executable,
|
||||
"-m",
|
||||
"dex_workbench_tracking.cli",
|
||||
"synthetic",
|
||||
"--manifest",
|
||||
str(MANIFEST),
|
||||
"--profile",
|
||||
"range_4s",
|
||||
"--output",
|
||||
str(output),
|
||||
]
|
||||
result = subprocess.run(command, capture_output=True, text=True, timeout=30)
|
||||
self.assertEqual(result.returncode, 0, result.stderr)
|
||||
self.assertEqual(json.loads(result.stdout)["provenance"], "synthetic")
|
||||
data = load(output, manifest)
|
||||
self.assertEqual(data.metadata["asset_sha256"], manifest["asset_sha256"])
|
||||
self.assertEqual(data.metadata["provenance"], "synthetic")
|
||||
self.assertIn("range_4s", data.metadata["source_description"])
|
||||
self.assertIn("NOT expert", data.metadata["source_description"])
|
||||
names = list(data.joint_names)
|
||||
self.assertEqual(names, [j["name"] for j in manifest["joints"]])
|
||||
episode = data.episodes["demo_000000"]
|
||||
self.assertEqual(len(episode.time), 241)
|
||||
self.assertEqual(episode.time[-1], 4)
|
||||
self.assertAlmostEqual(episode.wrist_position[:, 0].max(), 0.01)
|
||||
self.assertAlmostEqual(2 * np.arccos(episode.wrist_quaternion[:, 0].min()), 0.1, places=5)
|
||||
followers = {eq["joint"] for eq in manifest["source_urdf"]["mimic"]}
|
||||
for j in manifest["joints"]:
|
||||
if j["name"] not in followers:
|
||||
self.assertAlmostEqual(
|
||||
episode.joint_position[:, names.index(j["name"])].max(), min(0.1, 0.25 * j["upper_rad"])
|
||||
)
|
||||
for eq in manifest["source_urdf"]["mimic"]:
|
||||
np.testing.assert_allclose(
|
||||
episode.joint_position[:, names.index(eq["joint"])],
|
||||
eq["multiplier"] * episode.joint_position[:, names.index(eq["reference"])],
|
||||
atol=1e-7,
|
||||
)
|
||||
for field in ("wrist_position", "wrist_quaternion", "joint_position"):
|
||||
np.testing.assert_allclose(getattr(episode, field)[0], getattr(episode, field)[-1], atol=1e-7)
|
||||
validate_reference(episode, Limits())
|
||||
resampled = sample(episode, np.arange(961) / 240)
|
||||
validate_reference(resampled, Limits())
|
||||
self.assertEqual(len(resampled.time), 961)
|
||||
before = output.read_bytes()
|
||||
refused = subprocess.run(command, capture_output=True, text=True, timeout=30)
|
||||
self.assertNotEqual(refused.returncode, 0)
|
||||
self.assertEqual(output.read_bytes(), before)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Isolated CPU plugin-context regression; requires the installed Isaac USD schemas, not Kit."""
|
||||
|
||||
import json
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
from dex_workbench_tracking.prepared import inspect_prepared, prepare
|
||||
|
||||
REPO = Path(__file__).resolve().parents[3]
|
||||
ASSET = REPO / "assets/robots/dex_hand/linkerhand_g20_left"
|
||||
|
||||
|
||||
class TrackingSchemaContextTests(unittest.TestCase):
|
||||
def test_registered_schema_preparation_preserves_defaults_and_rejects_context_drift(self):
|
||||
installation = os.environ.get("ISAAC_PATH")
|
||||
if not installation:
|
||||
self.skipTest("ISAAC_PATH required for installed PhysX/Newton CPU schema regression")
|
||||
installation = Path(installation)
|
||||
physx = list(installation.glob("extscache/omni.usd.schema.physx-*/plugins/PhysxSchema/resources/plugInfo.json"))
|
||||
self.assertEqual(len(physx), 1, "Select an unambiguous installed PhysX schema for this test")
|
||||
newton = installation / "exts/omni.usd.schema.newton/usd/schema/newton/newton_usd_schemas/plugInfo.json"
|
||||
self.assertTrue(newton.is_file())
|
||||
manifest = json.loads((ASSET / "tracking_manifest.json").read_text())
|
||||
source = ASSET / "linkerhand_g20_left/linkerhand_g20_left.usda"
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
directory = Path(directory)
|
||||
unregistered = directory / "unregistered.usda"
|
||||
registered = directory / "registered.usda"
|
||||
result = prepare(source, unregistered, manifest)
|
||||
self.assertEqual(result["status"], "STATIC_ONLY_SCHEMA_UNREGISTERED")
|
||||
self.assertEqual(result, inspect_prepared(unregistered, manifest))
|
||||
# Registering plugins in a child prevents pollution of other CPU tests.
|
||||
code = """
|
||||
import json, sys
|
||||
from pathlib import Path
|
||||
from pxr import Usd
|
||||
from dex_workbench_tracking.asset import dependencies
|
||||
from dex_workbench_tracking.prepared import inspect_prepared, register_schema_plugins
|
||||
source, manifest_path, unregistered, registered, *plugins = sys.argv[1:]
|
||||
manifest = json.loads(Path(manifest_path).read_text())
|
||||
register_schema_plugins(plugins)
|
||||
try:
|
||||
inspect_prepared(unregistered, manifest)
|
||||
except ValueError as error:
|
||||
assert "regenerate" in str(error), error
|
||||
else:
|
||||
raise AssertionError("Unregistered authoring accepted with registered schemas")
|
||||
before = dependencies(source)
|
||||
# Exercise the actual CLI registration option in a fresh CPU interpreter too.
|
||||
import subprocess
|
||||
command = [sys.executable, "-m", "dex_workbench_tracking.prepared", source,
|
||||
"--manifest", manifest_path, "--output", registered]
|
||||
for plugin in plugins:
|
||||
command += ["--schema-plugin-path", plugin]
|
||||
run = subprocess.run(command, capture_output=True, text=True, timeout=60)
|
||||
assert run.returncode == 0, run.stderr
|
||||
result = inspect_prepared(registered, manifest)
|
||||
assert result["status"] == "STATIC_ELIGIBLE_RUNTIME_UNVERIFIED"
|
||||
assert all(result["schema_context"].values())
|
||||
assert dependencies(source) == before
|
||||
original = Usd.Stage.Open(source)
|
||||
stage = Usd.Stage.Open(registered)
|
||||
anchor = manifest["world_fixed_joints"][0]["path"]
|
||||
root = stage.GetPrimAtPath(manifest["root_body_path"])
|
||||
for attr in original.GetPrimAtPath(anchor).GetAttributes():
|
||||
if attr.GetName().startswith("physxArticulation:"):
|
||||
for prim in (stage.GetPrimAtPath(anchor), root):
|
||||
after = prim.GetAttribute(attr.GetName())
|
||||
assert after.Get() == attr.Get(), attr.GetPath()
|
||||
assert after.HasAuthoredValue(), attr.GetPath()
|
||||
assert not after.GetTimeSamples(), attr.GetPath()
|
||||
# Registered schema fallbacks do not relax the static/time-sample gate.
|
||||
root.GetAttribute("physxArticulation:sleepThreshold").Set(99, Usd.TimeCode(1))
|
||||
stage.GetRootLayer().Save()
|
||||
try:
|
||||
inspect_prepared(registered, manifest)
|
||||
except ValueError as error:
|
||||
assert "Time samples prohibited" in str(error), error
|
||||
else:
|
||||
raise AssertionError("Sampled solver property accepted")
|
||||
root.GetAttribute("physxArticulation:sleepThreshold").ClearAtTime(Usd.TimeCode(1))
|
||||
stage.GetRootLayer().Save()
|
||||
inspect_prepared(registered, manifest)
|
||||
print("PASS: isolated registered schema author/inspect, exact default preservation, strict time samples")
|
||||
"""
|
||||
result = subprocess.run(
|
||||
[
|
||||
sys.executable,
|
||||
"-c",
|
||||
code,
|
||||
str(source),
|
||||
str(ASSET / "tracking_manifest.json"),
|
||||
str(unregistered),
|
||||
str(registered),
|
||||
str(physx[0]),
|
||||
str(newton),
|
||||
],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=90,
|
||||
)
|
||||
self.assertEqual(result.returncode, 0, result.stdout + result.stderr)
|
||||
# The inverse schema mismatch is actionable too; never pretend unknown
|
||||
# vendor fallbacks are equivalent in an unregistered process.
|
||||
with self.assertRaisesRegex(ValueError, "regenerate"):
|
||||
inspect_prepared(registered, manifest)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,90 @@
|
||||
"""CPU startup/fast-shutdown regressions for the two source-named Kit entries."""
|
||||
|
||||
import builtins
|
||||
import contextlib
|
||||
import importlib.util
|
||||
import io
|
||||
import sys
|
||||
import types
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
from unittest.mock import patch
|
||||
|
||||
import numpy as np
|
||||
|
||||
REPO = Path(__file__).resolve().parents[3]
|
||||
|
||||
|
||||
class TrackingStartupTests(unittest.TestCase):
|
||||
def check_entry(self, filename, inspection_module, inspection_name):
|
||||
events = []
|
||||
|
||||
class FakeApp:
|
||||
def close(self, *, exit_code):
|
||||
events.append(("close", exit_code))
|
||||
# Model Kit fast shutdown, which never returns to a pending raise.
|
||||
raise SystemExit(exit_code)
|
||||
|
||||
class FakeLauncher:
|
||||
@staticmethod
|
||||
def add_app_launcher_args(parser):
|
||||
pass
|
||||
|
||||
def __init__(self, args):
|
||||
events.append("launch")
|
||||
self.app = FakeApp()
|
||||
|
||||
def reject_asset(*args):
|
||||
events.append("inspect")
|
||||
raise ValueError("deliberate asset rejection")
|
||||
|
||||
app_module = types.ModuleType("isaaclab.app")
|
||||
app_module.AppLauncher = FakeLauncher
|
||||
inspector = types.ModuleType(inspection_module)
|
||||
setattr(inspector, inspection_name, reject_asset)
|
||||
inspector.require_backend = inspector.validate_mimic = None
|
||||
original_import = builtins.__import__
|
||||
|
||||
def checked_import(name, *args, **kwargs):
|
||||
if name.startswith(("dex_workbench_tracking", "pxr")):
|
||||
self.assertIn("launch", events, f"USD-dependent import before AppLauncher: {name}")
|
||||
return original_import(name, *args, **kwargs)
|
||||
|
||||
entry = REPO / "scripts/tracking" / filename
|
||||
spec = importlib.util.spec_from_file_location("tracking_entry_test", entry)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(module)
|
||||
asset = REPO / "assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json"
|
||||
argv = [str(entry), str(asset)]
|
||||
if filename == "track_l20.py":
|
||||
argv += ["--manifest", str(asset), "--execute-experimental"]
|
||||
stderr = io.StringIO()
|
||||
with (
|
||||
patch.dict(
|
||||
sys.modules,
|
||||
{
|
||||
"isaaclab.app": app_module,
|
||||
inspection_module: inspector,
|
||||
"numpy": np,
|
||||
"torch": types.ModuleType("torch"),
|
||||
},
|
||||
),
|
||||
patch.object(sys, "argv", argv),
|
||||
patch.object(builtins, "__import__", checked_import),
|
||||
contextlib.redirect_stderr(stderr),
|
||||
):
|
||||
with self.assertRaises(SystemExit) as error:
|
||||
module.main()
|
||||
self.assertEqual(error.exception.code, 1)
|
||||
self.assertEqual(events, ["launch", "inspect", ("close", 1)], stderr.getvalue())
|
||||
self.assertIn("ValueError: deliberate asset rejection", stderr.getvalue())
|
||||
|
||||
def test_track_l20_launches_before_inspection_and_preserves_failure(self):
|
||||
self.check_entry("track_l20.py", "dex_workbench_tracking.prepared", "inspect_prepared")
|
||||
|
||||
def test_inspect_l20_scene_launches_before_inspection_and_preserves_failure(self):
|
||||
self.check_entry("inspect_l20_scene.py", "dex_workbench_tracking.asset", "inspect")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,303 @@
|
||||
"""CPU regression tests for dex_workbench_tracking.trajectory and CLI; no Isaac imports."""
|
||||
|
||||
import copy
|
||||
import json
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
import h5py
|
||||
import numpy as np
|
||||
from dex_workbench_tracking.cli import publish_validated, synthetic, write
|
||||
from dex_workbench_tracking.trajectory import ContractError, load, require_dynamic_replay_ready, sample
|
||||
|
||||
|
||||
class TrackingTrajectoryTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.directory = tempfile.TemporaryDirectory()
|
||||
self.addCleanup(self.directory.cleanup)
|
||||
self.path = Path(self.directory.name) / "fixture.hdf5"
|
||||
self.manifest = {
|
||||
"manifest_version": "l20_asset_manifest_v1",
|
||||
"asset_sha256": "a" * 64,
|
||||
"root_link": "test_root",
|
||||
"joints": [
|
||||
{"name": "finger_a", "lower_rad": -1, "upper_rad": 1},
|
||||
{"name": "finger_b", "lower_rad": -1, "upper_rad": 1},
|
||||
],
|
||||
"source_urdf": {
|
||||
"mimic": [{"joint": "finger_b", "reference": "finger_a", "multiplier": 0.5, "offset_rad": 0.0}]
|
||||
},
|
||||
}
|
||||
write(self.path, synthetic(self.manifest))
|
||||
|
||||
def edit(self, function):
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
function(file)
|
||||
|
||||
def test_load_roundtrip(self):
|
||||
data = load(self.path, self.manifest)
|
||||
self.assertEqual(data.joint_names, ("finger_a", "finger_b"))
|
||||
self.assertEqual(data.metadata["provenance"], "synthetic")
|
||||
self.assertEqual(data.episodes["demo_000000"].joint_position.shape, (121, 2))
|
||||
|
||||
def test_identity_and_order_mismatch(self):
|
||||
for key, value in (("asset_sha256", "b" * 64), ("root_link", "other")):
|
||||
manifest = copy.deepcopy(self.manifest)
|
||||
manifest[key] = value
|
||||
with self.subTest(key=key), self.assertRaises(ContractError):
|
||||
load(self.path, manifest)
|
||||
manifest = copy.deepcopy(self.manifest)
|
||||
manifest["joints"].reverse()
|
||||
with self.assertRaisesRegex(ContractError, "order"):
|
||||
load(self.path, manifest)
|
||||
|
||||
def test_missing_attributes(self):
|
||||
for name in ("root_link", "provenance", "source_description", "scale_to_meters", "metric_scale_provenance"):
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
value = file.attrs[name]
|
||||
del file.attrs[name]
|
||||
with self.subTest(name=name), self.assertRaises(ContractError):
|
||||
load(self.path)
|
||||
self.edit(lambda file: file.attrs.__setitem__(name, value))
|
||||
|
||||
def test_wrong_attributes(self):
|
||||
for name, bad in (
|
||||
("schema_version", "v2"),
|
||||
("embodiment", "L30"),
|
||||
("hand_side", "right"),
|
||||
("provenance", "unknown"),
|
||||
("asset_sha256", "xyz"),
|
||||
("root_link", ""),
|
||||
("scale_to_meters", 0.0),
|
||||
("scale_to_meters", float("nan")),
|
||||
("scale_to_meters", "1"),
|
||||
("scale_to_meters", [1.0]),
|
||||
):
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
original = file.attrs[name]
|
||||
file.attrs[name] = bad
|
||||
with self.subTest(name=name, bad=bad), self.assertRaises(ContractError):
|
||||
load(self.path)
|
||||
self.edit(lambda file: file.attrs.__setitem__(name, original))
|
||||
|
||||
def test_duplicate_or_wrong_joint_strings(self):
|
||||
self.edit(lambda file: file["metadata/joint_names"].__setitem__(1, "finger_a"))
|
||||
with self.assertRaisesRegex(ContractError, "duplicate"):
|
||||
load(self.path)
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
del file["metadata/joint_names"]
|
||||
file["metadata"].create_dataset("joint_names", data=[b"a", b"b"])
|
||||
with self.assertRaisesRegex(ContractError, "UTF-8"):
|
||||
load(self.path)
|
||||
|
||||
def test_invalid_transform(self):
|
||||
for bad in (np.diag([2, 1, 1, 1]), np.diag([-1, 1, 1, 1]), np.zeros((4, 4))):
|
||||
self.edit(lambda file: file["metadata/world_from_source"].__setitem__(slice(None), bad))
|
||||
with self.assertRaisesRegex(ContractError, "world_from_source"):
|
||||
load(self.path)
|
||||
|
||||
def test_transform_is_not_applied_again(self):
|
||||
self.edit(lambda file: file["metadata/world_from_source"].__setitem__((0, 3), 10))
|
||||
data = load(self.path)
|
||||
self.assertEqual(data.world_from_source[0, 3], 10)
|
||||
self.assertEqual(data.episodes["demo_000000"].wrist_position[0, 0], 0)
|
||||
|
||||
def test_episode_name_and_empty_group(self):
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
file.move("episodes/demo_000000", "episodes/demo_bad")
|
||||
with self.assertRaisesRegex(ContractError, "invalid episode"):
|
||||
load(self.path)
|
||||
self.edit(lambda file: file.__delitem__("episodes/demo_bad"))
|
||||
with self.assertRaisesRegex(ContractError, "nonempty"):
|
||||
load(self.path)
|
||||
|
||||
def test_shapes_and_dtypes(self):
|
||||
for name, value in (
|
||||
("time", np.arange(121, dtype=np.float32)),
|
||||
("wrist_position", np.zeros((121, 2), dtype=np.float32)),
|
||||
("joint_position", np.zeros((121, 2), dtype=np.float64)),
|
||||
("valid", np.ones(121, dtype=np.uint8)),
|
||||
):
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
group = file["episodes/demo_000000"]
|
||||
original = group[name][...]
|
||||
del group[name]
|
||||
group.create_dataset(name, data=value)
|
||||
with self.subTest(name=name), self.assertRaises(ContractError):
|
||||
load(self.path)
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
group = file["episodes/demo_000000"]
|
||||
del group[name]
|
||||
group.create_dataset(name, data=original)
|
||||
|
||||
def test_bad_time(self):
|
||||
for index, value in ((0, -1), (1, 0), (2, 0.001), (1, np.nan)):
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
time = file["episodes/demo_000000/time"]
|
||||
original = time[index]
|
||||
time[index] = value
|
||||
with self.subTest(index=index, value=value), self.assertRaises(ContractError):
|
||||
load(self.path)
|
||||
self.edit(lambda file: file["episodes/demo_000000/time"].__setitem__(index, original))
|
||||
|
||||
def test_nan_even_in_invalid_frame(self):
|
||||
self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(2, False))
|
||||
self.edit(lambda file: file["episodes/demo_000000/joint_position"].__setitem__((2, 0), np.nan))
|
||||
with self.assertRaisesRegex(ContractError, "NaN/Inf"):
|
||||
load(self.path)
|
||||
|
||||
def test_quaternion_normalization_and_sign(self):
|
||||
self.edit(lambda file: file["episodes/demo_000000/wrist_quaternion"].__setitem__(2, [2, 0, 0, 0]))
|
||||
with self.assertRaisesRegex(ContractError, "unit"):
|
||||
load(self.path)
|
||||
self.edit(lambda file: file["episodes/demo_000000/wrist_quaternion"].__setitem__(2, [-1, 0, 0, 0]))
|
||||
with self.assertRaisesRegex(ContractError, "sign"):
|
||||
load(self.path)
|
||||
|
||||
def test_all_invalid_rejected(self):
|
||||
self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(slice(None), False))
|
||||
with self.assertRaisesRegex(ContractError, "no valid"):
|
||||
load(self.path)
|
||||
|
||||
def test_limits_and_mimic_inconsistency(self):
|
||||
self.edit(lambda file: file["episodes/demo_000000/joint_position"].__setitem__((2, 0), 2))
|
||||
with self.assertRaisesRegex(ContractError, "limit violation"):
|
||||
load(self.path, self.manifest)
|
||||
self.edit(lambda file: file["episodes/demo_000000/joint_position"].__setitem__((2, 0), 0.2))
|
||||
with self.assertRaisesRegex(ContractError, "mimic"):
|
||||
load(self.path, self.manifest)
|
||||
|
||||
def test_slerp_and_linear_interpolation(self):
|
||||
episode = load(self.path).episodes["demo_000000"]
|
||||
# Isolated two-frame analytic rotation with a 180-degree endpoint, wxyz.
|
||||
from dex_workbench_tracking.trajectory import Episode
|
||||
|
||||
simple = Episode(
|
||||
np.array([0.0, 1.0]),
|
||||
np.array([[0, 0, 0], [2, 4, 6]], dtype=np.float32),
|
||||
np.array([[1, 0, 0, 0], [0, 0, 0, 1]], dtype=np.float32),
|
||||
np.array([[0, 0], [1, 2]], dtype=np.float32),
|
||||
np.array([True, True]),
|
||||
)
|
||||
sampled = sample(simple, [0, 0.5, 1])
|
||||
np.testing.assert_allclose(sampled.wrist_quaternion[1], [np.sqrt(0.5), 0, 0, np.sqrt(0.5)], atol=1e-7)
|
||||
np.testing.assert_allclose(sampled.wrist_position[1], [1, 2, 3])
|
||||
np.testing.assert_allclose(sampled.joint_position[1], [0.5, 1])
|
||||
np.testing.assert_allclose(sample(episode, episode.time).joint_position, episode.joint_position)
|
||||
np.testing.assert_allclose(np.linalg.norm(sample(episode, [0, 0.001]).wrist_quaternion, axis=1), 1, atol=1e-7)
|
||||
|
||||
def test_no_extrapolation_or_bad_queries(self):
|
||||
episode = load(self.path).episodes["demo_000000"]
|
||||
for query in ([], [-0.1], [2.1], [0, 0], [1, 0], [np.nan], [[0.1]]):
|
||||
with self.subTest(query=query), self.assertRaises(ContractError):
|
||||
sample(episode, query)
|
||||
|
||||
def test_invalid_gap_never_bridged(self):
|
||||
self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(60, False))
|
||||
episode = load(self.path).episodes["demo_000000"]
|
||||
for query in ([1], [0.99], [1.01], [0, 2]):
|
||||
with self.subTest(query=query), self.assertRaisesRegex(ContractError, "invalid"):
|
||||
sample(episode, query)
|
||||
self.assertEqual(len(sample(episode, [0, 0.5]).time), 2)
|
||||
self.assertEqual(len(sample(episode, [1.5, 2]).time), 2)
|
||||
|
||||
def test_no_overwrite(self):
|
||||
before = self.path.read_bytes()
|
||||
with self.assertRaises(OSError):
|
||||
write(self.path, synthetic(self.manifest))
|
||||
self.assertEqual(self.path.read_bytes(), before)
|
||||
|
||||
def test_validated_publication_failure_leaves_no_output(self):
|
||||
output = Path(self.directory.name) / "invalid.hdf5"
|
||||
data = synthetic(self.manifest)
|
||||
data.episodes["demo_000000"].wrist_quaternion[:] = 0
|
||||
with self.assertRaises(ContractError):
|
||||
publish_validated(output, data, self.manifest)
|
||||
self.assertFalse(output.exists())
|
||||
self.assertFalse(list(output.parent.glob(".l20-tracking-*")))
|
||||
|
||||
def test_validated_publication_preserves_existing_file(self):
|
||||
before = self.path.read_bytes()
|
||||
with self.assertRaises(FileExistsError):
|
||||
publish_validated(self.path, synthetic(self.manifest), self.manifest)
|
||||
self.assertEqual(self.path.read_bytes(), before)
|
||||
self.assertFalse(list(self.path.parent.glob(".l20-tracking-*")))
|
||||
|
||||
def test_cli_full_turn_downsampling_roundtrip(self):
|
||||
with h5py.File(self.path, "r+") as file:
|
||||
group = file["episodes/demo_000000"]
|
||||
time = group["time"][:]
|
||||
quaternion = np.zeros((len(time), 4), dtype=np.float32)
|
||||
quaternion[:, 0] = np.cos(np.pi * time / 2)
|
||||
quaternion[:, 3] = np.sin(np.pi * time / 2)
|
||||
group["wrist_quaternion"][:] = quaternion
|
||||
load(self.path, self.manifest)
|
||||
manifest = Path(self.directory.name) / "manifest.json"
|
||||
manifest.write_text(json.dumps(self.manifest))
|
||||
output = Path(self.directory.name) / "sparse.hdf5"
|
||||
result = self.run_cli("resample", self.path, "--manifest", manifest, "--hz", "0.5", "--output", output)
|
||||
self.assertEqual(result.returncode, 0, result.stderr)
|
||||
quaternion = load(output, self.manifest).episodes["demo_000000"].wrist_quaternion
|
||||
self.assertEqual(quaternion.shape, (2, 4))
|
||||
self.assertGreaterEqual(np.dot(quaternion[0], quaternion[1]), 0)
|
||||
np.testing.assert_allclose(quaternion[:, 0], 1, atol=1e-6)
|
||||
# Hemisphere repair cannot recover motion lost through undersampling.
|
||||
np.testing.assert_allclose(quaternion[:, 1:], 0, atol=1e-6)
|
||||
|
||||
def test_dynamic_gate_cannot_be_enabled_by_flag(self):
|
||||
for manifest in (self.manifest, {"dynamic_replay_ready": True, "coupling_status": "VERIFIED"}):
|
||||
with self.assertRaisesRegex(ContractError, "BLOCKED"):
|
||||
require_dynamic_replay_ready(manifest)
|
||||
|
||||
def run_cli(self, *args):
|
||||
return subprocess.run(
|
||||
[sys.executable, "-m", "dex_workbench_tracking.cli", *map(str, args)],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=30,
|
||||
env=os.environ.copy(),
|
||||
)
|
||||
|
||||
def test_cli_validate_and_resample(self):
|
||||
manifest = Path(self.directory.name) / "manifest.json"
|
||||
manifest.write_text(json.dumps(self.manifest))
|
||||
output = Path(self.directory.name) / "resampled.hdf5"
|
||||
result = self.run_cli("resample", self.path, "--manifest", manifest, "--hz", "100", "--output", output)
|
||||
self.assertEqual(result.returncode, 0, result.stderr)
|
||||
data = load(output, self.manifest)
|
||||
self.assertEqual(len(data.episodes["demo_000000"].time), 201)
|
||||
result = self.run_cli("validate", output)
|
||||
self.assertEqual(result.returncode, 0, result.stderr)
|
||||
self.assertEqual(json.loads(result.stdout)["asset_compatibility"], "NOT_CHECKED")
|
||||
self.edit(lambda file: file.attrs.__setitem__("hand_side", "right"))
|
||||
result = self.run_cli("validate", self.path)
|
||||
self.assertNotEqual(result.returncode, 0)
|
||||
self.assertIn("FAIL", result.stderr)
|
||||
|
||||
def test_cli_gate_and_invalid_gap_fail_closed(self):
|
||||
manifest = Path(self.directory.name) / "manifest.json"
|
||||
manifest.write_text(json.dumps(self.manifest))
|
||||
result = self.run_cli("replay-check", "--manifest", manifest)
|
||||
self.assertEqual(result.returncode, 2)
|
||||
self.assertIn("BLOCKED", result.stderr)
|
||||
output = Path(self.directory.name) / "resampled.hdf5"
|
||||
self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(60, False))
|
||||
result = self.run_cli("resample", self.path, "--manifest", manifest, "--hz", "60", "--output", output)
|
||||
self.assertEqual(result.returncode, 1)
|
||||
self.assertFalse(output.exists())
|
||||
|
||||
def test_cpu_import_does_not_register_isaac(self):
|
||||
code = (
|
||||
"import dex_workbench_tracking.trajectory, sys; "
|
||||
"assert not any(k.startswith(('isaac', 'omni', 'dex_workbench.')) for k in sys.modules)"
|
||||
)
|
||||
result = subprocess.run([sys.executable, "-c", code], capture_output=True, text=True, timeout=30)
|
||||
self.assertEqual(result.returncode, 0, result.stderr)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user