From 057f4c2cf6f7c1ea6ff704c9c2015bd459e7d688 Mon Sep 17 00:00:00 2001 From: cen617-code <1057290604@qq.com> Date: Fri, 11 Sep 2026 15:12:39 +0800 Subject: [PATCH] =?UTF-8?q?chore(release):=20v0.1.1=20USD=E6=96=87?= =?UTF-8?q?=E4=BB=B6=E5=88=9D=E6=AD=A5=E6=A0=A1=E9=AA=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 原因：记录 L20 USD 初步校验、严格浮动覆盖层、受限动力学回放及 HDF5 交付契约，包版本更新为 0.1.1。 验证：68 项 CPU/USD 回归测试通过，Ruff/format 与暂存 diff 检查通过；已核验单环境 small 2x480、合成 HDF5 2x960 步明确 PASS。独立暂存审查未发现问题。完整 pre-commit 因模块缺失未执行，干净环境安装未验证。 兼容性：Cartpole 及任务 ID 不变，原始 USD/URDF 未改；旧 prepared 覆盖层需重新生成。仅合成轨迹初步校验，不代表真实专家回放、训练或硬件验收。 --- HDF5_REQUIREMENTS.md | 217 +++++ L20_TRACKING.md | 348 +++++++ .../tracking_manifest.json | 870 ++++++++++++++++++ scripts/tracking/inspect_l20_scene.py | 91 ++ scripts/tracking/track_l20.py | 300 ++++++ source/dex_workbench/config/extension.toml | 2 +- .../dex_workbench_tracking/asset.py | 338 +++++++ .../dex_workbench_tracking/cli.py | 181 ++++ .../dex_workbench_tracking/control.py | 112 +++ .../dex_workbench_tracking/prepared.py | 299 ++++++ .../dex_workbench_tracking/trajectory.py | 225 +++++ source/dex_workbench/docs/CHANGELOG.rst | 47 + source/dex_workbench/setup.py | 5 +- .../tests/test_tracking_asset.py | 130 +++ .../tests/test_tracking_control.py | 103 +++ .../tests/test_tracking_prepared.py | 225 +++++ .../tests/test_tracking_range_profile.py | 89 ++ .../tests/test_tracking_schema_context.py | 114 +++ .../tests/test_tracking_startup.py | 90 ++ .../tests/test_tracking_trajectory.py | 303 ++++++ 20 files changed, 4086 insertions(+), 3 deletions(-) create mode 100644 HDF5_REQUIREMENTS.md create mode 100644 L20_TRACKING.md create mode 100644 assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json create mode 100644 scripts/tracking/inspect_l20_scene.py create mode 100644 scripts/tracking/track_l20.py create mode 100644 source/dex_workbench/dex_workbench_tracking/asset.py create mode 100644 source/dex_workbench/dex_workbench_tracking/cli.py create mode 100644 source/dex_workbench/dex_workbench_tracking/control.py create mode 100644 source/dex_workbench/dex_workbench_tracking/prepared.py create mode 100644 source/dex_workbench/dex_workbench_tracking/trajectory.py create mode 100644 source/dex_workbench/tests/test_tracking_asset.py create mode 100644 source/dex_workbench/tests/test_tracking_control.py create mode 100644 source/dex_workbench/tests/test_tracking_prepared.py create mode 100644 source/dex_workbench/tests/test_tracking_range_profile.py create mode 100644 source/dex_workbench/tests/test_tracking_schema_context.py create mode 100644 source/dex_workbench/tests/test_tracking_startup.py create mode 100644 source/dex_workbench/tests/test_tracking_trajectory.py diff --git a/HDF5_REQUIREMENTS.md b/HDF5_REQUIREMENTS.md new file mode 100644 index 0000000..c22a412 --- /dev/null +++ b/HDF5_REQUIREMENTS.md @@ -0,0 +1,217 @@ +# L20 左手专家轨迹 HDF5 交付要求 + +**接口版本：`l20_tracking_v1`** + +**交付对象：视频重定向/示教数据同事** + +**当前用途：浮动腕部与全部手指参考关节的轨迹回放，后续用于学习策略。** + +本文是可独立分享的数据交付说明，与当前仓库校验器一致。它不是通用 HDF5 标准，也不是完整 DexSchema。 +请先交付 **1 条短样例**，确认坐标、尺度、关节映射后再批量导出。 + +## 1. 要交什么 + +交付重定向后的 **L20 左手机器人参考状态轨迹**： + +- 腕部/手根的三维位置与姿态。 +- 约定模型的全部手指关节角，包括从动关节。 +- 同步时间戳与逐帧有效标记。 +- 模型身份、坐标变换、尺度和数据来源说明。 + +不是原始人手关键点、MANO 参数或电机实测指令。关节状态不能直接等同于执行器控制命令。 +本阶段不要求图像、物体位姿、奖励、接触标签、速度、加速度、电流或力矩；缺少这些内容不需要补零伪造。 + +## 2. 文件结构（必填） + +文件扩展名建议使用 `.hdf5`；HDF5 根属性通过 `file.attrs` 保存，**不要创建名为 `attrs` 的 group**。 + +```text +demonstrations.hdf5 +├── 根属性：schema_version、embodiment、hand_side、asset_sha256、root_link、 +│ provenance、source_description、metric_scale_provenance、scale_to_meters +├── metadata/ +│ ├── joint_names UTF-8 [J] +│ └── world_from_source float64 [4, 4] +└── episodes/ + ├── demo_000000/ + │ ├── time float64 [T] + │ ├── wrist_position float32 [T, 3] + │ ├── wrist_quaternion float32 [T, 4] + │ ├── joint_position float32 [T, J] + │ └── valid bool [T] + └── demo_000001/ + └── 同样结构，可以有不同帧数 T +``` + +- 至少一个 episode；名称必须为 `demo_` 加六位数字，如 `demo_000012`。 +- 每条 episode 至少两帧；各字段的 T 必须相同。 +- 一个文件共享模型、关节顺序及源世界标定；标定、模型或关节布局改变时另建文件。 +- 当前约定资产的 **J=21**，不是从“L20”名称推断出的自由度。21 个状态关节不代表 21 个独立执行器。 + +## 3. 根属性 + +除 `scale_to_meters` 外，下列属性均为非空 UTF-8 字符串。 + +| 属性 | 内容与要求 | +| --- | --- | +| `schema_version` | 固定为 `l20_tracking_v1` | +| `embodiment` | 固定为 `L20` | +| `hand_side` | 固定为 `left` | +| `asset_sha256` | 双方约定的原始模型依赖包哈希，64 位小写十六进制；见第 6 节 | +| `root_link` | 固定为当前模型的 `hand_base_link` | +| `provenance` | 实际专家重定向数据填 `expert_retargeted`；程序生成的测试数据填 `synthetic` | +| `source_description` | 视频/轨迹标识、重定向工具及版本、世界原点与 X/Y 朝向、源坐标约定、腕点到机器人根部的映射依据、质量限制 | +| `metric_scale_provenance` | 米制尺度如何获得，标定方法/版本或估算方法、可信程度与已知误差；不得把估算写成实测 | +| `scale_to_meters` | **有限正浮点标量**，记录已应用的源位置尺度系数；不是要求读取方再缩放 | + +例：源位置以毫米表示，则 `scale_to_meters=0.001`；输出位置仍须已经是米。 +源数据已经是米时可填 `1.0`，但必须有实际依据，不能仅为通过校验填写。 + +## 4. 字段与坐标语义 + +| 路径 | 类型 / shape | 单位及语义 | +| --- | --- | --- | +| `metadata/joint_names` | HDF5 UTF-8 string `[J]` | `joint_position` 每列对应的模型关节名称，严格使用第 5 节顺序 | +| `metadata/world_from_source` | `float64 [4,4]` | 已应用的源世界系到交付世界系刚体变换；矩阵平移部分单位 m | +| `episodes/.../time` | `float64 [T]` | 秒；每条轨迹从 0 开始，严格递增 | +| `episodes/.../wrist_position` | `float32 [T,3]` | `hand_base_link` 的 **link 原点**在交付世界系中的 `[x,y,z]`，单位 m | +| `episodes/.../wrist_quaternion` | `float32 [T,4]` | **`[w,x,y,z]`**；将根 link 局部向量主动旋转到交付世界系 | +| `episodes/.../joint_position` | `float32 [T,J]` | 参考关节角，单位 rad；零位和正方向与约定模型一致 | +| `episodes/.../valid` | `bool [T]` | 整帧是否有效；不是每关节标记，不使用 0/1 整数数组代替 bool | + +### 坐标与姿态 + +1. 交付世界系采用 **右手系、Z 向上**，在一个 episode 内固定，不随腕部移动。 +2. 根部位姿不是人手腕点、机器人质心或 USD default prim 位姿。重定向端必须先完成到 + `hand_base_link` 坐标系的映射，并说明映射依据。 +3. 记录的尺度和刚体变换满足： + + ```text + p_world = R_world_from_source @ (scale_to_meters * p_source) + t_world_from_source + ``` + + 此式用于源位置点；人手腕点到机器人根部的额外映射仍须由重定向端完成。 + 导出的 `wrist_position` 和 `wrist_quaternion` **已经在交付世界系**。 + 接收端不会再次应用该矩阵或尺度。 +4. `world_from_source` 最后一行为 `[0,0,0,1]`，旋转部分正交且行列式为 +1，不能混入尺度或镜像。 + 已统一世界系时可以使用单位矩阵，但不能用单位矩阵掩盖未知标定。 +5. 有效四元数必须单位化，范数容差 `1e-4`；相邻两帧都有效时，四元数点积必须非负。 + `q` 与 `-q` 表示同一旋转，导出时应统一相邻符号；不能混用 `xyzw` 或欧拉角。 +6. 不要为了适配当前合成测试而把真实轨迹自动平移到 `z=0.4`，或把起始姿态强制设为单位四元数。 + 场景对齐必须有明确、可追溯的坐标约定。 + +## 5. 关节顺序与联动 + +当前 `joint_names` 必须严格等于下列列表；这是数据清单顺序，**不是仿真运行时 DOF 顺序**。 + +```python +joint_names = [ + "index_dip", "index_mcp_pitch", "index_mcp_roll", "index_pip", + "middle_dip", "middle_mcp_pitch", "middle_mcp_roll", "middle_pip", + "pinky_dip", "pinky_mcp_pitch", "pinky_mcp_roll", "pinky_pip", + "ring_dip", "ring_mcp_pitch", "ring_mcp_roll", "ring_pip", + "thumb_cmc_pitch", "thumb_cmc_roll", "thumb_cmc_yaw", "thumb_ip", "thumb_mcp", +] +``` + +- 不允许重名、缺列、多列、静默重排或用其他手型的关节代替。 +- 每个有效帧必须满足清单 `joints[].lower_rad/upper_rad`，校验容差为 `1e-6 rad`。 + 精确限位以随交付约定的 JSON 清单为准，避免使用文档四舍五入值作为限位。 +- 当前源模型定义以下联动；数据中仍须包含这些从动关节列： + + ```text + thumb_ip = 1.02 * thumb_mcp + index_dip = 0.89 * index_pip + middle_dip = 0.89 * middle_pip + ring_dip = 0.89 * ring_pip + pinky_dip = 0.89 * pinky_pip + ``` + + 偏置均为 0 rad，有效帧等式残差绝对值不得超过 `1e-3 rad`。 +- 联动关系和各关节限位必须同时满足；不能单独裁剪从动关节而破坏联动。 +- 这些是约定模型的约束，不是已经核实的硬件电机协议。重定向映射有疑问时先确认，不要擅自独立控制从动关节。 + +## 6. 模型身份与随附清单 + +本仓库当前清单： + +[`assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json`](assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json) + +当前原始模型依赖包 `asset_sha256`： + +```text +6c8f35358f481cf604ee802588017f30c1661c0f38e1187c49e5340db3830538 +``` + +这是**当前版本快照**。请由仿真方提供同版 `tracking_manifest.json` 与约定模型，数据方确认使用该模型后填写哈希。 +不得把上述哈希复制到实际使用了其他模型的数据中。 + +- 哈希不是 HDF5 文件自身哈希，不是仅入口 USD 的哈希，也不是浮动控制覆盖层的哈希。 +- 它由完整解析资源包的相对路径与各文件 SHA-256 汇总生成，使用仓库工具获取，不自行猜测算法。 +- 模型依赖内容或名称变化时必须重新对齐。分享本文给同事时，**请同时附上同版清单**，不要只发送失效的仓库相对链接。 + +## 7. 时间同步、无效帧与缺失数据 + +- `time[0]` 必须精确等于 `0.0`，之后严格递增；不得有重复、倒序时间戳。 +- 允许非均匀采样，不强制视频帧率等于仿真控制频率。保留实际时间间隔，不通过伪造时间戳“拉齐”数据。 +- 同一帧腕部、姿态和关节角必须已经同步；异步数据须由交付方先明确对齐方式。 +- 任一必需部分缺失或不可信，整帧 `valid=false`。至少一帧有效；全部无效的 episode 会被拒绝。 +- **所有数值都必须有限，包括无效帧，不能出现 NaN/Inf。** 无效帧可以用明确的有限占位值， + 但必须保留 `valid=false`，并在来源说明中解释。无效帧四元数不要求单位化。 +- 必填 dataset 不能省略；禁止把缺失帧补零后标成有效，或把插值/估算值冒充测量。 +- 当前重采样 CLI 只接受全有效 episode，不跨无效片段插值。建议交付方将连续有效片段显式切成 + 独立 episode，并为每段重新设置从 0 开始的时间；原片段来源需可追溯。 + +## 8. 导出注意事项 + +使用 `h5py` 时，字符串必须显式声明 UTF-8，数值类型按契约转换： + +```python +# 仅展示 dtype 写法；不是完整数据生成器，不包含真实轨迹。 +import h5py +import numpy as np + +utf8 = h5py.string_dtype(encoding="utf-8") +# metadata.create_dataset("joint_names", data=joint_names, dtype=utf8) +# group.create_dataset("time", data=np.asarray(time, dtype=np.float64)) +# group.create_dataset("wrist_position", data=np.asarray(position, dtype=np.float32)) +# group.create_dataset("wrist_quaternion", data=np.asarray(quaternion_wxyz, dtype=np.float32)) +# group.create_dataset("joint_position", data=np.asarray(q, dtype=np.float32)) +# group.create_dataset("valid", data=np.asarray(valid, dtype=np.bool_)) +``` + +不要用 `dtype="S"` 或普通 ASCII bytes 数组写 `joint_names`。不要用 float64 替代契约中的 float32， +也不要用整数时间戳或整数 `valid` 代替约定类型。 + +## 9. 交付及验收流程 + +### 数据方交付 + +1. 一份包含一条短轨迹的 `.hdf5` 样例，优先选择连续、全有效、缓慢运动的片段。 +2. 对应模型清单/版本确认。 +3. 明确的标定与重定向说明；写入根属性，也可附独立说明文件。 +4. 可选：对应原视频片段或可视化，便于人工核对方向、尺度与动作，不是本格式必填字段。 + +### 仿真方校验 + +从仓库根目录运行： + +```bash +export PYTHONPATH="$PWD/source/dex_workbench${PYTHONPATH:+:$PYTHONPATH}" +~/isaacsim/python.sh -m dex_workbench_tracking.cli validate /path/to/demonstrations.hdf5 \ + --manifest assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json +``` + +该校验不启动仿真。仿真方本地的 `python.sh` 路径不是数据文件的依赖；具备项目模块、NumPy、h5py +的普通 Python 环境也可执行同一 `-m` 命令。 + +- **PASS**：schema、模型身份、关节顺序、限位和已记录联动一致。 +- **FAIL**：修正具体报错后重新交付，不绕过校验器或放宽断言。 +- 不加 `--manifest` 只检查格式，不能证明模型/关节兼容。 +- 校验通过后仍需人工核对坐标和标定，再做受限动力学回放。格式通过不等于重定向正确、动力学可执行、 + 策略已经可训练或真机可运行。 + +**注意：** 当前诊断脚本要求腕部及全部五组联动都有足够运动，用于发现失效约束；这是诊断用例要求， +不是 HDF5 格式要求。合法的静止或局部手指轨迹不应为了满足诊断而添加伪造动作。 + +仿真实现与已有验证结果另见 [`L20_TRACKING.md`](L20_TRACKING.md)。本文只约定数据交付，不要求数据方运行 Isaac Sim。 diff --git a/L20_TRACKING.md b/L20_TRACKING.md new file mode 100644 index 0000000..85c141e --- /dev/null +++ b/L20_TRACKING.md @@ -0,0 +1,348 @@ +# L20 左手轨迹跟踪：数据交付与准备入口 + +## 本次可用范围与验证状态 + +提供 **CPU 数据接口、资产清单、浮动控制覆盖层和实验性动力学跟踪入口**，不是完成的 RL 环境。 +覆盖层和控制数学有 CPU/USD 静态验证；**注册 schema 覆盖层的单环境合成轨迹动力学 smoke 已 PASS**。 +此前启动顺序、异常退出码和 schema 默认值保留问题已修复；后续授权重测完成两轮各480步。 +后续还完成4秒/10mm/0.1rad的合成 HDF5 文件加载与两轮各960步验收，并消除了旧世界 anchor 的 +CreateJoint 警告（见第4节）。通过范围仅限指定资产、后端、参数及两种合成参考，不代表真实专家轨迹、 +任意运动或 RL 环境验收。 +保留 `Template-Dex-Workbench-v0` / Cartpole 及其 checkpoint。没有新训练任务、物体、奖励、策略或训练结果。 + +实际组合资产检查发现： + +- 当前入口 `assets/robots/dex_hand/linkerhand_g20_left/linkerhand_g20_left/linkerhand_g20_left.usda` + 是**世界固定基座**，尽管转换缓存写 `fix_base: false`。 +- 22 个刚体、21 个 revolute 关节，另有一个世界 fixed joint；根 link 为 `hand_base_link`。 + **21 是导入状态关节数，不是独立执行器数**。实际名称、排序、限位、驱动属性、质量惯量见 + [`tracking_manifest.json`](assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json)。 +- 原始 URDF（清单记录其 SHA-256，未修改）含 5 个 mimic： + `thumb_ip = 1.02 * thumb_mcp`；`index/middle/ring/pinky` 各自 `dip = 0.89 * pip`；offset 均为 0 rad。 +- **纠正前轮结论**：安装的 Isaac Sim 6.0.1 / PhysX 110.1.13 原生解析 `NewtonMimicAPI`， + 旧 `PhysxMimicJointAPI` 已弃用。原资产的五个 Newton 方程正确，不能因缺少 Physx 前缀就认定缺约束。 + 依据包括安装 schema、PhysX migration checker/tests，以及 PhysX 插件中的 `parseNewtonMimicJoints` 与 + 对应解析诊断。它们是支持性证据，**不是本资产的动力学执行证明**。 + 清单改为 `MIMIC_SCHEMA_PRESENT_RUNTIME_UNVERIFIED`，仍 `dynamic_replay_ready=false`。 +- 原资产五个从动关节同时有独立 angular drive。新 `prepared.py` 覆盖层保留原生 Newton 约束， + 去掉这五个 DriveAPI 并将其 stiffness/damping/maxForce 置零；仅 16 个模型独立关节接收目标。 + 不新增第二套约束、不修改原始 USD/URDF，不改变关节限位、质量、惯量、碰撞或变换。 + 16 是根据该模型推导的实验控制映射，**不是已确认的硬件电机数**。 +- `require_dynamic_replay_ready()` 仍拒绝仅凭数据 JSON 启动控制。实验入口 `track_l20.py` 另行检查实际 + 覆盖层、源包哈希、逐项结构与约束、已启用后端版本、运行时从动增益和被动联动响应；不是改状态位绕过。 + +旧 `--floating-overlay` 仍仅是拓扑诊断，不应直接 Play。新 `prepared` 覆盖层也不是自带悬浮能力的资产： +必须配合实验控制器才有受限重力补偿；本次有合成参考的仿真跟踪指标，但不宣称可训练或 Sim2Real 有效。 + +## 1. 给数据同事的 HDF5 契约：`l20_tracking_v1` + +可直接分享的独立说明见 [`HDF5_REQUIREMENTS.md`](HDF5_REQUIREMENTS.md)，发送时请同时附同版模型清单。 + +这是本仓库的跟踪专用接口，不宣称已实现全项目 DexSchema。交付**重定向后的机器人参考状态**， +不是原始人手关键点，不是实测执行器命令。先交一条短样例，不要求物体、图片、力矩或速度。 + +```text +demonstrations.hdf5 +├── attrs +│ ├── schema_version = "l20_tracking_v1" +│ ├── embodiment = "L20" +│ ├── hand_side = "left" +│ ├── asset_sha256 = "<双方约定的清单 asset_sha256>" +│ ├── root_link = "hand_base_link" +│ ├── provenance = "expert_retargeted" 或 "synthetic" +│ ├── source_description = "采集/重定向版本、世界原点/朝向、校准来源、质量说明" +│ ├── metric_scale_provenance = "米制尺度如何获得，是否估算及可信程度" +│ └── scale_to_meters = 1.0 # 浮点标量，已应用的源尺度系数，不一定是1 +├── metadata +│ ├── joint_names UTF-8 [J] +│ └── world_from_source float64 [4,4] +└── episodes + └── demo_000000 # demo_ + 六位数字；可有多条 + ├── time float64 [T] + ├── wrist_position float32 [T,3] + ├── wrist_quaternion float32 [T,4] + ├── joint_position float32 [T,J] + └── valid bool [T] +``` + +### 坐标、单位和身份 + +- 所有字符串非空 UTF-8。`joint_names` 必须是 HDF5 UTF-8 string dtype，不能使用 ASCII byte arrays。 +- `J` 来自约定的机器人清单，不从 L20 名称推断。`joint_names` 必须**逐项等于清单 `joints[].name` 的排序**。 + 此排序为名字字典序，不是 Isaac 的 DOF 顺序；后续 Adapter 必须再显式按名称映射。 +- 根部位置对应 `hand_base_link` **link 原点**（不是质量中心、人手腕点或模型 default prim 原点）。 + 人手腕点到机器人根 link 的变换由重定向端应用；不能交付后再隐式猜测。 +- 位置单位 **m**，关节角 **rad**，时间 **s**；世界系右手、Z 向上。 + 世界原点、X/Y 朝向及源相机/重建坐标约定必须在 `source_description` 中注明并与仿真场景对齐。 + 一个文件共享一个源世界校准；相机/校准改变时另建文件，世界系不随腕部移动。 +- 四元数排列 **wxyz**，表示 root-link 局部向量到世界向量的主动旋转。 + 有效帧必须单位化（范数容差 `1e-4`）；相邻有效帧四元数点积必须非负，避免符号翻转。 +- `world_from_source` 表示已经应用的刚体变换： + `p_world = R * (scale_to_meters * p_source) + t`。该矩阵不包含尺度；平移单位为 m。 + 导出数据已在世界系，读取器**不会再次应用矩阵或尺度**。姿态同样已转换。 + `scale_to_meters` 必须为有限正浮点数；不能把估算尺度写成实测。 +- `asset_sha256` 是**完整本地 USD 依赖包**的内容身份，不是仅入口文件 SHA。 + 清单按相对入口目录的 POSIX 文件名排序，对每个 `relative_path + NUL + file_sha256 + LF` 的 UTF-8 + 记录累积 SHA-256。包含 USD 层和解析到的资源，重定位整个包身份不变；任何依赖内容/名称变化会改变身份。 + 清单本身不属于 USD 依赖；独立记录源 URDF SHA。不要把固定版与浮动 overlay 的包哈希混用。 + +### 时间、缺失和状态语义 + +- 每条至少 2 帧，`time[0] == 0`，严格递增；所有字段使用同一时间轴、同一 T。 + 允许非均匀采样，保留真实时间戳。不同步数据必须先由交付方明确对齐。 +- `joint_position` 包含所有列出的参考关节状态，包括 mimic 从动关节；不据此认定全部可独立驱动。 + 带清单校验会检查有效帧限位（容差 `1e-6` rad）以及已记录的 URDF mimic 等式（容差 `1e-3` rad）。 + 不静默换序、裁剪角度、补关节或归零修复。新数据误差超容差时应确认重定向质量，而非放宽验收。 +- 缺失/低可信帧必须 `valid=false`。本版本所有数值 payload 均要求有限（包括无效帧）；无效帧可用有限占位， + 但绝不能标成有效数据。无效四元数不要求单位化。至少一帧有效；完全缺失字段不能省略或静默补真值。 +- 读取器拒绝 NaN/Inf、重复名字、错误 shape/dtype、非单调时间、不合法刚体矩阵、未知版本/来源类别等。 + 元数据文字存在不证明标定正确，仍需人工核对来源。 +- API `sample()` 对腕位置/关节角线性插值，对姿态 shortest-arc SLERP；不外推。 + 输出四元数重新保持相邻符号连续；这不能恢复降采样丢失的旋转圈数或高频运动，频率需按轨迹带宽选择。 + **任何查询范围跨过/触及无效帧均拒绝**，包括仅查询无效间隙两侧的稀疏点。必须显式分割有效片段。 + CLI 重采样仅接受全有效 episode，不自动分段；输出从 0 开始的等间隔网格，离网格的尾帧省略。 +- 速度/加速度非必填。本次不估算并保存速度，不把未来派生值冒充硬件测量。 + +## 2. 本地命令 + +在仓库根目录运行。独立 namespace 包 `dex_workbench_tracking` **不导入** `dex_workbench` 的任务注册， +CPU 验证不要求 Kit/GPU。普通 Python 需要 numpy/h5py；本机系统 Python 缺 h5py，以下用已有 Isaac 启动器。 +安装环境可选 `pip install -e 'source/dex_workbench[tracking]'`，本次没有安装/升级任何依赖。 +也可不安装，使用： + +```bash +export PYTHONPATH="$PWD/source/dex_workbench${PYTHONPATH:+:$PYTHONPATH}" +ASSET=assets/robots/dex_hand/linkerhand_g20_left/linkerhand_g20_left/linkerhand_g20_left.usda +MANIFEST=assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json + +# 复查真实组合资产；输出必须是尚不存在的路径 +~/isaacsim/python.sh -m dex_workbench_tracking.asset "$ASSET" --output /tmp/l20-inspected.json +# 如有原始 URDF，用显式本地路径对照（不是运行时必需依赖） +# ... --source-urdf "$SOURCE_URDF" --output /tmp/l20-with-urdf.json + +# 生成明确标为 synthetic 的 2 秒参考样例，121 帧；不是专家数据或动态控制验收 +~/isaacsim/python.sh -m dex_workbench_tracking.cli synthetic \ + --manifest "$MANIFEST" --output /tmp/l20-synthetic.hdf5 +~/isaacsim/python.sh -m dex_workbench_tracking.cli validate \ + /tmp/l20-synthetic.hdf5 --manifest "$MANIFEST" +~/isaacsim/python.sh -m dex_workbench_tracking.cli resample \ + /tmp/l20-synthetic.hdf5 --manifest "$MANIFEST" --hz 240 --output /tmp/l20-resampled.hdf5 + +# 动态准备门禁：预期非零退出且 BLOCKED；直接 Python 返回2，本机 python.sh 将非零映射为1 +~/isaacsim/python.sh -m dex_workbench_tracking.cli replay-check --manifest "$MANIFEST" + +# 不提供 manifest 仅作结构检查，输出 asset_compatibility=NOT_CHECKED +~/isaacsim/python.sh -m dex_workbench_tracking.cli validate /path/to/demonstrations.hdf5 + +# 生成诊断层：保留原入口、fixed_base_setup.usda 和其他依赖原始字节 +~/isaacsim/python.sh -m dex_workbench_tracking.asset "$ASSET" \ + --floating-overlay /tmp/l20-floating.usda --output /tmp/l20-floating-manifest.json + +# 仅在没有冲突的 GPU/Kit 作业时运行；本次因已有 GUI 实例而未执行 +# 此入口只验证 Isaac Lab 引用后 root/关节/初始有限变换，不运行物理、reset 或 replay +# --help 的 AppLauncher 参数以本地 Isaac Lab 为准；默认 headless，单场景，seed=42 +timeout 300s ~/isaacsim/python.sh scripts/tracking/inspect_l20_scene.py /tmp/l20-floating.usda --headless + +# CPU 单测；第二组还需要 pxr（已有 Isaac Python 提供） +~/isaacsim/python.sh -m unittest discover -s source/dex_workbench/tests -p 'test_tracking_trajectory.py' -v +~/isaacsim/python.sh -m unittest discover -s source/dex_workbench/tests -p 'test_tracking_asset.py' -v +``` + +所有生成入口拒绝覆盖已有文件。HDF5 CLI 先在目标目录的临时文件中写入并校验，成功后以硬链接发布； +文件系统须支持硬链接，否则明确失败，不回退为覆盖写入。校验失败不留下目标文件。 +示例输出位置是临时产物，不应加入 Git；分享给同事时标清 synthetic。 +清单 snapshot 绑定当前依赖字节，资产变化后必须重新生成、审查并与数据方更新版本，不直接沿用旧哈希。 +普通 `validate` 的 PASS 只证明契约；即使带清单 PASS，`dynamic_replay` 仍为 BLOCKED。 + +## 3. 实验性 PhysX 浮动跟踪（单环境合成轨迹 smoke PASS） + +```bash +# 调用方显式选择本机插件；这些路径不是可移植资产依赖，其他安装应替换。 +# 多版本安装须人工选定一个 PhysX 插件，不能把多条路径合并为一个参数。 +PHYSX_PLUGIN=$(find "$HOME/isaacsim/extscache" -path '*/plugins/PhysxSchema/resources/plugInfo.json' -print) +NEWTON_PLUGIN="$HOME/isaacsim/exts/omni.usd.schema.newton/usd/schema/newton/newton_usd_schemas/plugInfo.json" +# 必须用原始数据/source manifest；覆盖层身份另外记录，不能静默改 HDF5 asset_sha256 +# 必须用新输出路径；旧覆盖层需重新生成，不覆盖已有文件。 +~/isaacsim/python.sh -m dex_workbench_tracking.prepared "$ASSET" \ + --manifest "$MANIFEST" --output /tmp/l20-tracking-registered.usda \ + --schema-plugin-path "$PHYSX_PLUGIN" --schema-plugin-path "$NEWTON_PLUGIN" +# 对已生成的实际 USD 重新检查；使用同样的插件上下文，不是读取 ready 状态位 +~/isaacsim/python.sh -m dex_workbench_tracking.prepared /tmp/l20-tracking-registered.usda \ + --manifest "$MANIFEST" --schema-plugin-path "$PHYSX_PLUGIN" --schema-plugin-path "$NEWTON_PLUGIN" + +# 先协调关闭/释放用户 GUI 会话；注册 schema 覆盖层已通过一次限定的合成轨迹验收。 +timeout 300s ~/isaacsim/python.sh scripts/tracking/track_l20.py /tmp/l20-tracking-registered.usda \ + --manifest "$MANIFEST" --execute-experimental --headless --steps 480 +# 默认使用明确标注的 synthetic 轨迹；已有合格 HDF5 时可加： +# --hdf5 /path/to/demonstrations.hdf5 --episode demo_000000 + +~/isaacsim/python.sh -m unittest discover -s source/dex_workbench/tests -p 'test_tracking_*.py' -v +``` + +### 修复过程记录（历史失败保留） + +- 旧覆盖层启动命令（单手、seed42、`--steps 480`、两次回放目标、外部 `timeout 300s`）实际执行失败， + 没有进入 scene/reset/physics loop，没有跟踪误差指标或 PASS JSON。 +- 初次启动：`pxr` 在 App 前导入，Kit 报 USD registry/free 崩溃，启动器退出1。 + 两个诊断入口均已改为先启动 App，再在 `try/finally` 内进行 USD 检查；资产门禁仍早于 scene/控制。 +- 第一次重试：App 启动成功但退出0且**没有 PASS**。确认 `SimulationApp.close()` 的 fast shutdown + 会掩盖进行中的异常；该次是失败，不是通过。已添加异常 traceback 和 `close(exit_code=1)`。 +- 第二次重试：退出1，明确报告旧 anchor 的 `physxArticulation:sleepThreshold` 默认值不一致。 + 注册 PhysX 的纯 CPU 复现表明：移除 API 后旧 anchor 的三个未显式写入的默认值消失； + root 默认值与原值一致，但严格保留检查正确拒绝旧 anchor 差异。没有通过放宽检查来修复。 +- 修复：移除 API 前快照全部已解析 articulation 属性（含 schema fallback），在旧 anchor 和新 root + 显式保留原值；记录 authoring schema context。跨上下文检查拒绝并要求重新生成。 + 不带插件选项的纯静态输出标为 `STATIC_ONLY_SCHEMA_UNREGISTERED`，不能推荐用于运行时。 +- **CPU 回归 60 项通过**（原57项加启动/异常退出与隔离注册 schema 测试），无跳过； + 新注册覆盖层生成和复查通过，source bundle SHA 不变。该修复交付内未做第三次 GPU 重试，增益和断言未放宽。 +- 本机诊断日志：`/tmp/l20-runtime-PpD1Ah/{smoke,retry1,retry2,schema-default-repro,tests}.log`； + 新覆盖层 `/tmp/l20-runtime-PpD1Ah/registered-tracking.usda` 当时仅静态验证。它们是临时制品，不提交。 + 修复交付后另获用户授权的 GPU 验收结果如下。 + +### 修正覆盖层的授权重测：PASS + +实际命令（仓库根目录，未更改增益或断言）： + +```bash +PYTHONPATH=source/dex_workbench timeout 300s ~/isaacsim/python.sh \ + scripts/tracking/track_l20.py /tmp/l20-runtime-PpD1Ah/registered-tracking.usda \ + --manifest assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json \ + --execute-experimental --headless --steps 480 +``` + +- Isaac Sim 6.0.1 / PhysX 110.1.13、RTX5080；seed42、num_envs1、240Hz、synthetic、两轮各480步。 +- 退出0且有完整 PASS JSON；两次 reset、位姿/关节/速度重复性、非零运动、有限值、限位与联动断言通过。 +- 两轮输出相同的汇总指标（这不承诺跨运行 GPU 确定性）： + +| 误差 | 最大值 | RMS | +| --- | ---: | ---: | +| 腕部位置 | 0.000492342 m | 0.000299964 m | +| 腕部姿态 | 0.003755143 rad | 0.002314660 rad | +| 各帧最大关节误差 | 0.008628675 rad | 0.005612279 rad | +| 各帧最大 mimic 残差 | 0.000221643 rad | 0.000059707 rad | + +- 源包 SHA `6c8f35358f481cf604ee802588017f30c1661c0f38e1187c49e5340db3830538`； + 覆盖层 SHA `ca874b29e4ee4f7c211d17c3e190487b0d6cc103dbe57035371d49fa64e98269`。 +- 日志 `/tmp/l20-registered-retest-N90YMI/smoke.log`，SHA256 + `b763f3abe35c780304a5a4bb587cdc639f63ab4fc4469e6ccde8551b10ff436d`；退出记录同目录 `result.txt`。 + 测试后未发现 Kit 进程，GPU占用739MiB（测试前708MiB）；这不证明所有资源位级恢复。 +- 仍有警告：旧 fixed joint 的 body transforms 不重合、TGS external-force iteration 配置、 + 16/21 actuator 数量、protobuf 重复注册和 visualizer 配置缺失。16/21对应有意保留的5个被动关节； + 其余警告未靠关闭检查消除，尤其 fixed joint 警告需在扩大运动范围前进一步核验。 +- 仅为2mm平移、0.01rad转动、小幅手指合成参考的 smoke；未测试真实 HDF5、大范围运动、接触、 + 多环境、训练或硬件。静态清单仍保留 runtime-unverified；本次证据不自动授权任意后续运行。 + +### 模型/版本门禁 + +- 验证 source manifest 的完整依赖包、关节列表、限位、质量记录与当前源 USD 一致；源 URDF 哈希和 + 五个方程必须为本轮已核验版本。重新检查五个实际 Newton leader 路径、系数、启用状态和单自由度轴； + 拒绝重复 legacy mimic、未知约束、独立从动电机以及覆盖层额外物理编辑。 + 本诊断仅接受静态资产：所有属性（含允许修改默认值的字段）均不得带时间采样。 +- `NewtonMimicAPI`: `q_follower = coef0 + coef1*q_leader`。revolute 的 coef0 用**度**，换为接口 rad； + coef1 无量纲。单 DOF 的轴由各自 RevoluteJoint 定义，不新增 rotX/rotY token。 + 旧 Physx 约定为 `q_follower + gearing*q_leader + offset = 0`，不能把正 multiplier 直接当 gearing。 +- 静态检查默认允许尚未注册的 codeless schema 元数据；运行时必须真正注册 NewtonMimicAPI、启用 + `omni.physx`，版本严格为 **110.1.13**。其他版本须重新核验，不静默降级/重复约束。 +- 当前 Isaac Lab 使用 `ProxyArray.torch`、root pose **xyzw**、index 写入方法；HDF5 始终 **wxyz**， + 在运行入口显式转换。旧版 Lab 未验证，不承诺兼容。 +- 本地依据（相对于 Isaac Sim 安装目录；未复制供应商实现）： + `extscache/omni.usd.schema.physx-*/plugins/PhysxSchema/resources/schema.usda` 的 PhysxMimicJointAPI； + `exts/omni.usd.schema.newton/usd/schema/newton/newton_usd_schemas/generatedSchema.usda` 的 NewtonMimicAPI； + `extscache/omni.physx.asset_validator-*/omni/physxassetvalidator/scripts/newtonMigrationChecker.py` 与 + `tests/newtonMigrationCheckerTest.py`；`extscache/omni.physx-*/bin/libomni.physx.plugin.so` 的 native parser 标识。 + 本轮读取了上述代码/二进制字符串，**未执行供应商 GPU 集成测试**。 + +### 控制与验收边界 + +- 单环境、seed42、240Hz；默认两次各480步，单次允许100–1200步，外部 timeout 300秒。 + 只创建手，无物体、桌面、任务注册或训练。不逐步 teleport；仅 reset 写 root/joint 初值并清零速度、 + wrench 与目标缓冲、轨迹时间，之后只发 master q_target 和 root COM wrench。 +- 世界系 PD 跟踪 link 原点；输出作用在 root COM，补上 `(p_link-p_rootCOM)×F_PD`。 + 全手重力补偿为 `sum(-m*g)` 和 `sum((p_bodyCOM-p_rootCOM)×(-m*g))`，重力维持 `(0,0,-9.81)`。 + 不是对整个手逐刚体取消重力；根部吸收补偿，手指仍承受物理重力/反作用。 +- **所有参数待校准**：位置 P/D=100 N/m、10 N·s/m，姿态 P/D=0.2 N·m/rad、0.02 N·m·s/rad； + 总力/力矩范数上限20N/1N·m（含重力项），需留至少20%静态重力余量。 + 16 个 master 的 implicit drive P/D=3/0.1，仿真 effort cap=0.2N·m、velocity cap=0.5rad/s； + 不改变 follower 限位或增加 armature。不是实测安全参数。 +- 可用 `--limits` 指定 `control.Limits` 字段 JSON（有限正值，SI），默认参考速度限制 + 0.05m/s、0.5rad/s、各关节0.5rad/s；位置不超过起点0.1m。超限/无效片段直接拒绝，不裁剪成成功。 +- 每帧检查有限值、速度、限位（0.01rad容差）、五条 mimic 残差<0.002rad，腕误差<0.05m/0.5rad、 + 关节误差<0.2rad。这些是预设诊断失败阈值，不是高质量跟踪指标。输出各项 max/RMS 原始指标。 + 每条 mimic 的 leader/follower 都必须有>0.002rad真实运动，根部需>0.5mm平移和>0.002rad转动， + 避免静止关节让缺约束假通过。输入需激励全部 mimic leader，纯静止/局部手指数据不适合此诊断。 +- 两次 reset 初值误差<=1e-6；重放的根部位姿/关节位置逐项差<=1e-3， + 根部线速度/角速度及关节速度逐项差<=1e-3（m/s 或 rad/s）；不承诺 GPU 位级确定性。 + 任一失败非零退出，`finally` 关闭本次 App，不修改其他进程或原资产。 +- **仍需扩展验证**：本次小幅合成参考的被动联动响应、跟踪与重复性已通过；真实轨迹、更大运动、 + 接触/自碰撞及跨运行稳定性尚未验证。任何失败需保留日志并诊断，不能放宽阈值充作通过。 + 后续再做少量并行环境、真实 HDF5、BC/PPO；硬件可控映射仍需外部确认。 + +本轮仅有上述合成参考的动力学误差实测，没有训练质量声明。没有提交/推送、版本升级或硬件动作。 + +## 4. 扩范围 HDF5 诊断（本轮预先固定的测试条件） + +- 新覆盖层仅将原有、已禁用的**叶节点世界 fixed joint**设为 inactive，阻止它继续进入活动关节解析； + 根部仍为 `hand_base_link`。旧 anchor 的属性、关系和 schema 默认值保留，`TraverseAll` 仍逐项检查； + 其他节点的活动状态、树结构与时间采样门禁不得改变。原始 USD 未修改，旧覆盖层需重新生成。 + 旧 `asset --floating-overlay` 仍仅禁用 anchor，用于拓扑诊断，不是本轮推荐的 prepared 控制资产。 +- 原警告与 world/local joint frames 不重合有关：原 world frame 为原点，spawn 后手根在 z=0.4m； + `jointEnabled=false` 不阻止解析阶段报告该警告。去激活不是改变局部变换以掩盖警告。 +- 新命名合成 profile `range_4s`：60Hz，241帧，4秒，`s(t)=sin²(pi*t/4)` 往返； + X向位移 `0.01*s` 米、Z保持0.4米、世界Z轴转角 `0.1*s` rad。 + 独立关节正向幅度 `min(0.1rad, 0.25*upper_limit)`，从动关节按源 mimic 计算。 + 解析曲线端点速度为0；采样后的线性/SLERP插值不保证连续加速度。明确为 synthetic，非专家。 +- 默认 `small` profile 保持原来的2秒/2mm/0.01rad和关节幅度，未改变默认参考或控制器。 +- 测试前固定：第1次新覆盖层 small，2x480步；第2次从磁盘载入 `range_4s` HDF5，2x960步。 + 每次单环境、seed42、240Hz、timeout300秒；不调增益/阈值，不将失败后减小幅度算通过。 +- 新入口在 spawn 后要求旧 anchor inactive、仅21个活动 revolute joints；运行时仍断言非固定基座、 + 22刚体、21状态关节、16独立控制目标与5个被动从动响应。PASS另记录HDF5 SHA/来源和活动关节数。 + +```bash +# 使用第3节显式插件选项重新生成新的覆盖层，例如 /tmp/l20-range/tracking.usda +~/isaacsim/python.sh -m dex_workbench_tracking.cli synthetic --manifest "$MANIFEST" \ + --profile range_4s --output /tmp/l20-range/range.hdf5 +~/isaacsim/python.sh -m dex_workbench_tracking.cli validate /tmp/l20-range/range.hdf5 --manifest "$MANIFEST" +timeout 300s ~/isaacsim/python.sh scripts/tracking/track_l20.py /tmp/l20-range/tracking.usda \ + --manifest "$MANIFEST" --execute-experimental --headless --steps 960 \ + --hdf5 /tmp/l20-range/range.hdf5 --episode demo_000000 +``` + +### 本轮结果：两个限定 GPU 用例均 PASS + +- 第1次命令：上述 `track_l20.py /tmp/l20-range/tracking.usda`，不传 `--hdf5`， + `--steps 480`，其余参数相同。第2次完整执行上方 `--steps 960 --hdf5 ...` 命令。 + 两次均退出0、有明确 PASS JSON、两轮期望步数及有限指标；没有第三次运行，没有调参或放宽断言。 +- 两次均输出 `world_anchor_inactive=true`、`active_state_joint_count=21`、 + `runtime_is_fixed_base=false`；两轮 reset/状态/速度重复性、非零运动与 mimic 断言通过。 + 两份日志均不再出现 `CreateJoint` / `disjointed body transforms`。小幅用例 max/RMS 汇总与 + 第3节历史基线逐值相同，支持此叶节点去激活在本用例的动力学等效性，不承诺任意场景等效。 +- HDF5 用例确实从磁盘读取 `range_4s`，输出 `reference_source=hdf5`、`provenance=synthetic`； + 60Hz的241帧按原时间戳插值至240Hz、961个参考点，执行960步/轮。 +- **68项 CPU/USD 回归通过，无跳过**，包括活动树/anchor/time samples、注册上下文、 + 实际L20清单下新profile的schema/限位/mimic/速度/来源与拒绝覆盖。 + +| 最大误差（两轮各自相同汇总） | 小幅基线 | range_4s HDF5 | +| --- | ---: | ---: | +| 腕部位置 | 0.000492342 m | 0.001044980 m | +| 腕部姿态 | 0.003755143 rad | 0.009153232 rad | +| 各帧最大关节误差 | 0.008628675 rad | 0.010852285 rad | +| 各帧最大 mimic 残差 | 0.000221643 rad | 0.000195338 rad | + +HDF5用例 RMS 依次为 `0.000646705 m`、`0.006378398 rad`、`0.007675343 rad`、`0.000050099 rad`。 +所有控制/参考上限与基线一致；这只是受限诊断误差，不是训练成功率或真机指标。 + +制品（临时，不提交）： + +- `/tmp/l20-range/{small,hdf5}.log`、`{small,hdf5}-metrics.json`、对应 `*-result.txt`、 + `acceptance-evidence.log`、CPU `tests-final.log`、各次 `preflight*.log` / `postflight*.log`。 +- 新覆盖层完整包 SHA `6179feb7a112c74b73bc003be6b0159757ed6ab3a9e2cb7015333e09225dc319`； + 原始源包身份仍为第3节的 `6c8f...0538`。 +- `/tmp/l20-range/range.hdf5` SHA256 + `9678a19a7bb983ceaf6ad715016fe784b001544de0dec1876831d986eb870072`，不是专家文件。 +- small日志 SHA256 `6e395fedf0796440ffc2f2fefac5661d97cb63ce6d015ebad0f8399e8e20f0f4`； + HDF5日志 SHA256 `10d255845d828e664d76f7d0119e5c2b5530cf696a1333afd41f39b95dd7c5f3`。 + +两次测试前后均未发现Kit残留；显存分别736→723MiB、734→775MiB，不承诺所有资源位级恢复。 +仍有TGS外力迭代、16/21 actuator（5个被动关节）、protobuf重复注册、visualizer/MaterialX与usdrt警告； +未关闭日志或放宽检查。更大范围、负向/多轴腕部运动、接触、真实专家文件、多环境、训练与硬件均未验证。 +完整 pre-commit 仍因未安装工具而 NOT_RUN；本次独立审查仍由父会话执行。 diff --git a/assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json b/assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json new file mode 100644 index 0000000..c2470d8 --- /dev/null +++ b/assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json @@ -0,0 +1,870 @@ +{ + "manifest_version": "l20_asset_manifest_v1", + "asset_sha256": "6c8f35358f481cf604ee802588017f30c1661c0f38e1187c49e5340db3830538", + "entry_file": "linkerhand_g20_left.usda", + "dependencies": [ + { + "path": "fixed_base_setup.usda", + "sha256": "1d4e01f8c0cb4eb6187dae4f7ba01660b69256d77a0d54f908dcb43fbdde55b7" + }, + { + "path": "linkerhand_g20_left.usda", + "sha256": "357d3b24d5a572592dafc4b4c16ea5a0b7cb95f15cc78b7a205ee1724ebe7fc5" + }, + { + "path": "payloads/Physics/mujoco.usda", + "sha256": "3234a6907b8259f72eb3d9f74b38c4d53473b42d67e2a797fce75a4a96c1a49f" + }, + { + "path": "payloads/Physics/physics.usda", + "sha256": "52c3799c0e494b7dc9795d0f6058901c6b0232702553c924cb42c866fdff7909" + }, + { + "path": "payloads/Physics/physx.usda", + "sha256": "5bd5628593ac5fb00b8a658d670b8d41a5205c930eb1f9edb15b7f3c3bd43ab2" + }, + { + "path": "payloads/base.usda", + "sha256": "084df26517fe2cfcfea6fb5bc2c2fe3382182bf07b04dc4547f34e1575658146" + }, + { + "path": "payloads/geometries.usd", + "sha256": "6ba619abc15fd30b10c05cd494ec8b4839647004f43a17376e105f4654f1bfcd" + }, + { + "path": "payloads/instances.usda", + "sha256": "6846beddde3bbdba8a5a39a329655314e40cebb6e29e323856e15dc50384d6a0" + }, + { + "path": "payloads/materials.usda", + "sha256": "eccbe067b9585993fe025e5ca0a533c048325d00023d5ec7c6cb3025cc7245e0" + }, + { + "path": "payloads/robot.usda", + "sha256": "8a1e950de525744a4ee32782a2cf67695175770c0c098b36cefd744df28f833b" + } + ], + "composition_errors": [], + "unresolved_dependencies": [], + "default_prim": "/tn__linkerhand_g20_lefturdf_cZ0", + "root_link": "hand_base_link", + "root_body_path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link", + "units": { + "length": "m", + "mass": "kg", + "up_axis": "Z" + }, + "physics_variant": "physx", + "articulation_roots": [ + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/fixed_base_joint" + ], + "world_fixed_joints": [ + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/fixed_base_joint", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link" + } + ], + "bodies": [ + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.00098641705699265, + 0.00026804901426658034, + 0.07737984508275986 + ], + "physics:diagonalInertia": [ + 0.00011751915008062497, + 0.0001602228730916977, + 0.00026441647787578404 + ], + "physics:mass": 0.11037349700927734, + "physics:principalAxes": "(0.70273006, 0.038762398, 0.7092034, -0.04121172)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + 0.011515513062477112, + 0.003980688285082579, + 0.00027288825367577374 + ], + "physics:diagonalInertia": [ + 2.0297929950174876e-06, + 2.4729044980631443e-06, + 4.054635155625874e-06 + ], + "physics:mass": 0.016035180538892746, + "physics:principalAxes": "(0.69315267, 0.09873541, 0.69662505, -0.15653834)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + 0.002867146162316203, + -0.00615726551041007, + 0.00472605274990201 + ], + "physics:diagonalInertia": [ + 6.547052322503077e-08, + 1.788231145383179e-07, + 2.448114173603244e-07 + ], + "physics:mass": 0.001832645502872765, + "physics:principalAxes": "(-0.4856348, 0.6328352, 0.57746726, 0.17381014)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + 0.006706578657031059, + 8.60758955241181e-05, + 0.03314129263162613 + ], + "physics:diagonalInertia": [ + 3.3777166663639946e-06, + 1.1335857379890513e-05, + 1.352877097815508e-05 + ], + "physics:mass": 0.028716035187244415, + "physics:principalAxes": "(0.5003233, 0.49646425, 0.50234294, 0.50085074)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals/thumb_proximal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.0026873440947383642, + 0.0005682445480488241, + 0.011850172653794289 + ], + "physics:diagonalInertia": [ + 2.2337233929192735e-07, + 4.898631118521735e-07, + 6.232804707906325e-07 + ], + "physics:mass": 0.005490154959261417, + "physics:principalAxes": "(0.4466405, 0.44347262, 0.5443941, 0.55450827)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals/thumb_proximal/thumb_distal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.0007100218208506703, + -0.0026757591404020786, + 0.012476266361773014 + ], + "physics:diagonalInertia": [ + 8.26387420715946e-08, + 2.520345105949673e-07, + 2.698974697068479e-07 + ], + "physics:mass": 0.0038204663433134556, + "physics:principalAxes": "(0.7076172, 0.49610376, 0.40506423, 0.29846585)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.00794299691915512, + -2.936739633696561e-07, + 0.0022010602988302708 + ], + "physics:diagonalInertia": [ + 2.8543546193304792e-08, + 3.1735115868514185e-08, + 4.670307163223697e-08 + ], + "physics:mass": 0.0008955164230428636, + "physics:principalAxes": "(0.15955442, 0.6888882, 0.68885684, -0.15953577)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.0011418547946959734, + 3.473157630651258e-05, + 0.022892840206623077 + ], + "physics:diagonalInertia": [ + 1.345547673281544e-07, + 6.678773729618115e-07, + 6.741959168721223e-07 + ], + "physics:mass": 0.004509213380515575, + "physics:principalAxes": "(0.5117973, 0.51168644, 0.48817277, 0.48777848)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal/index_middle", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.00020699194283224642, + 2.6061707103508525e-06, + 0.01670653373003006 + ], + "physics:diagonalInertia": [ + 9.467015615882701e-08, + 1.767613326819628e-07, + 2.3114837688353873e-07 + ], + "physics:mass": 0.002378274919465184, + "physics:principalAxes": "(0.7445136, -0.00005444173, 0.6676073, 0.000047438436)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal/index_middle/index_distal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.008162464946508408, + 2.270589175168425e-05, + 0.00811806507408619 + ], + "physics:diagonalInertia": [ + 8.608198243109655e-08, + 1.7809432506510348e-07, + 2.076314586929584e-07 + ], + "physics:mass": 0.003341702511534095, + "physics:principalAxes": "(0.90243715, -0.0029940915, 0.43080872, -0.0014403827)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.007942995987832546, + -2.9300665005393967e-07, + 0.0022010633256286383 + ], + "physics:diagonalInertia": [ + 2.8543560404159507e-08, + 3.173513363208258e-08, + 4.6703082290378006e-08 + ], + "physics:mass": 0.0008955169469118118, + "physics:principalAxes": "(0.1595545, 0.6888883, 0.6888567, -0.15953599)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.0011418547946959734, + 3.473157630651258e-05, + 0.022892840206623077 + ], + "physics:diagonalInertia": [ + 1.345547673281544e-07, + 6.678773729618115e-07, + 6.741959168721223e-07 + ], + "physics:mass": 0.004509213380515575, + "physics:principalAxes": "(0.5117973, 0.51168644, 0.48817277, 0.48777848)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal/middle_middle", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.00020699189917650074, + 2.6062498363899067e-06, + 0.01670653373003006 + ], + "physics:diagonalInertia": [ + 9.467015615882701e-08, + 1.767613326819628e-07, + 2.3114837688353873e-07 + ], + "physics:mass": 0.002378274919465184, + "physics:principalAxes": "(0.7445136, -0.000054456927, 0.6676073, 0.00004744315)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal/middle_middle/middle_distal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.008162465877830982, + 2.2705855371896178e-05, + 0.008118066005408764 + ], + "physics:diagonalInertia": [ + 8.60819895365239e-08, + 1.780943534868129e-07, + 2.0763148711466783e-07 + ], + "physics:mass": 0.003341702977195382, + "physics:principalAxes": "(0.90243715, -0.0029941155, 0.4308087, -0.0014403895)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.007942995987832546, + -2.9451308591887937e-07, + 0.002201061462983489 + ], + "physics:diagonalInertia": [ + 2.854355152237531e-08, + 3.173512297394154e-08, + 4.670307518495065e-08 + ], + "physics:mass": 0.0008955165976658463, + "physics:principalAxes": "(0.15955459, 0.6888881, 0.68885696, -0.1595357)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.0011418547946959734, + 3.4731579944491386e-05, + 0.022892840206623077 + ], + "physics:diagonalInertia": [ + 1.345547673281544e-07, + 6.678773729618115e-07, + 6.741959168721223e-07 + ], + "physics:mass": 0.004509213380515575, + "physics:principalAxes": "(0.5117973, 0.51168644, 0.48817277, 0.48777848)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal/ring_middle", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.0002069932088488713, + 2.6074692414113088e-06, + 0.01670653186738491 + ], + "physics:diagonalInertia": [ + 9.467012063169022e-08, + 1.767613184711081e-07, + 2.3114836267268402e-07 + ], + "physics:mass": 0.0023782742209732533, + "physics:principalAxes": "(0.74451363, -0.000054586842, 0.6676073, 0.000047385933)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal/ring_middle/ring_distal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.008162465877830982, + 2.2705731680616736e-05, + 0.008118066005408764 + ], + "physics:diagonalInertia": [ + 8.60819895365239e-08, + 1.780943392759582e-07, + 2.0763147290381312e-07 + ], + "physics:mass": 0.0033417027443647385, + "physics:principalAxes": "(0.90243715, -0.0029941578, 0.43080872, -0.0014404262)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.007942995987832546, + -2.930068490059057e-07, + 0.0022010633256286383 + ], + "physics:diagonalInertia": [ + 2.8543560404159507e-08, + 3.173513363208258e-08, + 4.6703082290378006e-08 + ], + "physics:mass": 0.0008955169469118118, + "physics:principalAxes": "(0.1595545, 0.6888883, 0.6888567, -0.15953599)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.0011418547946959734, + 3.4731579944491386e-05, + 0.022892840206623077 + ], + "physics:diagonalInertia": [ + 1.345547673281544e-07, + 6.678773729618115e-07, + 6.741959168721223e-07 + ], + "physics:mass": 0.004509213380515575, + "physics:principalAxes": "(0.5117973, 0.51168644, 0.48817277, 0.48777848)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal/pinky_middle", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.00020699325250461698, + 2.607489932415774e-06, + 0.01670653186738491 + ], + "physics:diagonalInertia": [ + 9.467012063169022e-08, + 1.767613184711081e-07, + 2.3114836267268402e-07 + ], + "physics:mass": 0.0023782742209732533, + "physics:principalAxes": "(0.74451363, -0.0000545887, 0.6676073, 0.000047383306)" + } + }, + { + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal/pinky_middle/pinky_distal", + "reset_xform_stack": false, + "mass_properties": { + "physics:centerOfMass": [ + -0.008162465877830982, + 2.2705418814439327e-05, + 0.008118066005408764 + ], + "physics:diagonalInertia": [ + 8.608198243109655e-08, + 1.780943534868129e-07, + 2.0763147290381312e-07 + ], + "physics:mass": 0.0033417027443647385, + "physics:principalAxes": "(0.90243715, -0.002994122, 0.43080872, -0.001440418)" + } + } + ], + "joints": [ + { + "name": "index_dip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/index_dip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal/index_middle", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal/index_middle/index_distal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.5499999681585768, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "index_mcp_pitch", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/index_mcp_pitch", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.219999954319697, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "index_mcp_roll", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/index_mcp_roll", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals", + "axis": "X", + "lower_rad": -0.22999999602684235, + "upper_rad": 0.22999999602684235, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "index_pip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/index_pip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/index_metacarpals/index_proximal/index_middle", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.740000000336972, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "middle_dip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/middle_dip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal/middle_middle", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal/middle_middle/middle_distal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.5499999681585768, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "middle_mcp_pitch", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/middle_mcp_pitch", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.219999954319697, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "middle_mcp_roll", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/middle_mcp_roll", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals", + "axis": "X", + "lower_rad": -0.22999999602684235, + "upper_rad": 0.22999999602684235, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "middle_pip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/middle_pip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/middle_metacarpals/middle_proximal/middle_middle", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.740000000336972, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "pinky_dip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/pinky_dip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal/pinky_middle", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal/pinky_middle/pinky_distal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.5499999681585768, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "pinky_mcp_pitch", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/pinky_mcp_pitch", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.219999954319697, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "pinky_mcp_roll", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/pinky_mcp_roll", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals", + "axis": "X", + "lower_rad": -0.22999999602684235, + "upper_rad": 0.22999999602684235, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "pinky_pip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/pinky_pip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/pinky_metacarpals/pinky_proximal/pinky_middle", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.740000000336972, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "ring_dip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/ring_dip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal/ring_middle", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal/ring_middle/ring_distal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.5499999681585768, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "ring_mcp_pitch", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/ring_mcp_pitch", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.219999954319697, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "ring_mcp_roll", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/ring_mcp_roll", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals", + "axis": "X", + "lower_rad": -0.22999999602684235, + "upper_rad": 0.22999999602684235, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "ring_pip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/ring_pip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/ring_metacarpals/ring_proximal/ring_middle", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.740000000336972, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.01745329238474369, + "drive:angular:physics:maxForce": 100.0, + "drive:angular:physics:stiffness": 1.7453292608261108, + "drive:angular:physics:type": "force", + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "thumb_cmc_pitch", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_cmc_pitch", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 0.8400000231209613, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.0003490658418741077, + "drive:angular:physics:maxForce": 1.0, + "drive:angular:physics:stiffness": 0.01745329238474369, + "drive:angular:physics:type": "force", + "physxJoint:armature": 9.999999747378752e-05, + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "thumb_cmc_roll", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_cmc_roll", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1", + "axis": "X", + "lower_rad": 0.0, + "upper_rad": 1.3999999497628992, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.0003490658418741077, + "drive:angular:physics:maxForce": 1.0, + "drive:angular:physics:stiffness": 0.01745329238474369, + "drive:angular:physics:targetPosition": 0.0, + "drive:angular:physics:type": "force", + "physxJoint:armature": 9.999999747378752e-05, + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "thumb_cmc_yaw", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_cmc_yaw", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2", + "axis": "Z", + "lower_rad": 0.0, + "upper_rad": 1.570000041628963, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.0003490658418741077, + "drive:angular:physics:maxForce": 1.0, + "drive:angular:physics:stiffness": 0.01745329238474369, + "drive:angular:physics:type": "force", + "physxJoint:armature": 9.999999747378752e-05, + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "thumb_ip", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_ip", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals/thumb_proximal", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals/thumb_proximal/thumb_distal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.2899999451499395, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.0003490658418741077, + "drive:angular:physics:maxForce": 1.0, + "drive:angular:physics:stiffness": 0.01745329238474369, + "drive:angular:physics:type": "force", + "physxJoint:armature": 9.999999747378752e-05, + "physxJoint:maxJointVelocity": 57.295780181884766 + } + }, + { + "name": "thumb_mcp", + "path": "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_mcp", + "body0": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals", + "body1": "/tn__linkerhand_g20_lefturdf_cZ0/Geometry/hand_base_link/thumb_metacarpals_base1/thumb_metacarpals_base2/thumb_metacarpals/thumb_proximal", + "axis": "Y", + "lower_rad": 0.0, + "upper_rad": 1.2599999681024148, + "authored_drive_properties_usd_units": { + "drive:angular:physics:damping": 0.0003490658418741077, + "drive:angular:physics:maxForce": 1.0, + "drive:angular:physics:stiffness": 0.01745329238474369, + "drive:angular:physics:type": "force", + "physxJoint:armature": 9.999999747378752e-05, + "physxJoint:maxJointVelocity": 57.295780181884766 + } + } + ], + "source_urdf": { + "status": "STRUCTURAL_MATCH_ONLY", + "file_name": "linkerhand_g20_left.urdf", + "sha256": "b8ef22e436ab311fb61f87091b72ae717821092471d628d0ea9475b5088daa5d", + "mimic": [ + { + "joint": "index_dip", + "reference": "index_pip", + "multiplier": 0.89, + "offset_rad": 0.0 + }, + { + "joint": "middle_dip", + "reference": "middle_pip", + "multiplier": 0.89, + "offset_rad": 0.0 + }, + { + "joint": "pinky_dip", + "reference": "pinky_pip", + "multiplier": 0.89, + "offset_rad": 0.0 + }, + { + "joint": "ring_dip", + "reference": "ring_pip", + "multiplier": 0.89, + "offset_rad": 0.0 + }, + { + "joint": "thumb_ip", + "reference": "thumb_mcp", + "multiplier": 1.02, + "offset_rad": 0.0 + } + ] + }, + "coupling_evidence": [ + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_ip.newton:mimicCoef1", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_ip.newton:mimicJoint", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_ip: SdfTokenListOp(Explicit Items: [PhysxJointAPI, NewtonMimicAPI, PhysicsDriveAPI:angular, PhysicsJointStateAPI:angular, IsaacJointAPI])", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/index_dip.newton:mimicCoef1", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/index_dip.newton:mimicJoint", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/index_dip: SdfTokenListOp(Explicit Items: [PhysxJointAPI, NewtonMimicAPI, PhysicsDriveAPI:angular, PhysicsJointStateAPI:angular, IsaacJointAPI])", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/middle_dip.newton:mimicCoef1", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/middle_dip.newton:mimicJoint", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/middle_dip: SdfTokenListOp(Explicit Items: [PhysxJointAPI, NewtonMimicAPI, PhysicsDriveAPI:angular, PhysicsJointStateAPI:angular, IsaacJointAPI])", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/ring_dip.newton:mimicCoef1", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/ring_dip.newton:mimicJoint", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/ring_dip: SdfTokenListOp(Explicit Items: [PhysxJointAPI, NewtonMimicAPI, PhysicsDriveAPI:angular, PhysicsJointStateAPI:angular, IsaacJointAPI])", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/pinky_dip.newton:mimicCoef1", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/pinky_dip.newton:mimicJoint", + "/tn__linkerhand_g20_lefturdf_cZ0/Physics/pinky_dip: SdfTokenListOp(Explicit Items: [PhysxJointAPI, NewtonMimicAPI, PhysicsDriveAPI:angular, PhysicsJointStateAPI:angular, IsaacJointAPI])" + ], + "physx_coupling_evidence": [], + "coupling_status": "MIMIC_SCHEMA_PRESENT_RUNTIME_UNVERIFIED", + "dynamic_replay_ready": false, + "blockers": [ + "Runtime mimic response is unverified; original follower drives need suppression for diagnostics.", + "No calibrated wrist support controller or whole-hand dynamic replay validation." + ] +} diff --git a/scripts/tracking/inspect_l20_scene.py b/scripts/tracking/inspect_l20_scene.py new file mode 100644 index 0000000..fcceb82 --- /dev/null +++ b/scripts/tracking/inspect_l20_scene.py @@ -0,0 +1,91 @@ +"""Isaac Lab scene-loading diagnostic ONLY: no physics steps, actuators, or replay. + +Run headless with an external timeout. This legacy topology diagnostic deliberately +performs no dynamic experiment (see track_l20.py for the experimental path). Create a disposable +floating overlay first; this script never edits/saves USD layers or existing scenes. +""" + +import argparse +import json +import random +import traceback +from pathlib import Path + + +def main(): + from isaaclab.app import AppLauncher + + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("asset", type=Path, help="Diagnostic floating overlay, not the fixed preview") + AppLauncher.add_app_launcher_args(parser) + parser.set_defaults(headless=True) + args = parser.parse_args() + asset = args.asset.resolve(strict=True) + + # USD inspection imports pxr: defer it until Kit has initialized its libraries. + launcher = AppLauncher(args) + app = launcher.app + exit_code = 0 + try: + import numpy as np + import torch + from dex_workbench_tracking.asset import inspect + + manifest = inspect(asset) + if manifest["world_fixed_joints"] or manifest["articulation_roots"] != [manifest["root_body_path"]]: + parser.error("Expected a floating diagnostic overlay; use dex_workbench_tracking.asset first") + + from pxr import UsdGeom, UsdPhysics + + import isaaclab.sim as sim_utils + + random.seed(42) + np.random.seed(42) + torch.manual_seed(42) + sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=1 / 240, device=args.device)) + spawn = sim_utils.UsdFileCfg(usd_path=str(asset)) + spawn.func("/World/Hand", spawn) + stage = sim.stage + roots, joint_names = [], [] + for prim in stage.Traverse(): + if not str(prim.GetPath()).startswith("/World/Hand/"): + continue + if prim.HasAPI(UsdPhysics.ArticulationRootAPI): + roots.append(str(prim.GetPath())) + if prim.IsA(UsdPhysics.RevoluteJoint): + joint_names.append(prim.GetName()) + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + matrix = UsdGeom.XformCache().GetLocalToWorldTransform(prim) + if not np.isfinite(np.asarray(matrix)).all(): + raise AssertionError(f"Nonfinite initial body transform: {prim.GetPath()}") + expected = "/World/Hand" + manifest["root_body_path"][len(manifest["default_prim"]) :] + assert roots == [expected], (roots, expected) + assert sorted(joint_names) == [joint["name"] for joint in manifest["joints"]] + assert not stage.GetCompositionErrors(), stage.GetCompositionErrors() + print( + json.dumps( + { + "status": "PASS", + "check": "Isaac_Lab_scene_loading_only", + "seed": 42, + "num_envs": 1, + "physics_steps": 0, + "root": expected, + "joints": len(joint_names), + "dynamic_replay": "BLOCKED", + "reset_and_tracking_metrics": "NOT_RUN", + } + ), + flush=True, + ) + except BaseException: + exit_code = 1 + traceback.print_exc() + raise + finally: + # No SimulationContext.reset()/step(): even passive dynamics are unvalidated. + app.close(exit_code=exit_code) + + +if __name__ == "__main__": + main() diff --git a/scripts/tracking/track_l20.py b/scripts/tracking/track_l20.py new file mode 100644 index 0000000..363638e --- /dev/null +++ b/scripts/tracking/track_l20.py @@ -0,0 +1,300 @@ +"""Bounded experimental L20 dynamic diagnostic, NOT a training or hardware entry. + +Requires inspected PhysX 110.1.13 + current Isaac Lab ProxyArray/xyzw API. Runtime +coupling is tested from passive follower response, not inferred from schema names. +Use an external timeout <=300s. Do not launch alongside another Kit/GPU job. +""" + +import argparse +import hashlib +import json +import random +import traceback +from pathlib import Path + + +def main(): + from isaaclab.app import AppLauncher + + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("asset", type=Path, help="Experimental prepared overlay, never original fixed preview") + parser.add_argument("--manifest", type=Path, required=True, help="Original data/source manifest") + parser.add_argument("--hdf5", type=Path, help="Omit for explicitly synthetic diagnostic reference") + parser.add_argument("--episode", default="demo_000000") + parser.add_argument("--steps", type=int, default=480, help="Per repetition at 240Hz; two repetitions") + parser.add_argument("--execute-experimental", action="store_true", help="Acknowledge uncalibrated controller") + parser.add_argument("--limits", type=Path, help="JSON fields of control.Limits, SI units; UNCALIBRATED") + AppLauncher.add_app_launcher_args(parser) + parser.set_defaults(headless=True, visualizer="none") + args = parser.parse_args() + if not args.execute_experimental or not 100 <= args.steps <= 1200: + parser.error("Require --execute-experimental and 100 <= steps <= 1200 per repetition") + + # Kit must choose/register its USD libraries before any pxr-dependent imports. + launcher = AppLauncher(args) + app = launcher.app + exit_code = 0 + try: + import numpy as np + import torch + from dex_workbench_tracking.cli import synthetic + from dex_workbench_tracking.control import ( + Limits, + reference_pose_to_xyzw, + rotation_error, + validate_reference, + wrench, + xyzw_pose_to_reference, + ) + from dex_workbench_tracking.prepared import inspect_prepared, require_backend, validate_mimic + from dex_workbench_tracking.trajectory import load, require, sample + + manifest = json.loads(args.manifest.read_text()) + prepared = inspect_prepared(args.asset, manifest) + limits = Limits(**json.loads(args.limits.read_text())) if args.limits else Limits() + data = load(args.hdf5, manifest) if args.hdf5 else synthetic(manifest) + episode = data.episodes[args.episode] + validate_reference(episode, limits) + dt = 1 / 240 + require(args.steps * dt <= episode.time[-1], "Reference shorter than requested steps") + reference = sample(episode, np.arange(args.steps + 1) * dt) + # Hold-start reset uses zero velocities; require a genuinely moving diagnostic + # for every mimic pair, so an ignored/disabled constraint cannot pass at zero. + names = list(data.joint_names) + for eq in prepared["mimic"]: + require( + np.ptp(reference.joint_position[:, names.index(eq["reference"])]) > 0.002, + "Each mimic leader must move >0.002rad", + ) + require( + np.max(np.linalg.norm(reference.wrist_position - reference.wrist_position[0], axis=1)) > 0.001, + "Wrist reference must translate >1mm", + ) + require( + max(np.linalg.norm(rotation_error(q, reference.wrist_quaternion[0])) for q in reference.wrist_quaternion) + > 0.005, + "Wrist reference must rotate >0.005rad", + ) + from isaaclab_physx.physics import PhysxCfg + + import omni.kit.app + from pxr import Usd, UsdPhysics + + import isaaclab.sim as sim_utils + from isaaclab.actuators import ImplicitActuatorCfg + from isaaclab.assets import Articulation, ArticulationCfg + + manager = omni.kit.app.get_app().get_extension_manager() + extension = manager.get_enabled_extension_id("omni.physx") + require(extension is not None, "PhysX extension not enabled") + version = manager.get_extension_dict(extension)["package"]["version"] + require_backend(version, bool(Usd.SchemaRegistry().FindAppliedAPIPrimDefinition("NewtonMimicAPI"))) + random.seed(42) + np.random.seed(42) + torch.manual_seed(42) + sim = sim_utils.SimulationContext( + sim_utils.SimulationCfg(dt=dt, gravity=(0, 0, -9.81), device=args.device, physics=PhysxCfg()) + ) + cfg = ArticulationCfg( + prim_path="/World/Hand", + spawn=sim_utils.UsdFileCfg(usd_path=str(args.asset.resolve())), + init_state=ArticulationCfg.InitialStateCfg(pos=(0, 0, 0.4)), + actuators={ + "model_independent": ImplicitActuatorCfg( + joint_names_expr=prepared["independent_joint_names"], + stiffness=limits.finger_stiffness, + damping=limits.finger_damping, + effort_limit_sim=limits.finger_effort, + velocity_limit_sim=limits.finger_velocity, + ) + }, + ) + hand = Articulation(cfg) + # Validate remapped USD relationships after reference spawning, before physics. + remapped = dict(manifest) + remapped["joints"] = [ + dict(j, path="/World/Hand" + j["path"][len(manifest["default_prim"]) :]) for j in manifest["joints"] + ] + validate_mimic(sim.stage, remapped, passive=True) + anchor_path = "/World/Hand" + manifest["world_fixed_joints"][0]["path"][len(manifest["default_prim"]) :] + anchor = sim.stage.GetPrimAtPath(anchor_path) + require(anchor.IsValid() and not anchor.IsActive(), "Obsolete world anchor must be inactive") + active_joints = [p for p in sim.stage.Traverse() if p.IsA(UsdPhysics.Joint)] + require(len(active_joints) == len(names), "Unexpected active joint count") + require(all(p.IsA(UsdPhysics.RevoluteJoint) for p in active_joints), "Unexpected active constraint") + sim.reset() + require(not hand.is_fixed_base and hand.num_instances == 1, "Expected one floating PhysX articulation") + require( + set(hand.joint_names) == set(names) and len(hand.joint_names) == len(names), "Runtime DOF mapping mismatch" + ) + require(hand.num_bodies == len(manifest["bodies"]), "Runtime body count mismatch") + require(hand.body_names[0] == manifest["root_link"], "Unexpected root body ordering") + runtime_ids = [hand.joint_names.index(n) for n in names] + master_ids = [hand.joint_names.index(n) for n in prepared["independent_joint_names"]] + master_columns = [names.index(n) for n in prepared["independent_joint_names"]] + follower_ids = [hand.joint_names.index(eq["joint"]) for eq in prepared["mimic"]] + + def array(proxy): + return proxy.torch.detach().cpu().numpy().copy() + + def tensor(values): + return torch.as_tensor(values, dtype=torch.float32, device=hand.device) + + def state(): + pose = array(hand.data.root_link_pose_w)[0] + # Installed Lab uses xyzw; HDF5/controller use wxyz. Explicit boundary. + pose = xyzw_pose_to_reference(pose) + velocity = array(hand.data.root_link_vel_w)[0] + q = array(hand.data.joint_pos)[0, runtime_ids] + return pose, velocity, q + + for gains in (hand.data.joint_stiffness, hand.data.joint_damping): + require((array(gains)[0, follower_ids] == 0).all(), "Runtime follower gains are nonzero") + masses = array(hand.data.body_mass)[0] + require(np.isfinite(masses).all() and (masses > 0).all(), "Invalid runtime mass") + require(masses.sum() * 9.81 < limits.force * 0.8, "Insufficient bounded gravity support headroom") + lower = np.array([j["lower_rad"] for j in manifest["joints"]]) + upper = np.array([j["upper_rad"] for j in manifest["joints"]]) + traces, reset_states, summaries = [], [], [] + for repetition in range(2): + hand.reset() + hand.permanent_wrench_composer.reset() + hand.instantaneous_wrench_composer.reset() + pose0 = reference_pose_to_xyzw(reference.wrist_position[0], reference.wrist_quaternion[0]) + hand.write_root_link_pose_to_sim_index(root_pose=tensor(pose0[None])) + hand.write_root_link_velocity_to_sim_index(root_velocity=tensor(np.zeros((1, 6)))) + hand.write_joint_position_to_sim_index( + position=tensor(reference.joint_position[0:1]), joint_ids=runtime_ids + ) + hand.write_joint_velocity_to_sim_index(velocity=tensor(np.zeros((1, len(names)))), joint_ids=runtime_ids) + hand.set_joint_position_target_index( + target=tensor(reference.joint_position[0:1, master_columns]), joint_ids=master_ids + ) + hand.set_joint_velocity_target_index(target=tensor(np.zeros((1, len(master_ids)))), joint_ids=master_ids) + hand.update(dt) + reset_pose, reset_vel, reset_q = state() + np.testing.assert_allclose(reset_pose[:3], reference.wrist_position[0], atol=1e-6) + require( + np.linalg.norm(rotation_error(reference.wrist_quaternion[0], reset_pose[3:])) < 1e-6, + "Reset orientation mismatch", + ) + np.testing.assert_allclose(reset_q, reference.joint_position[0], atol=1e-6) + np.testing.assert_allclose(reset_vel, 0, atol=1e-6) + np.testing.assert_allclose(array(hand.data.joint_vel), 0, atol=1e-6) + reset_states.append(np.r_[reset_pose, reset_vel, reset_q]) + trace, errors = [], [] + for step in range(args.steps): + require(app.is_running(), "Application stopped before finite test completed") + pose, velocity, q = state() + force, torque = wrench( + reference.wrist_position[step], + reference.wrist_quaternion[step], + pose, + velocity, + array(hand.data.root_com_pose_w)[0, :3], + array(hand.data.body_com_pose_w)[0, :, :3], + masses, + limits, + ) + hand.permanent_wrench_composer.set_forces_and_torques_index( + forces=tensor(force[None, None]), + torques=tensor(torque[None, None]), + body_ids=torch.tensor([0], dtype=torch.int32, device=hand.device), + is_global=True, + ) + hand.set_joint_position_target_index( + target=tensor(reference.joint_position[step : step + 1, master_columns]), joint_ids=master_ids + ) + hand.write_data_to_sim() + sim.step(render=False) + hand.update(dt) + pose, velocity, q = state() + qdot = array(hand.data.joint_vel)[0] + require(all(np.isfinite(v).all() for v in (pose, velocity, q, qdot)), "Nonfinite dynamic state") + require( + np.linalg.norm(velocity[:3]) < 0.5 + and np.linalg.norm(velocity[3:]) < 3 + and np.max(np.abs(qdot)) < 2, + "Measured velocity safety bound exceeded", + ) + require((q >= lower - 0.01).all() and (q <= upper + 0.01).all(), "Joint limit violation >0.01rad") + residual = max( + abs( + q[names.index(eq["joint"])] + - eq["multiplier"] * q[names.index(eq["reference"])] + - eq["offset_rad"] + ) + for eq in prepared["mimic"] + ) + position_error = np.linalg.norm(pose[:3] - reference.wrist_position[step + 1]) + angle_error = np.linalg.norm(rotation_error(reference.wrist_quaternion[step + 1], pose[3:])) + joint_error = np.max(np.abs(q - reference.joint_position[step + 1])) + require(residual < 0.002, "Mimic runtime residual >0.002rad; parser/constraint not verified") + require( + position_error < 0.05 and angle_error < 0.5 and joint_error < 0.2, + "Tracking safety envelope exceeded", + ) + trace.append(np.r_[pose, q, velocity, qdot]) + errors.append([position_error, angle_error, joint_error, residual]) + trace, errors = np.array(trace), np.array(errors) + for eq in prepared["mimic"]: + for name in (eq["joint"], eq["reference"]): + require(np.ptp(trace[:, 7 + names.index(name)]) > 0.002, f"No nontrivial runtime motion: {name}") + require( + np.max(np.linalg.norm(trace[:, :3] - reset_pose[:3], axis=1)) > 0.0005, "No nontrivial root translation" + ) + require( + max(np.linalg.norm(rotation_error(p[3:7], reset_pose[3:])) for p in trace) > 0.002, + "No nontrivial root rotation", + ) + traces.append(trace) + summaries.append( + { + "repetition": repetition, + "max_errors_m_rad_rad_rad": errors.max(axis=0).tolist(), + "rms_errors_m_rad_rad_rad": np.sqrt((errors**2).mean(axis=0)).tolist(), + } + ) + np.testing.assert_allclose(reset_states[0], reset_states[1], atol=1e-6, rtol=0) + position_end = 7 + len(names) + np.testing.assert_allclose(traces[0][:, :position_end], traces[1][:, :position_end], atol=1e-3, rtol=0) + # Root linear/angular and joint velocities: 1e-3 m/s or rad/s absolute. + np.testing.assert_allclose(traces[0][:, position_end:], traces[1][:, position_end:], atol=1e-3, rtol=0) + hand.permanent_wrench_composer.reset() + print( + json.dumps( + { + "status": "PASS", + "check": "bounded_experimental_dynamic_tracking", + "runtime_verified_for_this_run_only": True, + "provenance": data.metadata["provenance"], + "reference_source": "hdf5" if args.hdf5 else "analytic_in_memory", + "reference_hdf5_sha256": hashlib.sha256(args.hdf5.read_bytes()).hexdigest() if args.hdf5 else None, + "reference_description": data.metadata["source_description"], + "world_anchor_inactive": not anchor.IsActive(), + "active_state_joint_count": len(active_joints), + "runtime_is_fixed_base": hand.is_fixed_base, + "seed": 42, + "num_envs": 1, + "steps_per_repetition": args.steps, + "repetitions": 2, + "physx_version": version, + "prepared": prepared, + "limits_uncalibrated": vars(limits), + "metrics": summaries, + "hardware_and_training_validated": False, + } + ), + flush=True, + ) + except BaseException: + # Kit fast shutdown may not return; emit the failure before closing. + exit_code = 1 + traceback.print_exc() + raise + finally: + app.close(exit_code=exit_code) + + +if __name__ == "__main__": + main() diff --git a/source/dex_workbench/config/extension.toml b/source/dex_workbench/config/extension.toml index c591466..b1e1f52 100644 --- a/source/dex_workbench/config/extension.toml +++ b/source/dex_workbench/config/extension.toml @@ -1,7 +1,7 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "0.1.0" +version = "0.1.1" # Description category = "isaaclab" diff --git a/source/dex_workbench/dex_workbench_tracking/asset.py b/source/dex_workbench/dex_workbench_tracking/asset.py new file mode 100644 index 0000000..bd708d4 --- /dev/null +++ b/source/dex_workbench/dex_workbench_tracking/asset.py @@ -0,0 +1,338 @@ +"""Composed USD inspection and non-destructive, topology-only floating overlay. + +Requires pxr, not a running Kit application. No physical coupling is synthesized. +""" + +import hashlib +import json +import math +import os +import xml.etree.ElementTree as ET +from pathlib import Path + +from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics, UsdUtils + + +def _require(condition, message): + if not condition: + raise ValueError(message) + + +def _sha(path): + return hashlib.sha256(Path(path).read_bytes()).hexdigest() + + +def _plain(value): + if value is None or isinstance(value, (str, bool, int)): + return value + if isinstance(value, float): + return value if math.isfinite(value) else str(value) + try: + return [_plain(item) for item in value] + except TypeError: + return str(value) + + +def dependencies(source): + """Hash the local complete dependency bundle, including names relative to the entry. + + Sorted UTF-8 records are ``relative_posix_path + NUL + sha256 + LF``. + Renaming dependency files changes identity. Moving the entire bundle does not. + """ + source = Path(source).resolve(strict=True) + layers, assets, unresolved = UsdUtils.ComputeAllDependencies(str(source)) + _require(not unresolved, f"Unresolved dependencies: {unresolved}") + paths = {Path(layer.realPath).resolve(strict=True) for layer in layers} + paths.update(Path(asset).resolve(strict=True) for asset in assets) + paths.add(source) + entries = sorted((os.path.relpath(path, source.parent).replace(os.sep, "/"), _sha(path)) for path in paths) + digest = hashlib.sha256() + for name, sha in entries: + digest.update(f"{name}\0{sha}\n".encode()) + return [{"path": name, "sha256": sha} for name, sha in entries], digest.hexdigest() + + +def inspect(source, source_urdf=None): + source = Path(source).resolve(strict=True) + deps, bundle_sha = dependencies(source) + stage = Usd.Stage.Open(str(source)) + _require(stage and stage.GetDefaultPrim(), "USD must have a default prim") + _require(not stage.GetCompositionErrors(), f"Composition errors: {stage.GetCompositionErrors()}") + _require( + UsdGeom.GetStageUpAxis(stage) == "Z" + and UsdGeom.GetStageMetersPerUnit(stage) == 1 + and UsdPhysics.GetStageKilogramsPerUnit(stage) == 1, + "Expected Z-up, meter, kilogram asset", + ) + default = stage.GetDefaultPrim() + prims = list(Usd.PrimRange(default)) + bodies = [prim for prim in prims if prim.HasAPI(UsdPhysics.RigidBodyAPI)] + body_paths = {str(prim.GetPath()) for prim in bodies} + joints, world_joints, child_paths = [], [], set() + coupling_evidence = [] + for prim in prims: + for prop in prim.GetProperties(): + if any(term in prop.GetName().lower() for term in ("mimic", "tendon", "gearing", "coupling")): + coupling_evidence.append(str(prop.GetPath())) + # Raw schemas retain evidence even if a PhysX schema plugin is not loaded. + schemas = str(prim.GetMetadata("apiSchemas")) + if any(term in schemas.lower() for term in ("mimic", "tendon", "gearing", "coupling")): + coupling_evidence.append(f"{prim.GetPath()}: {schemas}") + if not prim.IsA(UsdPhysics.Joint): + continue + joint = UsdPhysics.Joint(prim) + if joint.GetJointEnabledAttr().Get() is False: + continue + b0 = [str(p) for p in joint.GetBody0Rel().GetTargets()] + b1 = [str(p) for p in joint.GetBody1Rel().GetTargets()] + _require(len(b1) == 1 and b1[0] in body_paths, f"Unresolved/non-body body1: {prim.GetPath()}") + if not b0: + _require(prim.IsA(UsdPhysics.FixedJoint), "Unsupported non-fixed world joint") + world_joints.append({"path": str(prim.GetPath()), "body1": b1[0]}) + continue + _require(len(b0) == 1 and b0[0] in body_paths, f"Unresolved/non-body body0: {prim.GetPath()}") + _require(prim.IsA(UsdPhysics.RevoluteJoint), f"Unsupported joint type {prim.GetTypeName()}") + child_paths.add(b1[0]) + revolute = UsdPhysics.RevoluteJoint(prim) + lower, upper = revolute.GetLowerLimitAttr().Get(), revolute.GetUpperLimitAttr().Get() + _require( + lower is not None + and upper is not None + and math.isfinite(lower) + and math.isfinite(upper) + and lower <= upper, + f"Missing/nonfinite limits: {prim.GetPath()}", + ) + joints.append( + { + "name": prim.GetName(), + "path": str(prim.GetPath()), + "body0": b0[0], + "body1": b1[0], + "axis": revolute.GetAxisAttr().Get(), + "lower_rad": math.radians(lower), + "upper_rad": math.radians(upper), + "authored_drive_properties_usd_units": { + attr.GetName(): _plain(attr.Get()) + for attr in prim.GetAttributes() + if attr.GetName().startswith(("drive:", "physxJoint:")) and attr.HasAuthoredValue() + }, + } + ) + _require(bodies and joints, "No articulated hand found") + roots = sorted(body_paths - child_paths) + _require(len(roots) == 1, f"Expected one body-tree root, got {roots}") + _require(len(joints) == len(bodies) - 1 and len(child_paths) == len(joints), "Not a simple articulated tree") + reachable = {roots[0]} + while True: + expanded = reachable | {joint["body1"] for joint in joints if joint["body0"] in reachable} + if expanded == reachable: + break + reachable = expanded + _require(reachable == body_paths, "Disconnected/cyclic body graph") + joints.sort(key=lambda joint: joint["name"]) + names = [joint["name"] for joint in joints] + _require(len(set(names)) == len(names), "Duplicate joint names") + articulation_roots = [str(p.GetPath()) for p in prims if p.HasAPI(UsdPhysics.ArticulationRootAPI)] + _require(len(articulation_roots) == 1, f"Expected one articulation root, got {articulation_roots}") + _require(len(world_joints) <= 1 and all(j["body1"] == roots[0] for j in world_joints), "Ambiguous world anchor") + source_info = {"status": "NOT_PROVIDED", "mimic": []} + if source_urdf is not None: + path = Path(source_urdf).resolve(strict=True) + xml = ET.parse(path).getroot() + urdf_joints = {j.attrib["name"]: j for j in xml.findall("joint") if j.attrib["type"] != "fixed"} + _require(set(urdf_joints) == set(names), "Source URDF/USD joint-name mismatch") + mimic = [] + for joint in joints: + node = urdf_joints[joint["name"]] + limit = node.find("limit") + _require(node.attrib["type"] == "revolute" and limit is not None, "Unsupported source URDF joint") + for key in ("lower", "upper"): + _require( + abs(float(limit.attrib[key]) - joint[f"{key}_rad"]) < 1e-5, + f"Source URDF/USD limit mismatch: {joint['name']}", + ) + parent, child = node.find("parent"), node.find("child") + _require( + parent is not None + and child is not None + and parent.attrib["link"] == joint["body0"].split("/")[-1] + and child.attrib["link"] == joint["body1"].split("/")[-1], + "Source URDF/USD link mismatch", + ) + equation = node.find("mimic") + if equation is not None: + _require(equation.attrib["joint"] in names, "Unresolved source mimic reference") + multiplier = float(equation.attrib.get("multiplier", "1")) + offset = float(equation.attrib.get("offset", "0")) + _require(math.isfinite(multiplier) and math.isfinite(offset), "Nonfinite mimic equation") + mimic.append( + { + "joint": joint["name"], + "reference": equation.attrib["joint"], + "multiplier": multiplier, + "offset_rad": offset, + } + ) + source_info = {"status": "STRUCTURAL_MATCH_ONLY", "file_name": path.name, "sha256": _sha(path), "mimic": mimic} + coupling_status = "UNVERIFIED" + physx_coupling = [ + item + for item in coupling_evidence + if "physxmimic" in item.lower() or "physxtendon" in item.lower() or ".physxmimic" in item.lower() + ] + newton_coupling = [item for item in coupling_evidence if "newtonmimic" in item.lower()] + if newton_coupling or physx_coupling: + # PhysX 110.1.13 parses NewtonMimicAPI natively. Prefix is not a backend gate. + coupling_status = "MIMIC_SCHEMA_PRESENT_RUNTIME_UNVERIFIED" + elif source_info["mimic"]: + coupling_status = "MISSING_MIMIC_SCHEMA" + return { + "manifest_version": "l20_asset_manifest_v1", + "asset_sha256": bundle_sha, + "entry_file": source.name, + "dependencies": deps, + "composition_errors": [], + "unresolved_dependencies": [], + "default_prim": str(default.GetPath()), + "root_link": roots[0].split("/")[-1], + "root_body_path": roots[0], + "units": {"length": "m", "mass": "kg", "up_axis": "Z"}, + "physics_variant": default.GetVariantSet("Physics").GetVariantSelection(), + "articulation_roots": articulation_roots, + "world_fixed_joints": world_joints, + "bodies": [ + { + "path": str(p.GetPath()), + "reset_xform_stack": UsdGeom.Xformable(p).GetResetXformStack(), + "mass_properties": { + a.GetName(): _plain(a.Get()) + for a in p.GetAttributes() + if a.GetName() + in ("physics:mass", "physics:centerOfMass", "physics:diagonalInertia", "physics:principalAxes") + }, + } + for p in bodies + ], + "joints": joints, + "source_urdf": source_info, + "coupling_evidence": coupling_evidence, + "physx_coupling_evidence": physx_coupling, + "coupling_status": coupling_status, + "dynamic_replay_ready": False, + "blockers": [ + "Runtime mimic response is unverified; original follower drives need suppression for diagnostics.", + "No calibrated wrist support controller or whole-hand dynamic replay validation.", + ], + } + + +def prepare_floating_overlay(source, output): + """Release only the world anchor; preserve transforms and source layer bytes. + + This is NOT a usable control asset: missing coupling remains missing. Output + is explicitly marked diagnostic and must never overwrite an existing file. + """ + source, output = Path(source).resolve(strict=True), Path(output).resolve() + _require(not output.exists(), "Refusing to overwrite output") + before = inspect(source) + _require(before["physics_variant"] == "physx", "Only the current PhysX variant is supported") + anchors = before["world_fixed_joints"] + _require( + len(anchors) == 1 and before["articulation_roots"] == [anchors[0]["path"]], + "Expected a fixed-world articulation root; will not guess topology", + ) + original = Usd.Stage.Open(str(source)) + stage = Usd.Stage.CreateInMemory() + stage.GetRootLayer().subLayerPaths = [str(source)] + stage.SetDefaultPrim(stage.GetPrimAtPath(before["default_prim"])) + UsdGeom.SetStageUpAxis(stage, "Z") + UsdGeom.SetStageMetersPerUnit(stage, 1) + UsdPhysics.SetStageKilogramsPerUnit(stage, 1) + anchor = stage.GetPrimAtPath(anchors[0]["path"]) + # Snapshot resolved schema fallbacks before removing the API. Preserve them + # explicitly on the inert anchor and the new root, not just authored values. + articulation_values = [] + for attr in anchor.GetAttributes(): + if attr.GetName().startswith("physxArticulation:") and attr.Get() is not None: + _require(not attr.GetTimeSamples(), f"Time samples prohibited: {attr.GetPath()}") + articulation_values.append((attr.GetName(), attr.GetTypeName(), attr.Get())) + anchor.RemoveAPI(UsdPhysics.ArticulationRootAPI) + anchor.RemoveAppliedSchema("PhysxArticulationAPI") + UsdPhysics.Joint(anchor).GetJointEnabledAttr().Set(False) + root = stage.GetPrimAtPath(before["root_body_path"]) + UsdPhysics.ArticulationRootAPI.Apply(root) + root.AddAppliedSchema("PhysxArticulationAPI") + for name, dtype, value in articulation_values: + anchor.CreateAttribute(name, dtype).Set(value) + root.CreateAttribute(name, dtype).Set(value) + stage.GetRootLayer().customLayerData = {"purpose": "L20 topology-only diagnostic; dynamic replay BLOCKED"} + cache0, cache1 = UsdGeom.XformCache(), UsdGeom.XformCache() + for prim in original.Traverse(): + after = stage.GetPrimAtPath(prim.GetPath()) + for attr in prim.GetAttributes(): + if str(prim.GetPath()) == anchors[0]["path"] and attr.GetName() == "physics:jointEnabled": + continue + _require( + attr.Get() == after.GetAttribute(attr.GetName()).Get(), f"Unexpected attribute edit: {attr.GetPath()}" + ) + for rel in prim.GetRelationships(): + _require( + rel.GetTargets() == after.GetRelationship(rel.GetName()).GetTargets(), "Unexpected relationship edit" + ) + if prim.IsA(UsdGeom.Xformable): + _require( + Gf.IsClose(cache0.GetLocalToWorldTransform(prim), cache1.GetLocalToWorldTransform(after), 1e-12), + f"Unexpected transform edit: {prim.GetPath()}", + ) + output.parent.mkdir(parents=True, exist_ok=True) + layer = Sdf.Layer.CreateAnonymous() + layer.TransferContent(stage.GetRootLayer()) + layer.subLayerPaths = [os.path.relpath(source, output.parent).replace(os.sep, "/")] + _require(layer.Export(str(output)), "USD export failed") + after = inspect(output) + _require( + not after["world_fixed_joints"] and after["articulation_roots"] == [before["root_body_path"]], + "Floating root topology check failed", + ) + _require(dependencies(source)[1] == before["asset_sha256"], "Source bundle unexpectedly changed") + return after + + +def main(): + import argparse + + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("asset", type=Path) + parser.add_argument("--source-urdf", type=Path, help="Optional local original; inspected, never modified") + parser.add_argument("--output", type=Path, required=True, help="New JSON manifest; no overwrite") + parser.add_argument("--floating-overlay", type=Path, help="New topology-only USD layer; no overwrite") + args = parser.parse_args() + try: + _require(not args.output.exists(), "Refusing to overwrite manifest") + manifest = inspect(args.asset, args.source_urdf) + if args.floating_overlay: + prepare_floating_overlay(args.asset, args.floating_overlay) + manifest = inspect(args.floating_overlay, args.source_urdf) + args.output.parent.mkdir(parents=True, exist_ok=True) + with args.output.open("x", encoding="utf-8") as stream: + json.dump(manifest, stream, indent=2, allow_nan=False) + stream.write("\n") + print( + json.dumps( + { + "status": "PASS", + "check": "static_asset_inspection_only", + "manifest": str(args.output), + "joints": len(manifest["joints"]), + "coupling_status": manifest["coupling_status"], + "dynamic_replay": "BLOCKED", + } + ) + ) + except (ValueError, OSError, RuntimeError, ET.ParseError) as error: + parser.exit(1, f"FAIL: {error}\n") + + +if __name__ == "__main__": + main() diff --git a/source/dex_workbench/dex_workbench_tracking/cli.py b/source/dex_workbench/dex_workbench_tracking/cli.py new file mode 100644 index 0000000..64c3ce7 --- /dev/null +++ b/source/dex_workbench/dex_workbench_tracking/cli.py @@ -0,0 +1,181 @@ +"""CPU preparation CLI: validate, synthesize test references, or resample HDF5.""" + +import argparse +import json +import os +import tempfile +from pathlib import Path + +import h5py +import numpy as np + +from .trajectory import ( + ContractError, + Demonstrations, + Episode, + load, + require, + require_dynamic_replay_ready, + sample, + validate_against_manifest, +) + + +def write(path, data): + """Never overwrite inputs, original demonstrations, or an earlier generated file.""" + path = Path(path) + with h5py.File(path, "x") as file: + for name, value in data.metadata.items(): + file.attrs[name] = value + meta = file.create_group("metadata") + meta.create_dataset("joint_names", data=data.joint_names, dtype=h5py.string_dtype("utf-8")) + meta.create_dataset("world_from_source", data=data.world_from_source, dtype="float64") + episodes = file.create_group("episodes") + for name, episode in data.episodes.items(): + group = episodes.create_group(name) + for field in ("time", "wrist_position", "wrist_quaternion", "joint_position", "valid"): + group.create_dataset(field, data=getattr(episode, field)) + + +def publish_validated(path, data, manifest): + """Validate privately, then publish without replacing any existing destination.""" + path = Path(path) + with tempfile.TemporaryDirectory(prefix=".l20-tracking-", dir=path.parent) as directory: + temporary = Path(directory) / "validated.hdf5" + write(temporary, data) + load(temporary, manifest) + # Same-filesystem hard link publishes atomically and refuses overwrites. + os.link(temporary, path) + + +def synthetic(manifest, profile="small"): + """Named analytic fixtures, not experts. ``range_4s`` has smooth out-and-back endpoints.""" + require(profile in ("small", "range_4s"), "Unknown synthetic profile") + larger = profile == "range_4s" + duration = 4 if larger else 2 + translation, yaw = (0.01, 0.1) if larger else (0.002, 0.01) + joints = manifest["joints"] + time = np.linspace(0, duration, duration * 60 + 1, dtype=np.float64) + phase = np.sin(np.pi * time / duration) ** 2 + position = np.zeros((len(time), 3), dtype=np.float32) + position[:, 0] = translation * phase + position[:, 2] = 0.4 + quaternion = np.zeros((len(time), 4), dtype=np.float32) + quaternion[:, 0] = np.cos(yaw / 2 * phase) + quaternion[:, 3] = np.sin(yaw / 2 * phase) + names = tuple(joint["name"] for joint in joints) + q = np.zeros((len(time), len(names)), dtype=np.float32) + for index, joint in enumerate(joints): + require(joint["lower_rad"] <= 0 <= joint["upper_rad"], "Synthetic fixture requires zero within limits") + amplitude = min(0.1, joint["upper_rad"] * 0.25) if larger else min(0.01, joint["upper_rad"] * 0.01) + q[:, index] = phase * amplitude + equations = {entry["joint"]: entry for entry in manifest.get("source_urdf", {}).get("mimic", [])} + resolved = set(names) - equations.keys() + while equations: + ready = [name for name, eq in equations.items() if eq["reference"] in resolved] + require(ready, "Cyclic/unresolved mimic graph") + for name in ready: + eq = equations.pop(name) + q[:, names.index(name)] = eq["multiplier"] * q[:, names.index(eq["reference"])] + eq["offset_rad"] + resolved.add(name) + metadata = { + "schema_version": "l20_tracking_v1", + "embodiment": "L20", + "hand_side": "left", + "asset_sha256": manifest["asset_sha256"], + "root_link": manifest["root_link"], + "metric_scale_provenance": "Analytic SI fixture, not video reconstruction or measurement", + "scale_to_meters": 1.0, + "provenance": "synthetic", + "source_description": ( + "Synthetic range_4s: 4 s sin^2 out-and-back, 10 mm translation, 0.1 rad yaw; " + "independent positive joint amplitude min(0.1 rad, 25% upper limit), followers obey source mimic; " + "60 Hz, analytic zero endpoint velocities; NOT expert data" + if larger + else "Deterministic analytic reference: 2 mm translation, 0.01 rad yaw, small joint motion; NOT expert data" + ), + } + data = Demonstrations( + metadata, + names, + np.eye(4, dtype=np.float64), + { + "demo_000000": Episode(time, position, quaternion, q, np.ones(len(time), dtype=bool)), + }, + ) + validate_against_manifest(data, manifest) + return data + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + commands = parser.add_subparsers(dest="command", required=True) + validate = commands.add_parser("validate") + validate.add_argument("input", type=Path) + validate.add_argument( + "--manifest", type=Path, help="Required to check asset identity/order/limits; otherwise schema only" + ) + generate = commands.add_parser("synthetic") + generate.add_argument("--manifest", required=True, type=Path) + generate.add_argument("--output", required=True, type=Path) + generate.add_argument("--profile", choices=("small", "range_4s"), default="small") + resample = commands.add_parser("resample") + resample.add_argument("input", type=Path) + resample.add_argument("--manifest", required=True, type=Path) + resample.add_argument("--hz", required=True, type=float) + resample.add_argument("--output", required=True, type=Path) + gate = commands.add_parser("replay-check", help="Fails closed until a physical tracking Adapter is implemented") + gate.add_argument("--manifest", required=True, type=Path) + args = parser.parse_args() + try: + manifest = json.loads(args.manifest.read_text(encoding="utf-8")) if args.manifest else None + if args.command == "replay-check": + try: + require_dynamic_replay_ready(manifest) + except ContractError as error: + parser.exit(2, f"{error}\n") + if args.command == "synthetic": + data = synthetic(manifest, args.profile) + publish_validated(args.output, data, manifest) + else: + data = load(args.input, manifest) + if args.command == "resample": + require(np.isfinite(args.hz) and 0 < args.hz <= 1000, "hz must be finite and in (0, 1000]") + episodes = {} + for name, episode in data.episodes.items(): + require( + episode.valid.all(), + f"{name}: CLI resampling requires a fully valid episode; segment explicitly first", + ) + count = int(np.floor(episode.time[-1] * args.hz)) + 1 + require(2 <= count <= 10_000_000, "Resampled frame count must be between 2 and 10 million") + query = np.arange(count, dtype=np.float64) / args.hz + # Endpoint is included only when on-grid; never append a shortened final interval. + query = query[query <= episode.time[-1]] + episodes[name] = sample(episode, query) + data = Demonstrations(dict(data.metadata), data.joint_names, data.world_from_source, episodes) + data.metadata["source_description"] += f"; resampled at {args.hz:g} Hz (off-grid end omitted)" + publish_validated(args.output, data, manifest) + print( + json.dumps( + { + "status": "PASS", + "command": args.command, + "schema": data.metadata["schema_version"], + "provenance": data.metadata["provenance"], + "asset_compatibility": "PASS" if manifest else "NOT_CHECKED", + "episodes": { + name: {"frames": len(ep.time), "valid_frames": int(ep.valid.sum())} + for name, ep in data.episodes.items() + }, + "joint_count": len(data.joint_names), + "dynamic_replay": "BLOCKED", + } + ) + ) + except (ContractError, OSError, KeyError, TypeError, ValueError) as error: + parser.exit(1, f"FAIL: {error}\n") + + +if __name__ == "__main__": + main() diff --git a/source/dex_workbench/dex_workbench_tracking/control.py b/source/dex_workbench/dex_workbench_tracking/control.py new file mode 100644 index 0000000..803a778 --- /dev/null +++ b/source/dex_workbench/dex_workbench_tracking/control.py @@ -0,0 +1,112 @@ +"""Single-hand experimental world-frame wrench PD. SI units; parameters UNCALIBRATED. + +The controller tracks a root *link* pose, applies one bounded wrench at root COM, +and compensates gravity for all links. No state writing, dynamics or hardware claims. +""" + +from dataclasses import dataclass + +import numpy as np + +from .trajectory import require + + +@dataclass(frozen=True) +class Limits: + position_gain: float = 100.0 # N/m + velocity_gain: float = 10.0 # N s/m + rotation_gain: float = 0.2 # Nm/rad + angular_velocity_gain: float = 0.02 # Nm s/rad + force: float = 20.0 # norm N, including gravity + torque: float = 1.0 # norm Nm, including gravity moment + reference_speed: float = 0.05 # m/s + reference_angular_speed: float = 0.5 # rad/s + reference_joint_speed: float = 0.5 # rad/s + workspace_radius: float = 0.1 # m from initial root link position + finger_stiffness: float = 3.0 # Nm/rad; model-independent joints only + finger_damping: float = 0.1 # Nm s/rad + finger_effort: float = 0.2 # Nm + finger_velocity: float = 0.5 # rad/s + + def __post_init__(self): + require(all(np.isfinite(v) and v > 0 for v in vars(self).values()), "Controller limits must be finite positive") + + +def reference_pose_to_xyzw(position, quaternion): + """HDF5 world link position + wxyz -> installed Isaac Lab world link pose.""" + position, quaternion = np.asarray(position), np.asarray(quaternion) + require(position.shape == (3,) and quaternion.shape == (4,), "Expected position[3], quaternion[4]") + return np.r_[position, quaternion[1:], quaternion[0]] + + +def xyzw_pose_to_reference(pose): + """Installed Isaac Lab world link pose -> controller position + wxyz.""" + pose = np.asarray(pose) + require(pose.shape == (7,), "Expected runtime pose[7]") + return np.r_[pose[:3], pose[6], pose[3:6]] + + +def rotation_error(target, current): + """Shortest world-frame rotation vector taking current to target, both wxyz.""" + target, current = np.asarray(target, dtype=float), np.asarray(current, dtype=float) + require(target.shape == current.shape == (4,), "quaternion[4] required") + for q in (target, current): + require(np.isfinite(q).all() and abs(np.linalg.norm(q) - 1) < 1e-4, "unit quaternion required") + w0, v0 = target[0], target[1:] + w1, v1 = current[0], -current[1:] + error = np.r_[w0 * w1 - np.dot(v0, v1), w0 * v1 + w1 * v0 + np.cross(v0, v1)] + if error[0] < 0: + error = -error + norm = np.linalg.norm(error[1:]) + return error[1:] * (2 * np.arctan2(norm, max(0, error[0])) / norm if norm > 1e-12 else 2) + + +def bounded(vector, maximum): + return vector * min(1.0, maximum / max(np.linalg.norm(vector), 1e-12)) + + +def wrench(target_position, target_quaternion, link_pose, link_velocity, root_com, body_com, masses, limits): + """All vectors are world-frame; poses use wxyz. Output acts at root COM. + + A link-origin PD force F needs (p_link-p_rootCOM) x F when applied at COM. + Gravity compensation is sum(-m*g) and sum((p_bodyCOM-p_rootCOM) x (-m*g)). + Both complete wrench vectors are norm-clamped; no unclamped gravity feedforward. + """ + inputs = (target_position, target_quaternion, link_pose, link_velocity, root_com, body_com, masses) + require(all(np.isfinite(v).all() for v in inputs), "Nonfinite controller state") + require(np.asarray(link_pose).shape == (7,) and np.asarray(link_velocity).shape == (6,), "Invalid root state shape") + require(np.asarray(body_com).shape == (len(masses), 3) and np.asarray(root_com).shape == (3,), "Invalid COM shape") + require(np.asarray(target_position).shape == (3,) and (np.asarray(masses) > 0).all(), "Invalid target/mass") + pd_force = limits.position_gain * (target_position - link_pose[:3]) - limits.velocity_gain * link_velocity[:3] + pd_torque = limits.rotation_gain * rotation_error(target_quaternion, link_pose[3:]) + pd_torque -= limits.angular_velocity_gain * link_velocity[3:] + compensation = np.asarray(masses)[:, None] * np.array([0.0, 0.0, 9.81]) + force = pd_force + compensation.sum(axis=0) + torque = pd_torque + np.cross(link_pose[:3] - root_com, pd_force) + torque += np.cross(body_com - root_com, compensation).sum(axis=0) + return bounded(force, limits.force), bounded(torque, limits.torque) + + +def validate_reference(episode, limits): + """Fail rather than silently clip reference speed/workspace or bridge invalid frames.""" + require(episode.valid.all(), "Dynamic diagnostic needs one fully valid episode") + dt = np.diff(episode.time) + require(len(dt) and (dt > 0).all(), "Increasing reference timestamps required") + speed = np.linalg.norm(np.diff(episode.wrist_position, axis=0), axis=1) / dt + omega = ( + np.array( + [ + np.linalg.norm(rotation_error(b, a)) + for a, b in zip(episode.wrist_quaternion[:-1], episode.wrist_quaternion[1:], strict=True) + ] + ) + / dt + ) + joint_speed = np.abs(np.diff(episode.joint_position, axis=0)) / dt[:, None] + require((speed <= limits.reference_speed).all(), "Reference translation too fast") + require((omega <= limits.reference_angular_speed).all(), "Reference rotation too fast") + require((joint_speed <= limits.reference_joint_speed).all(), "Reference joints too fast") + require( + (np.linalg.norm(episode.wrist_position - episode.wrist_position[0], axis=1) <= limits.workspace_radius).all(), + "Reference outside workspace", + ) diff --git a/source/dex_workbench/dex_workbench_tracking/prepared.py b/source/dex_workbench/dex_workbench_tracking/prepared.py new file mode 100644 index 0000000..9256d90 --- /dev/null +++ b/source/dex_workbench/dex_workbench_tracking/prepared.py @@ -0,0 +1,299 @@ +"""L20 model-derived floating control overlay for the inspected PhysX 110.1.13 backend. + +NewtonMimicAPI is parsed by this PhysX version. Preserve it; never add a second +legacy PhysxMimicJointAPI. Static eligibility is not runtime coupling verification. +""" + +import json +import math +import os +import tempfile +from pathlib import Path + +from pxr import Gf, Plug, Sdf, Usd, UsdGeom, UsdPhysics + +from .asset import _require, dependencies, inspect, prepare_floating_overlay + +SOURCE_URDF_SHA = "b8ef22e436ab311fb61f87091b72ae717821092471d628d0ea9475b5088daa5d" +BACKEND_VERSION = "110.1.13" +MIMIC = [ + {"joint": f"{finger}_dip", "reference": f"{finger}_pip", "multiplier": 0.89, "offset_rad": 0.0} + for finger in ("index", "middle", "pinky", "ring") +] + [{"joint": "thumb_ip", "reference": "thumb_mcp", "multiplier": 1.02, "offset_rad": 0.0}] +DRIVE_FIELDS = ("stiffness", "damping", "maxForce") + + +def require_backend(version, newton_schema_registered): + """Conservative capability binding; this does not certify runtime constraint response.""" + _require(version == BACKEND_VERSION, f"Unsupported PhysX version {version}; revalidate backend first") + _require(newton_schema_registered, "Newton mimic schema not registered") + + +def schema_context(): + """Resolved schema fallbacks are part of the authoring/inspection contract.""" + registry = Usd.SchemaRegistry() + return { + name: bool(registry.FindAppliedAPIPrimDefinition(name)) for name in ("PhysxArticulationAPI", "NewtonMimicAPI") + } + + +def register_schema_plugins(paths): + """Register caller-selected USD schema plugins; never discover vendor paths implicitly.""" + for path in paths: + Plug.Registry().RegisterPlugins(str(Path(path).resolve(strict=True))) + _require(all(schema_context().values()), "Schema paths must resolve PhysxArticulationAPI and NewtonMimicAPI") + + +def schemas(prim): + """Composed schema list, including codeless schemas not registered in CPU Python.""" + value = prim.GetMetadata("apiSchemas") + return list(value.GetAppliedItems()) if value else [] + + +def newton_mimic(prim): + """Read single-DOF Newton equation; fallbacks are from installed generatedSchema. + + Coef0 is degrees for revolute joints (not NewtonActuator's radians). + Coef1 is dimensionless; the USD joint axis implicitly selects the DOF. + """ + _require("NewtonMimicAPI" in schemas(prim), f"Missing NewtonMimicAPI: {prim.GetPath()}") + + def value(name, fallback): + attr = prim.GetAttribute("newton:" + name) + result = attr.Get() if attr else None + return fallback if result is None else result + + targets = prim.GetRelationship("newton:mimicJoint").GetTargets() + _require(len(targets) == 1, "Exactly one mimic leader required") + _require(value("mimicEnabled", True) is True, "Disabled mimic constraint") + return str(targets[0]), float(value("mimicCoef1", 1)), math.radians(float(value("mimicCoef0", 0))) + + +def bind_source(source, manifest): + """Reject stale identity/structural metadata before authoring or accepting an overlay.""" + actual = inspect(source) + for key in ("asset_sha256", "joints", "root_body_path", "root_link", "dependencies", "bodies"): + _require(actual[key] == manifest[key], f"Stale/inconsistent source manifest: {key}") + _require(manifest["source_urdf"]["sha256"] == SOURCE_URDF_SHA, "Unapproved URDF provenance") + _require(manifest["source_urdf"]["mimic"] == MIMIC, "Unexpected L20 mimic equations") + _require(actual["root_link"] == "hand_base_link" and len(actual["joints"]) == 21, "Wrong L20 model") + _require(actual["physics_variant"] == "physx", "PhysX variant required") + validate_mimic(Usd.Stage.Open(str(source)), actual) + return actual + + +def validate_mimic(stage, manifest, passive=False): + joints = {j["name"]: j for j in manifest["joints"]} + followers = {eq["joint"] for eq in MIMIC} + for name, joint in joints.items(): + prim = stage.GetPrimAtPath(joint["path"]) + _require(not any("PhysxMimic" in s or "Tendon" in s for s in schemas(prim)), "Duplicate/unsupported coupling") + _require(("NewtonMimicAPI" in schemas(prim)) == (name in followers), "Unexpected mimic set") + _require(not UsdPhysics.Joint(prim).GetExcludeFromArticulationAttr().Get(), "Excluded joint") + _require(joint["axis"] in ("X", "Y", "Z"), "Invalid revolute axis") + if name not in followers: + continue + equation = next(eq for eq in MIMIC if eq["joint"] == name) + leader, coefficient, offset = newton_mimic(prim) + _require(leader == joints[equation["reference"]]["path"], "Wrong mimic leader") + _require(abs(coefficient - equation["multiplier"]) < 1e-6, "Wrong mimic multiplier/sign") + _require(abs(offset - equation["offset_rad"]) < 1e-8, "Wrong mimic offset/units") + if passive: + _require("PhysicsDriveAPI:angular" not in schemas(prim), "Follower drive API still enabled") + for field in DRIVE_FIELDS: + _require(prim.GetAttribute(f"drive:angular:physics:{field}").Get() == 0, "Follower drive not zero") + return [name for name in joints if name not in followers] + + +def _preserved(original, stage, before): + """Only leaf world-anchor deactivation, root migration and follower drive suppression are allowed.""" + followers = {j["path"] for j in before["joints"] if j["name"] in {eq["joint"] for eq in MIMIC}} + anchor = before["world_fixed_joints"][0]["path"] + root = before["root_body_path"] + source_anchor = original.GetPrimAtPath(anchor) + _require( + source_anchor.IsA(UsdPhysics.FixedJoint) and not source_anchor.GetAllChildren(), "Expected leaf world anchor" + ) + joint = UsdPhysics.Joint(source_anchor) + _require(not joint.GetBody0Rel().GetTargets(), "Anchor must attach directly to world") + _require(joint.GetBody1Rel().GetTargets() == [Sdf.Path(root)], "Anchor must attach only to root") + c0, c1 = UsdGeom.XformCache(), UsdGeom.XformCache() + # TraverseAll retains the deliberately inactive leaf: no other deletion or + # deactivation may hide geometry, state joints or time-sampled attributes. + _require( + [str(p.GetPath()) for p in original.TraverseAll()] == [str(p.GetPath()) for p in stage.TraverseAll()], + "Prim tree changed", + ) + for prim in original.TraverseAll(): + path = str(prim.GetPath()) + after = stage.GetPrimAtPath(path) + _require(after.IsActive() == (False if path == anchor else prim.IsActive()), f"Unexpected active state: {path}") + allowed = set() + expected = set(schemas(prim)) + if path == anchor: + _require(after.GetAttribute("physics:jointEnabled").Get() is False, "Inactive anchor must remain disabled") + allowed.add("physics:jointEnabled") + expected -= {"PhysicsArticulationRootAPI", "PhysxArticulationAPI"} + if path == root: + expected |= {"PhysicsArticulationRootAPI", "PhysxArticulationAPI"} + allowed |= { + a.GetName() + for a in stage.GetPrimAtPath(anchor).GetAttributes() + if a.GetName().startswith("physxArticulation:") + } + for name in allowed: + _require( + after.GetAttribute(name).Get() == stage.GetPrimAtPath(anchor).GetAttribute(name).Get(), + "Root solver property changed", + ) + if path in followers: + expected.discard("PhysicsDriveAPI:angular") + allowed |= {f"drive:angular:physics:{field}" for field in DRIVE_FIELDS} + _require(set(schemas(after)) == expected, f"Unexpected schema edit: {path}") + attrs = {a.GetName() for a in prim.GetAttributes()} | {a.GetName() for a in after.GetAttributes()} + # This diagnostic accepts static assets only, including fields whose + # default values may be changed by the preparation allowlist. + for name in attrs: + for candidate in (prim.GetAttribute(name), after.GetAttribute(name)): + if candidate: + _require(not candidate.GetTimeSamples(), f"Time samples prohibited: {path}.{name}") + for name in attrs - allowed: + a, b = prim.GetAttribute(name), after.GetAttribute(name) + _require((a.Get() if a else None) == (b.Get() if b else None), f"Unexpected attribute: {path}.{name}") + rels = {r.GetName() for r in prim.GetRelationships()} | {r.GetName() for r in after.GetRelationships()} + for name in rels: + _require( + prim.GetRelationship(name).GetTargets() == after.GetRelationship(name).GetTargets(), + "Relationship changed", + ) + if prim.IsA(UsdGeom.Xformable): + _require( + Gf.IsClose(c0.GetLocalToWorldTransform(prim), c1.GetLocalToWorldTransform(after), 1e-12), + "Transform changed", + ) + + +def inspect_prepared(output, source_manifest): + """Inspect actual composed USD + original dependency hashes, never a JSON ready flag.""" + output = Path(output).resolve(strict=True) + layer = Sdf.Layer.FindOrOpen(str(output)) + _require( + len(layer.subLayerPaths) == 1 and not os.path.isabs(layer.subLayerPaths[0]), + "One relative source layer required", + ) + source = (output.parent / layer.subLayerPaths[0]).resolve(strict=True) + before = bind_source(source, source_manifest) + _require(layer.customLayerData.get("source_bundle_sha256") == before["asset_sha256"], "Overlay provenance mismatch") + _require(layer.customLayerData.get("source_urdf_sha256") == SOURCE_URDF_SHA, "Overlay URDF mismatch") + _require( + dict(layer.customLayerData.get("schema_context", {})) == schema_context(), + "Authoring schema context mismatch; regenerate from source with --schema-plugin-path for both " + "installed PhysX and Newton plugins, and use the same plugins when inspecting", + ) + after = inspect(output) + _require( + not after["world_fixed_joints"] and after["articulation_roots"] == [before["root_body_path"]], "Not floating" + ) + stage = Usd.Stage.Open(str(output)) + masters = validate_mimic(stage, after, passive=True) + _preserved(Usd.Stage.Open(str(source)), stage, before) + return { + "status": ( + "STATIC_ELIGIBLE_RUNTIME_UNVERIFIED" + if all(schema_context().values()) + else "STATIC_ONLY_SCHEMA_UNREGISTERED" + ), + "schema_context": schema_context(), + "preparation_note": "Runtime use requires matching registered PhysX/Newton schemas; regenerate if different", + "prepared_asset_sha256": after["asset_sha256"], + "source_asset_sha256": before["asset_sha256"], + "independent_joint_names": masters, + "mimic": MIMIC, + "required_physx_version": BACKEND_VERSION, + "root_body_path": before["root_body_path"], + "runtime_verified": False, + } + + +def prepare(source, output, manifest): + source, output = Path(source).resolve(strict=True), Path(output).resolve() + _require(not output.exists(), "Refusing to overwrite output") + before = bind_source(source, manifest) + output.parent.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory(prefix=".l20-prepared-", dir=output.parent) as directory: + temporary = Path(directory) / "floating.usda" + prepare_floating_overlay(source, temporary) + stage = Usd.Stage.Open(str(temporary)) + for joint in before["joints"]: + if joint["name"] not in {eq["joint"] for eq in MIMIC}: + continue + prim = stage.GetPrimAtPath(joint["path"]) + # Preserve resolved drive defaults as inert attributes when removing API. + drive_values = [ + (a.GetName(), a.GetTypeName(), a.Get()) + for a in prim.GetAttributes() + if a.GetName().startswith("drive:angular:") and a.Get() is not None + ] + prim.RemoveAPI(UsdPhysics.DriveAPI, "angular") + for name, dtype, value in drive_values: + prim.CreateAttribute(name, dtype).Set(value) + for field in DRIVE_FIELDS: + prim.CreateAttribute(f"drive:angular:physics:{field}", Sdf.ValueTypeNames.Float).Set(0) + # jointEnabled=false leaves a parsed joint with world/local frames that + # diverge when the floating hand is spawned. Remove that obsolete joint + # from active composition, not its data or any robot body/state joint. + stage.GetPrimAtPath(before["world_fixed_joints"][0]["path"]).SetActive(False) + stage.GetRootLayer().customLayerData = { + "purpose": "L20 experimental floating tracking; runtime UNVERIFIED", + "source_bundle_sha256": before["asset_sha256"], + "source_urdf_sha256": SOURCE_URDF_SHA, + "physx_evidence_version": BACKEND_VERSION, + "schema_context": schema_context(), + } + stage.GetRootLayer().Save() + inspect_prepared(temporary, manifest) + # Rebase the relative source path for the final location before publication. + final_layer = Sdf.Layer.CreateAnonymous() + final_layer.TransferContent(stage.GetRootLayer()) + final_layer.subLayerPaths = [os.path.relpath(source, output.parent).replace(os.sep, "/")] + # Validate with the final relative-path context before publishing. + descriptor, name = tempfile.mkstemp(prefix=".l20-publication-", suffix=".usda", dir=output.parent) + os.close(descriptor) + publication = Path(name) + try: + _require(final_layer.Export(str(publication)), "Export failed") + inspect_prepared(publication, manifest) + os.link(publication, output) + finally: + publication.unlink() + _require(dependencies(source)[1] == before["asset_sha256"], "Original changed") + return inspect_prepared(output, manifest) + + +def main(): + import argparse + + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("asset", type=Path) + parser.add_argument("--manifest", type=Path, required=True) + parser.add_argument("--output", type=Path, help="Create new experimental floating overlay") + parser.add_argument( + "--schema-plugin-path", + type=Path, + action="append", + default=[], + help="Repeat for caller-selected PhysX and Newton USD plugInfo.json paths; required for runtime preparation", + ) + args = parser.parse_args() + try: + if args.schema_plugin_path: + register_schema_plugins(args.schema_plugin_path) + manifest = json.loads(args.manifest.read_text()) + result = prepare(args.asset, args.output, manifest) if args.output else inspect_prepared(args.asset, manifest) + print(json.dumps(result, indent=2)) + except (ValueError, OSError, RuntimeError, KeyError) as error: + parser.exit(1, f"FAIL: {error}\n") + + +if __name__ == "__main__": + main() diff --git a/source/dex_workbench/dex_workbench_tracking/trajectory.py b/source/dex_workbench/dex_workbench_tracking/trajectory.py new file mode 100644 index 0000000..9c676df --- /dev/null +++ b/source/dex_workbench/dex_workbench_tracking/trajectory.py @@ -0,0 +1,225 @@ +"""Strict l20_tracking_v1 reference-state ingestion, without simulator imports.""" + +import re +from dataclasses import dataclass +from pathlib import Path + +import h5py +import numpy as np + + +class ContractError(ValueError): + """An input cannot be safely interpreted as a reference trajectory.""" + + +def require(condition, message): + if not condition: + raise ContractError(message) + + +def text(value, label): + if isinstance(value, bytes): + value = value.decode("utf-8", errors="strict") + require(isinstance(value, str) and bool(value.strip()), f"{label}: nonempty UTF-8 text required") + return value + + +@dataclass(frozen=True) +class Episode: + time: np.ndarray + wrist_position: np.ndarray + wrist_quaternion: np.ndarray + joint_position: np.ndarray + valid: np.ndarray + + +@dataclass(frozen=True) +class Demonstrations: + metadata: dict + joint_names: tuple[str, ...] + world_from_source: np.ndarray + episodes: dict[str, Episode] + + +def _dataset(group, name, dtype, shape): + require(name in group and isinstance(group[name], h5py.Dataset), f"{group.name}/{name}: dataset required") + data = group[name] + require( + data.dtype.kind == np.dtype(dtype).kind and data.dtype.itemsize == np.dtype(dtype).itemsize, + f"{data.name}: dtype must be {dtype}", + ) + require(data.shape == shape, f"{data.name}: expected shape {shape}, got {data.shape}") + value = data[...] + require(np.isfinite(value).all(), f"{data.name}: NaN/Inf prohibited, including invalid frames") + return value + + +def load(path: str | Path, manifest: dict | None = None) -> Demonstrations: + """Read and close an HDF5 file. Manifest checking never approves physical actuation.""" + with h5py.File(path, "r") as file: + attributes = {} + for name in ( + "schema_version", + "embodiment", + "hand_side", + "asset_sha256", + "root_link", + "metric_scale_provenance", + "provenance", + "source_description", + ): + require(name in file.attrs, f"missing root attribute {name}") + attributes[name] = text(file.attrs[name], name) + for name, expected in (("schema_version", "l20_tracking_v1"), ("embodiment", "L20"), ("hand_side", "left")): + require(attributes[name] == expected, f"{name}: expected {expected}") + require( + re.fullmatch(r"[0-9a-f]{64}", attributes["asset_sha256"]) is not None, + "asset_sha256: lowercase SHA-256 required", + ) + require(attributes["provenance"] in ("expert_retargeted", "synthetic"), "unknown provenance") + require("scale_to_meters" in file.attrs, "missing root attribute scale_to_meters") + scale = np.asarray(file.attrs["scale_to_meters"]) + require( + scale.shape == () and scale.dtype.kind == "f" and np.isfinite(scale) and scale > 0, + "scale_to_meters: positive finite floating scalar required", + ) + attributes["scale_to_meters"] = float(scale) + require("metadata" in file and isinstance(file["metadata"], h5py.Group), "metadata group required") + meta = file["metadata"] + require( + "joint_names" in meta and isinstance(meta["joint_names"], h5py.Dataset), + "metadata/joint_names dataset required", + ) + names = meta["joint_names"] + info = h5py.check_string_dtype(names.dtype) + require( + info is not None and info.encoding == "utf-8" and names.ndim == 1 and len(names) > 0, + "joint_names must be UTF-8[J], J > 0", + ) + joint_names = tuple(text(n, "joint_names") for n in names.asstr()[...]) + require(len(set(joint_names)) == len(joint_names), "duplicate joint names") + transform = _dataset(meta, "world_from_source", "float64", (4, 4)) + require( + np.allclose(transform[3], [0, 0, 0, 1], rtol=0, atol=1e-8), "world_from_source: invalid homogeneous row" + ) + rotation = transform[:3, :3] + require( + np.allclose(rotation.T @ rotation, np.eye(3), rtol=0, atol=1e-6) + and np.isclose(np.linalg.det(rotation), 1, rtol=0, atol=1e-6), + "world_from_source must be a proper rigid transform, not a scale/reflection", + ) + require( + "episodes" in file and isinstance(file["episodes"], h5py.Group) and len(file["episodes"]) > 0, + "nonempty episodes group required", + ) + episodes = {} + for name, group in file["episodes"].items(): + require( + re.fullmatch(r"demo_[0-9]{6}", name) is not None and isinstance(group, h5py.Group), + f"invalid episode group {name}", + ) + require( + "time" in group and isinstance(group["time"], h5py.Dataset) and group["time"].ndim == 1, + f"{name}: time[T] required", + ) + count = len(group["time"]) + require(count >= 2, f"{name}: at least two frames required") + time = _dataset(group, "time", "float64", (count,)) + require(time[0] == 0 and (np.diff(time) > 0).all(), f"{name}: time must start at 0 and increase strictly") + position = _dataset(group, "wrist_position", "float32", (count, 3)) + quaternion = _dataset(group, "wrist_quaternion", "float32", (count, 4)) + joints = _dataset(group, "joint_position", "float32", (count, len(joint_names))) + valid = _dataset(group, "valid", "bool", (count,)) + require(valid.any(), f"{name}: no valid frames") + require( + np.allclose(np.linalg.norm(quaternion[valid], axis=1), 1, rtol=0, atol=1e-4), + f"{name}: valid quaternions must be unit length", + ) + adjacent = valid[:-1] & valid[1:] + require( + (np.sum(quaternion[:-1] * quaternion[1:], axis=1)[adjacent] >= 0).all(), + f"{name}: adjacent valid quaternion signs must be continuous", + ) + episodes[name] = Episode(time, position, quaternion, joints, valid) + result = Demonstrations(attributes, joint_names, transform, episodes) + if manifest is not None: + validate_against_manifest(result, manifest) + return result + + +def validate_against_manifest(data: Demonstrations, manifest: dict): + """Check identity/order/limits and known URDF mimic equations, not actuator validity.""" + require(manifest.get("manifest_version") == "l20_asset_manifest_v1", "unsupported manifest") + require(data.metadata["asset_sha256"] == manifest["asset_sha256"], "asset bundle hash mismatch") + require(data.metadata["root_link"] == manifest["root_link"], "root link mismatch") + joints = manifest["joints"] + expected = tuple(joint["name"] for joint in joints) + require(data.joint_names == expected, "joint order/names must exactly match manifest (no implicit reorder)") + lower = np.array([joint["lower_rad"] for joint in joints]) + upper = np.array([joint["upper_rad"] for joint in joints]) + require( + np.isfinite(lower).all() and np.isfinite(upper).all() and (lower <= upper).all(), + "manifest requires finite ordered revolute limits", + ) + for name, episode in data.episodes.items(): + q = episode.joint_position[episode.valid] + require(((q >= lower - 1e-6) & (q <= upper + 1e-6)).all(), f"{name}: reference joint limit violation") + for mimic in manifest.get("source_urdf", {}).get("mimic", []): + child = expected.index(mimic["joint"]) + parent = expected.index(mimic["reference"]) + error = q[:, child] - (mimic["multiplier"] * q[:, parent] + mimic["offset_rad"]) + require((np.abs(error) <= 1e-3).all(), f"{name}: inconsistent mimic reference {mimic['joint']}") + + +def sample(episode: Episode, query_time) -> Episode: + """Linear position/joint interpolation and shortest-arc SLERP; never cross invalid gaps. + + Inputs must come from ``load``. Query a separate valid segment at a time; this + rejects even sparse queries spanning an invalid frame, and never extrapolates. + """ + query = np.asarray(query_time, dtype=np.float64) + require(query.ndim == 1 and len(query) > 0 and np.isfinite(query).all(), "finite query_time[N] required") + require((np.diff(query) > 0).all(), "query times must increase strictly") + time = episode.time + require(query[0] >= time[0] and query[-1] <= time[-1], "extrapolation prohibited") + left = np.searchsorted(time, query, side="right") - 1 + right = np.searchsorted(time, query, side="left") + require(episode.valid[left.min() : right.max() + 1].all(), "query crosses or touches an invalid frame") + denominator = time[right] - time[left] + alpha = np.divide(query - time[left], denominator, out=np.zeros_like(query), where=denominator > 0)[:, None] + position = (1 - alpha) * episode.wrist_position[left] + alpha * episode.wrist_position[right] + joints = (1 - alpha) * episode.joint_position[left] + alpha * episode.joint_position[right] + q0 = episode.wrist_quaternion[left].astype(np.float64) + q1 = episode.wrist_quaternion[right].astype(np.float64) + q0 /= np.linalg.norm(q0, axis=1, keepdims=True) + q1 /= np.linalg.norm(q1, axis=1, keepdims=True) + dot = np.sum(q0 * q1, axis=1, keepdims=True) + q1 = np.where(dot < 0, -q1, q1) + dot = np.clip(np.abs(dot), 0, 1) + theta = np.arccos(dot) + denominator = np.sin(theta) + curved = dot < 0.9995 + weight0 = np.divide(np.sin((1 - alpha) * theta), denominator, out=1 - alpha.copy(), where=curved) + weight1 = np.divide(np.sin(alpha * theta), denominator, out=alpha.copy(), where=curved) + quaternion = weight0 * q0 + weight1 * q1 + quaternion /= np.linalg.norm(quaternion, axis=1, keepdims=True) + # Sparse output samples may span many source arcs. Preserve rotations while + # choosing a continuous quaternion hemisphere on the output timeline too. + flips = np.where(np.sum(quaternion[:-1] * quaternion[1:], axis=1) < 0, -1, 1) + quaternion[1:] *= np.cumprod(flips)[:, None] + return Episode( + query, + position.astype(np.float32), + quaternion.astype(np.float32), + joints.astype(np.float32), + np.ones(len(query), dtype=bool), + ) + + +def require_dynamic_replay_ready(manifest: dict): + """A data manifest alone never authorizes dynamic control.""" + raise ContractError( + "BLOCKED: a data manifest cannot authorize actuation. Use the separate experimental track_l20.py " + "path with actual prepared-USD inspection, backend checks and runtime assertions. " + "Production replay and hardware control remain unvalidated." + ) diff --git a/source/dex_workbench/docs/CHANGELOG.rst b/source/dex_workbench/docs/CHANGELOG.rst index 3e3da67..463ac55 100644 --- a/source/dex_workbench/docs/CHANGELOG.rst +++ b/source/dex_workbench/docs/CHANGELOG.rst @@ -4,9 +4,56 @@ Changelog Unreleased ~~~~~~~~~~ +0.1.1 (2026-09-11) +~~~~~~~~~~~~~~~~~~ + +USD initial validation milestone (USD 文件初步校验). Synthetic single-hand +replay only; not a production training or hardware release. Cartpole and its task +ID are unchanged. Old prepared overlays require regeneration. Full pre-commit +remains unavailable in the validation environment. + +Added +^^^^^ + +* Add the explicitly synthetic ``range_4s`` HDF5 fixture profile (4 seconds, + 10 mm translation, 0.1 rad yaw, bounded master joints). Real-file replay passes + two 960-step repetitions with unchanged gains/thresholds; no training claim. + +* Add CPU-only ``l20_tracking_v1`` HDF5 validation, reference resampling, synthetic + fixtures, and regression tests, documented in ``L20_TRACKING.md``. Provide the + standalone colleague-facing contract in ``HDF5_REQUIREMENTS.md``. +* Add reproducible L20 composed-asset inspection and a non-destructive floating-root + topology overlay plus an Isaac Lab scene-loading diagnostic. Dynamic wrist/finger + production replay remains blocked pending runtime validation and hardware actuator mapping. + Cartpole is unchanged. +* Add an experimental floating tracking overlay that preserves native Newton mimic + constraints, suppresses follower drives, and validates source identity/allowed edits. + Add a bounded single-hand wrench-PD/master-position replay diagnostic with explicit + COM/world-frame gravity accounting, reset and passive-coupling assertions. Runtime + smoke passes on the corrected registered overlay: seed 42, one hand, two 480-step + synthetic repetitions on PhysX 110.1.13. Real demonstrations, broad motion/contact, + vectorized training and hardware remain unvalidated. Gains are uncalibrated. + Fixed ^^^^^ +* Deactivate only the obsolete disabled leaf world anchor in prepared control + overlays while retaining its data and strict all-prim/time-sample checks. + Both bounded runtime tests report a floating root and 21 active revolute joints, + without the prior disjointed-frame warning; small-profile metrics match baseline. + +* Launch Kit before USD-dependent diagnostic imports and preserve exceptions/nonzero + exit status across fast shutdown; an exit without PASS metrics is not acceptance. +* Preserve resolved articulation schema defaults on both the released anchor and + floating root. Add explicit preparation CLI schema-plugin paths and authoring-context + checks; old overlays require regeneration. CPU regressions and the subsequent + authorized synthetic dynamic smoke pass; metrics and remaining runtime warnings + are recorded in ``L20_TRACKING.md``. + +* Correct the prior missing-PhysX-coupling inference: installed PhysX 110.1.13 parses + NewtonMimicAPI natively. Schema presence is now reported as runtime-unverified, + not missing physics; it does not establish actual asset dynamics. + * Configure the L20 PhysX preview with a world-fixed articulation root, ground, lighting, and 240 Hz physics to prevent the observed 60 Hz instability. Add a bounded editor smoke script; Isaac Lab task integration is not included. diff --git a/source/dex_workbench/setup.py b/source/dex_workbench/setup.py index 182332e..074eb2b 100644 --- a/source/dex_workbench/setup.py +++ b/source/dex_workbench/setup.py @@ -24,7 +24,7 @@ INSTALL_REQUIRES = [ # Installation operation setup( name="dex_workbench", - packages=["dex_workbench"], + packages=["dex_workbench", "dex_workbench_tracking"], author=EXTENSION_TOML_DATA["package"]["author"], maintainer=EXTENSION_TOML_DATA["package"]["maintainer"], url=EXTENSION_TOML_DATA["package"]["repository"], @@ -32,6 +32,7 @@ setup( description=EXTENSION_TOML_DATA["package"]["description"], keywords=EXTENSION_TOML_DATA["package"]["keywords"], install_requires=INSTALL_REQUIRES, + extras_require={"tracking": ["numpy>=1.26", "h5py>=3.10"]}, license="Apache-2.0", include_package_data=True, python_requires=">=3.12", @@ -41,4 +42,4 @@ setup( "Isaac Sim :: 6.0.0", ], zip_safe=False, -) \ No newline at end of file +) diff --git a/source/dex_workbench/tests/test_tracking_asset.py b/source/dex_workbench/tests/test_tracking_asset.py new file mode 100644 index 0000000..7e8f2a8 --- /dev/null +++ b/source/dex_workbench/tests/test_tracking_asset.py @@ -0,0 +1,130 @@ +"""Static USD regression tests; run with Isaac Sim's pxr-capable Python, no GPU/Kit.""" + +import json +import shutil +import tempfile +import unittest +from pathlib import Path + +from dex_workbench_tracking.asset import dependencies, inspect, prepare_floating_overlay + +from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics + + +class TrackingAssetTests(unittest.TestCase): + def setUp(self): + self.directory = tempfile.TemporaryDirectory() + self.addCleanup(self.directory.cleanup) + self.path = Path(self.directory.name) / "robot.usda" + stage = Usd.Stage.CreateNew(str(self.path)) + root = UsdGeom.Xform.Define(stage, "/Robot").GetPrim() + stage.SetDefaultPrim(root) + variant = root.GetVariantSets().AddVariantSet("Physics") + variant.AddVariant("physx") + variant.SetVariantSelection("physx") + UsdGeom.SetStageMetersPerUnit(stage, 1) + UsdGeom.SetStageUpAxis(stage, "Z") + UsdPhysics.SetStageKilogramsPerUnit(stage, 1) + for name in ("base", "tip"): + prim = UsdGeom.Xform.Define(stage, f"/Robot/{name}").GetPrim() + UsdPhysics.RigidBodyAPI.Apply(prim) + UsdPhysics.MassAPI.Apply(prim).CreateMassAttr(0.1) + hinge = UsdPhysics.RevoluteJoint.Define(stage, "/Robot/hinge") + hinge.CreateBody0Rel().SetTargets(["/Robot/base"]) + hinge.CreateBody1Rel().SetTargets(["/Robot/tip"]) + hinge.CreateLowerLimitAttr(0) + hinge.CreateUpperLimitAttr(90) + fixed = UsdPhysics.FixedJoint.Define(stage, "/Robot/anchor") + fixed.CreateBody1Rel().SetTargets(["/Robot/base"]) + UsdPhysics.ArticulationRootAPI.Apply(fixed.GetPrim()) + stage.GetRootLayer().Save() + + def test_manifest_is_explicitly_not_actuator_map(self): + result = inspect(self.path) + self.assertEqual(result["root_link"], "base") + self.assertEqual([j["name"] for j in result["joints"]], ["hinge"]) + self.assertAlmostEqual(result["joints"][0]["upper_rad"], 1.5707963267948966) + self.assertFalse(result["dynamic_replay_ready"]) + self.assertEqual(result["coupling_status"], "UNVERIFIED") + json.dumps(result, allow_nan=False) + + def test_bundle_hash_is_portable_and_sensitive(self): + target = Path(self.directory.name) / "moved" + target.mkdir() + copy = target / self.path.name + shutil.copyfile(self.path, copy) + self.assertEqual(dependencies(self.path)[1], dependencies(copy)[1]) + with copy.open("a") as stream: + stream.write("\n# changed\n") + self.assertNotEqual(dependencies(self.path)[1], dependencies(copy)[1]) + + def test_missing_dependency_rejected(self): + stage = Usd.Stage.Open(str(self.path)) + stage.GetRootLayer().subLayerPaths = ["missing.usda"] + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "Unresolved"): + inspect(self.path) + + def test_floating_overlay_preserves_source_and_transforms(self): + before = self.path.read_bytes() + output = Path(self.directory.name) / "floating.usda" + result = prepare_floating_overlay(self.path, output) + self.assertEqual(result["world_fixed_joints"], []) + self.assertEqual(result["articulation_roots"], ["/Robot/base"]) + self.assertEqual(self.path.read_bytes(), before) + self.assertFalse(result["dynamic_replay_ready"]) + stage = Usd.Stage.Open(str(output)) + self.assertFalse(UsdPhysics.Joint(stage.GetPrimAtPath("/Robot/anchor")).GetJointEnabledAttr().Get()) + self.assertFalse(Sdf.Layer.FindOrOpen(str(output)).subLayerPaths[0].startswith("/")) + self.assertTrue( + Gf.IsClose( + UsdGeom.XformCache().GetLocalToWorldTransform(stage.GetPrimAtPath("/Robot/base")), Gf.Matrix4d(1), 1e-12 + ) + ) + with self.assertRaisesRegex(ValueError, "overwrite"): + prepare_floating_overlay(self.path, output) + with self.assertRaisesRegex(ValueError, "fixed-world"): + prepare_floating_overlay(output, Path(self.directory.name) / "again.usda") + + def test_ambiguous_root_rejected(self): + stage = Usd.Stage.Open(str(self.path)) + UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath("/Robot/base")) + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "one articulation"): + inspect(self.path) + + def test_urdf_matching_and_wrong_limits(self): + urdf = Path(self.directory.name) / "robot.urdf" + urdf.write_text( + '' + '' + '' + ) + result = inspect(self.path, urdf) + self.assertEqual(result["source_urdf"]["status"], "STRUCTURAL_MATCH_ONLY") + urdf.write_text(urdf.read_text().replace('upper="1.5707963267948966"', 'upper="1"')) + with self.assertRaisesRegex(ValueError, "limit mismatch"): + inspect(self.path, urdf) + + def test_newton_mimic_presence_is_not_runtime_verification(self): + stage = Usd.Stage.Open(str(self.path)) + hinge = stage.GetPrimAtPath("/Robot/hinge") + hinge.AddAppliedSchema("NewtonMimicAPI") + hinge.CreateAttribute("newton:mimicCoef1", Sdf.ValueTypeNames.Float).Set(1) + stage.GetRootLayer().Save() + urdf = Path(self.directory.name) / "robot.urdf" + # Deliberately self-referencing fixture only tests backend evidence, not valid transmission. + urdf.write_text( + '' + '' + '' + '' + ) + result = inspect(self.path, urdf) + self.assertTrue(result["coupling_evidence"]) + self.assertFalse(result["physx_coupling_evidence"]) + self.assertEqual(result["coupling_status"], "MIMIC_SCHEMA_PRESENT_RUNTIME_UNVERIFIED") + + +if __name__ == "__main__": + unittest.main() diff --git a/source/dex_workbench/tests/test_tracking_control.py b/source/dex_workbench/tests/test_tracking_control.py new file mode 100644 index 0000000..22f1900 --- /dev/null +++ b/source/dex_workbench/tests/test_tracking_control.py @@ -0,0 +1,103 @@ +"""Analytic CPU regressions for control.py; not simulated dynamics.""" + +import unittest + +import numpy as np +from dex_workbench_tracking.cli import synthetic +from dex_workbench_tracking.control import ( + Limits, + reference_pose_to_xyzw, + rotation_error, + validate_reference, + wrench, + xyzw_pose_to_reference, +) +from dex_workbench_tracking.trajectory import ContractError + + +class ControlTests(unittest.TestCase): + def setUp(self): + self.limits = Limits() + self.pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=float) + self.velocity = np.zeros(6) + + def compute(self, target=None, root_com=None, body_com=None, masses=None): + return wrench( + np.zeros(3) if target is None else np.array(target), + self.pose[3:], + self.pose, + self.velocity, + np.zeros(3) if root_com is None else np.array(root_com), + np.zeros((1, 3)) if body_com is None else np.array(body_com), + np.array([0.1]) if masses is None else np.array(masses), + self.limits, + ) + + def test_hdf5_wxyz_runtime_xyzw_boundary(self): + position = [0.1, 0.2, 0.3] + quaternion = [0.8, 0.6, 0, 0] + runtime = reference_pose_to_xyzw(position, quaternion) + np.testing.assert_allclose(runtime, [0.1, 0.2, 0.3, 0.6, 0, 0, 0.8]) + np.testing.assert_allclose(xyzw_pose_to_reference(runtime), position + quaternion) + + def test_gravity_all_links_and_moment_arm(self): + force, torque = self.compute(body_com=[[0, 0, 0], [0.1, 0, 0]], masses=[0.1, 0.2]) + np.testing.assert_allclose(force, [0, 0, 2.943]) + np.testing.assert_allclose(torque, [0, -0.1962, 0]) + + def test_link_pd_force_shift_to_com(self): + force, torque = self.compute(target=[0, 0.01, 0], root_com=[0.1, 0, 0], body_com=[[0.1, 0, 0]]) + np.testing.assert_allclose(force, [0, 1, 0.981]) + np.testing.assert_allclose(torque, [0, 0, -0.1]) + + def test_total_wrench_including_gravity_is_bounded(self): + force, torque = self.compute(target=[100, 100, 100], body_com=[[1, 0, 0]], masses=[100]) + self.assertLessEqual(np.linalg.norm(force), self.limits.force + 1e-10) + self.assertLessEqual(np.linalg.norm(torque), self.limits.torque + 1e-10) + + def test_damping_opposes_world_velocity(self): + self.velocity[:3] = [0.01, 0, 0] + self.velocity[3:] = [0, 0, 0.1] + force, torque = self.compute() + self.assertAlmostEqual(force[0], -0.1) + self.assertAlmostEqual(torque[2], -0.002) + + def test_orientation_world_rotation_and_antipodes(self): + angle = 0.2 + target = np.array([np.cos(angle / 2), 0, 0, np.sin(angle / 2)]) + np.testing.assert_allclose(rotation_error(target, self.pose[3:]), [0, 0, angle]) + np.testing.assert_allclose(rotation_error(-target, self.pose[3:]), [0, 0, angle]) + np.testing.assert_allclose(rotation_error(self.pose[3:], target), [0, 0, -angle]) + # Current local-X quarter-turn followed by world-Z quarter-turn. + np.testing.assert_allclose( + rotation_error([0.5, 0.5, 0.5, 0.5], [2**-0.5, 2**-0.5, 0, 0]), [0, 0, np.pi / 2], atol=1e-12 + ) + + def test_nonfinite_state_bad_mass_and_limits_rejected(self): + for mass in (0, -1, float("nan")): + with self.assertRaises(ContractError): + self.compute(masses=[mass]) + for value in (0, -1, float("nan"), float("inf")): + with self.assertRaises(ContractError): + Limits(force=value) + + def test_reference_envelope_and_invalid_gap(self): + manifest = { + "manifest_version": "l20_asset_manifest_v1", + "asset_sha256": "a" * 64, + "root_link": "test", + "joints": [{"name": "a", "lower_rad": 0, "upper_rad": 1}], + } + episode = synthetic(manifest).episodes["demo_000000"] + validate_reference(episode, self.limits) + episode.valid[60] = False + with self.assertRaisesRegex(ContractError, "fully valid"): + validate_reference(episode, self.limits) + episode.valid[:] = True + episode.wrist_position[60, 0] = 1 + with self.assertRaisesRegex(ContractError, "too fast"): + validate_reference(episode, self.limits) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/dex_workbench/tests/test_tracking_prepared.py b/source/dex_workbench/tests/test_tracking_prepared.py new file mode 100644 index 0000000..048aad9 --- /dev/null +++ b/source/dex_workbench/tests/test_tracking_prepared.py @@ -0,0 +1,225 @@ +"""Source-named CPU/USD tests of prepared.py; actual L20 dependency copies, no Kit.""" + +import copy +import json +import shutil +import tempfile +import unittest +from pathlib import Path + +from dex_workbench_tracking.asset import dependencies +from dex_workbench_tracking.prepared import ( + BACKEND_VERSION, + MIMIC, + bind_source, + inspect_prepared, + newton_mimic, + prepare, + require_backend, + schemas, +) + +from pxr import Sdf, Usd, UsdPhysics + +REPO = Path(__file__).resolve().parents[3] +ASSET_DIR = REPO / "assets/robots/dex_hand/linkerhand_g20_left" + + +class PreparedTests(unittest.TestCase): + def setUp(self): + self.temp = tempfile.TemporaryDirectory() + self.addCleanup(self.temp.cleanup) + self.bundle = Path(self.temp.name) / "bundle" + shutil.copytree(ASSET_DIR / "linkerhand_g20_left", self.bundle / "original") + self.source = self.bundle / "original/linkerhand_g20_left.usda" + self.output = self.bundle / "tracking.usda" + self.manifest = json.loads((ASSET_DIR / "tracking_manifest.json").read_text()) + + def build(self): + return prepare(self.source, self.output, self.manifest) + + def test_unsupported_backend_or_unregistered_schema_rejected(self): + require_backend(BACKEND_VERSION, True) + for version, registered in (("110.1.12", True), ("future", True), (BACKEND_VERSION, False)): + with self.assertRaises(ValueError): + require_backend(version, registered) + + def test_overlay_suppresses_only_followers_and_preserves_original(self): + original = dependencies(self.source) + result = self.build() + self.assertEqual(dependencies(self.source), original) + self.assertFalse(result["runtime_verified"]) + self.assertEqual(len(result["independent_joint_names"]), 16) + stage = Usd.Stage.Open(str(self.output)) + for eq in MIMIC: + joint = next(j for j in self.manifest["joints"] if j["name"] == eq["joint"]) + prim = stage.GetPrimAtPath(joint["path"]) + self.assertIn("NewtonMimicAPI", schemas(prim)) + self.assertNotIn("PhysicsDriveAPI:angular", schemas(prim)) + leader, multiplier, offset = newton_mimic(prim) + self.assertTrue(leader.endswith("/" + eq["reference"])) + self.assertAlmostEqual(multiplier, eq["multiplier"], places=6) + self.assertEqual(offset, 0) + self.assertEqual(result, inspect_prepared(self.output, self.manifest)) + + def test_only_obsolete_leaf_world_anchor_is_inactive(self): + self.build() + stage = Usd.Stage.Open(str(self.output)) + anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"]) + self.assertFalse(anchor.IsActive()) + self.assertFalse(anchor.GetAttribute("physics:jointEnabled").Get()) + self.assertFalse(anchor.GetAllChildren()) + self.assertIn(anchor, list(stage.TraverseAll())) + self.assertNotIn(anchor, list(stage.Traverse())) + joints = [p for p in stage.Traverse() if p.IsA(UsdPhysics.Joint)] + self.assertEqual(len(joints), 21) + self.assertTrue(all(p.IsA(UsdPhysics.RevoluteJoint) for p in joints)) + anchor.SetActive(True) + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "active state"): + inspect_prepared(self.output, self.manifest) + + def test_unrelated_deactivation_rejected(self): + self.edit_follower(lambda p: p.SetActive(False)) + + def test_unrelated_prim_addition_rejected(self): + self.build() + stage = Usd.Stage.Open(str(self.output)) + stage.DefinePrim(self.manifest["default_prim"] + "/Extra", "Scope") + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "Prim tree changed"): + inspect_prepared(self.output, self.manifest) + + def test_inactive_anchor_attributes_still_checked(self): + self.build() + stage = Usd.Stage.Open(str(self.output)) + anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"]) + anchor.GetAttribute("physics:jointEnabled").Set(True, Usd.TimeCode(1)) + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "Time samples prohibited"): + inspect_prepared(self.output, self.manifest) + + def test_inactive_anchor_must_remain_disabled(self): + self.build() + stage = Usd.Stage.Open(str(self.output)) + anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"]) + anchor.GetAttribute("physics:jointEnabled").Set(True) + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "remain disabled"): + inspect_prepared(self.output, self.manifest) + + def test_inactive_anchor_relationships_still_checked(self): + self.build() + stage = Usd.Stage.Open(str(self.output)) + anchor = stage.GetPrimAtPath(self.manifest["world_fixed_joints"][0]["path"]) + anchor.GetRelationship("physics:body1").SetTargets([]) + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "Relationship changed"): + inspect_prepared(self.output, self.manifest) + + def test_complete_bundle_relocation_preserves_identity(self): + before = self.build() + moved = Path(self.temp.name) / "moved" + shutil.copytree(self.bundle, moved) + self.assertEqual(before, inspect_prepared(moved / "tracking.usda", self.manifest)) + layer = Sdf.Layer.FindOrOpen(str(self.output)) + self.assertEqual(layer.subLayerPaths, ["original/linkerhand_g20_left.usda"]) + + def test_stale_manifest_or_original_bytes_rejected(self): + wrong = copy.deepcopy(self.manifest) + wrong["asset_sha256"] = "0" * 64 + with self.assertRaisesRegex(ValueError, "Stale"): + prepare(self.source, self.output, wrong) + self.assertFalse(self.output.exists()) + self.build() + with self.source.open("a") as stream: + stream.write("\n# change identity\n") + with self.assertRaisesRegex(ValueError, "Stale"): + inspect_prepared(self.output, self.manifest) + + def test_altered_model_mapping_rejected(self): + wrong = copy.deepcopy(self.manifest) + wrong["source_urdf"]["mimic"][0]["multiplier"] = -0.89 + with self.assertRaisesRegex(ValueError, "Unexpected L20 mimic"): + bind_source(self.source, wrong) + + def test_fixed_preview_and_json_flag_cannot_enable(self): + self.manifest["dynamic_replay_ready"] = True + with self.assertRaises(ValueError): + inspect_prepared(self.source, self.manifest) + + def test_no_overwrite(self): + self.build() + before = self.output.read_bytes() + with self.assertRaisesRegex(ValueError, "overwrite"): + self.build() + self.assertEqual(self.output.read_bytes(), before) + + def edit_follower(self, callback): + self.build() + stage = Usd.Stage.Open(str(self.output)) + path = next(j["path"] for j in self.manifest["joints"] if j["name"] == "thumb_ip") + callback(stage.GetPrimAtPath(path)) + stage.GetRootLayer().Save() + with self.assertRaises(ValueError): + inspect_prepared(self.output, self.manifest) + + def test_sampled_mimic_rejected(self): + self.edit_follower(lambda p: p.GetAttribute("newton:mimicCoef1").Set(-1.02, Usd.TimeCode(1))) + + def test_sampled_allowlisted_follower_gain_rejected(self): + self.edit_follower(lambda p: p.GetAttribute("drive:angular:physics:stiffness").Set(99, Usd.TimeCode(1))) + + def test_sampled_mass_rejected(self): + self.build() + stage = Usd.Stage.Open(str(self.output)) + root = stage.GetPrimAtPath(self.manifest["root_body_path"]) + root.GetAttribute("physics:mass").Set(99, Usd.TimeCode(1)) + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "Time samples prohibited"): + inspect_prepared(self.output, self.manifest) + + def test_sampled_transform_rejected(self): + self.build() + stage = Usd.Stage.Open(str(self.output)) + attr = next( + a + for p in stage.Traverse() + for a in p.GetAttributes() + if a.GetName().startswith("xformOp:") and a.Get() is not None + ) + # Even a sample equal to the default is prohibited for this static asset. + attr.Set(attr.Get(), Usd.TimeCode(1)) + stage.GetRootLayer().Save() + with self.assertRaisesRegex(ValueError, "Time samples prohibited"): + inspect_prepared(self.output, self.manifest) + + def test_wrong_sign_rejected(self): + self.edit_follower(lambda p: p.GetAttribute("newton:mimicCoef1").Set(-1.02)) + + def test_disabled_native_constraint_rejected(self): + self.edit_follower(lambda p: p.CreateAttribute("newton:mimicEnabled", Sdf.ValueTypeNames.Bool).Set(False)) + + def test_duplicate_legacy_constraint_rejected(self): + self.edit_follower(lambda p: p.AddAppliedSchema("PhysxMimicJointAPI:rotX")) + + def test_follower_motor_rejected(self): + self.edit_follower(lambda p: UsdPhysics.DriveAPI.Apply(p, "angular")) + + def test_changed_mass_or_limits_rejected(self): + self.edit_follower(lambda p: p.GetAttribute("physics:upperLimit").Set(180.0)) + + def test_newton_offset_degree_conversion(self): + stage = Usd.Stage.CreateInMemory() + prim = UsdPhysics.RevoluteJoint.Define(stage, "/follower").GetPrim() + prim.AddAppliedSchema("NewtonMimicAPI") + prim.CreateRelationship("newton:mimicJoint").SetTargets(["/leader"]) + prim.CreateAttribute("newton:mimicCoef0", Sdf.ValueTypeNames.Float).Set(90) + prim.CreateAttribute("newton:mimicCoef1", Sdf.ValueTypeNames.Float).Set(-2) + _, coefficient, offset = newton_mimic(prim) + self.assertEqual(coefficient, -2) + self.assertAlmostEqual(offset, 1.5707963267948966) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/dex_workbench/tests/test_tracking_range_profile.py b/source/dex_workbench/tests/test_tracking_range_profile.py new file mode 100644 index 0000000..abc01b7 --- /dev/null +++ b/source/dex_workbench/tests/test_tracking_range_profile.py @@ -0,0 +1,89 @@ +"""Regression of CLI's named range_4s HDF5 reference against the actual L20 manifest.""" + +import json +import subprocess +import sys +import tempfile +import unittest +from pathlib import Path + +import numpy as np +from dex_workbench_tracking.cli import synthetic +from dex_workbench_tracking.control import Limits, validate_reference +from dex_workbench_tracking.trajectory import ContractError, load, sample + +REPO = Path(__file__).resolve().parents[3] +MANIFEST = REPO / "assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json" + + +class TrackingRangeProfileTests(unittest.TestCase): + def test_default_small_profile_is_unchanged(self): + manifest = json.loads(MANIFEST.read_text()) + implicit, explicit = synthetic(manifest), synthetic(manifest, "small") + self.assertEqual(implicit.metadata, explicit.metadata) + a, b = implicit.episodes["demo_000000"], explicit.episodes["demo_000000"] + for field in ("time", "wrist_position", "wrist_quaternion", "joint_position", "valid"): + np.testing.assert_array_equal(getattr(a, field), getattr(b, field)) + self.assertEqual(len(a.time), 121) + self.assertEqual(a.time[-1], 2) + self.assertAlmostEqual(a.wrist_position[:, 0].max(), 0.002) + with self.assertRaises(ContractError): + synthetic(manifest, "unknown") + + def test_range_cli_publication_mapping_and_reference_limits(self): + manifest = json.loads(MANIFEST.read_text()) + with tempfile.TemporaryDirectory() as directory: + output = Path(directory) / "range.hdf5" + command = [ + sys.executable, + "-m", + "dex_workbench_tracking.cli", + "synthetic", + "--manifest", + str(MANIFEST), + "--profile", + "range_4s", + "--output", + str(output), + ] + result = subprocess.run(command, capture_output=True, text=True, timeout=30) + self.assertEqual(result.returncode, 0, result.stderr) + self.assertEqual(json.loads(result.stdout)["provenance"], "synthetic") + data = load(output, manifest) + self.assertEqual(data.metadata["asset_sha256"], manifest["asset_sha256"]) + self.assertEqual(data.metadata["provenance"], "synthetic") + self.assertIn("range_4s", data.metadata["source_description"]) + self.assertIn("NOT expert", data.metadata["source_description"]) + names = list(data.joint_names) + self.assertEqual(names, [j["name"] for j in manifest["joints"]]) + episode = data.episodes["demo_000000"] + self.assertEqual(len(episode.time), 241) + self.assertEqual(episode.time[-1], 4) + self.assertAlmostEqual(episode.wrist_position[:, 0].max(), 0.01) + self.assertAlmostEqual(2 * np.arccos(episode.wrist_quaternion[:, 0].min()), 0.1, places=5) + followers = {eq["joint"] for eq in manifest["source_urdf"]["mimic"]} + for j in manifest["joints"]: + if j["name"] not in followers: + self.assertAlmostEqual( + episode.joint_position[:, names.index(j["name"])].max(), min(0.1, 0.25 * j["upper_rad"]) + ) + for eq in manifest["source_urdf"]["mimic"]: + np.testing.assert_allclose( + episode.joint_position[:, names.index(eq["joint"])], + eq["multiplier"] * episode.joint_position[:, names.index(eq["reference"])], + atol=1e-7, + ) + for field in ("wrist_position", "wrist_quaternion", "joint_position"): + np.testing.assert_allclose(getattr(episode, field)[0], getattr(episode, field)[-1], atol=1e-7) + validate_reference(episode, Limits()) + resampled = sample(episode, np.arange(961) / 240) + validate_reference(resampled, Limits()) + self.assertEqual(len(resampled.time), 961) + before = output.read_bytes() + refused = subprocess.run(command, capture_output=True, text=True, timeout=30) + self.assertNotEqual(refused.returncode, 0) + self.assertEqual(output.read_bytes(), before) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/dex_workbench/tests/test_tracking_schema_context.py b/source/dex_workbench/tests/test_tracking_schema_context.py new file mode 100644 index 0000000..2fbd747 --- /dev/null +++ b/source/dex_workbench/tests/test_tracking_schema_context.py @@ -0,0 +1,114 @@ +"""Isolated CPU plugin-context regression; requires the installed Isaac USD schemas, not Kit.""" + +import json +import os +import subprocess +import sys +import tempfile +import unittest +from pathlib import Path + +from dex_workbench_tracking.prepared import inspect_prepared, prepare + +REPO = Path(__file__).resolve().parents[3] +ASSET = REPO / "assets/robots/dex_hand/linkerhand_g20_left" + + +class TrackingSchemaContextTests(unittest.TestCase): + def test_registered_schema_preparation_preserves_defaults_and_rejects_context_drift(self): + installation = os.environ.get("ISAAC_PATH") + if not installation: + self.skipTest("ISAAC_PATH required for installed PhysX/Newton CPU schema regression") + installation = Path(installation) + physx = list(installation.glob("extscache/omni.usd.schema.physx-*/plugins/PhysxSchema/resources/plugInfo.json")) + self.assertEqual(len(physx), 1, "Select an unambiguous installed PhysX schema for this test") + newton = installation / "exts/omni.usd.schema.newton/usd/schema/newton/newton_usd_schemas/plugInfo.json" + self.assertTrue(newton.is_file()) + manifest = json.loads((ASSET / "tracking_manifest.json").read_text()) + source = ASSET / "linkerhand_g20_left/linkerhand_g20_left.usda" + with tempfile.TemporaryDirectory() as directory: + directory = Path(directory) + unregistered = directory / "unregistered.usda" + registered = directory / "registered.usda" + result = prepare(source, unregistered, manifest) + self.assertEqual(result["status"], "STATIC_ONLY_SCHEMA_UNREGISTERED") + self.assertEqual(result, inspect_prepared(unregistered, manifest)) + # Registering plugins in a child prevents pollution of other CPU tests. + code = """ +import json, sys +from pathlib import Path +from pxr import Usd +from dex_workbench_tracking.asset import dependencies +from dex_workbench_tracking.prepared import inspect_prepared, register_schema_plugins +source, manifest_path, unregistered, registered, *plugins = sys.argv[1:] +manifest = json.loads(Path(manifest_path).read_text()) +register_schema_plugins(plugins) +try: + inspect_prepared(unregistered, manifest) +except ValueError as error: + assert "regenerate" in str(error), error +else: + raise AssertionError("Unregistered authoring accepted with registered schemas") +before = dependencies(source) +# Exercise the actual CLI registration option in a fresh CPU interpreter too. +import subprocess +command = [sys.executable, "-m", "dex_workbench_tracking.prepared", source, + "--manifest", manifest_path, "--output", registered] +for plugin in plugins: + command += ["--schema-plugin-path", plugin] +run = subprocess.run(command, capture_output=True, text=True, timeout=60) +assert run.returncode == 0, run.stderr +result = inspect_prepared(registered, manifest) +assert result["status"] == "STATIC_ELIGIBLE_RUNTIME_UNVERIFIED" +assert all(result["schema_context"].values()) +assert dependencies(source) == before +original = Usd.Stage.Open(source) +stage = Usd.Stage.Open(registered) +anchor = manifest["world_fixed_joints"][0]["path"] +root = stage.GetPrimAtPath(manifest["root_body_path"]) +for attr in original.GetPrimAtPath(anchor).GetAttributes(): + if attr.GetName().startswith("physxArticulation:"): + for prim in (stage.GetPrimAtPath(anchor), root): + after = prim.GetAttribute(attr.GetName()) + assert after.Get() == attr.Get(), attr.GetPath() + assert after.HasAuthoredValue(), attr.GetPath() + assert not after.GetTimeSamples(), attr.GetPath() +# Registered schema fallbacks do not relax the static/time-sample gate. +root.GetAttribute("physxArticulation:sleepThreshold").Set(99, Usd.TimeCode(1)) +stage.GetRootLayer().Save() +try: + inspect_prepared(registered, manifest) +except ValueError as error: + assert "Time samples prohibited" in str(error), error +else: + raise AssertionError("Sampled solver property accepted") +root.GetAttribute("physxArticulation:sleepThreshold").ClearAtTime(Usd.TimeCode(1)) +stage.GetRootLayer().Save() +inspect_prepared(registered, manifest) +print("PASS: isolated registered schema author/inspect, exact default preservation, strict time samples") +""" + result = subprocess.run( + [ + sys.executable, + "-c", + code, + str(source), + str(ASSET / "tracking_manifest.json"), + str(unregistered), + str(registered), + str(physx[0]), + str(newton), + ], + capture_output=True, + text=True, + timeout=90, + ) + self.assertEqual(result.returncode, 0, result.stdout + result.stderr) + # The inverse schema mismatch is actionable too; never pretend unknown + # vendor fallbacks are equivalent in an unregistered process. + with self.assertRaisesRegex(ValueError, "regenerate"): + inspect_prepared(registered, manifest) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/dex_workbench/tests/test_tracking_startup.py b/source/dex_workbench/tests/test_tracking_startup.py new file mode 100644 index 0000000..23c5f03 --- /dev/null +++ b/source/dex_workbench/tests/test_tracking_startup.py @@ -0,0 +1,90 @@ +"""CPU startup/fast-shutdown regressions for the two source-named Kit entries.""" + +import builtins +import contextlib +import importlib.util +import io +import sys +import types +import unittest +from pathlib import Path +from unittest.mock import patch + +import numpy as np + +REPO = Path(__file__).resolve().parents[3] + + +class TrackingStartupTests(unittest.TestCase): + def check_entry(self, filename, inspection_module, inspection_name): + events = [] + + class FakeApp: + def close(self, *, exit_code): + events.append(("close", exit_code)) + # Model Kit fast shutdown, which never returns to a pending raise. + raise SystemExit(exit_code) + + class FakeLauncher: + @staticmethod + def add_app_launcher_args(parser): + pass + + def __init__(self, args): + events.append("launch") + self.app = FakeApp() + + def reject_asset(*args): + events.append("inspect") + raise ValueError("deliberate asset rejection") + + app_module = types.ModuleType("isaaclab.app") + app_module.AppLauncher = FakeLauncher + inspector = types.ModuleType(inspection_module) + setattr(inspector, inspection_name, reject_asset) + inspector.require_backend = inspector.validate_mimic = None + original_import = builtins.__import__ + + def checked_import(name, *args, **kwargs): + if name.startswith(("dex_workbench_tracking", "pxr")): + self.assertIn("launch", events, f"USD-dependent import before AppLauncher: {name}") + return original_import(name, *args, **kwargs) + + entry = REPO / "scripts/tracking" / filename + spec = importlib.util.spec_from_file_location("tracking_entry_test", entry) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + asset = REPO / "assets/robots/dex_hand/linkerhand_g20_left/tracking_manifest.json" + argv = [str(entry), str(asset)] + if filename == "track_l20.py": + argv += ["--manifest", str(asset), "--execute-experimental"] + stderr = io.StringIO() + with ( + patch.dict( + sys.modules, + { + "isaaclab.app": app_module, + inspection_module: inspector, + "numpy": np, + "torch": types.ModuleType("torch"), + }, + ), + patch.object(sys, "argv", argv), + patch.object(builtins, "__import__", checked_import), + contextlib.redirect_stderr(stderr), + ): + with self.assertRaises(SystemExit) as error: + module.main() + self.assertEqual(error.exception.code, 1) + self.assertEqual(events, ["launch", "inspect", ("close", 1)], stderr.getvalue()) + self.assertIn("ValueError: deliberate asset rejection", stderr.getvalue()) + + def test_track_l20_launches_before_inspection_and_preserves_failure(self): + self.check_entry("track_l20.py", "dex_workbench_tracking.prepared", "inspect_prepared") + + def test_inspect_l20_scene_launches_before_inspection_and_preserves_failure(self): + self.check_entry("inspect_l20_scene.py", "dex_workbench_tracking.asset", "inspect") + + +if __name__ == "__main__": + unittest.main() diff --git a/source/dex_workbench/tests/test_tracking_trajectory.py b/source/dex_workbench/tests/test_tracking_trajectory.py new file mode 100644 index 0000000..e4684ed --- /dev/null +++ b/source/dex_workbench/tests/test_tracking_trajectory.py @@ -0,0 +1,303 @@ +"""CPU regression tests for dex_workbench_tracking.trajectory and CLI; no Isaac imports.""" + +import copy +import json +import os +import subprocess +import sys +import tempfile +import unittest +from pathlib import Path + +import h5py +import numpy as np +from dex_workbench_tracking.cli import publish_validated, synthetic, write +from dex_workbench_tracking.trajectory import ContractError, load, require_dynamic_replay_ready, sample + + +class TrackingTrajectoryTests(unittest.TestCase): + def setUp(self): + self.directory = tempfile.TemporaryDirectory() + self.addCleanup(self.directory.cleanup) + self.path = Path(self.directory.name) / "fixture.hdf5" + self.manifest = { + "manifest_version": "l20_asset_manifest_v1", + "asset_sha256": "a" * 64, + "root_link": "test_root", + "joints": [ + {"name": "finger_a", "lower_rad": -1, "upper_rad": 1}, + {"name": "finger_b", "lower_rad": -1, "upper_rad": 1}, + ], + "source_urdf": { + "mimic": [{"joint": "finger_b", "reference": "finger_a", "multiplier": 0.5, "offset_rad": 0.0}] + }, + } + write(self.path, synthetic(self.manifest)) + + def edit(self, function): + with h5py.File(self.path, "r+") as file: + function(file) + + def test_load_roundtrip(self): + data = load(self.path, self.manifest) + self.assertEqual(data.joint_names, ("finger_a", "finger_b")) + self.assertEqual(data.metadata["provenance"], "synthetic") + self.assertEqual(data.episodes["demo_000000"].joint_position.shape, (121, 2)) + + def test_identity_and_order_mismatch(self): + for key, value in (("asset_sha256", "b" * 64), ("root_link", "other")): + manifest = copy.deepcopy(self.manifest) + manifest[key] = value + with self.subTest(key=key), self.assertRaises(ContractError): + load(self.path, manifest) + manifest = copy.deepcopy(self.manifest) + manifest["joints"].reverse() + with self.assertRaisesRegex(ContractError, "order"): + load(self.path, manifest) + + def test_missing_attributes(self): + for name in ("root_link", "provenance", "source_description", "scale_to_meters", "metric_scale_provenance"): + with h5py.File(self.path, "r+") as file: + value = file.attrs[name] + del file.attrs[name] + with self.subTest(name=name), self.assertRaises(ContractError): + load(self.path) + self.edit(lambda file: file.attrs.__setitem__(name, value)) + + def test_wrong_attributes(self): + for name, bad in ( + ("schema_version", "v2"), + ("embodiment", "L30"), + ("hand_side", "right"), + ("provenance", "unknown"), + ("asset_sha256", "xyz"), + ("root_link", ""), + ("scale_to_meters", 0.0), + ("scale_to_meters", float("nan")), + ("scale_to_meters", "1"), + ("scale_to_meters", [1.0]), + ): + with h5py.File(self.path, "r+") as file: + original = file.attrs[name] + file.attrs[name] = bad + with self.subTest(name=name, bad=bad), self.assertRaises(ContractError): + load(self.path) + self.edit(lambda file: file.attrs.__setitem__(name, original)) + + def test_duplicate_or_wrong_joint_strings(self): + self.edit(lambda file: file["metadata/joint_names"].__setitem__(1, "finger_a")) + with self.assertRaisesRegex(ContractError, "duplicate"): + load(self.path) + with h5py.File(self.path, "r+") as file: + del file["metadata/joint_names"] + file["metadata"].create_dataset("joint_names", data=[b"a", b"b"]) + with self.assertRaisesRegex(ContractError, "UTF-8"): + load(self.path) + + def test_invalid_transform(self): + for bad in (np.diag([2, 1, 1, 1]), np.diag([-1, 1, 1, 1]), np.zeros((4, 4))): + self.edit(lambda file: file["metadata/world_from_source"].__setitem__(slice(None), bad)) + with self.assertRaisesRegex(ContractError, "world_from_source"): + load(self.path) + + def test_transform_is_not_applied_again(self): + self.edit(lambda file: file["metadata/world_from_source"].__setitem__((0, 3), 10)) + data = load(self.path) + self.assertEqual(data.world_from_source[0, 3], 10) + self.assertEqual(data.episodes["demo_000000"].wrist_position[0, 0], 0) + + def test_episode_name_and_empty_group(self): + with h5py.File(self.path, "r+") as file: + file.move("episodes/demo_000000", "episodes/demo_bad") + with self.assertRaisesRegex(ContractError, "invalid episode"): + load(self.path) + self.edit(lambda file: file.__delitem__("episodes/demo_bad")) + with self.assertRaisesRegex(ContractError, "nonempty"): + load(self.path) + + def test_shapes_and_dtypes(self): + for name, value in ( + ("time", np.arange(121, dtype=np.float32)), + ("wrist_position", np.zeros((121, 2), dtype=np.float32)), + ("joint_position", np.zeros((121, 2), dtype=np.float64)), + ("valid", np.ones(121, dtype=np.uint8)), + ): + with h5py.File(self.path, "r+") as file: + group = file["episodes/demo_000000"] + original = group[name][...] + del group[name] + group.create_dataset(name, data=value) + with self.subTest(name=name), self.assertRaises(ContractError): + load(self.path) + with h5py.File(self.path, "r+") as file: + group = file["episodes/demo_000000"] + del group[name] + group.create_dataset(name, data=original) + + def test_bad_time(self): + for index, value in ((0, -1), (1, 0), (2, 0.001), (1, np.nan)): + with h5py.File(self.path, "r+") as file: + time = file["episodes/demo_000000/time"] + original = time[index] + time[index] = value + with self.subTest(index=index, value=value), self.assertRaises(ContractError): + load(self.path) + self.edit(lambda file: file["episodes/demo_000000/time"].__setitem__(index, original)) + + def test_nan_even_in_invalid_frame(self): + self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(2, False)) + self.edit(lambda file: file["episodes/demo_000000/joint_position"].__setitem__((2, 0), np.nan)) + with self.assertRaisesRegex(ContractError, "NaN/Inf"): + load(self.path) + + def test_quaternion_normalization_and_sign(self): + self.edit(lambda file: file["episodes/demo_000000/wrist_quaternion"].__setitem__(2, [2, 0, 0, 0])) + with self.assertRaisesRegex(ContractError, "unit"): + load(self.path) + self.edit(lambda file: file["episodes/demo_000000/wrist_quaternion"].__setitem__(2, [-1, 0, 0, 0])) + with self.assertRaisesRegex(ContractError, "sign"): + load(self.path) + + def test_all_invalid_rejected(self): + self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(slice(None), False)) + with self.assertRaisesRegex(ContractError, "no valid"): + load(self.path) + + def test_limits_and_mimic_inconsistency(self): + self.edit(lambda file: file["episodes/demo_000000/joint_position"].__setitem__((2, 0), 2)) + with self.assertRaisesRegex(ContractError, "limit violation"): + load(self.path, self.manifest) + self.edit(lambda file: file["episodes/demo_000000/joint_position"].__setitem__((2, 0), 0.2)) + with self.assertRaisesRegex(ContractError, "mimic"): + load(self.path, self.manifest) + + def test_slerp_and_linear_interpolation(self): + episode = load(self.path).episodes["demo_000000"] + # Isolated two-frame analytic rotation with a 180-degree endpoint, wxyz. + from dex_workbench_tracking.trajectory import Episode + + simple = Episode( + np.array([0.0, 1.0]), + np.array([[0, 0, 0], [2, 4, 6]], dtype=np.float32), + np.array([[1, 0, 0, 0], [0, 0, 0, 1]], dtype=np.float32), + np.array([[0, 0], [1, 2]], dtype=np.float32), + np.array([True, True]), + ) + sampled = sample(simple, [0, 0.5, 1]) + np.testing.assert_allclose(sampled.wrist_quaternion[1], [np.sqrt(0.5), 0, 0, np.sqrt(0.5)], atol=1e-7) + np.testing.assert_allclose(sampled.wrist_position[1], [1, 2, 3]) + np.testing.assert_allclose(sampled.joint_position[1], [0.5, 1]) + np.testing.assert_allclose(sample(episode, episode.time).joint_position, episode.joint_position) + np.testing.assert_allclose(np.linalg.norm(sample(episode, [0, 0.001]).wrist_quaternion, axis=1), 1, atol=1e-7) + + def test_no_extrapolation_or_bad_queries(self): + episode = load(self.path).episodes["demo_000000"] + for query in ([], [-0.1], [2.1], [0, 0], [1, 0], [np.nan], [[0.1]]): + with self.subTest(query=query), self.assertRaises(ContractError): + sample(episode, query) + + def test_invalid_gap_never_bridged(self): + self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(60, False)) + episode = load(self.path).episodes["demo_000000"] + for query in ([1], [0.99], [1.01], [0, 2]): + with self.subTest(query=query), self.assertRaisesRegex(ContractError, "invalid"): + sample(episode, query) + self.assertEqual(len(sample(episode, [0, 0.5]).time), 2) + self.assertEqual(len(sample(episode, [1.5, 2]).time), 2) + + def test_no_overwrite(self): + before = self.path.read_bytes() + with self.assertRaises(OSError): + write(self.path, synthetic(self.manifest)) + self.assertEqual(self.path.read_bytes(), before) + + def test_validated_publication_failure_leaves_no_output(self): + output = Path(self.directory.name) / "invalid.hdf5" + data = synthetic(self.manifest) + data.episodes["demo_000000"].wrist_quaternion[:] = 0 + with self.assertRaises(ContractError): + publish_validated(output, data, self.manifest) + self.assertFalse(output.exists()) + self.assertFalse(list(output.parent.glob(".l20-tracking-*"))) + + def test_validated_publication_preserves_existing_file(self): + before = self.path.read_bytes() + with self.assertRaises(FileExistsError): + publish_validated(self.path, synthetic(self.manifest), self.manifest) + self.assertEqual(self.path.read_bytes(), before) + self.assertFalse(list(self.path.parent.glob(".l20-tracking-*"))) + + def test_cli_full_turn_downsampling_roundtrip(self): + with h5py.File(self.path, "r+") as file: + group = file["episodes/demo_000000"] + time = group["time"][:] + quaternion = np.zeros((len(time), 4), dtype=np.float32) + quaternion[:, 0] = np.cos(np.pi * time / 2) + quaternion[:, 3] = np.sin(np.pi * time / 2) + group["wrist_quaternion"][:] = quaternion + load(self.path, self.manifest) + manifest = Path(self.directory.name) / "manifest.json" + manifest.write_text(json.dumps(self.manifest)) + output = Path(self.directory.name) / "sparse.hdf5" + result = self.run_cli("resample", self.path, "--manifest", manifest, "--hz", "0.5", "--output", output) + self.assertEqual(result.returncode, 0, result.stderr) + quaternion = load(output, self.manifest).episodes["demo_000000"].wrist_quaternion + self.assertEqual(quaternion.shape, (2, 4)) + self.assertGreaterEqual(np.dot(quaternion[0], quaternion[1]), 0) + np.testing.assert_allclose(quaternion[:, 0], 1, atol=1e-6) + # Hemisphere repair cannot recover motion lost through undersampling. + np.testing.assert_allclose(quaternion[:, 1:], 0, atol=1e-6) + + def test_dynamic_gate_cannot_be_enabled_by_flag(self): + for manifest in (self.manifest, {"dynamic_replay_ready": True, "coupling_status": "VERIFIED"}): + with self.assertRaisesRegex(ContractError, "BLOCKED"): + require_dynamic_replay_ready(manifest) + + def run_cli(self, *args): + return subprocess.run( + [sys.executable, "-m", "dex_workbench_tracking.cli", *map(str, args)], + capture_output=True, + text=True, + timeout=30, + env=os.environ.copy(), + ) + + def test_cli_validate_and_resample(self): + manifest = Path(self.directory.name) / "manifest.json" + manifest.write_text(json.dumps(self.manifest)) + output = Path(self.directory.name) / "resampled.hdf5" + result = self.run_cli("resample", self.path, "--manifest", manifest, "--hz", "100", "--output", output) + self.assertEqual(result.returncode, 0, result.stderr) + data = load(output, self.manifest) + self.assertEqual(len(data.episodes["demo_000000"].time), 201) + result = self.run_cli("validate", output) + self.assertEqual(result.returncode, 0, result.stderr) + self.assertEqual(json.loads(result.stdout)["asset_compatibility"], "NOT_CHECKED") + self.edit(lambda file: file.attrs.__setitem__("hand_side", "right")) + result = self.run_cli("validate", self.path) + self.assertNotEqual(result.returncode, 0) + self.assertIn("FAIL", result.stderr) + + def test_cli_gate_and_invalid_gap_fail_closed(self): + manifest = Path(self.directory.name) / "manifest.json" + manifest.write_text(json.dumps(self.manifest)) + result = self.run_cli("replay-check", "--manifest", manifest) + self.assertEqual(result.returncode, 2) + self.assertIn("BLOCKED", result.stderr) + output = Path(self.directory.name) / "resampled.hdf5" + self.edit(lambda file: file["episodes/demo_000000/valid"].__setitem__(60, False)) + result = self.run_cli("resample", self.path, "--manifest", manifest, "--hz", "60", "--output", output) + self.assertEqual(result.returncode, 1) + self.assertFalse(output.exists()) + + def test_cpu_import_does_not_register_isaac(self): + code = ( + "import dex_workbench_tracking.trajectory, sys; " + "assert not any(k.startswith(('isaac', 'omni', 'dex_workbench.')) for k in sys.modules)" + ) + result = subprocess.run([sys.executable, "-c", code], capture_output=True, text=True, timeout=30) + self.assertEqual(result.returncode, 0, result.stderr) + + +if __name__ == "__main__": + unittest.main()