feat(tracking): 汇总 v0.1.3 回放与 ACT 准备进度
新增专家轨迹诊断、原视频估计相机入口和状态参考ACT数据门禁/训练链路；更新版本与进度文档。 验证：120项CPU/USD回归和4步synthetic CPU smoke通过；1333帧默认GUI历史回放PASS。原视频相机完整回放超时124，策略GPU E2E未执行，完整pre-commit工具缺失。 兼容性：HDF5、Cartpole、USD和控制阈值保持不变。本提交为实验进度快照，不宣称完整发布验收通过；数据、视频和权重不纳入。
This commit is contained in:
@@ -1,7 +1,7 @@
|
||||
[package]
|
||||
|
||||
# Semantic Versioning is used: https://semver.org/
|
||||
version = "0.1.2"
|
||||
version = "0.1.3"
|
||||
|
||||
# Description
|
||||
category = "isaaclab"
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
"""State-only reference imitation, separate from Isaac Lab task registration.
|
||||
|
||||
Importing this package does not launch Kit or initialize CUDA. It is not a robot
|
||||
hardware controller, grasping task, or implementation of the full DexSchema.
|
||||
"""
|
||||
@@ -0,0 +1,182 @@
|
||||
"""Manifest-bound geometry/posture encoding and bounded reference target adapter.
|
||||
|
||||
Shared geometry is root-link pose relative to the episode's initial root frame.
|
||||
Joint layout/coupling lives ONLY in this adapter. Targets are state-derived
|
||||
proxies, not measured actuator commands. No hardware or collision guarantees.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from dex_workbench_tracking.control import bounded, rotation_error
|
||||
from dex_workbench_tracking.identity import manifest_side
|
||||
from dex_workbench_tracking.trajectory import require
|
||||
|
||||
|
||||
def qmul(a, b):
|
||||
return np.r_[a[0] * b[0] - np.dot(a[1:], b[1:]), a[0] * b[1:] + b[0] * a[1:] + np.cross(a[1:], b[1:])]
|
||||
|
||||
|
||||
def qmatrix(q):
|
||||
q = np.asarray(q, dtype=float)
|
||||
require(q.shape == (4,) and np.isfinite(q).all() and abs(np.linalg.norm(q) - 1) < 1e-4, "Unit wxyz required")
|
||||
w, x, y, z = q / np.linalg.norm(q)
|
||||
return np.array(
|
||||
[
|
||||
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
||||
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
||||
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def matrixq(matrix):
|
||||
"""Rotation matrix -> unit wxyz, stable at half turns (largest component branch)."""
|
||||
m = np.asarray(matrix, dtype=float)
|
||||
candidates = np.array(
|
||||
[
|
||||
1 + np.trace(m),
|
||||
1 + m[0, 0] - m[1, 1] - m[2, 2],
|
||||
1 - m[0, 0] + m[1, 1] - m[2, 2],
|
||||
1 - m[0, 0] - m[1, 1] + m[2, 2],
|
||||
]
|
||||
)
|
||||
i = int(candidates.argmax())
|
||||
s = 2 * np.sqrt(max(candidates[i], 0))
|
||||
require(s > 1e-8, "Degenerate rotation")
|
||||
if i == 0:
|
||||
q = [s / 4, (m[2, 1] - m[1, 2]) / s, (m[0, 2] - m[2, 0]) / s, (m[1, 0] - m[0, 1]) / s]
|
||||
elif i == 1:
|
||||
q = [(m[2, 1] - m[1, 2]) / s, s / 4, (m[0, 1] + m[1, 0]) / s, (m[0, 2] + m[2, 0]) / s]
|
||||
elif i == 2:
|
||||
q = [(m[0, 2] - m[2, 0]) / s, (m[0, 1] + m[1, 0]) / s, s / 4, (m[1, 2] + m[2, 1]) / s]
|
||||
else:
|
||||
q = [(m[1, 0] - m[0, 1]) / s, (m[0, 2] + m[2, 0]) / s, (m[1, 2] + m[2, 1]) / s, s / 4]
|
||||
q = np.asarray(q)
|
||||
return q / np.linalg.norm(q)
|
||||
|
||||
|
||||
def rotation6d(values):
|
||||
a, b = np.asarray(values, dtype=float).reshape(2, 3)
|
||||
require(np.isfinite(a).all() and np.isfinite(b).all(), "Nonfinite policy rotation")
|
||||
require(np.linalg.norm(a) > 1e-6, "Degenerate policy rotation first axis")
|
||||
a = a / np.linalg.norm(a)
|
||||
b = b - np.dot(a, b) * a
|
||||
require(np.linalg.norm(b) > 1e-6, "Degenerate policy rotation second axis")
|
||||
b = b / np.linalg.norm(b)
|
||||
return np.column_stack((a, b, np.cross(a, b)))
|
||||
|
||||
|
||||
class Adapter:
|
||||
def __init__(self, manifest):
|
||||
self.side = manifest_side(manifest)
|
||||
self.names = [j["name"] for j in manifest["joints"]]
|
||||
require(len(set(self.names)) == len(self.names) and len(self.names) > 0, "Unique joint names required")
|
||||
self.lower = np.array([j["lower_rad"] for j in manifest["joints"]], dtype=float)
|
||||
self.upper = np.array([j["upper_rad"] for j in manifest["joints"]], dtype=float)
|
||||
require(
|
||||
np.isfinite(self.lower).all() and np.isfinite(self.upper).all() and (self.lower <= self.upper).all(),
|
||||
"Invalid limits",
|
||||
)
|
||||
self.mimic = manifest.get("source_urdf", {}).get("mimic", [])
|
||||
followers = [eq["joint"] for eq in self.mimic]
|
||||
require(len(followers) == len(set(followers)), "Duplicate follower")
|
||||
self.master_names = [n for n in self.names if n not in followers]
|
||||
self.columns = [self.names.index(n) for n in self.master_names]
|
||||
self.target_lower = self.lower[self.columns].copy()
|
||||
self.target_upper = self.upper[self.columns].copy()
|
||||
self.rate_multiplier = np.ones(len(self.columns))
|
||||
for eq in self.mimic:
|
||||
require(
|
||||
eq["joint"] in self.names and eq["reference"] in self.master_names,
|
||||
"Only inspected direct master/follower equations supported",
|
||||
)
|
||||
m, o = eq["multiplier"], eq["offset_rad"]
|
||||
require(np.isfinite(m) and np.isfinite(o) and m != 0, "Invalid mimic coefficients")
|
||||
child, master = self.names.index(eq["joint"]), self.master_names.index(eq["reference"])
|
||||
bounds = sorted([(self.lower[child] - o) / m, (self.upper[child] - o) / m])
|
||||
self.target_lower[master] = max(self.target_lower[master], bounds[0])
|
||||
self.target_upper[master] = min(self.target_upper[master], bounds[1])
|
||||
self.rate_multiplier[master] = max(self.rate_multiplier[master], abs(m))
|
||||
require((self.target_lower <= self.target_upper).all(), "Infeasible coupled limits")
|
||||
self.spec = {
|
||||
"hand_side": self.side,
|
||||
"asset_sha256": manifest["asset_sha256"],
|
||||
"root_link": manifest["root_link"],
|
||||
"joint_names": self.names,
|
||||
"master_names": self.master_names,
|
||||
"lower_rad": self.lower.tolist(),
|
||||
"upper_rad": self.upper.tolist(),
|
||||
"mimic": self.mimic,
|
||||
}
|
||||
self.action_dim = 9 + len(self.columns)
|
||||
# Current root pose/all state joints + terminal geometric/posture goal + phase/duration.
|
||||
self.observation_dim = 9 + len(self.names) + self.action_dim + 2
|
||||
|
||||
def encode_pose(self, position, quaternion, origin):
|
||||
p0, q0 = origin
|
||||
r0 = qmatrix(q0)
|
||||
relative = r0.T @ qmatrix(quaternion)
|
||||
return np.r_[r0.T @ (np.asarray(position) - p0), relative[:, 0], relative[:, 1]]
|
||||
|
||||
def action(self, position, quaternion, joints, origin):
|
||||
return np.r_[self.encode_pose(position, quaternion, origin), np.asarray(joints)[self.columns]].astype(
|
||||
np.float32
|
||||
)
|
||||
|
||||
def observation(self, position, quaternion, joints, origin, goal, elapsed, duration):
|
||||
require(duration > 0 and 0 <= elapsed <= duration + 1e-8, "Invalid trajectory phase")
|
||||
result = np.r_[
|
||||
self.encode_pose(position, quaternion, origin), joints, goal, min(elapsed / duration, 1), duration
|
||||
]
|
||||
require(result.shape == (self.observation_dim,) and np.isfinite(result).all(), "Invalid policy observation")
|
||||
return result.astype(np.float32)
|
||||
|
||||
def expand(self, masters):
|
||||
result = np.zeros(len(self.names), dtype=float)
|
||||
result[self.columns] = masters
|
||||
for eq in self.mimic:
|
||||
result[self.names.index(eq["joint"])] = (
|
||||
eq["multiplier"] * result[self.names.index(eq["reference"])] + eq["offset_rad"]
|
||||
)
|
||||
return result
|
||||
|
||||
def decode(self, action, origin):
|
||||
a = np.asarray(action, dtype=float)
|
||||
require(a.shape == (self.action_dim,) and np.isfinite(a).all(), "Invalid policy action")
|
||||
p0, q0 = origin
|
||||
r0 = qmatrix(q0)
|
||||
return p0 + r0 @ a[:3], matrixq(r0 @ rotation6d(a[3:9])), a[9:]
|
||||
|
||||
|
||||
class TargetLimiter:
|
||||
"""Bound commands at each physics step; never teleport simulated state."""
|
||||
|
||||
def __init__(self, adapter, limits, position, quaternion, joints):
|
||||
self.adapter, self.limits = adapter, limits
|
||||
self.origin = np.array(position, dtype=float)
|
||||
self.position, self.quaternion = np.array(position, dtype=float), np.array(quaternion, dtype=float)
|
||||
self.masters = np.clip(np.asarray(joints)[adapter.columns], adapter.target_lower, adapter.target_upper)
|
||||
self.limited_steps = 0
|
||||
self.steps = 0
|
||||
|
||||
def step(self, desired, dt):
|
||||
require(np.isfinite(dt) and dt > 0, "Positive command dt required")
|
||||
position, quaternion, masters = desired
|
||||
limits, adapter = self.limits, self.adapter
|
||||
require(all(np.isfinite(v).all() for v in desired), "Nonfinite desired command")
|
||||
bounded_position = self.origin + bounded(np.asarray(position) - self.origin, limits.workspace_radius)
|
||||
p = self.position + bounded(bounded_position - self.position, limits.reference_speed * dt)
|
||||
error = rotation_error(quaternion, self.quaternion)
|
||||
angle = np.linalg.norm(error)
|
||||
limited = min(angle, limits.reference_angular_speed * dt)
|
||||
increment = np.r_[np.cos(limited / 2), error * (np.sin(limited / 2) / angle if angle > 1e-12 else 0.5)]
|
||||
q = qmul(increment, self.quaternion)
|
||||
q /= np.linalg.norm(q)
|
||||
clipped = np.clip(masters, adapter.target_lower, adapter.target_upper)
|
||||
maximum = min(limits.reference_joint_speed, limits.finger_velocity) * dt / adapter.rate_multiplier
|
||||
m = self.masters + np.clip(clipped - self.masters, -maximum, maximum)
|
||||
self.limited_steps += int(
|
||||
np.linalg.norm(p - position) > 1e-8 or angle - limited > 1e-8 or np.max(np.abs(m - masters)) > 1e-8
|
||||
)
|
||||
self.steps += 1
|
||||
self.position, self.quaternion, self.masters = p, q, m
|
||||
return p.copy(), q.copy(), adapter.expand(m)
|
||||
@@ -0,0 +1,193 @@
|
||||
"""Offline ACT preparation/training commands. No command here starts Isaac Sim."""
|
||||
|
||||
import argparse
|
||||
import importlib.util
|
||||
import json
|
||||
import platform
|
||||
import sys
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
from dex_workbench_tracking.cli import publish_validated, synthetic
|
||||
from dex_workbench_tracking.trajectory import ContractError, Demonstrations, Episode, require
|
||||
|
||||
from .config import Config
|
||||
from .data import digest, prepare
|
||||
from .engine import json_write, load_checkpoint, score, train
|
||||
|
||||
|
||||
def read_json(path):
|
||||
return json.loads(Path(path).read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def provenance(metadata):
|
||||
package = Path(__file__).parent
|
||||
metadata["implementation_sha256"] = {p.name: digest(p) for p in sorted(package.glob("*.py"))}
|
||||
metadata["python"] = platform.python_version()
|
||||
metadata["numpy"] = np.__version__
|
||||
metadata["torch"] = str(torch.__version__)
|
||||
return metadata
|
||||
|
||||
|
||||
def evaluate(checkpoint, hdf5, manifest, partition):
|
||||
model, config, adapter, normalization, metadata = load_checkpoint(checkpoint, manifest)
|
||||
require(
|
||||
digest(hdf5) == metadata["hdf5_sha256"], "Evaluation HDF5 must match checkpoint; re-review new data explicitly"
|
||||
)
|
||||
_, datasets, fitted, _ = prepare(
|
||||
hdf5, manifest, metadata["splits"], config, metadata["data_review"], synthetic_smoke=metadata["synthetic_smoke"]
|
||||
)
|
||||
require(partition in ("validation", "test") and partition in datasets, "Select a nonempty held-out split")
|
||||
for key in normalization:
|
||||
np.testing.assert_array_equal(normalization[key], fitted[key])
|
||||
torch.set_num_threads(1)
|
||||
metric = score(model, datasets[partition], config.batch_size, "cpu")
|
||||
return {
|
||||
"status": "PASS",
|
||||
"scope": "offline_heldout_reference_prediction_not_dynamic_success",
|
||||
"split": partition,
|
||||
"windows": len(datasets[partition]),
|
||||
"l1_normalized_z0": metric,
|
||||
"checkpoint_sha256": digest(checkpoint),
|
||||
"hdf5_sha256": digest(hdf5),
|
||||
"synthetic_smoke": metadata["synthetic_smoke"],
|
||||
"simulation_e2e": "NOT_RUN",
|
||||
}
|
||||
|
||||
|
||||
def smoke(manifest, output):
|
||||
output = Path(output)
|
||||
output.mkdir(parents=True, exist_ok=False)
|
||||
side = manifest.get("hand_side", "left")
|
||||
config = replace(
|
||||
Config(),
|
||||
hand_side=side,
|
||||
chunk_size=4,
|
||||
execute_steps=2,
|
||||
hidden_dim=32,
|
||||
heads=4,
|
||||
layers=1,
|
||||
latent_dim=4,
|
||||
batch_size=4,
|
||||
max_updates=4,
|
||||
evaluate_every=2,
|
||||
max_seconds=120,
|
||||
)
|
||||
original = synthetic(manifest)
|
||||
episode = original.episodes["demo_000000"]
|
||||
episodes = {}
|
||||
for index, factor in enumerate((1.0, 1.3, 1.6)):
|
||||
position = episode.wrist_position.copy()
|
||||
position[:, 0] *= factor
|
||||
episodes[f"demo_{index:06d}"] = Episode(
|
||||
episode.time.copy(),
|
||||
position,
|
||||
episode.wrist_quaternion.copy(),
|
||||
episode.joint_position.copy() * factor,
|
||||
episode.valid.copy(),
|
||||
)
|
||||
data = Demonstrations(dict(original.metadata), original.joint_names, original.world_from_source.copy(), episodes)
|
||||
data.metadata["source_description"] += "; THREE ANALYTIC CPU SMOKE FIXTURES, not independent expert recordings"
|
||||
split = {
|
||||
"schema": "l20_episode_splits_v1",
|
||||
"train": ["demo_000000"],
|
||||
"validation": ["demo_000001"],
|
||||
"test": ["demo_000002"],
|
||||
"episode_groups": {name: f"analytic_fixture_{i}" for i, name in enumerate(episodes)},
|
||||
}
|
||||
hdf5 = output / "synthetic.hdf5"
|
||||
publish_validated(hdf5, data, manifest)
|
||||
json_write(output / "splits.json", split)
|
||||
json_write(output / "config.json", config.to_dict())
|
||||
adapter, datasets, normalization, metadata = prepare(hdf5, manifest, split, config, synthetic_smoke=True)
|
||||
result = train(adapter, datasets, normalization, provenance(metadata), config, output / "train", device="cpu")
|
||||
evaluation = evaluate(output / "train/last.pt", hdf5, manifest, "test")
|
||||
json_write(output / "offline-evaluation.json", evaluation)
|
||||
return {
|
||||
"status": "PASS",
|
||||
"scope": "synthetic_CPU_training_save_reload_evaluate_only",
|
||||
"training": result,
|
||||
"evaluation": evaluation,
|
||||
"simulation_e2e": "NOT_RUN",
|
||||
"output": str(output),
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
commands = parser.add_subparsers(dest="command", required=True)
|
||||
doctor = commands.add_parser("doctor", help="CPU dependency check; CUDA probing is opt-in")
|
||||
doctor.add_argument("--cuda", action="store_true")
|
||||
for command in ("preflight", "train"):
|
||||
item = commands.add_parser(command)
|
||||
item.add_argument("--hdf5", type=Path, required=True)
|
||||
item.add_argument("--manifest", type=Path, required=True)
|
||||
item.add_argument("--config", type=Path, required=True)
|
||||
item.add_argument("--splits", type=Path, required=True)
|
||||
item.add_argument("--data-review", type=Path, required=True)
|
||||
item.add_argument("--output", type=Path, required=True)
|
||||
if command == "train":
|
||||
item.add_argument("--device", choices=("cpu", "cuda"), default="cpu")
|
||||
item.add_argument(
|
||||
"--execute", action="store_true", help="Acknowledge configured updates/time/device budget"
|
||||
)
|
||||
evaluation = commands.add_parser("evaluate", help="CPU held-out prediction, not simulation rollout")
|
||||
evaluation.add_argument("--checkpoint", type=Path, required=True)
|
||||
evaluation.add_argument("--hdf5", type=Path, required=True)
|
||||
evaluation.add_argument("--manifest", type=Path, required=True)
|
||||
evaluation.add_argument("--split", choices=("validation", "test"), default="test")
|
||||
evaluation.add_argument("--output", type=Path, required=True)
|
||||
test = commands.add_parser("smoke", help="Four tiny CPU updates on labeled synthetic fixtures; no GPU/Kit")
|
||||
test.add_argument("--manifest", type=Path, required=True)
|
||||
test.add_argument("--output", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
try:
|
||||
if args.command == "doctor":
|
||||
require(sys.version_info >= (3, 12), "Python >=3.12 required")
|
||||
major_minor = tuple(int(v) for v in torch.__version__.split("+")[0].split(".")[:2])
|
||||
require(major_minor >= (2, 6), "Torch >=2.6 required for tensor-only checkpoint loading")
|
||||
if args.cuda:
|
||||
require(torch.cuda.is_available(), "CUDA unavailable; do not install/replace Isaac Torch automatically")
|
||||
report = {
|
||||
"status": "PASS",
|
||||
"scope": "dependency_imports_only",
|
||||
"python": platform.python_version(),
|
||||
"torch": str(torch.__version__),
|
||||
"numpy": np.__version__,
|
||||
"isaaclab_discoverable": importlib.util.find_spec("isaaclab") is not None,
|
||||
"cuda": "PASS" if args.cuda else "NOT_RUN",
|
||||
"simulation_e2e": "NOT_RUN",
|
||||
}
|
||||
elif args.command == "smoke":
|
||||
report = smoke(read_json(args.manifest), args.output)
|
||||
elif args.command == "evaluate":
|
||||
report = evaluate(args.checkpoint, args.hdf5, read_json(args.manifest), args.split)
|
||||
json_write(args.output, report)
|
||||
else:
|
||||
if args.command == "train":
|
||||
require(args.execute, "Training requires --execute; review configured compute budget first")
|
||||
config = Config.load(args.config)
|
||||
manifest, split, review = (read_json(p) for p in (args.manifest, args.splits, args.data_review))
|
||||
adapter, datasets, normalization, metadata = prepare(args.hdf5, manifest, split, config, review)
|
||||
metadata = provenance(metadata)
|
||||
if args.command == "preflight":
|
||||
report = {
|
||||
"status": "PASS",
|
||||
"scope": "data_and_training_configuration_only",
|
||||
"config": config.to_dict(),
|
||||
"metadata": metadata,
|
||||
"windows": {k: len(v) for k, v in datasets.items()},
|
||||
"simulation_e2e": "NOT_RUN",
|
||||
}
|
||||
json_write(args.output, report)
|
||||
else:
|
||||
report = train(adapter, datasets, normalization, metadata, config, args.output, args.device)
|
||||
print(json.dumps(report, allow_nan=False))
|
||||
except (ContractError, OSError, KeyError, TypeError, ValueError, RuntimeError, AssertionError) as error:
|
||||
parser.exit(1, f"FAIL: {error}\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,69 @@
|
||||
"""Versioned, bounded configuration for goal-conditioned reference ACT."""
|
||||
|
||||
import json
|
||||
from dataclasses import asdict, dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from dex_workbench_tracking.trajectory import require
|
||||
|
||||
CONTRACT = "l20_goal_reference_act_v1"
|
||||
CHECKPOINT = "l20_reference_act_checkpoint_v1"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Config:
|
||||
contract: str = CONTRACT
|
||||
hand_side: str = "right"
|
||||
control_hz: int = 30
|
||||
chunk_size: int = 16
|
||||
execute_steps: int = 4
|
||||
hidden_dim: int = 128
|
||||
heads: int = 4
|
||||
layers: int = 2
|
||||
latent_dim: int = 32
|
||||
batch_size: int = 32
|
||||
learning_rate: float = 0.0001
|
||||
kl_weight: float = 10.0
|
||||
max_updates: int = 1000
|
||||
evaluate_every: int = 100
|
||||
max_seconds: float = 600.0
|
||||
max_total_frames: int = 500000
|
||||
workspace_radius: float = 0.8
|
||||
seed: int = 42
|
||||
|
||||
def __post_init__(self):
|
||||
require(self.contract == CONTRACT, "Unsupported observation/action contract")
|
||||
require(self.hand_side in ("left", "right"), "Explicit left/right side required")
|
||||
for key in (
|
||||
"control_hz",
|
||||
"chunk_size",
|
||||
"execute_steps",
|
||||
"hidden_dim",
|
||||
"heads",
|
||||
"layers",
|
||||
"latent_dim",
|
||||
"batch_size",
|
||||
"max_updates",
|
||||
"evaluate_every",
|
||||
"max_total_frames",
|
||||
"seed",
|
||||
):
|
||||
value = getattr(self, key)
|
||||
require(type(value) is int and value > 0, f"{key} must be a positive integer")
|
||||
require(240 % self.control_hz == 0, "control_hz must divide the validated 240Hz physics rate")
|
||||
require(self.execute_steps <= self.chunk_size <= 256, "Require execute_steps <= chunk_size <= 256")
|
||||
require(self.hidden_dim % self.heads == 0 and self.hidden_dim <= 512, "Invalid transformer width/heads")
|
||||
require(self.layers <= 6 and self.latent_dim <= 256 and self.batch_size <= 512, "Model/batch resource cap")
|
||||
require(self.max_updates <= 500000 and self.max_total_frames <= 2000000, "Training/data resource cap")
|
||||
for key in ("learning_rate", "kl_weight", "max_seconds", "workspace_radius"):
|
||||
value = getattr(self, key)
|
||||
require(type(value) in (int, float) and np.isfinite(value) and value > 0, f"Invalid {key}")
|
||||
require(self.max_seconds <= 86400, "Maximum one-day process budget; choose bounded experiments")
|
||||
|
||||
def to_dict(self):
|
||||
return asdict(self)
|
||||
|
||||
@classmethod
|
||||
def load(cls, path):
|
||||
return cls(**json.loads(Path(path).read_text(encoding="utf-8")))
|
||||
@@ -0,0 +1,168 @@
|
||||
"""Episode/group-isolated reference windows; training-only normalization."""
|
||||
|
||||
import hashlib
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
from dex_workbench_tracking.control import Limits, validate_reference
|
||||
from dex_workbench_tracking.trajectory import load, require, sample
|
||||
from torch.utils.data import Dataset
|
||||
|
||||
from .adapter import Adapter
|
||||
|
||||
|
||||
def digest(path):
|
||||
value = hashlib.sha256()
|
||||
with Path(path).open("rb") as stream:
|
||||
for block in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
value.update(block)
|
||||
return value.hexdigest()
|
||||
|
||||
|
||||
def validate_splits(data, split):
|
||||
require(split.get("schema") == "l20_episode_splits_v1", "Unsupported split schema")
|
||||
require(
|
||||
set(split) == {"schema", "train", "validation", "test", "episode_groups"}, "Unexpected/missing split fields"
|
||||
)
|
||||
groups = split["episode_groups"]
|
||||
require(isinstance(groups, dict) and set(groups) == set(data.episodes), "Provide capture group for every episode")
|
||||
require(all(isinstance(v, str) and v.strip() for v in groups.values()), "Nonempty source capture groups required")
|
||||
require(split["train"] and split["validation"], "At least one independent train and validation episode required")
|
||||
seen, group_owner, geometry_owner = set(), {}, {}
|
||||
for partition in ("train", "validation", "test"):
|
||||
require(isinstance(split[partition], list), "Split entries must be episode lists")
|
||||
for name in split[partition]:
|
||||
require(
|
||||
isinstance(name, str) and name in data.episodes and name not in seen,
|
||||
"Unknown/overlapping episode split",
|
||||
)
|
||||
seen.add(name)
|
||||
group = groups[name]
|
||||
require(group_owner.get(group, partition) == partition, "Capture group leakage across splits")
|
||||
group_owner[group] = partition
|
||||
# Catch exact renamed/slowdown copies even if the caller supplies false group IDs.
|
||||
ep = data.episodes[name]
|
||||
fingerprint = hashlib.sha256()
|
||||
for field in (ep.wrist_position, ep.wrist_quaternion, ep.joint_position, ep.valid):
|
||||
fingerprint.update(np.ascontiguousarray(field).tobytes())
|
||||
fingerprint = fingerprint.hexdigest()
|
||||
require(
|
||||
geometry_owner.get(fingerprint, partition) == partition, "Duplicate trajectory geometry across splits"
|
||||
)
|
||||
geometry_owner[fingerprint] = partition
|
||||
require(seen == set(data.episodes), "Every episode must have an explicit split; no silently ignored data")
|
||||
|
||||
|
||||
def validate_review(review, data_hash, manifest):
|
||||
require(review.get("schema") == "l20_training_review_v1", "Training review sidecar required")
|
||||
require(review.get("hdf5_sha256") == data_hash, "Review must bind exact HDF5 bytes")
|
||||
require(review.get("asset_sha256") == manifest["asset_sha256"], "Review asset identity mismatch")
|
||||
for flag in ("coordinate_and_scale_reviewed", "reference_state_targets_accepted", "capture_groups_reviewed"):
|
||||
require(review.get(flag) is True, f"BLOCKED: explicit data-owner approval missing: {flag}")
|
||||
for key in ("reviewer", "evidence"):
|
||||
require(isinstance(review.get(key), str) and review[key].strip(), f"Review {key} required")
|
||||
|
||||
|
||||
class Windows(Dataset):
|
||||
def __init__(self, series, names, chunk_size, normalization=None):
|
||||
self.series, self.names, self.chunk_size = series, list(names), chunk_size
|
||||
self.normalization = normalization
|
||||
self.ends = np.cumsum([len(series[n][0]) - 1 for n in names])
|
||||
require(len(self.ends) > 0 and self.ends[-1] > 0, "Empty partition")
|
||||
|
||||
def __len__(self):
|
||||
return int(self.ends[-1])
|
||||
|
||||
def __getitem__(self, index):
|
||||
require(0 <= index < len(self), "Window index out of range")
|
||||
i = int(np.searchsorted(self.ends, index, side="right"))
|
||||
start = index - (int(self.ends[i - 1]) if i else 0)
|
||||
obs, actions = self.series[self.names[i]]
|
||||
stop = min(start + 1 + self.chunk_size, len(actions))
|
||||
length = stop - start - 1
|
||||
target = np.repeat(actions[stop - 1 : stop], self.chunk_size, axis=0)
|
||||
target[:length] = actions[start + 1 : stop]
|
||||
mask = np.arange(self.chunk_size) < length
|
||||
x = obs[start].copy()
|
||||
if self.normalization is not None:
|
||||
n = self.normalization
|
||||
x = (x - n["observation_mean"]) / n["observation_std"]
|
||||
target = (target - n["action_mean"]) / n["action_std"]
|
||||
return (
|
||||
torch.from_numpy(x.astype(np.float32)),
|
||||
torch.from_numpy(target.astype(np.float32)),
|
||||
torch.from_numpy(mask),
|
||||
)
|
||||
|
||||
|
||||
def fit_normalization(series, train_names):
|
||||
result = {}
|
||||
for label, column in (("observation", 0), ("action", 1)):
|
||||
# Fit each TRAIN frame once; not padded windows and never validation/test.
|
||||
values = np.concatenate([series[n][column][:-1] if column == 0 else series[n][column][1:] for n in train_names])
|
||||
require(np.isfinite(values).all(), "Nonfinite training features")
|
||||
result[label + "_mean"] = values.mean(axis=0, dtype=np.float64).astype(np.float32)
|
||||
result[label + "_std"] = np.maximum(values.std(axis=0, dtype=np.float64), 1e-4).astype(np.float32)
|
||||
return result
|
||||
|
||||
|
||||
def prepare(hdf5, manifest, split, config, review=None, *, synthetic_smoke=False):
|
||||
require(Path(hdf5).stat().st_size <= 2 * 1024**3, "Single HDF5 exceeds 2GiB preflight cap; shard explicitly")
|
||||
data = load(hdf5, manifest)
|
||||
require(data.metadata["hand_side"] == config.hand_side, "Config/data hand side mismatch")
|
||||
data_hash = digest(hdf5)
|
||||
if synthetic_smoke:
|
||||
require(data.metadata["provenance"] == "synthetic", "Smoke cannot relabel real expert data")
|
||||
else:
|
||||
require(data.metadata["provenance"] == "expert_retargeted", "Real training requires expert_retargeted data")
|
||||
validate_review(review or {}, data_hash, manifest)
|
||||
validate_splits(data, split)
|
||||
adapter = Adapter(manifest)
|
||||
limits = replace(Limits(), workspace_radius=config.workspace_radius)
|
||||
total, series, counts = 0, {}, {}
|
||||
for name, episode in data.episodes.items():
|
||||
validate_reference(episode, limits) # No implicit slowdown, clipping or invalid-gap bridging.
|
||||
steps = int(round(episode.time[-1] * config.control_hz))
|
||||
require(
|
||||
steps >= 1 and abs(steps / config.control_hz - episode.time[-1]) < 1e-8,
|
||||
f"{name}: duration must end on the configured control grid; no silent tail trimming",
|
||||
)
|
||||
total += steps + 1
|
||||
require(total <= config.max_total_frames, "Resampled dataset exceeds configured frame budget")
|
||||
query = np.arange(steps + 1, dtype=float) / config.control_hz
|
||||
query[-1] = episode.time[-1] # Only the checked floating-point grid roundoff, not time warping.
|
||||
ep = sample(episode, query)
|
||||
origin = ep.wrist_position[0], ep.wrist_quaternion[0]
|
||||
goal = adapter.action(ep.wrist_position[-1], ep.wrist_quaternion[-1], ep.joint_position[-1], origin)
|
||||
observations, actions = [], []
|
||||
for t, p, q, joints in zip(ep.time, ep.wrist_position, ep.wrist_quaternion, ep.joint_position, strict=True):
|
||||
observations.append(adapter.observation(p, q, joints, origin, goal, t, ep.time[-1]))
|
||||
actions.append(adapter.action(p, q, joints, origin))
|
||||
series[name] = np.asarray(observations), np.asarray(actions)
|
||||
counts[name] = {
|
||||
"source_frames": len(episode.time),
|
||||
"control_frames": len(ep.time),
|
||||
"duration_s": float(ep.time[-1]),
|
||||
}
|
||||
normalization = fit_normalization(series, split["train"])
|
||||
datasets = {
|
||||
key: Windows(series, split[key], config.chunk_size, normalization)
|
||||
for key in ("train", "validation", "test")
|
||||
if split[key]
|
||||
}
|
||||
metadata = {
|
||||
"hdf5_sha256": data_hash,
|
||||
"provenance": data.metadata["provenance"],
|
||||
"splits": split,
|
||||
"data_review": review if not synthetic_smoke else None,
|
||||
"synthetic_smoke": synthetic_smoke,
|
||||
"episodes": counts,
|
||||
"adapter": adapter.spec,
|
||||
"limits_uncalibrated": vars(limits),
|
||||
"target_semantics": "next_reference_root_pose_and_independent_q_target_proxy_not_measured_commands",
|
||||
"observation_semantics": "current_start_relative_pose_all_q_terminal_goal_phase_duration",
|
||||
"normalization_source": "train_frames_only",
|
||||
}
|
||||
return adapter, datasets, normalization, metadata
|
||||
@@ -0,0 +1,183 @@
|
||||
"""Bounded offline training, trusted tensor-only checkpoints and deterministic inference."""
|
||||
|
||||
import json
|
||||
import os
|
||||
import random
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
from dex_workbench_tracking.trajectory import require
|
||||
from torch.utils.data import DataLoader
|
||||
|
||||
from .adapter import Adapter
|
||||
from .config import CHECKPOINT, Config
|
||||
from .data import digest
|
||||
from .model import ReferenceACT, objective
|
||||
|
||||
|
||||
def json_write(path, value):
|
||||
with Path(path).open("x", encoding="utf-8") as stream:
|
||||
json.dump(value, stream, indent=2, allow_nan=False)
|
||||
|
||||
|
||||
def score(model, dataset, batch_size, device, deadline=None):
|
||||
model.eval()
|
||||
total, count = 0.0, 0
|
||||
with torch.inference_mode():
|
||||
for obs, target, valid in DataLoader(dataset, batch_size=batch_size, shuffle=False, num_workers=0):
|
||||
require(deadline is None or time.monotonic() < deadline, "Training/evaluation wall-time budget exhausted")
|
||||
prediction = model(obs.to(device))[0]
|
||||
error = (prediction - target.to(device)).abs() * valid.to(device).unsqueeze(-1)
|
||||
require(torch.isfinite(error).all().item(), "Nonfinite held-out prediction")
|
||||
total += error.sum().item()
|
||||
count += valid.sum().item() * target.shape[-1]
|
||||
return total / count
|
||||
|
||||
|
||||
def checkpoint_payload(model, config, normalization, metadata, update):
|
||||
return {
|
||||
"format": CHECKPOINT,
|
||||
"config": config.to_dict(),
|
||||
"metadata": metadata,
|
||||
"update": update,
|
||||
"normalization": {k: torch.tensor(v.copy()) for k, v in normalization.items()},
|
||||
"state_dict": {k: v.detach().cpu().clone() for k, v in model.state_dict().items()},
|
||||
"torch_version": str(torch.__version__),
|
||||
}
|
||||
|
||||
|
||||
def save_checkpoint(path, payload):
|
||||
"""Owned new run directory: atomic replace of this run's best/last checkpoint only."""
|
||||
path = Path(path)
|
||||
temporary = path.with_suffix(".pending")
|
||||
with temporary.open("xb") as stream:
|
||||
torch.save(payload, stream)
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
os.replace(temporary, path)
|
||||
|
||||
|
||||
def load_checkpoint(path, manifest):
|
||||
require(Path(path).stat().st_size <= 512 * 1024**2, "Checkpoint exceeds 512MiB local loading cap")
|
||||
# Caller-supplied local file only; no unsafe custom pickle globals.
|
||||
payload = torch.load(path, map_location="cpu", weights_only=True)
|
||||
require(payload.get("format") == CHECKPOINT, "Unsupported checkpoint")
|
||||
config = Config(**payload["config"])
|
||||
adapter = Adapter(manifest)
|
||||
require(payload["metadata"]["adapter"] == adapter.spec, "Checkpoint embodiment/asset/adapter mismatch")
|
||||
require(config.hand_side == adapter.side, "Checkpoint config side mismatch")
|
||||
normalization = {k: v.cpu().numpy() for k, v in payload["normalization"].items()}
|
||||
require(
|
||||
set(normalization) == {"observation_mean", "observation_std", "action_mean", "action_std"},
|
||||
"Normalization fields mismatch",
|
||||
)
|
||||
for key, value in normalization.items():
|
||||
dim = adapter.observation_dim if key.startswith("observation") else adapter.action_dim
|
||||
require(value.shape == (dim,) and np.isfinite(value).all(), "Invalid checkpoint normalization")
|
||||
if key.endswith("std"):
|
||||
require((value > 0).all(), "Normalization scales must be positive")
|
||||
model = ReferenceACT(adapter.observation_dim, adapter.action_dim, config)
|
||||
model.load_state_dict(payload["state_dict"], strict=True)
|
||||
require(all(torch.isfinite(p).all().item() for p in model.parameters()), "Nonfinite checkpoint parameters")
|
||||
model.eval()
|
||||
return model, config, adapter, normalization, payload["metadata"]
|
||||
|
||||
|
||||
def predict(model, normalization, observation):
|
||||
obs = np.asarray(observation, dtype=np.float32)
|
||||
require(
|
||||
obs.shape == normalization["observation_mean"].shape and np.isfinite(obs).all(), "Invalid inference observation"
|
||||
)
|
||||
obs = (obs - normalization["observation_mean"]) / normalization["observation_std"]
|
||||
with torch.inference_mode():
|
||||
result = model(torch.from_numpy(obs[None]))[0][0].cpu().numpy()
|
||||
result = result * normalization["action_std"] + normalization["action_mean"]
|
||||
require(np.isfinite(result).all(), "Nonfinite policy prediction")
|
||||
return result
|
||||
|
||||
|
||||
def train(adapter, datasets, normalization, metadata, config, output, device="cpu"):
|
||||
require(device in ("cpu", "cuda"), "Choose explicit cpu or cuda device")
|
||||
require(device != "cuda" or torch.cuda.is_available(), "CUDA unavailable; no silent CPU fallback")
|
||||
if metadata["synthetic_smoke"]:
|
||||
require(
|
||||
device == "cpu" and config.max_updates <= 10 and config.hidden_dim <= 64 and config.max_seconds <= 120,
|
||||
"Synthetic smoke is CPU-only and tightly bounded",
|
||||
)
|
||||
output = Path(output)
|
||||
output.mkdir(parents=True, exist_ok=False) # Never mix new data/config/checkpoints with an old run.
|
||||
json_write(output / "run.json", {"config": config.to_dict(), "metadata": metadata, "device": device})
|
||||
random.seed(config.seed)
|
||||
np.random.seed(config.seed)
|
||||
torch.manual_seed(config.seed)
|
||||
# Keep CPU smoke inexpensive; this only affects this dedicated CLI/test process.
|
||||
torch.set_num_threads(1)
|
||||
model = ReferenceACT(adapter.observation_dim, adapter.action_dim, config).to(device)
|
||||
optimizer = torch.optim.AdamW(model.parameters(), lr=config.learning_rate, weight_decay=1e-4)
|
||||
generator = torch.Generator().manual_seed(config.seed)
|
||||
started = time.monotonic()
|
||||
deadline = started + config.max_seconds
|
||||
best, completed = float("inf"), 0
|
||||
last_loss = None
|
||||
try:
|
||||
with (output / "metrics.jsonl").open("x", encoding="utf-8") as log:
|
||||
for update in range(1, config.max_updates + 1):
|
||||
require(time.monotonic() < deadline, "Training wall-time budget exhausted; incomplete run")
|
||||
indices = torch.randint(len(datasets["train"]), (config.batch_size,), generator=generator).tolist()
|
||||
batch = [datasets["train"][index] for index in indices]
|
||||
obs, target, valid = (torch.stack(values).to(device) for values in zip(*batch, strict=True))
|
||||
model.train()
|
||||
optimizer.zero_grad(set_to_none=True)
|
||||
prediction, mu, logvar = model(obs, target, valid)
|
||||
loss, l1, kl = objective(prediction, target, valid, mu, logvar, config.kl_weight)
|
||||
require(torch.isfinite(loss).item(), "Nonfinite training loss")
|
||||
loss.backward()
|
||||
torch.nn.utils.clip_grad_norm_(model.parameters(), 1.0, error_if_nonfinite=True)
|
||||
optimizer.step()
|
||||
completed = update
|
||||
last_loss = {"update": update, "loss": loss.item(), "l1": l1.item(), "kl": kl.item()}
|
||||
if update % config.evaluate_every == 0 or update == config.max_updates:
|
||||
validation = score(model, datasets["validation"], config.batch_size, device, deadline)
|
||||
last_loss["validation_l1_normalized_z0"] = validation
|
||||
if validation < best:
|
||||
best = validation
|
||||
save_checkpoint(
|
||||
output / "best.pt", checkpoint_payload(model, config, normalization, metadata, update)
|
||||
)
|
||||
log.write(json.dumps(last_loss, allow_nan=False) + "\n")
|
||||
log.flush()
|
||||
model.eval()
|
||||
payload = checkpoint_payload(model, config, normalization, metadata, completed)
|
||||
save_checkpoint(output / "last.pt", payload)
|
||||
reloaded = ReferenceACT(adapter.observation_dim, adapter.action_dim, config)
|
||||
reloaded.load_state_dict(
|
||||
torch.load(output / "last.pt", map_location="cpu", weights_only=True)["state_dict"], strict=True
|
||||
)
|
||||
reloaded.eval()
|
||||
probe = datasets["validation"][0][0].unsqueeze(0)
|
||||
with torch.inference_mode():
|
||||
expected = model.cpu()(probe)[0]
|
||||
actual = reloaded(probe)[0]
|
||||
torch.testing.assert_close(actual, expected, rtol=0, atol=1e-6)
|
||||
require(time.monotonic() <= deadline, "Training exceeded budget during finalization")
|
||||
result = {
|
||||
"status": "PASS",
|
||||
"scope": "bounded_offline_reference_act_training_not_policy_quality",
|
||||
"synthetic_smoke": metadata["synthetic_smoke"],
|
||||
"completed_updates": completed,
|
||||
"elapsed_seconds": time.monotonic() - started,
|
||||
"last_metrics": last_loss,
|
||||
"best_validation_l1_normalized": best,
|
||||
"best_checkpoint_sha256": digest(output / "best.pt"),
|
||||
"last_checkpoint_sha256": digest(output / "last.pt"),
|
||||
"checkpoint_reload_prediction_max_abs_diff": float((actual - expected).abs().max()),
|
||||
"simulation_e2e": "NOT_RUN",
|
||||
"hardware_and_training_quality_validated": False,
|
||||
}
|
||||
json_write(output / "result.json", result)
|
||||
return result
|
||||
except BaseException as error:
|
||||
json_write(output / "failure.json", {"status": "FAIL", "completed_updates": completed, "error": str(error)})
|
||||
raise
|
||||
@@ -0,0 +1,51 @@
|
||||
"""State-only ACT-style CVAE Transformer (no vision backbone or official-weight compatibility)."""
|
||||
|
||||
import torch
|
||||
from torch import nn
|
||||
|
||||
|
||||
class ReferenceACT(nn.Module):
|
||||
def __init__(self, observation_dim, action_dim, config):
|
||||
super().__init__()
|
||||
self.config = config
|
||||
width = config.hidden_dim
|
||||
self.observation = nn.Linear(observation_dim, width)
|
||||
self.action = nn.Linear(action_dim, width)
|
||||
self.cls = nn.Parameter(torch.zeros(1, 1, width))
|
||||
self.posterior_position = nn.Parameter(torch.randn(1, config.chunk_size + 2, width) * 0.02)
|
||||
encoder = nn.TransformerEncoderLayer(
|
||||
width, config.heads, width * 4, dropout=0, batch_first=True, norm_first=True
|
||||
)
|
||||
self.posterior = nn.TransformerEncoder(encoder, config.layers, enable_nested_tensor=False)
|
||||
self.distribution = nn.Linear(width, config.latent_dim * 2)
|
||||
self.latent = nn.Linear(config.latent_dim, width)
|
||||
self.queries = nn.Parameter(torch.randn(1, config.chunk_size, width) * 0.02)
|
||||
decoder = nn.TransformerDecoderLayer(
|
||||
width, config.heads, width * 4, dropout=0, batch_first=True, norm_first=True
|
||||
)
|
||||
self.decoder = nn.TransformerDecoder(decoder, config.layers)
|
||||
self.output = nn.Linear(width, action_dim)
|
||||
|
||||
def forward(self, observation, actions=None, valid=None):
|
||||
batch = len(observation)
|
||||
obs = self.observation(observation).unsqueeze(1)
|
||||
mu = logvar = None
|
||||
if actions is not None:
|
||||
tokens = torch.cat((self.cls.expand(batch, -1, -1), obs, self.action(actions)), dim=1)
|
||||
padding = torch.cat((torch.zeros((batch, 2), dtype=torch.bool, device=obs.device), ~valid), dim=1)
|
||||
encoded = self.posterior(tokens + self.posterior_position, src_key_padding_mask=padding)[:, 0]
|
||||
mu, logvar = self.distribution(encoded).chunk(2, dim=-1)
|
||||
logvar = logvar.clamp(-10, 10)
|
||||
z = mu + torch.exp(0.5 * logvar) * torch.randn_like(mu)
|
||||
else:
|
||||
# Deterministic ACT inference: posterior is never supplied reference future actions.
|
||||
z = observation.new_zeros((batch, self.config.latent_dim))
|
||||
memory = torch.cat((obs, self.latent(z).unsqueeze(1)), dim=1)
|
||||
prediction = self.output(self.decoder(self.queries.expand(batch, -1, -1), memory))
|
||||
return prediction, mu, logvar
|
||||
|
||||
|
||||
def objective(prediction, actions, valid, mu, logvar, kl_weight):
|
||||
reconstruction = ((prediction - actions).abs() * valid.unsqueeze(-1)).sum() / (valid.sum() * actions.shape[-1])
|
||||
kl = -0.5 * (1 + logvar - mu.square() - logvar.exp()).sum(dim=-1).mean()
|
||||
return reconstruction + kl_weight * kl, reconstruction, kl
|
||||
@@ -0,0 +1,77 @@
|
||||
"""Receding action-chunk policy for the existing bounded Isaac Lab replay runner."""
|
||||
|
||||
import torch
|
||||
from dex_workbench_tracking.trajectory import require
|
||||
|
||||
from .adapter import TargetLimiter
|
||||
from .data import digest
|
||||
from .engine import load_checkpoint, predict
|
||||
|
||||
|
||||
class ClosedLoopReferenceACT:
|
||||
def __init__(self, checkpoint, manifest, hdf5, episode_name, episode, limits):
|
||||
# This optional branch owns the dedicated evaluation process; avoid a large
|
||||
# CPU thread pool for tiny, latency-sensitive chunk inference.
|
||||
torch.set_num_threads(1)
|
||||
self.model, self.config, self.adapter, self.normalization, self.metadata = load_checkpoint(checkpoint, manifest)
|
||||
require(
|
||||
hdf5 is not None and digest(hdf5) == self.metadata["hdf5_sha256"],
|
||||
"Policy evaluation must bind exact trained dataset",
|
||||
)
|
||||
split = self.metadata["splits"]
|
||||
require(
|
||||
episode_name in split["validation"] + split["test"] and episode_name not in split["train"],
|
||||
"Policy evaluation requires a held-out episode, never train replay",
|
||||
)
|
||||
require(vars(limits) == self.metadata["limits_uncalibrated"], "Policy/runtime controller limits mismatch")
|
||||
self.episode, self.limits = episode, limits
|
||||
self.checkpoint_hash = digest(checkpoint)
|
||||
self.physics_per_control = 240 // self.config.control_hz
|
||||
self.duration = float(episode.time[-1])
|
||||
self.origin = episode.wrist_position[0], episode.wrist_quaternion[0]
|
||||
self.goal = self.adapter.action(
|
||||
episode.wrist_position[-1], episode.wrist_quaternion[-1], episode.joint_position[-1], self.origin
|
||||
)
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
e = self.episode
|
||||
self.limiter = TargetLimiter(
|
||||
self.adapter, self.limits, e.wrist_position[0], e.wrist_quaternion[0], e.joint_position[0]
|
||||
)
|
||||
self.chunk = None
|
||||
self.next_step = 0
|
||||
self.inference_calls = 0
|
||||
self.desired = None
|
||||
|
||||
def target(self, step, pose, joints):
|
||||
require(step == self.next_step, "Policy clock must advance one physics step at a time; reset explicitly")
|
||||
if step % self.physics_per_control == 0:
|
||||
control_step = step // self.physics_per_control
|
||||
slot = control_step % self.config.execute_steps
|
||||
if slot == 0:
|
||||
observation = self.adapter.observation(
|
||||
pose[:3], pose[3:], joints, self.origin, self.goal, step / 240, self.duration
|
||||
)
|
||||
self.chunk = predict(self.model, self.normalization, observation)
|
||||
self.inference_calls += 1
|
||||
self.desired = self.adapter.decode(self.chunk[slot], self.origin)
|
||||
result = self.limiter.step(self.desired, 1 / 240)
|
||||
self.next_step += 1
|
||||
return result
|
||||
|
||||
def summary(self):
|
||||
return {
|
||||
"checkpoint_sha256": self.checkpoint_hash,
|
||||
"contract": self.config.contract,
|
||||
"control_hz": self.config.control_hz,
|
||||
"chunk_size": self.config.chunk_size,
|
||||
"execute_steps": self.config.execute_steps,
|
||||
"inference_calls": self.inference_calls,
|
||||
"limited_target_steps": self.limiter.limited_steps,
|
||||
"physics_steps": self.limiter.steps,
|
||||
"synthetic_smoke_checkpoint": self.metadata["synthetic_smoke"],
|
||||
"observation_source": "measured_current_state_plus_terminal_goal_phase_duration",
|
||||
"future_reference_used_for_policy": "terminal_goal_only; no teacher forcing",
|
||||
"limiting_is_not_force_saturation_telemetry": True,
|
||||
}
|
||||
@@ -0,0 +1,216 @@
|
||||
"""CPU-only reference diagnostics; no dynamics, identity overrides or data rewriting.
|
||||
|
||||
Requires a schema/manifest-validated, fully valid episode. Finite differences are
|
||||
reference estimates, not measured velocities. Slowdown bounds cover reference
|
||||
speed gates only, never force/torque, tracking quality, workspace or hardware.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .cli import stretch_time
|
||||
from .control import Limits, rotation_error, validate_reference
|
||||
from .trajectory import ContractError, load, require, validate_against_manifest
|
||||
|
||||
|
||||
def stats(values):
|
||||
values = np.asarray(values, dtype=np.float64)
|
||||
require(values.size > 0 and np.isfinite(values).all(), "Finite nonempty diagnostic values required")
|
||||
return {
|
||||
"min": float(values.min()),
|
||||
"median": float(np.median(values)),
|
||||
"p95": float(np.percentile(values, 95)),
|
||||
"max": float(values.max()),
|
||||
}
|
||||
|
||||
|
||||
def speed_summary(values, time, limit):
|
||||
"""Unweighted interval statistics, with zero-based source frame locations."""
|
||||
index = int(np.argmax(values))
|
||||
return {
|
||||
**stats(values),
|
||||
"reference_limit": limit,
|
||||
"peak_interval_frames": [index, index + 1],
|
||||
"peak_interval_time_s": [float(time[index]), float(time[index + 1])],
|
||||
"over_limit_interval_starts": np.flatnonzero(values > limit).tolist(),
|
||||
"speed_only_slowdown_lower_bound": max(1.0, float(np.max(values)) / limit),
|
||||
}
|
||||
|
||||
|
||||
def diagnose(data, manifest, episode_name, limits=None, factors=(1, 60, 69, 70, 75, 80)):
|
||||
"""No implicit name reorder, invalid-gap bridging, clipping or asset rebinding."""
|
||||
limits = Limits() if limits is None else limits
|
||||
validate_against_manifest(data, manifest)
|
||||
episode = data.episodes[episode_name]
|
||||
require(episode.valid.all(), "Offline diagnostic needs one fully valid episode; segment explicitly")
|
||||
time = episode.time
|
||||
dt = np.diff(time)
|
||||
require(len(dt) > 0 and np.isfinite(dt).all() and (dt > 0).all(), "Increasing finite time required")
|
||||
# Promote float32 payload before subtraction to avoid extra cancellation rounding.
|
||||
position = episode.wrist_position.astype(np.float64)
|
||||
joints = episode.joint_position.astype(np.float64)
|
||||
linear = np.diff(position, axis=0) / dt[:, None]
|
||||
angular = (
|
||||
np.array(
|
||||
[
|
||||
rotation_error(b, a)
|
||||
for a, b in zip(episode.wrist_quaternion[:-1], episode.wrist_quaternion[1:], strict=True)
|
||||
]
|
||||
)
|
||||
/ dt[:, None]
|
||||
)
|
||||
joint_velocity = np.diff(joints, axis=0) / dt[:, None]
|
||||
speed = np.linalg.norm(linear, axis=1)
|
||||
omega = np.linalg.norm(angular, axis=1)
|
||||
qspeed = np.abs(joint_velocity)
|
||||
displacement = np.linalg.norm(position - position[0], axis=1)
|
||||
lower = np.array([j["lower_rad"] for j in manifest["joints"]])
|
||||
upper = np.array([j["upper_rad"] for j in manifest["joints"]])
|
||||
followers = {eq["joint"] for eq in manifest.get("source_urdf", {}).get("mimic", [])}
|
||||
per_joint = {}
|
||||
for column, name in enumerate(data.joint_names):
|
||||
per_joint[name] = {
|
||||
"role": "model_follower" if name in followers else "model_independent_target_not_hardware_mapping",
|
||||
"position_range_rad": [float(joints[:, column].min()), float(joints[:, column].max())],
|
||||
"minimum_limit_margin_rad": float(
|
||||
np.minimum(joints[:, column] - lower[column], upper[column] - joints[:, column]).min()
|
||||
),
|
||||
"speed_rad_s": speed_summary(qspeed[:, column], time, limits.reference_joint_speed),
|
||||
}
|
||||
mimic = {}
|
||||
for eq in manifest.get("source_urdf", {}).get("mimic", []):
|
||||
child, parent = (data.joint_names.index(eq[key]) for key in ("joint", "reference"))
|
||||
residual = joints[:, child] - eq["multiplier"] * joints[:, parent] - eq["offset_rad"]
|
||||
mimic[eq["joint"]] = {
|
||||
"max_abs_residual_rad": float(np.abs(residual).max()),
|
||||
"peak_frame": int(np.argmax(np.abs(residual))),
|
||||
"reference_tolerance_rad": 1e-3,
|
||||
"leader_range_rad": float(np.ptp(joints[:, parent])),
|
||||
}
|
||||
summaries = {
|
||||
"translation_m_s": speed_summary(speed, time, limits.reference_speed),
|
||||
"rotation_rad_s": speed_summary(omega, time, limits.reference_angular_speed),
|
||||
"all_joint_max_rad_s": speed_summary(qspeed.max(axis=1), time, limits.reference_joint_speed),
|
||||
}
|
||||
speed_bound = max(s["speed_only_slowdown_lower_bound"] for s in summaries.values())
|
||||
candidates = []
|
||||
for factor in factors:
|
||||
# Reuse the real float32 reference gate, rather than treating our double
|
||||
# precision estimates as an exact reproduction of boundary comparisons.
|
||||
stretched = stretch_time(data, factor).episodes[episode_name]
|
||||
try:
|
||||
validate_reference(stretched, limits)
|
||||
gate_status, reason = "PASS", None
|
||||
except ContractError as error:
|
||||
gate_status, reason = "FAIL", str(error)
|
||||
candidates.append(
|
||||
{
|
||||
"factor": float(factor),
|
||||
"duration_s": float(stretched.time[-1]),
|
||||
"estimated_peak_translation_m_s": float(speed.max() / factor),
|
||||
"estimated_peak_rotation_rad_s": float(omega.max() / factor),
|
||||
"estimated_peak_joint_rad_s": float(qspeed.max() / factor),
|
||||
"reference_gate": gate_status,
|
||||
"first_gate_failure": reason,
|
||||
"dynamics": "NOT_RUN",
|
||||
}
|
||||
)
|
||||
# Midpoint finite differences describe reference roughness only. The replay
|
||||
# interpolator is piecewise linear/SLERP: knot acceleration is not bounded by
|
||||
# these estimates, and no dynamics/force inference is justified from them.
|
||||
roughness = None
|
||||
if len(dt) > 1:
|
||||
midpoint_dt = (dt[:-1] + dt[1:]) / 2
|
||||
roughness = {
|
||||
"translation_m_s2": stats(np.linalg.norm(np.diff(linear, axis=0), axis=1) / midpoint_dt),
|
||||
"rotation_rad_s2": stats(np.linalg.norm(np.diff(angular, axis=0), axis=1) / midpoint_dt),
|
||||
"all_joint_max_rad_s2": stats((np.abs(np.diff(joint_velocity, axis=0)) / midpoint_dt[:, None]).max(axis=1)),
|
||||
}
|
||||
return {
|
||||
"report_version": "l20_reference_diagnostic_v1",
|
||||
"status": "PASS",
|
||||
"status_scope": "offline report generated; NOT dynamic or replay approval",
|
||||
"asset_compatibility": "PASS_MANIFEST_ONLY_NOT_USD_REINSPECTION",
|
||||
"episode": episode_name,
|
||||
"hand_side": data.metadata["hand_side"],
|
||||
"asset_sha256": data.metadata["asset_sha256"],
|
||||
"provenance": data.metadata["provenance"],
|
||||
"frames": len(time),
|
||||
"valid_frames": int(episode.valid.sum()),
|
||||
"duration_s": float(time[-1]),
|
||||
"sample_dt_s": stats(dt),
|
||||
"effective_sample_hz": float(1 / np.median(dt)),
|
||||
"quaternion_max_norm_error": float(
|
||||
np.abs(np.linalg.norm(episode.wrist_quaternion.astype(float), axis=1) - 1).max()
|
||||
),
|
||||
"limits_uncalibrated": vars(limits),
|
||||
"speed_estimates": summaries,
|
||||
"speed_only_slowdown_lower_bound": speed_bound,
|
||||
"workspace": {
|
||||
"max_displacement_from_start_m": float(displacement.max()),
|
||||
"peak_frame": int(displacement.argmax()),
|
||||
"radius_m": limits.workspace_radius,
|
||||
"outside_frames": np.flatnonzero(displacement > limits.workspace_radius).tolist(),
|
||||
"fixable_by_time_stretch": False,
|
||||
"path_length_m": float(np.linalg.norm(np.diff(position, axis=0), axis=1).sum()),
|
||||
},
|
||||
"joints": per_joint,
|
||||
"mimic": mimic,
|
||||
"midpoint_acceleration_estimates_not_physical_bounds": roughness,
|
||||
"intervals": [
|
||||
{
|
||||
"frames": [i, i + 1],
|
||||
"time_s": [float(time[i]), float(time[i + 1])],
|
||||
"translation_m_s": float(speed[i]),
|
||||
"rotation_rad_s": float(omega[i]),
|
||||
"max_joint_rad_s": float(qspeed[i].max()),
|
||||
"fastest_joint": data.joint_names[int(qspeed[i].argmax())],
|
||||
}
|
||||
for i in range(len(dt))
|
||||
],
|
||||
"candidates": candidates,
|
||||
"dynamic_replay": "NOT_RUN",
|
||||
"limitations": [
|
||||
"Derived reference differences, not measured hardware or simulator velocities.",
|
||||
"No position/rotation rescaling, recentering, smoothing or invalid-gap interpolation.",
|
||||
"Speed-only lower bound is necessary for source intervals, not a dynamics guarantee.",
|
||||
"Piecewise interpolation has velocity jumps; midpoint accelerations are not knot bounds.",
|
||||
"No force/torque saturation, collision, calibration or tracking stability measured.",
|
||||
"Candidate reference gate is the existing validate_reference, not full runtime acceptance.",
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("input", type=Path)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--episode", default="demo_000000")
|
||||
parser.add_argument("--output", type=Path, required=True, help="New JSON path, never overwrite")
|
||||
parser.add_argument("--workspace-radius", type=float, help="Comparison only; does not change runtime defaults")
|
||||
parser.add_argument("--factors", nargs="+", type=float, default=[1, 60, 69, 70, 75, 80])
|
||||
args = parser.parse_args()
|
||||
try:
|
||||
manifest = json.loads(args.manifest.read_text())
|
||||
data = load(args.input, manifest)
|
||||
limits = Limits()
|
||||
if args.workspace_radius is not None:
|
||||
limits = replace(limits, workspace_radius=args.workspace_radius)
|
||||
report = diagnose(data, manifest, args.episode, limits, args.factors)
|
||||
report["input_hdf5_sha256"] = hashlib.sha256(args.input.read_bytes()).hexdigest()
|
||||
report["manifest_file_sha256"] = hashlib.sha256(args.manifest.read_bytes()).hexdigest()
|
||||
encoded = json.dumps(report, indent=2, allow_nan=False) + "\n"
|
||||
with args.output.open("x", encoding="utf-8") as stream:
|
||||
stream.write(encoded)
|
||||
print(json.dumps({"status": "PASS", "scope": report["status_scope"], "output": str(args.output)}))
|
||||
except (ContractError, OSError, KeyError, TypeError, ValueError) as error:
|
||||
parser.exit(1, f"FAIL: {error}\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,65 @@
|
||||
"""Presentation-only estimated source camera; no physics or reference changes."""
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def load_camera(path, reference_time, time_factor):
|
||||
"""Validate source timestamps against the replay's explicitly stretched time."""
|
||||
if not np.isfinite(time_factor) or time_factor < 1:
|
||||
raise ValueError("Camera time factor must be finite and >= 1")
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
time = data["time"].copy()
|
||||
poses = data["final_world_from_camera"].copy()
|
||||
inverse = data["camera_from_final_world"].copy()
|
||||
k = data["K"].copy()
|
||||
if time.ndim != 1 or len(time) < 2 or poses.shape != (len(time), 4, 4):
|
||||
raise ValueError("Invalid camera array shapes")
|
||||
if not all(np.isfinite(a).all() for a in (time, poses, inverse, k)):
|
||||
raise ValueError("Nonfinite camera data")
|
||||
if not (np.diff(time) > 0).all() or time[0] != 0:
|
||||
raise ValueError("Invalid camera timestamps")
|
||||
np.testing.assert_allclose(time * time_factor, reference_time, atol=1e-9, rtol=0)
|
||||
np.testing.assert_allclose(poses @ inverse, np.broadcast_to(np.eye(4), poses.shape), atol=2e-6, rtol=0)
|
||||
rotation = poses[:, :3, :3]
|
||||
np.testing.assert_allclose(rotation.transpose(0, 2, 1) @ rotation,
|
||||
np.broadcast_to(np.eye(3), rotation.shape), atol=3e-6, rtol=0)
|
||||
np.testing.assert_allclose(np.linalg.det(rotation), 1, atol=3e-6, rtol=0)
|
||||
np.testing.assert_allclose(poses[:, 3], np.tile([0, 0, 0, 1], (len(time), 1)), atol=1e-9)
|
||||
# This delivery uses centered 1280x720 full-image intrinsics, no crop/distortion.
|
||||
if k.shape != (3, 3) or k[0, 0] <= 0 or k[1, 1] <= 0:
|
||||
raise ValueError("Invalid camera intrinsics")
|
||||
np.testing.assert_allclose(k, [[k[0, 0], 0, 640], [0, k[1, 1], 360], [0, 0, 1]], atol=1e-9)
|
||||
return time * time_factor, poses, k
|
||||
|
||||
|
||||
def usd_pose(cv_pose):
|
||||
"""OpenCV +Y down/+Z forward to USD +Y up/-Z forward, column-vector SE3."""
|
||||
return cv_pose @ np.diag([1.0, -1.0, -1.0, 1.0])
|
||||
|
||||
|
||||
class SourceCamera:
|
||||
def __init__(self, stage, path, reference_time, time_factor):
|
||||
from pxr import Gf, UsdGeom
|
||||
from omni.kit.viewport.utility import get_active_viewport
|
||||
|
||||
self.time, self.poses, k = load_camera(path, reference_time, time_factor)
|
||||
self.Gf = Gf
|
||||
camera = UsdGeom.Camera.Define(stage, "/World/SourceVideoCamera")
|
||||
camera.CreateProjectionAttr("perspective")
|
||||
camera.CreateFocalLengthAttr(20.0)
|
||||
camera.CreateHorizontalApertureAttr(20.0 * 1280 / k[0, 0])
|
||||
camera.CreateVerticalApertureAttr(20.0 * 720 / k[1, 1])
|
||||
camera.CreateClippingRangeAttr(Gf.Vec2f(0.01, 100.0))
|
||||
self.op = UsdGeom.Xformable(camera.GetPrim()).AddTransformOp()
|
||||
viewport = get_active_viewport()
|
||||
if viewport is None:
|
||||
raise RuntimeError("Source camera requires an active Kit viewport")
|
||||
viewport.set_texture_resolution((1280, 720))
|
||||
viewport.camera_path = str(camera.GetPath())
|
||||
self.update(0)
|
||||
|
||||
def update(self, replay_time):
|
||||
# Hold each exported source frame until the next timestamp (no guessed poses).
|
||||
index = int(np.clip(np.searchsorted(self.time, replay_time, side="right") - 1, 0, len(self.time) - 1))
|
||||
matrix = usd_pose(self.poses[index])
|
||||
self.op.Set(self.Gf.Matrix4d(*matrix.T.reshape(-1).tolist()))
|
||||
@@ -4,6 +4,47 @@ Changelog
|
||||
Unreleased
|
||||
~~~~~~~~~~
|
||||
|
||||
0.1.3 (2026-09-15)
|
||||
~~~~~~~~~~~~~~~~~~
|
||||
|
||||
Experimental progress snapshot; not a full simulation release-gate PASS.
|
||||
|
||||
Added
|
||||
^^^^^
|
||||
|
||||
* Add bounded state-only reference ACT (CVAE Transformer) training, train-only
|
||||
normalization, capture-group/episode splits, hash-bound data-review requirements,
|
||||
tensor-only checkpoint loading and offline held-out evaluation. Provide explicit
|
||||
configuration templates, an Isaac-Python wrapper and synthetic CPU smoke.
|
||||
* Add an opt-in checkpoint branch to the existing bounded tracking runner with
|
||||
measured-state feedback, terminal goal/phase conditioning, receding action chunks
|
||||
and manifest-bound coupled position/rate limits. Existing runtime assertions remain;
|
||||
the new policy GPU E2E and real-data training are NOT_RUN. No new Gym/PPO task or
|
||||
hardware mapping is claimed; existing tracking HDF5 schema and Cartpole are unchanged.
|
||||
* Add a CPU-only, identity-validated reference diagnostic with source-interval
|
||||
speeds, workspace/limit/mimic metrics and actual existing reference-gate checks
|
||||
for in-memory slowdown candidates. Preserve inputs and refuse output overwrites.
|
||||
Document the right expert trajectory's translation-dominated ~69.37x speed-only
|
||||
slowdown bound; this is not dynamic feasibility or original-speed acceptance.
|
||||
Runtime controls, assets and data schema remain unchanged.
|
||||
* Record one separately authorized right expert-reference replay at 75x slowdown:
|
||||
full 2x30000 steps at 240Hz, exit0/PASS in 364 seconds with unchanged controls
|
||||
and assertions; 98 CPU/USD regressions pass. Preserve the upstream world-frame
|
||||
and metric-scale verification blocker; no original-speed, GUI, task or hardware claim.
|
||||
|
||||
* Add opt-in estimated source-video camera replay with explicit OpenCV-to-USD
|
||||
axes, centered 1280x720 intrinsics, source-frame hold and validated time stretch.
|
||||
Camera data is external; no calibration or pixel-alignment claim. The first
|
||||
full GUI run reached GUI_READY but timed out at 600 seconds (exit 124); full
|
||||
camera replay acceptance is FAIL, not PASS.
|
||||
* Record the new 1333-frame right reference: verified source model binding and
|
||||
unchanged geometry, 3x slowdown / 0.4 m workspace, full default-camera GUI
|
||||
replay 2x31968 steps at 240 Hz, exit 0/PASS. Maximum wrist position error is
|
||||
3.582 mm; this does not validate original speed, object contact or grasp success.
|
||||
* Verify 120 CPU/USD regression tests without skips and a fresh four-update
|
||||
synthetic CPU ACT save/reload/held-out smoke. Full pre-commit is BLOCKED by
|
||||
missing tooling; policy GPU E2E and real-data training remain NOT_RUN.
|
||||
|
||||
0.1.2 (2026-09-14)
|
||||
~~~~~~~~~~~~~~~~~~
|
||||
|
||||
|
||||
@@ -24,7 +24,7 @@ INSTALL_REQUIRES = [
|
||||
# Installation operation
|
||||
setup(
|
||||
name="dex_workbench",
|
||||
packages=["dex_workbench", "dex_workbench_tracking"],
|
||||
packages=["dex_workbench", "dex_workbench_tracking", "dex_workbench_imitation"],
|
||||
author=EXTENSION_TOML_DATA["package"]["author"],
|
||||
maintainer=EXTENSION_TOML_DATA["package"]["maintainer"],
|
||||
url=EXTENSION_TOML_DATA["package"]["repository"],
|
||||
@@ -32,7 +32,10 @@ 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"]},
|
||||
extras_require={
|
||||
"tracking": ["numpy>=1.26", "h5py>=3.10"],
|
||||
"imitation": ["numpy>=1.26", "h5py>=3.10", "torch>=2.6"],
|
||||
},
|
||||
license="Apache-2.0",
|
||||
include_package_data=True,
|
||||
python_requires=">=3.12",
|
||||
|
||||
@@ -0,0 +1,427 @@
|
||||
"""Analytic CPU tests; no Kit, GPU, real demonstrations or policy-quality claims."""
|
||||
|
||||
import copy
|
||||
import importlib.util
|
||||
import json
|
||||
import subprocess
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
from dex_workbench_imitation.adapter import Adapter, TargetLimiter, matrixq, qmatrix, qmul, rotation6d
|
||||
from dex_workbench_imitation.cli import evaluate, smoke
|
||||
from dex_workbench_imitation.config import Config
|
||||
from dex_workbench_imitation.data import digest, fit_normalization, prepare, validate_review, validate_splits
|
||||
from dex_workbench_imitation.engine import load_checkpoint, predict, train
|
||||
from dex_workbench_imitation.model import ReferenceACT, objective
|
||||
from dex_workbench_imitation.policy import ClosedLoopReferenceACT
|
||||
from dex_workbench_tracking.cli import synthetic, write
|
||||
from dex_workbench_tracking.control import Limits, rotation_error
|
||||
from dex_workbench_tracking.trajectory import ContractError, Demonstrations, Episode, load
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
|
||||
|
||||
def manifest_fixture():
|
||||
return {
|
||||
"manifest_version": "l20_asset_manifest_v1",
|
||||
"hand_side": "left",
|
||||
"asset_sha256": "a" * 64,
|
||||
"root_link": "analytic_base",
|
||||
"joints": [
|
||||
{"name": "follower", "lower_rad": 0.0, "upper_rad": 0.8},
|
||||
{"name": "leader", "lower_rad": 0.0, "upper_rad": 1.0},
|
||||
],
|
||||
"source_urdf": {"mimic": [{"joint": "follower", "reference": "leader", "multiplier": 2.0, "offset_rad": 0.0}]},
|
||||
}
|
||||
|
||||
|
||||
def fixtures(manifest):
|
||||
data = synthetic(manifest)
|
||||
ep = data.episodes["demo_000000"]
|
||||
episodes = {}
|
||||
for i, factor in enumerate((1.0, 1.3, 1.6)):
|
||||
p = ep.wrist_position.copy()
|
||||
p[:, 0] *= factor
|
||||
episodes[f"demo_{i:06d}"] = Episode(
|
||||
ep.time.copy(), p, ep.wrist_quaternion.copy(), ep.joint_position * factor, ep.valid.copy()
|
||||
)
|
||||
data = Demonstrations(dict(data.metadata), data.joint_names, data.world_from_source.copy(), episodes)
|
||||
split = {
|
||||
"schema": "l20_episode_splits_v1",
|
||||
"train": ["demo_000000"],
|
||||
"validation": ["demo_000001"],
|
||||
"test": ["demo_000002"],
|
||||
"episode_groups": {n: f"analytic_{i}" for i, n in enumerate(episodes)},
|
||||
}
|
||||
return data, split
|
||||
|
||||
|
||||
class AdapterTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.adapter = Adapter(manifest_fixture())
|
||||
|
||||
def test_half_turn_and_sign_equivalent_geometry_round_trip(self):
|
||||
for q in ([1.0, 0, 0, 0], [0.0, 1, 0, 0], [0.0, 0, 1, 0], [0.0, 0, 0, 1], [0.5, 0.5, 0.5, 0.5]):
|
||||
with self.subTest(q=q):
|
||||
np.testing.assert_allclose(qmatrix(matrixq(qmatrix(q))), qmatrix(q), atol=1e-12)
|
||||
np.testing.assert_allclose(qmatrix(-np.array(q)), qmatrix(q), atol=1e-12)
|
||||
r = qmatrix(q)
|
||||
np.testing.assert_allclose(rotation6d(np.r_[r[:, 0], r[:, 1]]), r, atol=1e-12)
|
||||
|
||||
def test_start_relative_encoding_invariant_to_global_rigid_transform(self):
|
||||
origin = np.array([1.0, 2, 3]), np.array([1.0, 0, 0, 0])
|
||||
p, q, joints = np.array([1.1, 2.2, 3.3]), np.array([0.5, 0.5, 0.5, 0.5]), np.array([0.4, 0.2])
|
||||
a = self.adapter.action(p, q, joints, origin)
|
||||
global_q = np.array([0.5, -0.5, 0.5, 0.5])
|
||||
r, shift = qmatrix(global_q), np.array([4.0, 5, 6])
|
||||
transformed = self.adapter.action(
|
||||
r @ p + shift, qmul(global_q, q), joints, (r @ origin[0] + shift, qmul(global_q, origin[1]))
|
||||
)
|
||||
np.testing.assert_allclose(a, transformed, atol=1e-6)
|
||||
p2, q2, m = self.adapter.decode(a, origin)
|
||||
np.testing.assert_allclose(p2, p, atol=1e-6)
|
||||
np.testing.assert_allclose(qmatrix(q2), qmatrix(q), atol=1e-6)
|
||||
np.testing.assert_allclose(self.adapter.expand(m), joints, atol=1e-6)
|
||||
|
||||
def test_degenerate_action_is_rejected_not_silently_repaired(self):
|
||||
for values in ([0] * 6, [1, 0, 0, 2, 0, 0], [float("nan")] * 6):
|
||||
with self.assertRaises(ContractError):
|
||||
rotation6d(values)
|
||||
|
||||
def test_coupled_limit_and_all_state_joint_rate_envelope(self):
|
||||
self.assertEqual(self.adapter.master_names, ["leader"])
|
||||
np.testing.assert_array_equal(self.adapter.target_upper, [0.4])
|
||||
limiter = TargetLimiter(self.adapter, Limits(), [0, 0, 0], [1, 0, 0, 0], [0, 0])
|
||||
desired = np.array([10.0, 0, 0]), np.array([0.0, 0, 0, 1]), np.array([10.0])
|
||||
p0, q0, j0 = np.zeros(3), np.array([1.0, 0, 0, 0]), np.zeros(2)
|
||||
for _ in range(250):
|
||||
p, q, j = limiter.step(desired, 1 / 240)
|
||||
self.assertLessEqual(np.linalg.norm(p - p0), 0.05 / 240 + 1e-12)
|
||||
self.assertLessEqual(np.linalg.norm(rotation_error(q, q0)), 0.5 / 240 + 1e-12)
|
||||
self.assertLessEqual(np.max(np.abs(j - j0)), 0.5 / 240 + 1e-12)
|
||||
self.assertLessEqual(np.linalg.norm(p), 0.1 + 1e-12)
|
||||
self.assertAlmostEqual(j[0], 2 * j[1])
|
||||
p0, q0, j0 = p, q, j
|
||||
self.assertEqual(limiter.limited_steps, 250)
|
||||
|
||||
def test_negative_mimic_intersection_and_unsupported_cascade(self):
|
||||
manifest = manifest_fixture()
|
||||
manifest["source_urdf"]["mimic"][0].update(multiplier=-2.0, offset_rad=0.8)
|
||||
adapter = Adapter(manifest)
|
||||
np.testing.assert_allclose(adapter.target_lower, [0])
|
||||
np.testing.assert_allclose(adapter.target_upper, [0.4])
|
||||
manifest["source_urdf"]["mimic"][0]["reference"] = "follower"
|
||||
with self.assertRaises(ContractError):
|
||||
Adapter(manifest)
|
||||
|
||||
|
||||
class DatasetTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.directory = tempfile.TemporaryDirectory()
|
||||
self.addCleanup(self.directory.cleanup)
|
||||
self.path = Path(self.directory.name) / "data.hdf5"
|
||||
self.manifest = manifest_fixture()
|
||||
self.data, self.split = fixtures(self.manifest)
|
||||
self.config = replace(Config(), hand_side="left", chunk_size=4)
|
||||
write(self.path, self.data)
|
||||
|
||||
def prepared(self, **kwargs):
|
||||
return prepare(self.path, self.manifest, self.split, self.config, synthetic_smoke=True, **kwargs)
|
||||
|
||||
def test_future_windows_padding_and_split_isolation(self):
|
||||
_, datasets, norm, metadata = self.prepared()
|
||||
training = datasets["train"]
|
||||
self.assertEqual(training.names, ["demo_000000"])
|
||||
obs, actions, valid = training[len(training) - 1]
|
||||
self.assertEqual(valid.tolist(), [True, False, False, False])
|
||||
n = norm
|
||||
actual = actions[0].numpy() * n["action_std"] + n["action_mean"]
|
||||
np.testing.assert_allclose(actual, training.series["demo_000000"][1][-1], atol=1e-6)
|
||||
self.assertEqual(metadata["episodes"]["demo_000000"]["control_frames"], 61)
|
||||
self.assertTrue(torch.isfinite(obs).all())
|
||||
|
||||
def test_normalization_cannot_see_heldout_values(self):
|
||||
_, datasets, norm, _ = self.prepared()
|
||||
series = copy.deepcopy(datasets["train"].series)
|
||||
for name in self.split["validation"] + self.split["test"]:
|
||||
series[name] = (series[name][0] * 10000, series[name][1] * 10000)
|
||||
new = fit_normalization(series, self.split["train"])
|
||||
for k in norm:
|
||||
np.testing.assert_array_equal(norm[k], new[k])
|
||||
|
||||
def test_cross_partition_episode_group_and_duplicate_geometry_rejected(self):
|
||||
for kind in ("episode", "group", "geometry", "unassigned"):
|
||||
split, data = copy.deepcopy(self.split), copy.deepcopy(self.data)
|
||||
if kind == "episode":
|
||||
split["test"] = split["train"]
|
||||
elif kind == "group":
|
||||
split["episode_groups"]["demo_000001"] = "analytic_0"
|
||||
elif kind == "geometry":
|
||||
data.episodes["demo_000001"] = copy.deepcopy(data.episodes["demo_000000"])
|
||||
else:
|
||||
split["test"] = []
|
||||
with self.subTest(kind=kind), self.assertRaises(ContractError):
|
||||
validate_splits(data, split)
|
||||
|
||||
def test_review_requires_hash_owner_and_all_flags(self):
|
||||
review = {
|
||||
"schema": "l20_training_review_v1",
|
||||
"hdf5_sha256": digest(self.path),
|
||||
"asset_sha256": "a" * 64,
|
||||
"coordinate_and_scale_reviewed": True,
|
||||
"reference_state_targets_accepted": True,
|
||||
"capture_groups_reviewed": True,
|
||||
"reviewer": "analytic test fixture only",
|
||||
"evidence": "test",
|
||||
}
|
||||
validate_review(review, digest(self.path), self.manifest)
|
||||
for key in (
|
||||
"hdf5_sha256",
|
||||
"coordinate_and_scale_reviewed",
|
||||
"reference_state_targets_accepted",
|
||||
"capture_groups_reviewed",
|
||||
"reviewer",
|
||||
):
|
||||
bad = dict(review)
|
||||
bad[key] = False
|
||||
with self.subTest(key=key), self.assertRaises(ContractError):
|
||||
validate_review(bad, digest(self.path), self.manifest)
|
||||
|
||||
def test_real_training_cannot_accept_synthetic_or_unreviewed_expert_flags(self):
|
||||
with self.assertRaisesRegex(ContractError, "expert_retargeted"):
|
||||
prepare(self.path, self.manifest, self.split, self.config, {})
|
||||
# Deliberately forged expert flag in an analytic contract test, never real training data.
|
||||
self.data.metadata["provenance"] = "expert_retargeted"
|
||||
expert = self.path.with_name("expert-flag-contract-fixture.hdf5")
|
||||
write(expert, self.data)
|
||||
with self.assertRaisesRegex(ContractError, "review"):
|
||||
prepare(expert, self.manifest, self.split, self.config, {})
|
||||
with self.assertRaisesRegex(ContractError, "relabel"):
|
||||
prepare(expert, self.manifest, self.split, self.config, synthetic_smoke=True)
|
||||
|
||||
def test_speed_invalid_gap_off_grid_and_frame_budget_fail_closed(self):
|
||||
for kind in ("speed", "gap", "offgrid", "budget"):
|
||||
data = copy.deepcopy(self.data)
|
||||
config = self.config
|
||||
e = data.episodes["demo_000000"]
|
||||
if kind == "speed":
|
||||
e.wrist_position[1, 0] = 0.09
|
||||
elif kind == "gap":
|
||||
e.valid[1] = False
|
||||
elif kind == "offgrid":
|
||||
e.time[-1] += 0.001
|
||||
else:
|
||||
config = replace(config, max_total_frames=10)
|
||||
path = self.path.with_name(kind + ".hdf5")
|
||||
write(path, data)
|
||||
with self.subTest(kind=kind), self.assertRaises(ContractError):
|
||||
prepare(path, self.manifest, self.split, config, synthetic_smoke=True)
|
||||
|
||||
def test_prepare_does_not_modify_file_or_manifest(self):
|
||||
before, manifest = self.path.read_bytes(), copy.deepcopy(self.manifest)
|
||||
self.prepared()
|
||||
self.assertEqual(before, self.path.read_bytes())
|
||||
self.assertEqual(manifest, self.manifest)
|
||||
|
||||
def test_config_rejects_invalid_dimensions_and_resource_values(self):
|
||||
for fields in (
|
||||
{"control_hz": 29},
|
||||
{"hidden_dim": 31},
|
||||
{"execute_steps": 17},
|
||||
{"max_updates": True},
|
||||
{"max_seconds": float("nan")},
|
||||
{"max_total_frames": 2000001},
|
||||
):
|
||||
with self.subTest(fields=fields), self.assertRaises(ContractError):
|
||||
Config(**fields)
|
||||
|
||||
|
||||
class ModelTests(unittest.TestCase):
|
||||
def test_real_cvae_gradient_padding_and_deterministic_zero_latent_inference(self):
|
||||
torch.manual_seed(42)
|
||||
torch.set_num_threads(1)
|
||||
config = replace(Config(), hidden_dim=32, layers=1, latent_dim=4, chunk_size=4, execute_steps=2)
|
||||
model = ReferenceACT(6, 3, config)
|
||||
obs, target = torch.randn(2, 6), torch.randn(2, 4, 3)
|
||||
valid = torch.tensor([[True, True, False, False], [True, False, False, False]])
|
||||
pred, mu, logvar = model(obs, target, valid)
|
||||
changed = target.clone()
|
||||
changed[~valid] = 10000
|
||||
_, mu2, lv2 = model(obs, changed, valid)
|
||||
torch.testing.assert_close(mu, mu2, rtol=0, atol=1e-6)
|
||||
torch.testing.assert_close(logvar, lv2, rtol=0, atol=1e-6)
|
||||
loss, _, _ = objective(pred, target, valid, mu, logvar, config.kl_weight)
|
||||
loss.backward()
|
||||
self.assertTrue(all(p.grad is None or torch.isfinite(p.grad).all() for p in model.parameters()))
|
||||
model.eval()
|
||||
with torch.inference_mode():
|
||||
a = model(obs)[0]
|
||||
b = model(obs)[0]
|
||||
torch.testing.assert_close(a, b, rtol=0, atol=0)
|
||||
|
||||
|
||||
class TrainingAndPolicyTests(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
cls.directory = tempfile.TemporaryDirectory()
|
||||
cls.path = Path(cls.directory.name) / "smoke"
|
||||
cls.manifest = manifest_fixture()
|
||||
cls.result = smoke(cls.manifest, cls.path)
|
||||
cls.checkpoint = cls.path / "train/last.pt"
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
cls.directory.cleanup()
|
||||
|
||||
def test_four_update_save_reload_and_heldout_evaluation(self):
|
||||
self.assertEqual(self.result["training"]["completed_updates"], 4)
|
||||
self.assertEqual(self.result["training"]["checkpoint_reload_prediction_max_abs_diff"], 0)
|
||||
self.assertEqual(self.result["evaluation"]["split"], "test")
|
||||
self.assertEqual(self.result["simulation_e2e"], "NOT_RUN")
|
||||
self.assertTrue(self.checkpoint.stat().st_size > 0)
|
||||
model, config, adapter, norm, _ = load_checkpoint(self.checkpoint, self.manifest)
|
||||
values = predict(model, norm, norm["observation_mean"])
|
||||
self.assertEqual(values.shape, (config.chunk_size, adapter.action_dim))
|
||||
with self.assertRaises(ContractError):
|
||||
evaluate(self.checkpoint, self.path / "synthetic.hdf5", self.manifest, "train")
|
||||
|
||||
def test_checkpoint_side_identity_and_bad_normalization_rejected(self):
|
||||
manifest = copy.deepcopy(self.manifest)
|
||||
manifest["asset_sha256"] = "b" * 64
|
||||
with self.assertRaisesRegex(ContractError, "mismatch"):
|
||||
load_checkpoint(self.checkpoint, manifest)
|
||||
payload = torch.load(self.checkpoint, weights_only=True)
|
||||
payload["normalization"]["action_std"][0] = 0
|
||||
path = self.path / "invalid-norm.pt"
|
||||
torch.save(payload, path)
|
||||
with self.assertRaisesRegex(ContractError, "positive"):
|
||||
load_checkpoint(path, self.manifest)
|
||||
|
||||
def test_policy_reset_clock_rate_limits_and_no_future_teacher_forcing(self):
|
||||
hdf5 = self.path / "synthetic.hdf5"
|
||||
ep = load(hdf5, self.manifest).episodes["demo_000001"]
|
||||
limits = replace(Limits(), workspace_radius=0.8)
|
||||
policy = ClosedLoopReferenceACT(self.checkpoint, self.manifest, hdf5, "demo_000001", ep, limits)
|
||||
pose = np.r_[ep.wrist_position[0], ep.wrist_quaternion[0]]
|
||||
traces = []
|
||||
for repetition in range(2):
|
||||
policy.reset()
|
||||
trace = []
|
||||
for step in range(128):
|
||||
# Analytic observations only: not a simulated physics trace.
|
||||
p, q, j = policy.target(step, pose, ep.joint_position[0])
|
||||
self.assertAlmostEqual(j[0], 2 * j[1])
|
||||
self.assertTrue((j >= policy.adapter.lower - 1e-8).all())
|
||||
self.assertTrue((j <= policy.adapter.upper + 1e-8).all())
|
||||
trace.append(np.r_[p, q, j])
|
||||
traces.append(trace)
|
||||
np.testing.assert_array_equal(traces[0], traces[1])
|
||||
self.assertEqual(policy.summary()["inference_calls"], 8)
|
||||
self.assertEqual(policy.summary()["physics_steps"], 128)
|
||||
with self.assertRaisesRegex(ContractError, "clock"):
|
||||
policy.target(130, pose, ep.joint_position[0])
|
||||
with self.assertRaisesRegex(ContractError, "held-out"):
|
||||
ClosedLoopReferenceACT(self.checkpoint, self.manifest, hdf5, "demo_000000", ep, limits)
|
||||
with self.assertRaisesRegex(ContractError, "limits"):
|
||||
ClosedLoopReferenceACT(self.checkpoint, self.manifest, hdf5, "demo_000001", ep, Limits())
|
||||
|
||||
def test_run_directory_is_never_overwritten_and_budget_failure_is_not_pass(self):
|
||||
with self.assertRaises(FileExistsError):
|
||||
smoke(self.manifest, self.path)
|
||||
config = replace(
|
||||
Config(),
|
||||
hand_side="left",
|
||||
hidden_dim=32,
|
||||
layers=1,
|
||||
latent_dim=4,
|
||||
chunk_size=4,
|
||||
execute_steps=2,
|
||||
batch_size=2,
|
||||
max_updates=1,
|
||||
max_seconds=1e-12,
|
||||
)
|
||||
split = json.loads((self.path / "splits.json").read_text())
|
||||
adapter, datasets, norm, metadata = prepare(
|
||||
self.path / "synthetic.hdf5", self.manifest, split, config, synthetic_smoke=True
|
||||
)
|
||||
output = self.path / "deadline-failure"
|
||||
with self.assertRaisesRegex(ContractError, "budget"):
|
||||
train(adapter, datasets, norm, metadata, config, output)
|
||||
self.assertFalse((output / "result.json").exists())
|
||||
self.assertEqual(json.loads((output / "failure.json").read_text())["status"], "FAIL")
|
||||
|
||||
def test_cli_heldout_eval_and_missing_inputs_fail_without_kit(self):
|
||||
manifest = self.path / "manifest.json"
|
||||
manifest.write_text(json.dumps(self.manifest))
|
||||
output = self.path / "cli-evaluation.json"
|
||||
base = [sys.executable, "-m", "dex_workbench_imitation.cli"]
|
||||
result = subprocess.run(
|
||||
base
|
||||
+ [
|
||||
"evaluate",
|
||||
"--checkpoint",
|
||||
str(self.checkpoint),
|
||||
"--hdf5",
|
||||
str(self.path / "synthetic.hdf5"),
|
||||
"--manifest",
|
||||
str(manifest),
|
||||
"--output",
|
||||
str(output),
|
||||
],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
self.assertEqual(result.returncode, 0, result.stderr)
|
||||
self.assertEqual(json.loads(output.read_text())["simulation_e2e"], "NOT_RUN")
|
||||
result = subprocess.run(
|
||||
base
|
||||
+ [
|
||||
"preflight",
|
||||
"--hdf5",
|
||||
"missing.hdf5",
|
||||
"--manifest",
|
||||
str(manifest),
|
||||
"--config",
|
||||
str(ROOT / "configs/imitation/l20_right_act.json"),
|
||||
"--splits",
|
||||
str(self.path / "splits.json"),
|
||||
"--data-review",
|
||||
str(ROOT / "configs/imitation/data_review.example.json"),
|
||||
"--output",
|
||||
str(self.path / "missing.json"),
|
||||
],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
self.assertNotEqual(result.returncode, 0)
|
||||
self.assertFalse((self.path / "missing.json").exists())
|
||||
|
||||
|
||||
class EntryTests(unittest.TestCase):
|
||||
def test_optional_policy_argument_preserves_default_and_requires_full_hdf5_before_kit(self):
|
||||
script = ROOT / "scripts/tracking/track_l20.py"
|
||||
spec = importlib.util.spec_from_file_location("act_entry_test", script)
|
||||
entry = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(entry)
|
||||
parser = entry.build_parser(lambda parser: None)
|
||||
args = parser.parse_args(["asset.usda", "--manifest", "manifest.json"])
|
||||
self.assertIsNone(args.policy_checkpoint)
|
||||
args = parser.parse_args(["asset.usda", "--manifest", "manifest.json", "--policy-checkpoint", "checkpoint.pt"])
|
||||
self.assertEqual(args.policy_checkpoint, Path("checkpoint.pt"))
|
||||
# AST/source invariant: original runtime assertion thresholds have not been disabled.
|
||||
source = script.read_text()
|
||||
for assertion in (
|
||||
"residual < 0.002",
|
||||
"position_error < 0.05 and angle_error < 0.5 and joint_error < 0.2",
|
||||
"np.testing.assert_allclose(reset_states[0], reset_states[1], atol=1e-6, rtol=0)",
|
||||
):
|
||||
self.assertIn(assertion, source)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,40 @@
|
||||
"""CPU regression for camera axes and explicit slow-replay synchronization."""
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from dex_workbench_tracking.source_camera import load_camera, usd_pose
|
||||
|
||||
|
||||
class SourceCameraTests(unittest.TestCase):
|
||||
def test_axes_and_projection(self):
|
||||
cv = np.eye(4)
|
||||
cv[:3, 3] = [1, 2, 3]
|
||||
usd = usd_pose(cv)
|
||||
np.testing.assert_array_equal(usd[:3, 3], cv[:3, 3])
|
||||
point = cv @ np.array([.1, .2, 1, 1])
|
||||
np.testing.assert_allclose(np.linalg.inv(usd) @ point, [.1, -.2, -1, 1])
|
||||
|
||||
def test_time_binding_and_invalid_rotation(self):
|
||||
with tempfile.TemporaryDirectory() as folder:
|
||||
path = Path(folder) / 'camera.npz'
|
||||
poses = np.tile(np.eye(4), (2, 1, 1))
|
||||
k = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]])
|
||||
np.savez(path, time=[0, 1], final_world_from_camera=poses,
|
||||
camera_from_final_world=poses, K=k)
|
||||
time, _, _ = load_camera(path, np.array([0, 3]), 3)
|
||||
np.testing.assert_array_equal(time, [0, 3])
|
||||
with self.assertRaises(AssertionError):
|
||||
load_camera(path, np.array([0, 3]), 1)
|
||||
with self.assertRaises(ValueError):
|
||||
load_camera(path, np.array([0, 3]), float('nan'))
|
||||
poses[:, 0, 0] = -1
|
||||
np.savez(path, time=[0, 1], final_world_from_camera=poses,
|
||||
camera_from_final_world=poses, K=k)
|
||||
with self.assertRaises(AssertionError):
|
||||
load_camera(path, np.array([0, 3]), 3)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,212 @@
|
||||
"""CPU analytic tests: offline reference metrics never authorize physical replay."""
|
||||
|
||||
import copy
|
||||
import json
|
||||
import subprocess
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from dex_workbench_tracking.cli import stretch_time, write
|
||||
from dex_workbench_tracking.control import Limits
|
||||
from dex_workbench_tracking.diagnostic import diagnose
|
||||
from dex_workbench_tracking.identity import RIGHT_URDF_SHA
|
||||
from dex_workbench_tracking.trajectory import ContractError, Demonstrations, Episode, load
|
||||
|
||||
|
||||
class DiagnosticTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.manifest = {
|
||||
"manifest_version": "l20_asset_manifest_v1",
|
||||
"hand_side": "right",
|
||||
"asset_sha256": "a" * 64,
|
||||
"root_link": "hand_base_link",
|
||||
"joints": [
|
||||
{"name": "follower", "lower_rad": 0, "upper_rad": 2},
|
||||
{"name": "leader", "lower_rad": 0, "upper_rad": 1},
|
||||
],
|
||||
"source_urdf": {
|
||||
# Identity token required by right-side schema; geometry below is synthetic.
|
||||
"sha256": RIGHT_URDF_SHA,
|
||||
"mimic": [
|
||||
{"joint": "follower", "reference": "leader", "multiplier": 2, "offset_rad": 0},
|
||||
],
|
||||
},
|
||||
}
|
||||
time = np.array([0, 0.25, 1.0], dtype=np.float64)
|
||||
position = np.zeros((3, 3), dtype=np.float32)
|
||||
position[:, 0] = 0.2 * time
|
||||
quaternion = np.zeros((3, 4), dtype=np.float32)
|
||||
quaternion[:, 0], quaternion[:, 3] = np.cos(time / 2), np.sin(time / 2)
|
||||
joints = np.array([time * 0.8, time * 0.4], dtype=np.float32).T
|
||||
self.data = Demonstrations(
|
||||
{
|
||||
"schema_version": "l20_tracking_v1",
|
||||
"embodiment": "L20",
|
||||
"hand_side": "right",
|
||||
"asset_sha256": "a" * 64,
|
||||
"root_link": "hand_base_link",
|
||||
"provenance": "synthetic",
|
||||
"metric_scale_provenance": "Analytic test, not measured",
|
||||
"scale_to_meters": 1.0,
|
||||
"source_description": "Analytic nonuniform samples",
|
||||
},
|
||||
("follower", "leader"),
|
||||
np.eye(4),
|
||||
{"demo_000000": Episode(time, position, quaternion, joints, np.ones(3, dtype=bool))},
|
||||
)
|
||||
|
||||
def report(self, **kwargs):
|
||||
return diagnose(self.data, self.manifest, "demo_000000", **kwargs)
|
||||
|
||||
def test_nonuniform_si_derivatives_and_passive_joint_roles(self):
|
||||
report = self.report()
|
||||
self.assertAlmostEqual(report["speed_estimates"]["translation_m_s"]["max"], 0.2, places=6)
|
||||
self.assertAlmostEqual(report["speed_estimates"]["rotation_rad_s"]["max"], 1.0, places=6)
|
||||
self.assertAlmostEqual(report["speed_estimates"]["all_joint_max_rad_s"]["max"], 0.8, places=6)
|
||||
self.assertAlmostEqual(report["speed_only_slowdown_lower_bound"], 4.0, places=6)
|
||||
self.assertEqual(report["joints"]["follower"]["role"], "model_follower")
|
||||
self.assertEqual(report["mimic"]["follower"]["max_abs_residual_rad"], 0)
|
||||
self.assertEqual(report["sample_dt_s"]["min"], 0.25)
|
||||
|
||||
def test_workspace_cannot_be_fixed_by_slowdown(self):
|
||||
report = self.report(factors=(1, 80))
|
||||
self.assertEqual(report["workspace"]["outside_frames"], [2])
|
||||
self.assertFalse(report["workspace"]["fixable_by_time_stretch"])
|
||||
self.assertEqual(report["candidates"][1]["reference_gate"], "FAIL")
|
||||
self.assertIn("workspace", report["candidates"][1]["first_gate_failure"])
|
||||
extended = self.report(limits=replace(Limits(), workspace_radius=0.8), factors=(80,))
|
||||
self.assertEqual(extended["candidates"][0]["reference_gate"], "PASS")
|
||||
self.assertEqual(extended["dynamic_replay"], "NOT_RUN")
|
||||
self.assertEqual(extended["candidates"][0]["dynamics"], "NOT_RUN")
|
||||
|
||||
def test_slowdown_scaling_and_no_input_mutation(self):
|
||||
before = copy.deepcopy(self.data)
|
||||
report = self.report()
|
||||
slow = diagnose(stretch_time(self.data, 10), self.manifest, "demo_000000")
|
||||
for metric in report["speed_estimates"]:
|
||||
self.assertAlmostEqual(
|
||||
slow["speed_estimates"][metric]["max"] * 10, report["speed_estimates"][metric]["max"]
|
||||
)
|
||||
self.assertEqual(self.data.metadata, before.metadata)
|
||||
for field in vars(self.data.episodes["demo_000000"]):
|
||||
np.testing.assert_array_equal(
|
||||
getattr(self.data.episodes["demo_000000"], field), getattr(before.episodes["demo_000000"], field)
|
||||
)
|
||||
|
||||
def test_peak_interval_locations_and_limit_margin(self):
|
||||
episode = self.data.episodes["demo_000000"]
|
||||
episode.wrist_position[1, 0] = 0.15
|
||||
report = self.report()
|
||||
metric = report["speed_estimates"]["translation_m_s"]
|
||||
self.assertEqual(metric["peak_interval_frames"], [0, 1])
|
||||
self.assertEqual(metric["peak_interval_time_s"], [0, 0.25])
|
||||
self.assertEqual(metric["over_limit_interval_starts"], [0, 1])
|
||||
self.assertEqual(report["joints"]["leader"]["minimum_limit_margin_rad"], 0)
|
||||
|
||||
def test_identity_side_order_limits_mimic_fail_closed(self):
|
||||
for mutation in ("hash", "side", "order", "limits", "mimic"):
|
||||
manifest = copy.deepcopy(self.manifest)
|
||||
if mutation == "hash":
|
||||
manifest["asset_sha256"] = "b" * 64
|
||||
elif mutation == "side":
|
||||
manifest["hand_side"] = "left"
|
||||
manifest["source_urdf"].pop("sha256")
|
||||
elif mutation == "order":
|
||||
manifest["joints"].reverse()
|
||||
elif mutation == "limits":
|
||||
manifest["joints"][1]["upper_rad"] = 0.1
|
||||
else:
|
||||
manifest["source_urdf"]["mimic"][0]["multiplier"] = 1.5
|
||||
with self.subTest(mutation=mutation), self.assertRaises(ContractError):
|
||||
diagnose(self.data, manifest, "demo_000000")
|
||||
|
||||
def test_invalid_gap_and_invalid_factors_rejected(self):
|
||||
for factor in (0, 0.5, float("inf"), float("nan")):
|
||||
with self.subTest(factor=factor), self.assertRaises(ContractError):
|
||||
self.report(factors=(factor,))
|
||||
self.data.episodes["demo_000000"].valid[1] = False
|
||||
with self.assertRaisesRegex(ContractError, "fully valid"):
|
||||
self.report()
|
||||
|
||||
def test_two_frame_and_stationary_episode(self):
|
||||
episode = self.data.episodes["demo_000000"]
|
||||
episode = Episode(*(getattr(episode, field)[:2].copy() for field in vars(episode)))
|
||||
episode.wrist_position[:] = 0
|
||||
episode.wrist_quaternion[:] = [1, 0, 0, 0]
|
||||
episode.joint_position[:] = 0
|
||||
self.data.episodes["demo_000000"] = episode
|
||||
report = self.report()
|
||||
self.assertEqual(report["speed_only_slowdown_lower_bound"], 1)
|
||||
self.assertIsNone(report["midpoint_acceleration_estimates_not_physical_bounds"])
|
||||
# Stationary data can satisfy the reference gate, not the runtime motion checks.
|
||||
self.assertEqual(report["candidates"][0]["reference_gate"], "PASS")
|
||||
self.assertEqual(report["dynamic_replay"], "NOT_RUN")
|
||||
|
||||
def test_cli_no_overwrite_and_rejects_hash_without_output(self):
|
||||
with tempfile.TemporaryDirectory() as temporary:
|
||||
directory = Path(temporary)
|
||||
source, manifest, output = (directory / name for name in ("input.hdf5", "manifest.json", "report.json"))
|
||||
write(source, self.data)
|
||||
manifest.write_text(json.dumps(self.manifest))
|
||||
before = source.read_bytes()
|
||||
command = [
|
||||
sys.executable,
|
||||
"-m",
|
||||
"dex_workbench_tracking.diagnostic",
|
||||
str(source),
|
||||
"--manifest",
|
||||
str(manifest),
|
||||
"--output",
|
||||
str(output),
|
||||
]
|
||||
first = subprocess.run(command, capture_output=True, text=True)
|
||||
self.assertEqual(first.returncode, 0, first.stderr)
|
||||
self.assertEqual(json.loads(output.read_text())["dynamic_replay"], "NOT_RUN")
|
||||
report_bytes = output.read_bytes()
|
||||
second = subprocess.run(command, capture_output=True, text=True)
|
||||
self.assertNotEqual(second.returncode, 0)
|
||||
self.assertEqual(output.read_bytes(), report_bytes)
|
||||
self.assertEqual(source.read_bytes(), before)
|
||||
self.manifest["asset_sha256"] = "b" * 64
|
||||
manifest.write_text(json.dumps(self.manifest))
|
||||
rejected = directory / "rejected.json"
|
||||
command[-1] = str(rejected)
|
||||
third = subprocess.run(command, capture_output=True, text=True)
|
||||
self.assertNotEqual(third.returncode, 0)
|
||||
self.assertIn("hash mismatch", third.stderr)
|
||||
self.assertFalse(rejected.exists())
|
||||
|
||||
def test_loader_rejects_bad_time_and_nan_before_diagnostic(self):
|
||||
with tempfile.TemporaryDirectory() as temporary:
|
||||
for kind in ("duplicate_time", "nan"):
|
||||
data = copy.deepcopy(self.data)
|
||||
episode = data.episodes["demo_000000"]
|
||||
if kind == "duplicate_time":
|
||||
episode.time[1] = 0
|
||||
else:
|
||||
episode.wrist_position[1, 0] = np.nan
|
||||
path = Path(temporary) / (kind + ".hdf5")
|
||||
write(path, data)
|
||||
with self.subTest(kind=kind), self.assertRaises(ContractError):
|
||||
load(path, self.manifest)
|
||||
|
||||
def test_import_does_not_load_simulator_or_torch(self):
|
||||
result = subprocess.run(
|
||||
[
|
||||
sys.executable,
|
||||
"-c",
|
||||
"import sys; import dex_workbench_tracking.diagnostic; "
|
||||
"assert not any(n in sys.modules for n in ('pxr', 'torch', 'isaacsim', 'isaaclab', 'omni'))",
|
||||
],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
self.assertEqual(result.returncode, 0, result.stderr)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user