release: SkillFactory v0.0.1

Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
This commit is contained in:
2026-10-10 09:58:30 +08:00
parent 180f8f7f97
commit 7f39abb613
52 changed files with 4370 additions and 147 deletions
+6 -2
View File
@@ -13,6 +13,8 @@
**/*.usda
**/*.usdc
**/*.usdz
# Hand-authored offline ground is source data, not a generated asset cache.
!source/SkillFactory/SkillFactory/legs/resources/ground.usda
# Python
.DS_Store
@@ -64,8 +66,8 @@ _build
# Teleop Recorded Dataset
/datasets/
# Tests
/tests/
# Reproducible local assets (URDF, mesh and USD cache)
/assets/generated/
# Docker history
.isaac-lab-docker-history
@@ -82,6 +84,8 @@ _build
# Dev-time files, generated stuff
**/__*
# Python package entry points must be versioned for task auto-discovery.
!**/__init__.py
# Isaac Lab CI environments in native mode
**/_isaaclab_install_ci_*
+48 -116
View File
@@ -1,135 +1,67 @@
# Template for Isaac Lab Projects
# SkillFactory v0.0.1
## Overview
仓库:[chenlin/SkillFactory](https://gitea.robotquan.com/chenlin/SkillFactory)。
This project/repository serves as a template for building projects or extensions based on Isaac Lab.
It allows you to develop in an isolated environment, outside of the core Isaac Lab repository.
真实单腿资产的导入、固定支座仿真与共享策略训练原型。当前覆盖 **Go2 FL / Go1 FL**;第一周 Go2 基准已完成验收,共享 PPO 仅通过短更新测试,**尚无已收敛、冻结或验证泛化的 Skill**。
**Key Features:**
## 运行环境
- `Isolation` Work outside the core Isaac Lab repository, ensuring that your development efforts remain self-contained.
- `Flexibility` This template is set up to allow your code to be run as an extension in Omniverse.
**Keywords:** extension, template, isaaclab
## Installation
- Install Isaac Lab by following the [installation guide](https://isaac-sim.github.io/IsaacLab/main/source/setup/installation/index.html).
We recommend using the conda or uv installation as it simplifies calling Python scripts from the terminal.
- Clone or copy this project/repository separately from the Isaac Lab installation (i.e. outside the `IsaacLab` directory):
- Using a python interpreter that has Isaac Lab installed, install the library in editable mode using:
```bash
# use 'PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda
python -m pip install -e source/SkillFactory
- Verify that the extension is correctly installed by:
- Listing the available tasks:
Note: It the task name changes, it may be necessary to update the search pattern `"Template-"`
(in the `scripts/list_envs.py` file) so that it can be listed.
```bash
# use 'FULL_PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda
python scripts/list_envs.py
```
- Running a task:
```bash
# use 'FULL_PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda
python scripts/<RL_LIBRARY>/train.py --task=<TASK_NAME>
```
- Running a task with dummy agents:
These include dummy agents that output zero or random agents. They are useful to ensure that the environments are configured correctly.
- Zero-action agent
```bash
# use 'FULL_PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda
python scripts/zero_agent.py --task=<TASK_NAME>
```
- Random-action agent
```bash
# use 'FULL_PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda
python scripts/random_agent.py --task=<TASK_NAME>
```
### Set up IDE (Optional)
To setup the IDE, please follow these instructions:
- Run VSCode Tasks, by pressing `Ctrl+Shift+P`, selecting `Tasks: Run Task` and running the `setup_python_env` in the drop down menu.
When running this task, you will be prompted to add the absolute path to your Isaac Sim installation.
If everything executes correctly, it should create a file .python.env in the `.vscode` directory.
The file contains the python paths to all the extensions provided by Isaac Sim and Omniverse.
This helps in indexing all the python modules for intelligent suggestions while writing code.
### Setup as Omniverse Extension (Optional)
We provide an example UI extension that will load upon enabling your extension defined in `source/SkillFactory/SkillFactory/ui_extension_example.py`.
To enable your extension, follow these steps:
1. **Add the search path of this project/repository** to the extension manager:
- Navigate to the extension manager using `Window` -> `Extensions`.
- Click on the **Hamburger Icon**, then go to `Settings`.
- In the `Extension Search Paths`, enter the absolute path to the `source` directory of this project/repository.
- If not already present, in the `Extension Search Paths`, enter the path that leads to Isaac Lab's extension directory directory (`IsaacLab/source`)
- Click on the **Hamburger Icon**, then click `Refresh`.
2. **Search and enable your extension**:
- Find your extension under the `Third Party` category.
- Toggle it to enable your extension.
## Code formatting
We have a pre-commit template to automatically format your code.
To install pre-commit:
本机已验证:Isaac Sim 6.0.1、Isaac Lab 3.0.0(core 6.1.17)、Python 3.12、Torch 2.10 CUDA、Kit `PhysxCfg`。不修改外部 Lab/Unitree,不安装新运行时。
```bash
pip install pre-commit
# 仓库根目录;避免旧 conda/venv 抢占解释器
unset CONDA_PREFIX VIRTUAL_ENV
export PYTHONPATH="$PWD/source/SkillFactory${PYTHONPATH:+:$PYTHONPATH}"
export PYTHONUNBUFFERED=1
LAB=/home/cen/IsaacLab/isaaclab.sh
```
Then you can run pre-commit with:
路径可按机器调整。**不要在仓库根执行 `uv sync`**:根 `pyproject.toml` 的依赖源仍是 Lab 模板配置;使用已有 Lab 解释器。需要安装项目时用该解释器 `-m pip install -e source/SkillFactory`。Kit 必须在任务配置/动作类导入之前启动;以下仿真入口已处理。
## 通用单腿:导入 → 验证 → 短训练
```bash
pre-commit run --all-files
for leg in go2_fl go1_fl; do
"$LAB" -p scripts/assets/extract_leg.py --spec "configs/legs/$leg.json"
"$LAB" -p scripts/assets/convert_leg.py --bundle "assets/generated/legs/$leg"
done
"$LAB" -p scripts/leg_worker.py --bundle assets/generated/legs/go2_fl \
--num_envs 64 --steps 120 --output logs/leg_interface/verify
"$LAB" -p scripts/train_leg_skill.py \
--bundles assets/generated/legs/go2_fl assets/generated/legs/go1_fl \
--num_envs 16 --horizon 32 --updates 1 --output logs/leg_interface/train_smoke
```
## Troubleshooting
新腿通过 JSON 声明源路径、安装切口、辅助支链、受控关节及足端,不按 Go2 名称硬编码。`extract_leg.py --source-root` 可覆盖示例机器路径。重提取后必须重新转换;分层 USD、网格、许可证与 manifest 整体保留。
### Pylance Missing Indexing of Extensions
GUI:给验证命令加 `--num_envs 1 --viz kit`;默认无 GUI。接触验证加 `--contact-action 0,0,1`,该动作只适用于示例 Go2/Go1 的声明顺序,不是任意腿的通用压地指令。
In some VsCode versions, the indexing of part of the extensions is missing.
In this case, add the path to your extension in `.vscode/settings.json` under the key `"python.analysis.extraPaths"`.
## 保留的第一周基准
```json
{
"python.analysis.extraPaths": [
"<path-to-ext-repo>/source/SkillFactory"
]
}
```bash
"$LAB" -p scripts/assets/extract_go2_leg.py
"$LAB" -p scripts/assets/convert_go2_leg.py
"$LAB" -p scripts/anchor_smoke.py --num_envs 1 --mode contact --steps 600 --viz kit
# 完整验收约 11 分钟;开发时用 --suite contract
"$LAB" -p scripts/verify_anchor.py --suite full --num_envs 64 \
--duration 600 --reset_count 1334 --output logs/week1_anchor/acceptance
```
### Pylance Crash
## 测试与接口
If you encounter a crash in `pylance`, it is probable that too many files are indexed and you run out of memory.
A possible solution is to exclude some of omniverse packages that are not used in your project.
To do so, modify `.vscode/settings.json` and comment out packages under the key `"python.analysis.extraPaths"`
Some examples of packages that can likely be excluded are:
```bash
"$LAB" -p scripts/list_envs.py --show_presets
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 "$LAB" -p -m pytest tests -q
# 可选:额外启动旧任务 B=1/16/64 Kit 集成测试
PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 ANCHOR_RUNTIME_TESTS=1 "$LAB" -p -m pytest tests/test_anchor_contract.py -q
"$LAB" -p -m ruff check source/SkillFactory scripts tests
```
```json
"<path-to-isaac-sim>/extscache/omni.anim.*" // Animation packages
"<path-to-isaac-sim>/extscache/omni.kit.*" // Kit UI tools
"<path-to-isaac-sim>/extscache/omni.graph.*" // Graph UI tools
"<path-to-isaac-sim>/extscache/omni.services.*" // Services tools
...
```
- [`docs/leg_interface.md`](docs/leg_interface.md):资产/token/动作契约、验收阈值与证据、实现边界。
- `SkillFactory-LegSkill-v0`:结构化 token 与专用 PPO 原型入口;不能直接交给仅接受平坦观测的旧 RSL-RL trainer。
- `SkillFactory-SingleLegAnchor-Go2-v0`:保留 `[B,19]` 验收接口。
- `Template-Skillfactory-v0`:原 Cartpole 与 `scripts/rsl_rl/` 入口保留。
生成资产、日志、checkpoint 不入库;换机器需重建复验。当前采样器每轮重启 Kit,仅适合短接口测试,**不要据此直接长训或宣称高吞吐**。后续优先常驻采样、留出形态评估,再实现 latent 学习和冻结调用;固定支座结果不证明整机迁移。
+17
View File
@@ -0,0 +1,17 @@
{
"schema_version": 1,
"part_id": "go1_fl",
"source_root": "/home/cen/Embodied_Workspace/unitree_ros/robots/go1_description",
"urdf": "urdf/go1.urdf",
"package_name": "go1_description",
"license_file": "../../LICENSE",
"license_id": "BSD-3-Clause",
"mount_joint": "FL_hip_joint",
"auxiliary_root_joints": ["FL_hip_rotor_joint"],
"joints": [
{"name": "FL_hip_joint", "q_default": 0.0, "action_scale": 0.25, "kp": 25.0, "kd": 0.5},
{"name": "FL_thigh_joint", "q_default": 0.8, "action_scale": 0.25, "kp": 25.0, "kd": 0.5},
{"name": "FL_calf_joint", "q_default": -1.5, "action_scale": 0.25, "kp": 25.0, "kd": 0.5}
],
"foot_body": "FL_foot"
}
+17
View File
@@ -0,0 +1,17 @@
{
"schema_version": 1,
"part_id": "go2_fl",
"source_root": "/home/cen/Embodied_Workspace/unitree_ros/robots/go2_description",
"urdf": "urdf/go2_description.urdf",
"package_name": "go2_description",
"license_file": "../../LICENSE",
"license_id": "BSD-3-Clause",
"mount_joint": "FL_hip_joint",
"auxiliary_root_joints": ["FL_hip_rotor_joint"],
"joints": [
{"name": "FL_hip_joint", "q_default": 0.0, "action_scale": 0.25, "kp": 25.0, "kd": 0.5},
{"name": "FL_thigh_joint", "q_default": 0.8, "action_scale": 0.25, "kp": 25.0, "kd": 0.5},
{"name": "FL_calf_joint", "q_default": -1.5, "action_scale": 0.25, "kp": 25.0, "kd": 0.5}
],
"foot_body": "FL_foot"
}
+109
View File
@@ -0,0 +1,109 @@
# 单腿接口与验收
## 1. 实现范围
```text
LegSpec JSON → URDF 子树/资源/许可 → USD 与映射
→ 固定支座环境 → 形态/状态 tokens → 共享 actor/critic → PPO
```
- 已实现真实 Go2 FL、Go1 FL 导入、CUDA 批量仿真、indexed reset、目标条件共享 PPO 短更新。
- 支持独立 bounded revolute 树、显式辅助 fixed 支链、一个足端、sphere/box 间隙探针;mimic、prismatic/continuous、闭链、柔性结构和 mesh 足底近似明确不支持。
- **未实现** ModSkill 式学习 latent、跨部件协调、冻结 SkillBundle、高层调用及真实留出形态泛化。人工 goal、URMA 内部 action intent、保存 checkpoint 都不等于已学得可复用 Skill。
- Go2/Go1 都是 3 DoF;生成的 1/2/4 DoF 夹具验证接口,不代替真实不同 DoF 或迁移实验。
## 2. 资产与物理契约
公共入口:`SkillFactory.legs.LegSpec` / `JointSpec`、`legs.urdf.extract_leg`、`legs.spec.load_bundle`。示例 `configs/legs/{go2_fl,go1_fl}.json`。
| 声明 | 含义 |
| --- | --- |
| source_root / urdf / package_name / license_file / license_id | 来源、资源解析及许可 |
| mount_joint / auxiliary_root_joints | 切口与保留支链;辅助 roots 必须 fixed 且共用源 parent |
| joints | 受控关节的**声明顺序**、q_default、action_scale、Kp/Kd |
| foot_body / foot_collision_index | 独立传感 body 与用于 gap 的碰撞体 |
| soft_limit_factor / physics_dt / decimation | 软限位与物理/控制时间步 |
提取按树而非名称前缀;全部 moving joints 必须受控,保留足端/rotor/固定碰撞辅助。合成 `anchor_base` 无碰撞、质量 1 kg、惯性对角 0.001 kg·m²;固定安装座,不冻结 hip。anchor frame 为安装关节零位 frame,源 parent 到 anchor 的变换保存在 `manifest.mount`,供未来装配使用。
输出:`leg.urdf`、asset manifest v2、完整资源、`LICENSE`、分层 USD、`conversion.json`。加载校验 URDF/资源/许可 hash、执行顺序、转换 manifest hash、独立 body/joint 映射、固定支座及层依赖;拒绝逃逸路径、输出覆盖源目录及越界 symlink。改描述/重提取后必须重转换,资产目录整体迁移。
真实 Unitree 资产保留 BSD-3-Clause 许可,不修改原文件。有源惯性保留完整 tensor 并检验正定;Go2 `FL_calflower/FL_calflower1` 缺少惯性,PhysX 实测估算质量约 0.0247086/0.0226430 kg,不能冒充厂家数据。质量尺度为源已知惯性质量和,排除 anchor 和未知估算质量;有效导入 body masses 随运行报告保存。
默认显式 Ideal PD,示例 Kp=25、Kd=0.5,导入器 drive 关闭;限矩/限速取 URDF,目标为 `q_default + action_scale * clamp(action,-1,1)`,再按软范围裁剪。Go2 默认 `[0,0.8,-1.5]` rad、scale=0.25;限矩 `[23.7,23.7,45.43]` N·m、限速 `[30.1,30.1,15.7]` rad/s。物理 0.002 s / 控制 0.01 s。
新任务用本地 `legs/resources/ground.usda`(z=0、静/动摩擦 0.8/0.6、恢复系数 0);旧基准地面保持原配置。足端显式启用 contact reporter;运行入口验证 ground collider 和传感器唯一映射。
## 3. 通用任务 `SkillFactory-LegSkill-v0`
**Kit 启动后**构造 `LegEnvCfg(bundle_dir=..., max_joints=8)` 并 `gym.make(...)`。默认悬空高度为保守足端路径长度 +0.1 m;ReachCommand 对实际动作范围内姿态做 FK,生成运动学可达的足端 link 位置目标。reward 为归一化 tracking、effort 与 action-rate 惩罚,不保证目标动态可行或已学到步态。
`obs['policy']` 字典:浮点 CUDA float32、mask bool;anchor 坐标、米/弧度/SI;四元数 **xyzw**、世界 z 向上。完整字段顺序/归一化由 `legs.tokens.FEATURE_SCHEMA` 定义,并存入 checkpoint。
| 字段 | 形状 |
| --- | --- |
| joint_description / joint_observation / joint_mask | `[B,Jmax,25]` / `[B,Jmax,3]` / `[B,Jmax]` |
| effector_description / effector_observation / effector_mask | `[B,1,16]` / `[B,1,11]` / `[B,1]` |
| context / goal | `[B,10]` / `[B,3]` |
- joint description 含 nominal 轴/位置、范围/控制规格、source child mass、COM 惯性及偏移;COM/inertia 转到 anchor nominal frame,**不做 fixed-subtree 复合惯性聚合**。
- joint observation:`(q-q_nominal)/scale`、`dq/limit`、最后已施加动作(ActionManager.action)。
- effector:link 原点位置/线速度、ground-filtered **法向**力、接触标志(>1 N)、选定碰撞体 gap。不是切向摩擦力、实测电机扭矩或六维 wrench;其他碰撞体的全脚掌最小 gap 未实现。
- context 保留重力、dt、地面相对高度、长度/已知质量、限矩对重力力矩比、绝对限速、缺失惯性标志,避免归一化擦掉物理差异。
- 当前必需传感器,无噪声/DR;未来随机化物理属性必须同步描述,不能沿用静态缓存。
`mdp.state(env).snapshot()` 返回新 token 视图;跨 step/reset 保存须 clone。`state(env).actions([B,Jmax])` 去 padding 后再调用 `env.step([B,N])`;**Gym action space 是实际 N**。虚拟槽不驱动,也不参与路由/log-prob/entropy/梯度。
`reset(env_ids=...)` 支持全量、索引、空集合 no-op;拒绝负/越界/重复/非整数索引及空 reset reseed。复位清理目标、动作/历史、episode;不重建 stage、不推进物理时间,不改变未选实例。非法原生动作在 ActionManager 改历史前拒绝。timeout 使用 reset 前的 terminal tokens bootstrap,真 termination 不 bootstrap。
## 4. 训练与失败语义
`LegActorCritic`:静态描述 MLP 的逐 latent 特征 softmax 路由动态状态、共享核心、共享逐关节高斯 decoder。不是大型 Transformer 或完整 ModSkill 复现。
不同拓扑按独立同构 worker 采样,只在 PPO 层合并,不能认为 `replicate_physics=True` 支持任意拓扑混装。各资产等量采样、共用采样权重/optimizer,必须同 physics/control dt;device 显式传给 worker。保存未裁剪动作的 log-prob,实际执行再裁剪。更新前检验概率一致性;GAE/旧 rollout 梯度分离,拒绝空/非有限输入。
当前每轮重启 Kit、CPU 文件交换,**仅适合短测试,不适合直接长训或吞吐结论**。checkpoint 仍标 `trained_skill=false`。
运行报告先写 `passed=false/status=running`,环境成功关闭后才提交成功;JSON 原子替换并拒绝 NaN/Inf。关闭异常/子进程失败/critical log 均保留非零退出码,检查 error/fatal/contact buffer overflow,不能只看进程退出 0。优化按字段读取观测,不缓存跨 reset 状态;FK 常量与 box 投影复用/向量化。
## 5. 保留基准 `SkillFactory-SingleLegAnchor-Go2-v0`
接口不变:CUDA float32 actions `[B,3]`,policy `[B,19]`:
| 切片 | 字段 |
| --- | --- |
| 0:3 / 3:6 | q-q_default [rad] / dq [rad/s] |
| 6:9 / 9:12 | foot link 位置 [m] / 线速度 [m/s],anchor frame |
| 12:15 / 15:16 | ground 法向力 [N] / contact float 0/1 |
| 16:19 | ActionManager.prev_action(旧接口历史语义,区别于新任务) |
旧默认支座高度 0.344598 m、足球 r=0.022 m / local offset=(-0.002,0,0);默认 timeout 10 s,验收禁用。旧空 reset 在验证调用端做 no-op;不要假设底层普通 ManagerBasedRLEnv 支持空索引。诊断 `single_leg_anchor.mdp.observations.diagnostics` 包含 q/dq/目标、限幅前后命令 torque、gap、anchor pose、`[B,5,1,3]` 历史;torque 非实测、`reaction_wrench=None/valid=False`。
## 6. 验收证据与限制
**第一周历史完整验收**:`logs/week1_anchor/acceptance/result.json`,`passed=true/milestone_complete=true`;原 Go2 URDF SHA256 `7d19fe48e2e689ee1a032ab99f2a4a8b671d87e73de48d3e65811682a5b48b9e`,源提交 `daadf41ee9afce8f90fdc09a98506012691fa122`。
| 项目 | 实测 | 保留阈值 |
| --- | --- | --- |
| B=1/16/64 接口 | `[B,19]` CUDA、顺序/有限性通过 | shape/order/finite 匹配 |
| 悬空 60 s + 2 s warmup,0.1 rad / 0.5 Hz | RMS `[0.043313,0.017097,0.009654]` rad,根漂移/转角 0 | RMS≤0.10,根≤1e-4 m/1e-3 rad,硬限位容差0.01 rad、torque不越界 |
| 20 次抬脚/压地 | 离地 0 N;最小稳定平均 Fz 31.3321 N | gap≥1 cm/力<0.1 N,压地>1 N |
| 接触响应/穿透 | 延迟0控制周期、最大1.40e-6 m | ≤1周期 / ≤5 mm |
| 独立1 kg小球标定 | 9.810026 N | m·g误差≤20% |
| 耐久 | 64×600 s、60,000控制/300,000物理步 | 完整时长、无非有限/意外reset/限位异常 |
| reset隔离 | 1,334操作=1,001非空+333no-op;误差0、prim数2384不变 | ≥1000真reset,q/dq≤1e-5、未选≤1e-6、历史清零、不重建 |
| 资源 | device used 3,814,981,632 B前后不变 | warmup后增长≤5% |
历史墙钟 576.748 s(约6,658环境控制步/s)、reset均值/最大1.785/4.620 ms;没有墙钟实时门槛。显存包含 Warp/PhysX,不能只报 Torch 分配。固定支座承担载荷,足端力**不等于腿重量**。
**本次代码复验**放在 `logs/code_review/`,不覆盖历史验收:
- 全部定向测试 **43 passed**(含旧任务 B=1/16/64 Kit 集成),全仓库 Ruff / `git diff --check` 通过。
- `go2_64`、`go1_contact_64`、`fixture2_16`:原生/token动作、快照一致性、任意轴/box、索引/空reset与接触复验通过。
- `train_mixed_smoke`:三组各16环境、32步、1次共享PPO,概率与checkpoint一致性通过;`trained_skill=false`。
- `anchor_debug`:缩短的旧full套件通过,明确 `milestone_complete=false`;最终运行日志无error/fatal/buffer overflow。
缩短时长的通过不等于重做600 s验收;生成夹具或一次 PPO 更新不证明真实泛化/收敛。
生成缓存与证据不入库,迁移机器需重建复验。后续顺序:常驻采样与训练/留出评估 → reference/goal-to-latent 学习 → 冻结低层与高层复用 → 单独评估移动基座/整机协调。
研究参考:[URMA](https://arxiv.org/html/2409.06366v2)、[ModSkill](https://arxiv.org/html/2502.14140v1)。本项目是固定支座单腿工程改编,不是原论文全身模仿或零样本迁移复现。
+132
View File
@@ -0,0 +1,132 @@
"""Shared finite benchmark launcher; preserve failures through Kit fast shutdown."""
import argparse
import json
import os
import sys
import traceback
from pathlib import Path
from runtime_common import finish_run, write_json
TASK = "SkillFactory-SingleLegAnchor-Go2-v0"
def launch(description, configure_parser, run):
if not __debug__:
raise RuntimeError("Benchmark assertions require Python without -O/PYTHONOPTIMIZE")
if sys.version_info[:2] != (3, 12):
raise RuntimeError("Use /home/cen/IsaacLab/isaaclab.sh -p with Python 3.12, not the old conda environment")
for key in ("CONDA_PREFIX", "VIRTUAL_ENV"):
if os.environ.get(key):
print(f"[WARNING] Active {key}={os.environ[key]}; actual interpreter: {sys.executable}")
from isaaclab_tasks.utils import add_launcher_args, launch_simulation, resolve_task_config, setup_preset_cli
parser = argparse.ArgumentParser(description=description)
parser.add_argument("--task", default=TASK)
parser.add_argument("--num_envs", type=int, default=64)
parser.add_argument("--seed", type=int, default=42)
parser.add_argument("--anchor_height", type=float, default=None)
parser.add_argument("--output", type=Path, default=Path("logs/week1_anchor/smoke"))
configure_parser(parser)
add_launcher_args(parser)
args, hydra_args = setup_preset_cli(parser)
sys.argv = [sys.argv[0]] + hydra_args
# Our selected backend is known to require Kit. Start it before resolving
# configs: action classes can import pxr and preload an incompatible USD.
from isaaclab.app import AppLauncher
app = AppLauncher(args).app
code = 0
env = None
args.output.mkdir(parents=True, exist_ok=True)
result = {"passed": False, "status": "running"}
write_json(args.output / "result.json", result)
try:
import SkillFactory.tasks # noqa: F401
cfg, _ = resolve_task_config(args.task, "")
cfg.scene.num_envs = args.num_envs
cfg.seed = args.seed
cfg.terminations.time_out = None # no automatic reset hiding verification failures
if getattr(args, "suite", None) == "stability" and args.anchor_height is None:
args.anchor_height = 0.6
if args.anchor_height is not None:
cfg.scene.robot.init_state.pos = (0.0, 0.0, args.anchor_height)
if getattr(args, "suite", None) == "calibration":
import isaaclab.sim as sim_utils
from isaaclab.assets import RigidObjectCfg
from isaaclab.sensors import ContactSensorCfg
cfg.scene.calibration_probe = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Probe",
spawn=sim_utils.SphereCfg(
radius=0.05,
mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
rigid_props=sim_utils.RigidBodyPropertiesCfg(),
collision_props=sim_utils.CollisionPropertiesCfg(),
activate_contact_sensors=True,
),
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.7, 0.0, 0.3)),
)
cfg.scene.probe_contact = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Probe",
update_period=0.0,
history_length=5,
filter_prim_paths_expr=cfg.scene.foot_contact.filter_prim_paths_expr,
)
# Kit is already launched, so this context performs backend setup but
# does not own app.close(). We retain the failure exit code ourselves.
with launch_simulation(cfg, args):
import gymnasium as gym
import torch
torch.manual_seed(args.seed)
env = gym.make(args.task, cfg=cfg)
u = env.unwrapped
from pxr import UsdPhysics
from isaaclab.sim import get_current_stage
stage = get_current_stage()
grounds = [
str(p.GetPath())
for p in stage.Traverse()
if str(p.GetPath()).startswith("/World/ground/") and p.HasAPI(UsdPhysics.CollisionAPI)
]
expected = cfg.scene.foot_contact.filter_prim_paths_expr
if grounds != expected:
raise RuntimeError(f"Ground collision mapping mismatch: actual={grounds}, expected={expected}")
assert u.scene["foot_contact"].num_sensors == 1
assert u.scene["foot_contact"].data.force_matrix_w.torch.shape == (args.num_envs, 1, 1, 3)
import importlib.metadata
from SkillFactory.assets.go2_leg import CONVERSION, MANIFEST
body_masses = u.scene["robot"].data.body_mass.torch[0].tolist()
asset_report = {
"source": MANIFEST["source"],
"derived_urdf_sha256": MANIFEST["derived_urdf_sha256"],
"conversion": CONVERSION,
"backend": "Kit PhysxCfg",
"python": sys.version,
"packages": {n: importlib.metadata.version(n) for n in ("isaaclab", "isaaclab-physx", "torch")},
"effective_body_mass_kg": dict(zip(u.scene["robot"].body_names, body_masses)),
"missing_inertial_fixed_links": MANIFEST["missing_inertial_fixed_links"],
"inertial_handling": (
"Fixed auxiliary colliders preserved; PhysX mass inferred from geometry/default density"
),
"pd": MANIFEST["control"],
"ground_colliders": grounds,
}
write_json(args.output / "physical_asset.json", asset_report)
result = run(env, args)
print(json.dumps(result, indent=2), flush=True)
except BaseException:
traceback.print_exc()
code = 1
error = traceback.format_exc()
(args.output / "failure.txt").write_text(error)
result = {"passed": False, "error": error}
finally:
finish_run(app, env, args.output / "result.json", result, code)
+55
View File
@@ -0,0 +1,55 @@
#!/usr/bin/env python3
"""Finite Go2 anchor demonstrations; no training and no unbounded random agent."""
import csv
import math
from anchor_common import launch
def configure(parser):
parser.add_argument("--steps", type=int, default=600)
parser.add_argument("--mode", choices=["hold", "sine", "contact"], default="sine")
def run(env, args):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
obs, _ = env.reset()
assert obs["policy"].shape == (args.num_envs, 19)
actions = torch.zeros((args.num_envs, 3), device=u.device)
rows = []
for step in range(args.steps):
if args.mode == "sine":
actions[:] = 0.4 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
elif args.mode == "contact":
actions.zero_()
actions[:, 2] = -1.0 if int(step * u.step_dt) % 2 == 0 else 1.0
obs, _, terminated, timedout, _ = env.step(actions)
assert not terminated.any() and not timedout.any()
assert torch.isfinite(obs["policy"]).all()
d = diagnostics(u)
rows.append([step * u.step_dt, *d["q"][0].tolist(), d["gap"][0].item(), *d["normal_force_w"][0].tolist()])
with (args.output / "trace.csv").open("w", newline="") as f:
writer = csv.writer(f)
writer.writerow(["time_s", "q_hip", "q_thigh", "q_calf", "gap_m", "normal_fx_N", "normal_fy_N", "normal_fz_N"])
writer.writerows(rows)
return {
"passed": True,
"mode": args.mode,
"num_envs": args.num_envs,
"steps": args.steps,
"physics_dt": u.physics_dt,
"control_dt": u.step_dt,
"policy_shape": list(obs["policy"].shape),
"joint_names": u.scene["robot"].joint_names,
"final_q": d["q"][0].tolist(),
"min_gap_m": min(r[4] for r in rows),
"max_normal_fz_N": max(r[7] for r in rows),
}
if __name__ == "__main__":
launch(__doc__, configure, run)
+92
View File
@@ -0,0 +1,92 @@
#!/usr/bin/env python3
"""Convert the extracted Go2 leg with Kit and verify independent physics bodies."""
import argparse
import json
from pathlib import Path
ROOT = Path(__file__).resolve().parents[2]
def main():
if not __debug__:
raise RuntimeError("Asset checks require Python without -O/PYTHONOPTIMIZE")
from isaaclab.app import AppLauncher
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--asset_dir", type=Path, default=ROOT / "assets/generated/go2_fl")
AppLauncher.add_app_launcher_args(parser)
args = parser.parse_args()
app = AppLauncher(args).app
exit_code = 0
try:
from pxr import PhysxSchema, Usd, UsdPhysics
from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg
directory = args.asset_dir.resolve()
cfg = UrdfConverterCfg(
asset_path=str(directory / "go2_fl.urdf"),
usd_dir=str(directory / "usd"),
fix_base=True,
merge_fixed_joints=False,
force_usd_conversion=True,
joint_drive=UrdfConverterCfg.JointDriveCfg(target_type="none"),
run_multi_physics_conversion=False,
)
converter = UrdfConverter(cfg)
stage = Usd.Stage.Open(converter.usd_path)
if stage is None:
raise RuntimeError("Converter produced no readable stage")
root = stage.GetDefaultPrim()
assert root, "No default prim"
bodies = {p.GetName(): str(p.GetPath()) for p in stage.Traverse() if p.HasAPI(UsdPhysics.RigidBodyAPI)}
revolute = [p for p in stage.Traverse() if p.IsA(UsdPhysics.RevoluteJoint)]
fixed = [p for p in stage.Traverse() if p.IsA(UsdPhysics.FixedJoint)]
expected = ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint"]
assert sorted(p.GetName() for p in revolute) == sorted(expected)
assert "FL_foot" in bodies and "anchor_base" in bodies
def is_world_fixed(p):
targets = UsdPhysics.Joint(p).GetBody0Rel().GetTargets() + UsdPhysics.Joint(p).GetBody1Rel().GetTargets()
return len(targets) < 2 or any(not stage.GetPrimAtPath(t).HasAPI(UsdPhysics.RigidBodyAPI) for t in targets)
assert any(is_world_fixed(p) for p in fixed), "No fixed-to-world joint"
foot = stage.GetPrimAtPath(bodies["FL_foot"])
assert any(p.HasAPI(UsdPhysics.CollisionAPI) for p in Usd.PrimRange(foot, Usd.TraverseInstanceProxies())), (
"No foot collision"
)
# Importer 3.0 produces nested rigid bodies. The generic spawner stops
# at the first rigid body (anchor), so explicitly enable the foot here.
PhysxSchema.PhysxContactReportAPI.Apply(foot).CreateThresholdAttr(0.0)
stage.GetRootLayer().Save()
assert foot.HasAPI(PhysxSchema.PhysxContactReportAPI)
used_layers = stage.GetUsedLayers()
missing = [
layer.identifier for layer in used_layers if not layer.anonymous and not Path(layer.realPath).is_file()
]
assert not missing, missing
mapping = {
"usd_path": str(Path(converter.usd_path).relative_to(directory)),
"default_prim": str(root.GetPath()),
"body_paths": {k: v.removeprefix(str(root.GetPath())) for k, v in bodies.items()},
"joint_paths": {p.GetName(): str(p.GetPath()).removeprefix(str(root.GetPath())) for p in revolute},
"used_layers": [
str(Path(layer.realPath).relative_to(directory)) for layer in used_layers if layer.realPath
],
"fixed_to_world": True,
"merge_fixed_joints": False,
}
(directory / "conversion.json").write_text(json.dumps(mapping, indent=2) + "\n")
print(json.dumps(mapping, indent=2))
except BaseException:
import traceback
traceback.print_exc()
exit_code = 1
finally:
app.close(exit_code=exit_code)
if __name__ == "__main__":
main()
+85
View File
@@ -0,0 +1,85 @@
#!/usr/bin/env python3
"""Convert a PartSpec-derived single-leg bundle and validate its USD mapping."""
import argparse
import json
from pathlib import Path
def main():
if not __debug__:
raise RuntimeError("Conversion assertions require Python without -O")
from isaaclab.app import AppLauncher
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--bundle", type=Path, required=True)
AppLauncher.add_app_launcher_args(parser)
args = parser.parse_args()
app = AppLauncher(args).app
code = 0
try:
from SkillFactory.legs.spec import load_bundle
from SkillFactory.legs.urdf import digest
from pxr import PhysxSchema, Usd, UsdPhysics
from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg
directory = args.bundle.resolve()
bundle = load_bundle(directory)
converter = UrdfConverter(
UrdfConverterCfg(
asset_path=str(directory / "leg.urdf"),
usd_dir=str(directory / "usd"),
fix_base=True,
merge_fixed_joints=False,
force_usd_conversion=True,
joint_drive=UrdfConverterCfg.JointDriveCfg(target_type="none"),
run_multi_physics_conversion=False,
)
)
stage = Usd.Stage.Open(converter.usd_path)
assert stage and stage.GetDefaultPrim()
root = stage.GetDefaultPrim()
bodies = {
prim.GetName(): str(prim.GetPath()) for prim in stage.Traverse() if prim.HasAPI(UsdPhysics.RigidBodyAPI)
}
revolute = [prim for prim in stage.Traverse() if prim.IsA(UsdPhysics.RevoluteJoint)]
assert sorted(prim.GetName() for prim in revolute) == sorted(bundle["joint_order"])
assert set(bundle["links"]) | {bundle["anchor_body"]} == set(bodies)
fixed_world = False
for prim in stage.Traverse():
if prim.IsA(UsdPhysics.FixedJoint):
joint = UsdPhysics.Joint(prim)
targets = joint.GetBody0Rel().GetTargets() + joint.GetBody1Rel().GetTargets()
fixed_world |= len(targets) < 2 or any(
not stage.GetPrimAtPath(path).HasAPI(UsdPhysics.RigidBodyAPI) for path in targets
)
assert fixed_world, "Missing fixed-to-world support"
foot = stage.GetPrimAtPath(bodies[bundle["foot"]["body"]])
assert any(prim.HasAPI(UsdPhysics.CollisionAPI) for prim in Usd.PrimRange(foot, Usd.TraverseInstanceProxies()))
PhysxSchema.PhysxContactReportAPI.Apply(foot).CreateThresholdAttr(0.0)
stage.GetRootLayer().Save()
layers = [Path(layer.realPath).resolve() for layer in stage.GetUsedLayers() if layer.realPath]
assert layers and all(layer.is_relative_to(directory) and layer.is_file() for layer in layers)
conversion = {
"usd_path": str(Path(converter.usd_path).relative_to(directory)),
"manifest_sha256": digest(directory / "manifest.json"),
"fixed_to_world": True,
"body_paths": {name: path.removeprefix(str(root.GetPath())) for name, path in bodies.items()},
"joint_paths": {prim.GetName(): str(prim.GetPath()).removeprefix(str(root.GetPath())) for prim in revolute},
"used_layers": sorted(str(layer.relative_to(directory)) for layer in layers),
}
(directory / "conversion.json").write_text(json.dumps(conversion, indent=2) + "\n")
print(json.dumps(conversion, indent=2))
except BaseException:
import traceback
traceback.print_exc()
code = 1
finally:
app.close(exit_code=code)
if __name__ == "__main__":
main()
+260
View File
@@ -0,0 +1,260 @@
#!/usr/bin/env python3
"""Extract the real Go2 FL subtree without importing Isaac Lab (SI units)."""
from __future__ import annotations
import argparse
import copy
import hashlib
import json
import math
import shutil
import subprocess
import xml.etree.ElementTree as ET
from pathlib import Path
ROOT = Path(__file__).resolve().parents[2]
DEFAULT_SOURCE = Path("/home/cen/Embodied_Workspace/unitree_ros/robots/go2_description")
JOINTS = ("FL_hip_joint", "FL_thigh_joint", "FL_calf_joint")
Q_DEFAULT = (0.0, 0.8, -1.5)
def digest(path: Path) -> str:
return hashlib.sha256(path.read_bytes()).hexdigest()
def numbers(text: str) -> list[float]:
result = [float(x) for x in text.split()]
if not all(math.isfinite(x) for x in result):
raise ValueError(f"Non-finite numeric field: {text}")
return result
def origin(element: ET.Element) -> dict:
value = element.find("origin")
return {key: numbers(value.get(key, "0 0 0") if value is not None else "0 0 0") for key in ("xyz", "rpy")}
def inertia_record(link: ET.Element) -> dict | None:
inertial = link.find("inertial")
if inertial is None:
return None
mass = float(inertial.find("mass").get("value"))
values = {k: float(inertial.find("inertia").get(k, "0")) for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")}
a, b, c, d, e, f = (values[k] for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz"))
# Sylvester's criterion for the complete symmetric inertia, not just its diagonal.
determinant = a * (d * f - e * e) - b * (b * f - c * e) + c * (b * e - c * d)
if (
not all(math.isfinite(x) for x in (mass, a, b, c, d, e, f))
or mass <= 0
or min(a, a * d - b * b, determinant) <= 0
):
raise ValueError(f"Invalid inertia on {link.get('name')}")
return {"mass": mass, "inertia": values, "origin": origin(inertial)}
def matmul(a, b):
return [[sum(a[i][k] * b[k][j] for k in range(len(b))) for j in range(len(b[0]))] for i in range(len(a))]
def transform(xyz, rpy):
r, p, y = rpy
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
return [
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, xyz[0]],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, xyz[1]],
[-sp, cp * sr, cp * cr, xyz[2]],
[0, 0, 0, 1],
]
def foot_center(robot: ET.Element, q=Q_DEFAULT) -> list[float]:
"""Collision sphere center in the anchor frame at the specified joint angles [m]."""
t = transform([0, 0, 0], [0, 0, 0])
by_name = {j.get("name"): j for j in robot.findall("joint")}
for name in (*JOINTS, "FL_foot_joint"):
j = by_name[name]
o = origin(j)
t = matmul(t, transform(o["xyz"], o["rpy"]))
if name in JOINTS:
angle = q[JOINTS.index(name)]
axis = numbers(j.find("axis").get("xyz"))
# Go2 uses the canonical x and y axes. Refuse unhandled topologies.
if axis not in ([1.0, 0.0, 0.0], [0.0, 1.0, 0.0]):
raise ValueError(f"Unsupported axis {axis}")
t = matmul(t, transform([0, 0, 0], [angle * axis[0], angle * axis[1], 0]))
foot = next(link for link in robot.findall("link") if link.get("name") == "FL_foot")
o = origin(foot.find("collision"))
t = matmul(t, transform(o["xyz"], o["rpy"]))
return [t[i][3] for i in range(3)]
def extract(source: Path, output: Path) -> dict:
source = source.resolve()
source_urdf = source / "urdf/go2_description.urdf"
original = ET.parse(source_urdf).getroot()
links = {link.get("name"): link for link in original.findall("link")}
joints = {j.get("name"): j for j in original.findall("joint")}
for name in (*JOINTS, "FL_foot_joint", "FL_hip_rotor_joint"):
if name not in joints:
raise ValueError(f"Missing required joint: {name}")
keep = {"FL_hip", "FL_hip_rotor"}
while True:
expanded = keep | {j.find("child").get("link") for j in joints.values() if j.find("parent").get("link") in keep}
if expanded == keep:
break
keep = expanded
if "FL_foot" not in keep or any(not n.startswith("FL_") for n in keep):
raise ValueError("FL branch must contain only the FL leg and its independent foot")
robot = ET.Element("robot", name="go2_fl_anchor")
anchor = ET.SubElement(robot, "link", name="anchor_base")
anchor.append(
ET.fromstring(
'<inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="1"/>'
'<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/></inertial>'
)
)
shift = origin(joints[JOINTS[0]])["xyz"]
for link in original.findall("link"):
if link.get("name") in keep:
inertia_record(link)
robot.append(copy.deepcopy(link))
for j in original.findall("joint"):
if j.find("child").get("link") in keep:
j = copy.deepcopy(j)
if j.find("parent").get("link") == "base":
j.find("parent").set("link", "anchor_base")
old_xyz = origin(j)["xyz"]
j.find("origin").set("xyz", " ".join(f"{x - s:.12g}" for x, s in zip(old_xyz, shift)))
robot.append(j)
revolute = [j.get("name") for j in robot.findall("joint") if j.get("type") != "fixed"]
if tuple(revolute) != JOINTS:
raise ValueError(f"Expected exactly the three ordered joints, got {revolute}")
output.mkdir(parents=True, exist_ok=True)
resources = {}
for mesh in robot.findall(".//mesh"):
filename = mesh.get("filename")
prefix = "package://go2_description/"
if not filename.startswith(prefix):
raise ValueError(f"Unexpected mesh URI: {filename}")
relative = Path(filename[len(prefix) :])
if relative.is_absolute() or ".." in relative.parts:
raise ValueError(f"Unsafe mesh path: {relative}")
src, dst = source / relative, output / relative
dst.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(src, dst)
mesh.set("filename", relative.as_posix())
resources[relative.as_posix()] = digest(src)
if src.suffix.lower() == ".dae":
for image in ET.parse(src).getroot().findall(".//{*}image/{*}init_from"):
image_relative = Path(image.text.strip())
if image_relative.is_absolute() or ".." in image_relative.parts:
raise ValueError("Unsafe DAE image reference")
image_src, image_dst = src.parent / image_relative, dst.parent / image_relative
image_dst.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(image_src, image_dst)
resources[image_dst.relative_to(output).as_posix()] = digest(image_src)
license_path = source.parents[1] / "LICENSE"
shutil.copy2(license_path, output / "LICENSE")
records = []
for name in JOINTS:
j = joints[name]
limits = {k: float(j.find("limit").get(k)) for k in ("lower", "upper", "effort", "velocity")}
if (
not all(math.isfinite(x) for x in limits.values())
or limits["lower"] >= limits["upper"]
or min(limits["effort"], limits["velocity"]) <= 0
):
raise ValueError(f"Invalid limits: {name}")
midpoint = (limits["lower"] + limits["upper"]) / 2
records.append(
{
"name": name,
"axis": numbers(j.find("axis").get("xyz")),
"limits": limits,
"soft_limits": [midpoint + 0.9 * (limits[k] - midpoint) for k in ("lower", "upper")],
}
)
radius = float(links["FL_foot"].find("collision/geometry/sphere").get("radius"))
center = foot_center(robot)
manifest = {
"schema_version": 1,
"asset": "Go2 FL fixed anchor",
"source": {
"urdf": str(source_urdf),
"sha256": digest(source_urdf),
"commit": subprocess.check_output(["git", "-C", str(source), "rev-parse", "HEAD"], text=True).strip(),
},
"resources": dict(sorted(resources.items())),
"license": "BSD-3-Clause",
"joint_order": list(JOINTS),
"joints": records,
"links": {link.get("name"): inertia_record(link) for link in robot.findall("link")},
"transforms": {
j.get("name"): {"parent": j.find("parent").get("link"), "child": j.find("child").get("link"), **origin(j)}
for j in robot.findall("joint")
},
"missing_inertial_fixed_links": [
link.get("name") for link in robot.findall("link") if link.find("inertial") is None
],
"anchor": {
"fixed": True,
"original_mount_translation": shift,
"synthetic_root_mass": 1.0,
"default_height": radius - center[2] + 0.01,
},
"foot": {"frame": "FL_foot", "radius": radius, "collision_origin": origin(links["FL_foot"].find("collision"))},
"control": {
"q_default": list(Q_DEFAULT),
"action_scale": 0.25,
"kp": 25.0,
"kd": 0.5,
"physics_dt": 0.002,
"decimation": 5,
},
"interface": {
"action_shape": ["B", 3],
"policy_shape": ["B", 19],
"device": "cuda",
"dtype": "float32",
"quaternion_order": "xyzw",
"policy_order": [
"q_rel",
"dq",
"foot_pos_anchor",
"foot_vel_anchor",
"foot_normal_force_anchor",
"contact",
"last_action",
],
"force_semantics": "normal contact force, not full friction wrench",
},
"skill": {"weights": None, "latent": None, "reaction_wrench": None, "valid": False},
"modifications": [
"Replace whole body with fixed synthetic anchor root",
"Translate mount origin to anchor origin",
"Preserve FL fixed subtrees and root-side rotor",
"Relative mesh URIs; no ROS runtime dependency",
],
}
ET.indent(robot)
ET.ElementTree(robot).write(output / "go2_fl.urdf", encoding="utf-8", xml_declaration=True)
manifest["derived_urdf_sha256"] = digest(output / "go2_fl.urdf")
(output / "manifest.json").write_text(json.dumps(manifest, indent=2, sort_keys=True) + "\n")
return manifest
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--source", type=Path, default=DEFAULT_SOURCE)
parser.add_argument("--output", type=Path, default=ROOT / "assets/generated/go2_fl")
args = parser.parse_args()
manifest = extract(args.source, args.output)
print(
f"Extracted {manifest['joint_order']} to {args.output}; "
f"anchor height={manifest['anchor']['default_height']:.6f} m"
)
if __name__ == "__main__":
main()
+24
View File
@@ -0,0 +1,24 @@
#!/usr/bin/env python3
"""Extract an explicitly described real single leg; no simulator startup."""
import argparse
from pathlib import Path
from SkillFactory.legs import LegSpec
from SkillFactory.legs.urdf import extract_leg
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--spec", type=Path, required=True)
parser.add_argument("--source-root", type=Path)
parser.add_argument("--output", type=Path)
args = parser.parse_args()
spec = LegSpec.load(args.spec, args.source_root)
output = args.output or Path("assets/generated/legs") / spec.part_id
bundle = extract_leg(spec, output)
print(f"Extracted {bundle['joint_order']} to {output}; source {bundle['source']['sha256']}")
if __name__ == "__main__":
main()
+71
View File
@@ -0,0 +1,71 @@
#!/usr/bin/env python3
"""Finite single-leg GUI physics preview before registering the RL environment."""
import argparse
import json
import math
import traceback
from pathlib import Path
def main():
from isaaclab.app import AppLauncher
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--steps", type=int, default=1500)
AppLauncher.add_app_launcher_args(parser)
args = parser.parse_args()
app = AppLauncher(args).app
code = 0
try:
import torch
from isaaclab_physx.physics import PhysxCfg
from SkillFactory.assets.go2_leg import GO2_LEG_CFG, JOINT_NAMES
import isaaclab.sim as sim_utils
from isaaclab.assets import Articulation
sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.002, device="cuda:0", physics=PhysxCfg()))
sim.set_camera_view((1.0, 1.0, 0.8), (0.0, 0.0, 0.35))
ground = sim_utils.GroundPlaneCfg()
ground.func("/World/ground", ground)
light = sim_utils.DomeLightCfg(intensity=2000)
light.func("/World/light", light)
cfg = GO2_LEG_CFG.replace(prim_path="/World/Robot")
cfg.init_state.pos = (0.0, 0.0, 0.60)
robot = Articulation(cfg)
sim.reset()
robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch)
robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch)
ids, names = robot.find_joints(JOINT_NAMES, preserve_order=True)
assert names == JOINT_NAMES and robot.num_joints == 3
start = robot.data.root_pos_w.torch.clone()
for step in range(args.steps):
target = robot.data.default_joint_pos.torch.clone()
target[:, ids] += 0.05 * math.sin(2 * math.pi * 0.5 * step * 0.002)
robot.set_joint_position_target_index(target=target)
robot.write_data_to_sim()
sim.step()
robot.update(0.002)
assert torch.isfinite(robot.data.joint_pos.torch).all()
result = {
"joint_names": names,
"num_joints": robot.num_joints,
"body_names": robot.body_names,
"q": robot.data.joint_pos.torch.tolist(),
"root_drift_m": (robot.data.root_pos_w.torch - start).abs().max().item(),
"steps": args.steps,
"gui_requested": not args.headless,
}
assert result["root_drift_m"] <= 1e-4
Path("logs/week1_anchor/preview.json").write_text(json.dumps(result, indent=2))
print(json.dumps(result, indent=2))
except BaseException:
traceback.print_exc()
code = 1
finally:
app.close(exit_code=code)
if __name__ == "__main__":
main()
+359
View File
@@ -0,0 +1,359 @@
#!/usr/bin/env python3
"""Finite single-leg verification or on-policy rollout in a fresh Kit worker."""
import argparse
import json
import math
import sys
import traceback
from pathlib import Path
from runtime_common import finish_run, write_json
def clone_batch(batch):
return {name: value.detach().clone() for name, value in batch.items()}
def verify(env, args):
import torch
from SkillFactory.tasks.manager_based.leg_skill.mdp import state
from isaaclab.sim import get_current_stage
adapter = state(env)
stage = get_current_stage()
count = sum(1 for _ in stage.Traverse())
initial_anchor = adapter.robot.data.body_link_pose_w.torch[:, adapter.anchor].clone()
worst_fk, max_torque = 0.0, 0.0
timeouts = 0
for step in range(args.steps):
action = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device)
phase = torch.arange(env.num_envs, device=env.device) * 0.1 + step * env.step_dt * 3.141592653589793
joint_phase = torch.arange(len(adapter.joint_ids), device=env.device) * 0.35
action[:, : len(adapter.joint_ids)] = 0.4 * (phase[:, None] + joint_phase).sin()
expected = (adapter.nominal + action[:, : len(adapter.joint_ids)] * adapter.scale).clamp(
min=adapter.soft[:, 0], max=adapter.soft[:, 1]
)
obs, _, terminated, truncated, info = env.step(adapter.actions(action))
if terminated.any():
raise RuntimeError("Unexpected invalid-state termination")
snapshot = adapter.snapshot()
for name, value in obs["policy"].items():
assert torch.isfinite(value).all()
assert torch.equal(value, snapshot[name]), ("Observation term/snapshot mismatch", name)
assert (
(adapter.robot.data.joint_pos_target.torch[:, adapter.joint_ids][~truncated] - expected[~truncated])
.abs()
.max()
.item()
< 1e-6
if (~truncated).any()
else True
)
assert torch.equal(
obs["policy"]["joint_observation"][:, : len(adapter.joint_ids), 2][~truncated],
action[:, : len(adapter.joint_ids)][~truncated],
)
fk_error = (adapter.fk.foot_position(adapter.q()) - adapter.foot_position()).abs().max().item()
worst_fk = max(worst_fk, fk_error)
assert fk_error < 1e-4, ("URDF/PhysX frame mismatch", fk_error)
torque = (adapter.robot.data.applied_torque.torch[:, adapter.joint_ids] / adapter.effort).abs().max().item()
max_torque = max(max_torque, torque)
assert torque <= 1.00001
if truncated.any():
assert info["terminal_tokens"] is not None
assert len(info["terminal_ids"]) == truncated.sum().item()
terminal = info["terminal_tokens"]
terminal_q = adapter.nominal + terminal["joint_observation"][:, : len(adapter.joint_ids), 0] * adapter.scale
assert (
adapter.fk.foot_position(terminal_q) / adapter.length - terminal["effector_observation"][:, 0, :3]
).abs().max().item() < 1e-4
assert torch.equal(
terminal["joint_observation"][:, : len(adapter.joint_ids), 2],
action[info["terminal_ids"], : len(adapter.joint_ids)],
)
assert obs["policy"]["joint_observation"][truncated].abs().max().item() == 0.0
timeouts += truncated.sum().item()
robot = adapter.robot
q_before, dq_before, action_before = (
adapter.q().clone(),
robot.data.joint_vel.torch[:, adapter.joint_ids].clone(),
env.action_manager.action.clone(),
)
ids = torch.arange(0, env.num_envs, 2, device=env.device)
untouched = torch.arange(1, env.num_envs, 2, device=env.device)
old_goal = env.command_manager.get_command("reach").clone()
old_time = env._sim_step_counter
env.reset(env_ids=ids)
assert (adapter.q()[ids] - adapter.nominal).abs().max().item() < 1e-5
assert robot.data.joint_vel.torch[ids].abs().max().item() < 1e-5
assert (robot.data.joint_pos_target.torch[ids][:, adapter.joint_ids] - adapter.nominal).abs().max().item() < 1e-5
assert env.action_manager.action[ids].abs().max().item() == 0.0
assert env.scene["foot_contact"].data.net_forces_w_history.torch[ids].abs().max().item() == 0.0
assert env.episode_length_buf[ids].max().item() == 0
if len(untouched):
assert (adapter.q()[untouched] - q_before[untouched]).abs().max().item() <= 1e-6
assert (
robot.data.joint_vel.torch[untouched][:, adapter.joint_ids] - dq_before[untouched]
).abs().max().item() <= 1e-6
assert torch.equal(env.action_manager.action[untouched], action_before[untouched])
assert torch.equal(env.command_manager.get_command("reach")[untouched], old_goal[untouched])
before_empty = clone_batch(adapter.snapshot())
env.reset(env_ids=[])
assert all(torch.equal(v, adapter.snapshot()[k]) for k, v in before_empty.items())
for invalid in ([-1], [env.num_envs], [0, 0], [0.5]):
try:
env.reset(env_ids=invalid)
except ValueError:
pass
else:
raise AssertionError("Invalid reset indices accepted")
native_bad = env.action_manager.action.clone()
native_bad[:, 0] = torch.nan
manager_before = (env.action_manager.action.clone(), env.action_manager.prev_action.clone())
try:
env.step(native_bad)
except ValueError:
pass
else:
raise AssertionError("Non-finite native action accepted")
assert torch.equal(env.action_manager.action, manager_before[0])
assert torch.equal(env.action_manager.prev_action, manager_before[1])
assert env.single_observation_space["policy"]["joint_mask"].dtype.kind == "b"
bad = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device)
bad[:, 0] = torch.nan
try:
adapter.actions(bad)
except ValueError:
pass
else:
raise AssertionError("Non-finite active action accepted")
padded = torch.zeros_like(bad)
padded[:, len(adapter.joint_ids) :] = torch.nan
assert torch.isfinite(adapter.actions(padded)).all()
assert env._sim_step_counter == old_time
assert sum(1 for _ in stage.Traverse()) == count
drift = (robot.data.body_link_pose_w.torch[:, adapter.anchor] - initial_anchor).abs().max().item()
assert drift <= 1e-4
return {
"passed": True,
"mode": "verify",
"part_id": adapter.bundle["spec"]["part_id"],
"num_envs": env.num_envs,
"steps": args.steps,
"physics_dt": env.physics_dt,
"control_dt": env.step_dt,
"joint_order": adapter.bundle["joint_order"],
"max_fk_error_m": worst_fk,
"max_normalized_applied_torque": max_torque,
"fixed_anchor_pose_error": drift,
"timeouts_checked": timeouts,
"indexed_reset": True,
"empty_reset_noop": True,
"stage_prim_count": count,
"policy_shapes": {k: list(v.shape) for k, v in adapter.snapshot().items()},
"trained_skill": False,
}
def rollout(env, args):
import torch
from SkillFactory.legs.policy import load_policy
from SkillFactory.tasks.manager_based.leg_skill.mdp import state
if args.weights is None:
raise ValueError("rollout requires --weights")
model = load_policy(args.weights, env.device).eval()
adapter = state(env)
observation, _ = env.reset(seed=args.seed)
snapshots, actions, logs, values, rewards, next_values, done = [], [], [], [], [], [], []
with torch.no_grad():
for _ in range(args.horizon):
batch = clone_batch(observation["policy"])
action, log_prob, value = model.act(batch)
observation, reward, terminated, truncated, info = env.step(adapter.actions(action))
if terminated.any():
raise RuntimeError("Invalid physical state during rollout")
nxt = model.value(observation["policy"])
if truncated.any():
terminal_value = model.value(info["terminal_tokens"])
ids = info["terminal_ids"]
nxt[ids] = torch.where(truncated[ids], terminal_value, nxt[ids])
nxt[terminated] = 0.0
assert torch.isfinite(reward).all() and torch.isfinite(nxt).all()
assert all(torch.isfinite(v).all() for v in observation["policy"].values())
snapshots.append(batch)
actions.append(action)
logs.append(log_prob)
values.append(value)
rewards.append(reward.clone())
next_values.append(nxt)
done.append((terminated | truncated).clone())
data = {"observation": {name: torch.stack([batch[name] for batch in snapshots]).cpu() for name in snapshots[0]}}
for name, items in {
"actions": actions,
"log_prob": logs,
"values": values,
"rewards": rewards,
"next_values": next_values,
"done": done,
}.items():
data[name] = torch.stack(items).cpu()
torch.save(data, args.output / "rollout.pt")
return {
"passed": True,
"mode": "rollout",
"part_id": adapter.bundle["spec"]["part_id"],
"horizon": args.horizon,
"num_envs": env.num_envs,
"mean_reward_per_control_step": data["rewards"].mean().item(),
"episode_end_count": data["done"].sum().item(),
"source": adapter.bundle["source"],
"trained_skill": False,
}
def contact_trial(env, args):
import torch
from SkillFactory.tasks.manager_based.leg_skill.mdp import state
adapter = state(env)
press = [float(v) for v in args.contact_action.split(",")]
if len(press) != len(adapter.joint_ids):
raise ValueError("contact-action must have one value per declared actuator")
action = torch.zeros(env.num_envs, env.cfg.max_joints, device=env.device)
action[:, : len(press)] = torch.tensor(press, device=env.device)
rows = {}
for phase, sign in (("air", -1.0), ("press", 1.0)):
forces, gaps = [], []
for step in range(100):
env.step(adapter.actions(action * sign))
if step >= 70:
forces.append(env.scene["foot_contact"].data.force_matrix_w.torch[:, 0, 0].clone())
gaps.append(adapter.gap().clone())
force, gap = torch.stack(forces), torch.stack(gaps)
rows[phase] = {
"mean_fz_N": force[:, :, 2].mean(dim=0).min().item(),
"max_force_N": force.norm(dim=-1).max().item(),
"min_gap_m": gap.min().item(),
}
if phase == "air":
assert gap.min().item() >= 0.01 and force.norm(dim=-1).max().item() < 0.1, rows
else:
assert force[:, :, 2].mean(dim=0).min().item() > 1.0 and gap.min().item() >= -0.005, rows
ids = torch.arange(0, env.num_envs, 2, device=env.device)
env.reset(env_ids=ids)
assert env.scene["foot_contact"].data.net_forces_w_history.torch[ids].abs().max().item() == 0.0
return rows
def main():
if not __debug__:
raise RuntimeError("Worker checks require Python without -O")
from isaaclab.app import AppLauncher
from isaaclab_tasks.utils import add_launcher_args, launch_simulation
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--bundle", type=Path, required=True)
parser.add_argument("--mode", choices=("verify", "rollout"), default="verify")
parser.add_argument("--num_envs", type=int, default=16)
parser.add_argument("--max-joints", type=int, default=8)
parser.add_argument("--steps", type=int, default=300)
parser.add_argument("--horizon", type=int, default=32)
parser.add_argument("--episode-length", type=float, default=0.2)
parser.add_argument("--weights", type=Path)
parser.add_argument("--contact-action", help="Optional configured press action, e.g. 0,0,1; verify only")
parser.add_argument("--seed", type=int, default=42)
parser.add_argument("--output", type=Path, default=Path("logs/leg_interface/verify"))
add_launcher_args(parser)
args = parser.parse_args()
if (
args.num_envs < 1
or args.max_joints < 1
or args.steps < 10
or args.horizon < 1
or not math.isfinite(args.episode_length)
or args.episode_length <= 0
):
parser.error("Require positive finite counts/duration and steps>=10")
if args.mode == "rollout" and args.weights is None:
parser.error("rollout requires --weights")
if args.contact_action is not None:
if args.mode != "verify":
parser.error("contact-action is for verification only")
try:
press = [float(value) for value in args.contact_action.split(",")]
except ValueError:
parser.error("contact-action requires comma-separated finite numbers")
if not press or not all(math.isfinite(value) for value in press):
parser.error("contact-action requires comma-separated finite numbers")
args.output.mkdir(parents=True, exist_ok=True)
result = {"passed": False, "status": "running"}
write_json(args.output / "result.json", result)
app = AppLauncher(args).app
code, env = 0, None
try:
import gymnasium as gym
import SkillFactory.tasks # noqa: F401
import torch
from SkillFactory.tasks.manager_based.leg_skill.leg_env_cfg import LegEnvCfg
from pxr import UsdPhysics
from isaaclab.sim import get_current_stage
cfg = LegEnvCfg(bundle_dir=str(args.bundle.resolve()), max_joints=args.max_joints)
cfg.scene.num_envs = args.num_envs
cfg.seed = args.seed
cfg.episode_length_s = args.episode_length
if args.contact_action is not None:
from SkillFactory.legs.urdf import forward, multiply, pose
bundle = cfg.leg_bundle
nominal = {j["name"]: j["q_default"] for j in bundle["spec"]["joints"]}
poses, _ = forward(bundle["tree"], nominal)
foot = bundle["foot"]
collision = multiply(poses[foot["body"]], pose(foot["xyz"], foot["rpy"]))
extent = (
foot["radius"]
if foot["type"] == "sphere"
else sum(abs(collision[2][i]) * foot["size"][i] / 2 for i in range(3))
)
cfg.scene.robot.init_state.pos = (0.0, 0.0, extent - collision[2][3] + 0.01)
cfg.terminations.time_out = None
with launch_simulation(cfg, args):
torch.manual_seed(args.seed)
torch.set_float32_matmul_precision("highest")
env = gym.make("SkillFactory-LegSkill-v0", cfg=cfg).unwrapped
env.reset(seed=args.seed)
stage = get_current_stage()
grounds = [
str(p.GetPath())
for p in stage.Traverse()
if str(p.GetPath()).startswith("/World/ground/") and p.HasAPI(UsdPhysics.CollisionAPI)
]
assert grounds == cfg.scene.foot_contact.filter_prim_paths_expr
assert env.scene["foot_contact"].num_sensors == 1
assert env.scene["foot_contact"].data.force_matrix_w.torch.shape == (args.num_envs, 1, 1, 3)
result = verify(env, args) if args.mode == "verify" else rollout(env, args)
if args.contact_action is not None:
result["contact_trial"] = contact_trial(env, args)
masses = dict(zip(env.scene["robot"].body_names, env.scene["robot"].data.body_mass.torch[0].tolist()))
result["effective_body_mass_kg"] = masses
result["manifest_sha256"] = cfg.leg_bundle["conversion"]["manifest_sha256"]
result["seed"] = args.seed
result["device"] = env.device
print(json.dumps(result, indent=2))
except BaseException:
error = traceback.format_exc()
print(error, file=sys.stderr, flush=True)
result = {"passed": False, "error": error}
code = 1
finally:
finish_run(app, env, args.output / "result.json", result, code)
if __name__ == "__main__":
main()
+15 -13
View File
@@ -9,8 +9,8 @@ Script to print all the available environments in Isaac Lab.
The script iterates over all registered environments and stores the details in a table.
It prints the name of the environment, the entry point and the config file.
All the environments are registered in the `SkillFactory` extension. They start
with `Isaac` in their name.
List only environments whose config entry points belong to SkillFactory.
Both the original Cartpole template and the single-leg anchor are retained.
"""
"""Launch Isaac Sim Simulator first."""
@@ -44,9 +44,8 @@ simulation_app = app_launcher.app
"""Rest everything follows."""
import gymnasium as gym
from prettytable import PrettyTable
import SkillFactory.tasks # noqa: F401
from prettytable import PrettyTable
# PLACEHOLDER: Extension template (do not remove this comment)
with contextlib.suppress(ImportError):
@@ -57,7 +56,7 @@ def _format_presets(preset_map: dict | None) -> str:
"""Format a preset map returned by :func:`enumerate_task_presets` into a human-readable string.
Args:
preset_map: Mapping of :class:`~SkillFactory.utils.preset_target.PresetTarget`
preset_map: Mapping of :class:`~isaaclab_tasks.utils.preset_target.PresetTarget`
to sorted preset name lists, or ``None`` when the env cfg could not be loaded.
Returns:
@@ -66,7 +65,7 @@ def _format_presets(preset_map: dict | None) -> str:
"""
if preset_map is None:
return "(unavailable)"
from SkillFactory.utils.preset_target import PresetTarget
from isaaclab_tasks.utils.preset_target import PresetTarget
lines = []
labels = {
@@ -87,11 +86,12 @@ def main():
task_specs = [
spec
for spec in gym.registry.values()
if "Template-" in spec.id and (args_cli.keyword is None or args_cli.keyword in spec.id)
if str(spec.kwargs.get("env_cfg_entry_point", "")).startswith("SkillFactory.")
and (args_cli.keyword is None or args_cli.keyword in spec.id)
]
if args_cli.show_presets:
from SkillFactory.utils.preset_cli import enumerate_task_presets
from isaaclab_tasks.utils.preset_cli import enumerate_task_presets
table = PrettyTable(["S. No.", "Task Name", "Entry Point", "Config", "Presets"])
table.title = "Available Environments in Isaac Lab"
@@ -125,11 +125,13 @@ def main():
if __name__ == "__main__":
exit_code = 0
try:
# run the main function
main()
except Exception as e:
raise e
except BaseException:
import traceback
traceback.print_exc()
exit_code = 1
finally:
# close the app
simulation_app.close()
simulation_app.close(exit_code=exit_code)
Binary file not shown.
+1
View File
@@ -55,6 +55,7 @@ from isaaclab_tasks.utils.hydra import hydra_task_config
import cli_args # isort: skip
import SkillFactory.tasks # noqa: F401
with contextlib.suppress(ImportError):
import isaaclab_tasks_experimental # noqa: F401
+1
View File
@@ -44,6 +44,7 @@ from isaaclab_tasks.utils.hydra import hydra_task_config
import cli_args # isort: skip
import SkillFactory.tasks # noqa: F401
with contextlib.suppress(ImportError):
import isaaclab_tasks_experimental # noqa: F401
+1
View File
@@ -53,6 +53,7 @@ import cli_args # isort: skip
logger = logging.getLogger(__name__)
import SkillFactory.tasks # noqa: F401
with contextlib.suppress(ImportError):
import isaaclab_tasks_experimental # noqa: F401
+1
View File
@@ -41,6 +41,7 @@ RL_ROOT = Path(__file__).resolve().parents[1]
CLI_ARGS = import_local_module("isaaclab_rsl_rl_cli_args", RL_ROOT / "rsl_rl" / "cli_args.py")
import SkillFactory.tasks # noqa: F401
with contextlib.suppress(ImportError):
import isaaclab_tasks_experimental # noqa: F401
+56
View File
@@ -0,0 +1,56 @@
"""Stdlib-only reporting and shutdown helpers; safe to import before Kit."""
import json
import os
import re
import tempfile
import traceback
from pathlib import Path
CRITICAL_LOG = re.compile(r"\[(?:Error|Fatal)\]|buffer\s+overflow", re.IGNORECASE)
def write_json(path, data):
"""Replace complete reports atomically; never serialize NaN/Inf as valid JSON."""
path = Path(path)
content = json.dumps(data, indent=2, allow_nan=False) + "\n"
path.parent.mkdir(parents=True, exist_ok=True)
temporary = None
try:
with tempfile.NamedTemporaryFile(mode="w", encoding="utf-8", dir=path.parent, delete=False) as stream:
temporary = Path(stream.name)
stream.write(content)
os.replace(temporary, path)
finally:
if temporary is not None:
temporary.unlink(missing_ok=True)
def critical_log_lines(path):
with Path(path).open(errors="replace") as stream:
return [line.rstrip() for line in stream if CRITICAL_LOG.search(line)]
def finish_run(app, env, path, result, exit_code=0):
"""Close the environment before publishing success and Kit's fast exit."""
if env is not None:
try:
env.close()
except BaseException:
error = traceback.format_exc()
traceback.print_exc()
result.update(passed=False, shutdown_error=error)
if "milestone_complete" in result:
result["milestone_complete"] = False
exit_code = 1
if not result.get("passed"):
exit_code = exit_code or 1
if exit_code:
result["passed"] = False
result["status"] = "completed" if not exit_code else "failed"
try:
write_json(path, result)
except BaseException:
traceback.print_exc()
exit_code = 1
app.close(exit_code=exit_code)
+174
View File
@@ -0,0 +1,174 @@
#!/usr/bin/env python3
"""Finite shared-policy PPO prototype across asset-homogeneous Kit workers.
Workers restart for each rollout in this initial integration runner. Use small
update counts; this is not yet a throughput-optimized long-training service.
"""
import argparse
import json
import math
import subprocess
import sys
import time
import traceback
from pathlib import Path
from runtime_common import critical_log_lines, write_json
def main():
import torch
from SkillFactory.legs.policy import LegActorCritic, load_policy, ppo_update, save_policy
from SkillFactory.legs.spec import load_bundle
from SkillFactory.legs.tokens import FEATURE_SCHEMA
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--bundles", type=Path, nargs="+", required=True)
parser.add_argument("--num_envs", type=int, default=16, help="Per-asset environment count")
parser.add_argument("--max-joints", type=int, default=8)
parser.add_argument("--horizon", type=int, default=32)
parser.add_argument("--updates", type=int, default=1)
parser.add_argument("--episode-length", type=float, default=0.2)
parser.add_argument("--seed", type=int, default=42)
parser.add_argument("--device", default="cuda:0")
parser.add_argument("--output", type=Path, default=Path("logs/leg_interface/train_smoke"))
args = parser.parse_args()
if (
min(args.num_envs, args.max_joints, args.horizon, args.updates) < 1
or not math.isfinite(args.episode_length)
or args.episode_length <= 0
):
parser.error("Counts and episode length must be positive")
args.output.mkdir(parents=True, exist_ok=True)
report = {
"passed": False,
"status": "running",
"trained_skill": False,
"seed": args.seed,
"feature_schema": FEATURE_SCHEMA,
}
write_json(args.output / "result.json", report)
start = time.perf_counter()
try:
bundles = [load_bundle(path, require_usd=True) for path in args.bundles]
ids = [bundle["spec"]["part_id"] for bundle in bundles]
if len(set(ids)) != len(ids) or any(len(bundle["joints"]) > args.max_joints for bundle in bundles):
raise ValueError("Require unique assets fitting max-joints")
timing = {(bundle["spec"]["physics_dt"], bundle["spec"]["decimation"]) for bundle in bundles}
if len(timing) != 1:
raise ValueError("Initial mixed trainer requires a common physics/control timestep")
torch.manual_seed(args.seed)
torch.set_float32_matmul_precision("highest")
model = LegActorCritic().to(args.device)
args.device = str(next(model.parameters()).device)
optimizer = torch.optim.Adam(model.parameters(), lr=3e-4, eps=1e-5)
initial = {k: v.detach().clone() for k, v in model.state_dict().items()}
rows = []
for update in range(args.updates):
weights = args.output / "sampling_policy.pt"
save_policy(model, weights, {"update": update})
all_data, workers = [], []
for index, (bundle_path, part_id) in enumerate(zip(args.bundles, ids)):
directory = args.output / f"update_{update:04d}" / part_id
directory.mkdir(parents=True, exist_ok=True)
log_path = directory / "worker.log"
command = [
sys.executable,
str(Path(__file__).with_name("leg_worker.py")),
"--bundle",
str(bundle_path.resolve()),
"--mode",
"rollout",
"--weights",
str(weights.resolve()),
"--num_envs",
str(args.num_envs),
"--max-joints",
str(args.max_joints),
"--horizon",
str(args.horizon),
"--episode-length",
str(args.episode_length),
"--seed",
str(args.seed + index + update * 100),
"--device",
args.device,
"--output",
str(directory.resolve()),
]
with log_path.open("w") as log:
child = subprocess.run(command, stdout=log, stderr=subprocess.STDOUT)
critical = critical_log_lines(log_path)
if child.returncode or critical:
raise RuntimeError(f"Worker failed ({child.returncode}): {log_path}; {critical[-3:]}")
worker = json.loads((directory / "result.json").read_text())
if (
not worker["passed"]
or worker["device"] != str(torch.device(args.device))
or worker["manifest_sha256"] != bundles[index]["conversion"]["manifest_sha256"]
):
raise RuntimeError(f"Worker report/device/provenance mismatch: {directory}")
workers.append(worker)
all_data.append(torch.load(directory / "rollout.pt", weights_only=True, map_location=args.device))
# Equal per-asset environment counts yield balanced morphology sampling.
data = {
"observation": {
key: torch.cat([item["observation"][key] for item in all_data], dim=1)
for key in all_data[0]["observation"]
}
}
for key in ("actions", "log_prob", "values", "rewards", "next_values", "done"):
data[key] = torch.cat([item[key] for item in all_data], dim=1)
with torch.no_grad():
sampled = {key: value.flatten(0, 1) for key, value in data["observation"].items()}
current_log, _, _ = model.evaluate(sampled, data["actions"].flatten(0, 1))
mismatch = (current_log - data["log_prob"].flatten()).abs().max().item()
if not math.isfinite(mismatch) or mismatch > 1e-5:
raise RuntimeError(f"Sampling policy/log-prob mismatch: {mismatch}")
metrics = ppo_update(model, optimizer, data)
metrics["sampling_log_prob_error"] = mismatch
metrics.update(update=update, workers=workers, mean_reward=data["rewards"].mean().item())
rows.append(metrics)
write_json(args.output / "progress.json", rows)
print(
f"PPO update {update}: reward={metrics['mean_reward']:.6f}, value_loss={metrics['value_loss']:.6f}",
flush=True,
)
difference = max((model.state_dict()[key] - value).abs().max().item() for key, value in initial.items())
if not difference > 0 or not all(torch.isfinite(value).all() for value in model.state_dict().values()):
raise RuntimeError("PPO did not produce a finite parameter update")
checkpoint = args.output / "policy.pt"
save_policy(model, checkpoint, {"training_assets": ids, "seed": args.seed, "updates": args.updates})
loaded = load_policy(checkpoint, args.device).eval()
sample = {key: value[0] for key, value in data["observation"].items()}
with torch.no_grad():
before = model.act(sample, deterministic=True)
after = loaded.act(sample, deterministic=True)
if not all(torch.equal(a, b) for a, b in zip(before, after)):
raise RuntimeError("Checkpoint reload mismatch")
report.update(
passed=True,
status="completed",
training_assets=ids,
updates=rows,
environment_control_steps=args.updates * args.horizon * args.num_envs * len(ids),
max_parameter_change=difference,
checkpoint_reload_exact=True,
wall_time_s=time.perf_counter() - start,
device=args.device,
checkpoint=str(checkpoint),
capability="URMA-inspired goal-conditioned shared PPO smoke; not a converged/modular Skill",
)
except BaseException:
error = traceback.format_exc()
print(error, file=sys.stderr)
report.update(status="failed", error=error)
write_json(args.output / "result.json", report)
raise SystemExit(1) from None
write_json(args.output / "result.json", report)
print(f"Shared PPO integration passed: {args.output}/result.json; trained_skill=False", flush=True)
if __name__ == "__main__":
main()
+494
View File
@@ -0,0 +1,494 @@
#!/usr/bin/env python3
"""Deterministic exit-criteria tests for the real Go2 single-leg anchor."""
import math
import time
from anchor_common import launch
from runtime_common import critical_log_lines, write_json
def configure(parser):
parser.add_argument(
"--suite",
choices=["contract", "stability", "calibration", "contact", "reset", "long", "core", "full"],
default="full",
)
parser.add_argument("--duration", type=float, default=600.0)
parser.add_argument("--reset_count", type=int, default=1334)
def check_contract(env):
import torch
from SkillFactory.assets.go2_leg import JOINT_NAMES
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
obs, _ = env.reset()
robot = u.scene["robot"]
assert robot.joint_names == JOINT_NAMES
assert robot.num_joints == 3
assert obs["policy"].shape == (u.num_envs, 19)
assert obs["policy"].dtype == torch.float32 and obs["policy"].is_cuda
assert torch.isfinite(obs["policy"]).all()
d = diagnostics(u)
assert d["joint_mask"].shape == (u.num_envs, 3) and d["joint_mask"].dtype == torch.bool
assert d["force_history_w"].shape == (u.num_envs, 5, 1, 3)
actions = torch.zeros((u.num_envs, 3), device=u.device)
u.action_manager.process_action(actions)
term = u.action_manager.get_term("joint_position")
assert torch.allclose(term.processed_actions, robot.data.default_joint_pos.torch, atol=1e-7)
u.action_manager.process_action(actions + 10.0)
assert torch.allclose(term.processed_actions, robot.data.default_joint_pos.torch + 0.25, atol=1e-7)
assert u.action_manager.action.max() == 1.0
limits = robot.data.soft_joint_pos_limits.torch
assert ((term.processed_actions >= limits[:, :, 0]) & (term.processed_actions <= limits[:, :, 1])).all()
counter = u._sim_step_counter
try:
env.step(torch.full_like(actions, float("nan")))
except ValueError:
pass
else:
raise AssertionError("NaN input was accepted")
assert u._sim_step_counter == counter
env.reset()
obs, _, done, timeout, _ = env.step(actions)
assert torch.isfinite(obs["policy"]).all() and not done.any() and not timeout.any()
return {
"passed": True,
"policy_shape": list(obs["policy"].shape),
"device": str(obs["policy"].device),
"joint_order": robot.joint_names,
"force_history_shape": list(d["force_history_w"].shape),
"nan_rejected_without_physics_step": True,
}
def _step(env, actions):
import torch
obs, _, done, timeout, _ = env.step(actions)
assert not done.any() and not timeout.any(), "Unexpected automatic reset"
assert torch.isfinite(obs["policy"]).all(), "Non-finite observation"
return obs
def check_stability(env):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
env.reset()
robot = u.scene["robot"]
initial = diagnostics(u)["anchor_pose_w"].clone()
actions = torch.zeros((u.num_envs, 3), device=u.device)
errors = torch.zeros((u.num_envs, 3), device=u.device)
max_drift = max_angle = 0.0
for step in range(6200):
actions[:] = 0.4 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
_step(env, actions)
d = diagnostics(u)
assert (d["gap"] >= 0.01).all(), "Stability test must be in air"
limits = robot.data.joint_pos_limits.torch
assert ((d["q"] >= limits[:, :, 0] - 0.01) & (d["q"] <= limits[:, :, 1] + 0.01)).all()
assert (d["applied_torque"].abs() <= robot.data.joint_effort_limits.torch + 1e-4).all()
max_drift = max(max_drift, (d["anchor_pose_w"][:, :3] - initial[:, :3]).abs().max().item())
dot = (d["anchor_pose_w"][:, 3:] * initial[:, 3:]).sum(dim=-1).abs().clamp(max=1.0)
max_angle = max(max_angle, (2 * torch.acos(dot)).max().item())
if step >= 200:
errors += (d["q"] - d["q_target"]).square()
rms = (errors / 6000).sqrt().max(dim=0).values
assert (rms <= 0.10).all(), f"Tracking RMS: {rms.tolist()}"
assert max_drift <= 1e-4 and max_angle <= 1e-3
return {
"passed": True,
"warmup_s": 2.0,
"duration_s": 60.0,
"amplitude_rad": 0.1,
"frequency_hz": 0.5,
"worst_env_rms_rad": rms.tolist(),
"root_drift_m": max_drift,
"root_angle_rad": max_angle,
}
def check_calibration(env):
import torch
u = env.unwrapped
env.reset()
actions = torch.zeros((u.num_envs, 3), device=u.device)
force = torch.zeros(u.num_envs, device=u.device)
for step in range(500):
_step(env, actions)
if step >= 300:
force += u.scene["probe_contact"].data.force_matrix_w.torch[:, 0, 0, 2]
force /= 200
assert ((force - 9.81).abs() / 9.81 <= 0.2).all(), force.tolist()
return {
"passed": True,
"probe_mass_kg": 1.0,
"expected_N": 9.81,
"mean_fz_min_N": force.min().item(),
"mean_fz_max_N": force.max().item(),
}
def check_contact(env):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
env.reset()
actions = torch.zeros((u.num_envs, 3), device=u.device)
air_max, press_min, penetration = 0.0, float("inf"), 0.0
latency = 0
transitions = []
for cycle in range(20):
# Alternate assignments across environments: verify contact channels
# and independent commands without changing the fixed anchor poses.
signs = torch.where(torch.arange(u.num_envs, device=u.device) % 2 == 0, 1.0, -1.0)
for phase in ("air", "press"):
actions[:, 2] = -signs if phase == "air" else signs
air_ids = actions[:, 2] < 0
press_ids = ~air_ids
gap_crossed = torch.zeros(u.num_envs, dtype=torch.bool, device=u.device)
latency_steps = torch.zeros(u.num_envs, dtype=torch.int32, device=u.device)
air_window, press_window, gap_window = [], [], []
for step in range(100):
_step(env, actions)
d = diagnostics(u)
f = d["normal_force_w"]
contact = f.norm(dim=-1) > 1.0
crossed = (d["gap"] <= 0.0001) & press_ids
gap_crossed |= crossed
pending = gap_crossed & ~contact & press_ids
latency_steps[pending] += 1
if step >= 70:
if air_ids.any():
assert (d["gap"][air_ids] >= 0.01).all(), ("air gap", d["gap"].tolist())
air_window.append(f[air_ids].norm(dim=-1))
if press_ids.any():
press_window.append(f[press_ids, 2])
gap_window.append(d["gap"][press_ids])
latency = max(latency, latency_steps.max().item())
assert latency_steps.max() <= 1, f"Contact response >1 control cycle: {latency_steps.tolist()}"
if air_window:
current = torch.stack(air_window).max().item()
air_max = max(air_max, current)
assert current < 0.1, f"Air force: {current}"
if press_window:
mean = torch.stack(press_window).mean(dim=0).min().item()
press_min = min(press_min, mean)
assert mean > 1.0, f"Pressed force: {mean}"
penetration = max(penetration, max(0.0, -torch.stack(gap_window).min().item()))
assert penetration <= 0.005, f"Penetration: {penetration}"
transitions.append([cycle, phase, d["gap"][0].item(), d["normal_force_w"][0, 2].item()])
return {
"passed": True,
"cycles": 20,
"air_force_max_N": air_max,
"press_fz_min_N": press_min,
"penetration_max_m": penetration,
"latency_control_steps_max": latency,
"trace": transitions,
}
def check_resets(env, count):
import torch
from isaaclab.sim import get_current_stage
u = env.unwrapped
robot = u.scene["robot"]
actions = torch.zeros((u.num_envs, 3), device=u.device)
all_ids = torch.arange(u.num_envs, device=u.device, dtype=torch.int32)
env.reset()
for _ in range(50):
actions[:, 2] = torch.where(all_ids % 2 == 0, 1.0, -1.0)
_step(env, actions)
stage = get_current_stage()
prim_count = sum(1 for _ in stage.Traverse())
sensor_id, robot_id = id(u.scene["foot_contact"]), id(robot)
torch.cuda.synchronize()
mem0 = torch.cuda.memory_allocated()
memory = []
max_q_error = max_vel = max_unselected = max_history = 0.0
latencies = []
for iteration in range(count):
mode = iteration % 4
ids = (
all_ids
if mode == 0
else (
all_ids[iteration % u.num_envs : iteration % u.num_envs + 1]
if mode == 1
else all_ids[iteration % 2 :: 2]
)
)
if mode == 3:
ids = all_ids[:0] # documented no-op edge handling at caller
selected = torch.zeros(u.num_envs, dtype=torch.bool, device=u.device)
selected[ids.long()] = True
q0, dq0 = robot.data.joint_pos.torch.clone(), robot.data.joint_vel.torch.clone()
action0 = u.action_manager.action.clone()
sim_count = u._sim_step_counter
start = time.perf_counter()
if ids.numel():
u.reset(env_ids=ids)
torch.cuda.synchronize()
latencies.append(time.perf_counter() - start)
assert u._sim_step_counter == sim_count, "Reset advanced physical time"
if ids.numel():
max_q_error = max(
max_q_error,
(robot.data.joint_pos.torch[ids] - robot.data.default_joint_pos.torch[ids]).abs().max().item(),
)
max_vel = max(max_vel, robot.data.joint_vel.torch[ids].abs().max().item())
assert max_q_error <= 1e-5 and max_vel <= 1e-5
assert u.episode_length_buf[ids].max() == 0
assert u.action_manager.action[ids].abs().max() == 0
assert u.action_manager.prev_action[ids].abs().max() == 0
assert torch.allclose(
robot.data.joint_pos_target.torch[ids], robot.data.default_joint_pos.torch[ids], atol=1e-5
)
h = u.scene["foot_contact"].data.net_forces_w_history.torch[ids]
max_history = max(max_history, h.abs().max().item())
assert max_history == 0.0, f"Stale contact history after reset: {max_history}"
other = ~selected
if other.any():
change = max(
(robot.data.joint_pos.torch[other] - q0[other]).abs().max().item(),
(robot.data.joint_vel.torch[other] - dq0[other]).abs().max().item(),
(u.action_manager.action[other] - action0[other]).abs().max().item(),
)
max_unselected = max(max_unselected, change)
assert change <= 1e-6, f"Unselected state changed during reset: {change}"
_step(env, actions)
assert id(u.scene["foot_contact"]) == sensor_id and id(u.scene["robot"]) == robot_id
if iteration == min(7, count - 1):
# Include verification scratch tensors and warm-up all subset shapes.
# The baseline must not precede the test's own snapshot allocations.
torch.cuda.synchronize()
mem0 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu0 = total - free
if iteration % 100 == 0:
torch.cuda.synchronize()
memory.append(torch.cuda.memory_allocated())
torch.cuda.synchronize()
mem1 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu1 = total - free
assert gpu1 <= gpu0 * 1.05, f"Device memory growth (including Warp/PhysX): {gpu0}->{gpu1}"
assert get_current_stage() == stage and sum(1 for _ in stage.Traverse()) == prim_count
assert mem1 <= mem0 * 1.05, f"CUDA memory growth: {mem0}->{mem1}"
return {
"passed": True,
"reset_operations": count,
"nonempty_reset_operations": count - count // 4,
"q_error_max_rad": max_q_error,
"dq_max_rad_s": max_vel,
"unselected_change_max": max_unselected,
"stale_history_max_N": max_history,
"cuda_before_bytes": mem0,
"cuda_after_bytes": mem1,
"device_used_before_bytes": gpu0,
"device_used_after_bytes": gpu1,
"cuda_samples_bytes": memory,
"reset_latency_mean_ms": 1000 * sum(latencies) / len(latencies),
"reset_latency_max_ms": 1000 * max(latencies),
"stage_prim_count": prim_count,
}
def check_long(env, duration):
import torch
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.observations import diagnostics
u = env.unwrapped
env.reset()
steps = round(duration / u.step_dt)
assert steps > 0
actions = torch.zeros((u.num_envs, 3), device=u.device)
initial = diagnostics(u)["anchor_pose_w"].clone()
drift = 0.0
forces = 0.0
counter = u._sim_step_counter
for _ in range(200):
_step(env, actions)
d = diagnostics(u) # warm the same diagnostic/scratch allocations as rollout
counter = u._sim_step_counter
torch.cuda.synchronize()
start = time.perf_counter()
mem0 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu0 = total - free
rows = []
for step in range(steps):
actions[:, 0] = 0.2 * math.sin(2 * math.pi * 0.5 * step * u.step_dt)
actions[:, 2] = torch.where(
torch.arange(u.num_envs, device=u.device) % 2 == 0,
0.8 * math.sin(2 * math.pi * 0.25 * step * u.step_dt),
-0.8 * math.sin(2 * math.pi * 0.25 * step * u.step_dt),
)
_step(env, actions)
d = diagnostics(u)
assert all(torch.isfinite(d[k]).all() for k in ("q", "dq", "computed_torque", "normal_force_w"))
limits = u.scene["robot"].data.joint_pos_limits.torch
assert ((d["q"] >= limits[:, :, 0] - 0.01) & (d["q"] <= limits[:, :, 1] + 0.01)).all()
assert (d["applied_torque"].abs() <= u.scene["robot"].data.joint_effort_limits.torch + 1e-4).all()
drift = max(drift, (d["anchor_pose_w"][:, :3] - initial[:, :3]).abs().max().item())
forces = max(forces, d["normal_force_w"][:, 2].max().item())
if step % 1000 == 0:
rows.append(
[
step * u.step_dt,
*d["q"][0].tolist(),
d["gap"][0].item(),
d["normal_force_w"][0, 2].item(),
torch.cuda.memory_allocated(),
]
)
if step % 10000 == 0:
print(f"[VERIFY] long rollout {step}/{steps}", flush=True)
torch.cuda.synchronize()
elapsed = time.perf_counter() - start
assert u._sim_step_counter - counter == steps * 5
assert (u.episode_length_buf == steps + 200).all()
assert drift <= 1e-4
mem1 = torch.cuda.memory_allocated()
free, total = torch.cuda.mem_get_info()
gpu1 = total - free
assert mem1 <= mem0 * 1.05
assert gpu1 <= gpu0 * 1.05, f"Device memory growth (including Warp/PhysX): {gpu0}->{gpu1}"
return {
"passed": True,
"duration_s": steps * u.step_dt,
"control_steps": steps,
"physics_steps": steps * 5,
"physics_dt": u.physics_dt,
"control_dt": u.step_dt,
"wall_seconds": elapsed,
"env_steps_per_second": steps * u.num_envs / elapsed,
"root_drift_m": drift,
"max_normal_fz_N": forces,
"cuda_before_bytes": mem0,
"cuda_after_bytes": mem1,
"cuda_peak_bytes": torch.cuda.max_memory_allocated(),
"device_used_before_bytes": gpu0,
"device_used_after_bytes": gpu1,
"device_total_bytes": total,
"trace": rows,
}
def run(env, args):
import json
if args.num_envs < 1 or args.reset_count < 8 or args.duration <= 0 or not math.isfinite(args.duration):
raise ValueError("Require num_envs >=1, reset_count >=8, finite duration >0")
result = {
"suite": args.suite,
"num_envs": args.num_envs,
"seed": args.seed,
"backend": "Kit PhysxCfg",
"contract": check_contract(env),
}
checks = {
"stability": lambda: check_stability(env),
"calibration": lambda: check_calibration(env),
"contact": lambda: check_contact(env),
"reset": lambda: check_resets(env, args.reset_count),
"long": lambda: check_long(env, args.duration),
}
names = ["contact", "reset", "long"] if args.suite == "core" else ([args.suite] if args.suite in checks else [])
for name in names:
print(f"[VERIFY] Starting {name}", flush=True)
result[name] = checks[name]()
if "trace" in result[name]:
import csv
headers = (
["cycle", "phase", "gap_m", "normal_fz_N"]
if name == "contact"
else [
"time_s",
"q_hip_rad",
"q_thigh_rad",
"q_calf_rad",
"gap_m",
"normal_fz_N",
"torch_allocated_bytes",
]
)
with (args.output / f"{name}.csv").open("w", newline="") as stream:
writer = csv.writer(stream)
writer.writerow(headers)
writer.writerows(result[name]["trace"])
(args.output / "checkpoint.json").write_text(json.dumps(result, indent=2) + "\n")
result["passed"] = True
result["milestone_complete"] = False # individual suites are not full acceptance
return result
def main():
# Full acceptance runs fresh Kit processes so the air anchor and probe are
# independent physical scenes, not reset-time mutations of a fixed joint.
import argparse
import json
import subprocess
import sys
from pathlib import Path
parser = argparse.ArgumentParser(add_help=False)
parser.add_argument("--suite", default="full")
parser.add_argument("--output", type=Path, default=Path("logs/week1_anchor/acceptance"))
args, argv = parser.parse_known_args()
if args.suite != "full" or "--help" in sys.argv or "-h" in sys.argv:
launch(__doc__, configure, run)
return
args.output.mkdir(parents=True, exist_ok=True)
write_json(args.output / "result.json", {"passed": False, "status": "running"})
results = {}
for suite in ("stability", "calibration", "core"):
output = args.output / suite
print(f"[VERIFY] Fresh process: {suite}", flush=True)
with (args.output / f"{suite}.log").open("w") as log:
child = subprocess.run(
[sys.executable, str(Path(__file__).resolve()), *argv, "--suite", suite, "--output", str(output)],
stdout=log,
stderr=subprocess.STDOUT,
)
critical = critical_log_lines(args.output / f"{suite}.log")
if child.returncode or critical:
write_json(
args.output / "result.json",
{
"passed": False,
"failed_suite": suite,
"returncode": child.returncode,
"critical_log_lines": critical[-20:],
"completed": results,
},
)
raise SystemExit(child.returncode or 1)
report = json.loads((output / "result.json").read_text())
if not report.get("passed"):
write_json(args.output / "result.json", {"passed": False, "failed_suite": suite, "report": report})
raise SystemExit(1)
results[suite] = report
core = results["core"]
complete = (
core["num_envs"] == 64
and core["long"]["duration_s"] >= 600.0
and core["reset"]["nonempty_reset_operations"] >= 1000
)
result = {"passed": True, "milestone_complete": complete, "results": results}
write_json(args.output / "result.json", result)
print(f"[VERIFY] Passed; complete acceptance={complete}; {args.output}/result.json", flush=True)
if __name__ == "__main__":
main()
+1
View File
@@ -38,6 +38,7 @@ args_cli, hydra_args = setup_preset_cli(parser)
sys.argv = [sys.argv[0]] + hydra_args
import SkillFactory.tasks # noqa: F401
MAX_STEPS = 100
@@ -0,0 +1 @@
"""Local, reproducibly generated simulation assets."""
@@ -0,0 +1,55 @@
"""Go2 FL fixed-anchor configuration; asset conversion is always explicit."""
import json
import os
from pathlib import Path
import isaaclab.sim as sim_utils
from isaaclab.actuators import IdealPDActuatorCfg
from isaaclab.assets import ArticulationCfg
ROOT = Path(__file__).resolve().parents[4]
ASSET_DIR = Path(os.environ.get("SKILLFACTORY_GO2_ASSET_DIR", ROOT / "assets/generated/go2_fl"))
JOINT_NAMES = ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint"]
if not (ASSET_DIR / "conversion.json").is_file():
raise FileNotFoundError(
"Build Go2 asset first: scripts/assets/extract_go2_leg.py then scripts/assets/convert_go2_leg.py"
)
MANIFEST = json.loads((ASSET_DIR / "manifest.json").read_text())
CONVERSION = json.loads((ASSET_DIR / "conversion.json").read_text())
USD_PATH = ASSET_DIR / CONVERSION["usd_path"]
if not USD_PATH.is_file():
raise FileNotFoundError(f"Missing generated USD: {USD_PATH}; rerun converter")
FOOT_PATH = CONVERSION["body_paths"]["FL_foot"]
ANCHOR_PATH = CONVERSION["body_paths"]["anchor_base"]
EFFORT_LIMITS = {j["name"]: j["limits"]["effort"] for j in MANIFEST["joints"]}
VELOCITY_LIMITS = {j["name"]: j["limits"]["velocity"] for j in MANIFEST["joints"]}
GO2_LEG_CFG = ArticulationCfg(
prim_path="{ENV_REGEX_NS}/Robot",
spawn=sim_utils.UsdFileCfg(
usd_path=str(USD_PATH),
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False, max_depenetration_velocity=1.0),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4
),
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, MANIFEST["anchor"]["default_height"]),
joint_pos=dict(zip(JOINT_NAMES, MANIFEST["control"]["q_default"])),
joint_vel={".*": 0.0},
),
soft_joint_pos_limit_factor=0.9,
actuators={
"leg": IdealPDActuatorCfg(
joint_names_expr=JOINT_NAMES,
stiffness=MANIFEST["control"]["kp"],
damping=MANIFEST["control"]["kd"],
effort_limit=EFFORT_LIMITS,
effort_limit_sim=EFFORT_LIMITS,
velocity_limit=VELOCITY_LIMITS,
velocity_limit_sim=VELOCITY_LIMITS,
)
},
)
@@ -0,0 +1,5 @@
"""Single-leg asset and learning contracts; no Kit imports at module load."""
from .spec import JointSpec, LegSpec
__all__ = ["JointSpec", "LegSpec"]
@@ -0,0 +1,180 @@
"""URMA-inspired shared actor/critic; no claim of a pretrained modular Skill."""
from __future__ import annotations
import math
import torch
from torch import nn
from torch.distributions import Normal
from .tokens import FEATURE_SCHEMA
def mlp(inputs, hidden, outputs):
return nn.Sequential(nn.Linear(inputs, hidden), nn.LayerNorm(hidden), nn.ELU(), nn.Linear(hidden, outputs))
class DescriptionRouter(nn.Module):
"""Per-latent-dimension softmax over valid entities, not full Transformer attention."""
def __init__(self, descriptions, observations, hidden=64, latent=32):
super().__init__()
self.description = mlp(descriptions, hidden, latent)
self.observation = mlp(observations, hidden, latent)
self.temperature = nn.Parameter(torch.tensor(1.0))
def forward(self, description, observation, mask):
mask = mask.bool()
description = torch.where(mask[..., None], description, 0.0)
observation = torch.where(mask[..., None], observation, 0.0)
encoded = self.description(description)
logits = encoded / (self.temperature.clamp(min=0.0) + 0.015)
logits = logits.masked_fill(~mask[..., None], -torch.inf)
logits = torch.where(mask.any(dim=1)[:, None, None], logits, 0.0)
weights = logits.softmax(dim=1) * mask[..., None]
local = weights * self.observation(observation)
return local.sum(dim=1), local, encoded
class LegActorCritic(nn.Module):
def __init__(self, hidden=64, latent=32):
super().__init__()
self.config = {"hidden": hidden, "latent": latent}
self.joints = DescriptionRouter(len(FEATURE_SCHEMA["joint_description"]), 3, hidden, latent)
self.feet = DescriptionRouter(
len(FEATURE_SCHEMA["effector_description"]), len(FEATURE_SCHEMA["effector_observation"]), hidden, latent
)
general = len(FEATURE_SCHEMA["context"]) + len(FEATURE_SCHEMA["goal"])
self.core = mlp(latent * 2 + general, hidden, latent)
self.mean = mlp(latent * 3, hidden, 1)
self.log_std = mlp(latent, hidden, 1)
nn.init.zeros_(self.log_std[-1].weight)
nn.init.constant_(self.log_std[-1].bias, math.log(0.3))
self.critic = mlp(latent * 2 + general, hidden, 1)
self.float() # feature contract is float32, independent of torch's global default
def _features(self, batch):
pooled, local, descriptions = self.joints(
batch["joint_description"], batch["joint_observation"], batch["joint_mask"]
)
feet, _, _ = self.feet(batch["effector_description"], batch["effector_observation"], batch["effector_mask"])
shared = torch.cat((pooled, feet, batch["context"], batch["goal"]), dim=-1)
return shared, local, descriptions
def distribution_value(self, batch):
shared, local, description = self._features(batch)
intent = self.core(shared)[:, None, :].expand_as(local)
mean = self.mean(torch.cat((description, intent, local), dim=-1)).squeeze(-1)
mask = batch["joint_mask"].bool()
mean = torch.where(mask, mean, 0.0)
std = self.log_std(description).squeeze(-1).clamp(-5.0, 1.0).exp()
std = torch.where(mask, std, 1.0)
return Normal(mean, std), self.critic(shared).squeeze(-1)
def act(self, batch, deterministic=False):
distribution, value = self.distribution_value(batch)
mask = batch["joint_mask"].bool()
action = torch.where(mask, distribution.mean if deterministic else distribution.sample(), 0.0)
log_prob = torch.where(mask, distribution.log_prob(action), 0.0).sum(dim=-1)
return action, log_prob, value
def evaluate(self, batch, actions):
distribution, value = self.distribution_value(batch)
mask = batch["joint_mask"].bool()
actions = torch.where(mask, actions, 0.0)
return (
torch.where(mask, distribution.log_prob(actions), 0.0).sum(dim=-1),
torch.where(mask, distribution.entropy(), 0.0).sum(dim=-1),
value,
)
def value(self, batch):
return self.critic(self._features(batch)[0]).squeeze(-1)
def save_policy(model, path, metadata=None):
torch.save(
{
"model_config": model.config,
"feature_schema": FEATURE_SCHEMA,
"state_dict": {k: v.detach().cpu() for k, v in model.state_dict().items()},
"metadata": metadata or {},
"trained_skill": False,
},
path,
)
def load_policy(path, device="cpu"):
checkpoint = torch.load(path, map_location=device, weights_only=True)
if checkpoint["feature_schema"] != FEATURE_SCHEMA:
raise ValueError("Incompatible feature schema")
model = LegActorCritic(**checkpoint["model_config"]).to(device)
model.load_state_dict(checkpoint["state_dict"])
return model
@torch.no_grad()
def compute_gae(rewards, values, next_values, done, gamma=0.99, lam=0.95):
"""next_values includes terminal-state bootstrap for timeouts, zero for true termination."""
if (
not (rewards.shape == values.shape == next_values.shape == done.shape)
or rewards.ndim != 2
or min(rewards.shape) < 1
or done.dtype != torch.bool
):
raise ValueError("GAE requires nonempty matching [T,B] tensors and bool done")
if not 0 <= gamma <= 1 or not 0 <= lam <= 1:
raise ValueError("GAE discounts must lie in [0,1]")
if any(not torch.isfinite(value).all() for value in (rewards, values, next_values)):
raise ValueError("Non-finite GAE input")
advantage = torch.zeros_like(rewards)
carry = torch.zeros_like(rewards[0])
for t in reversed(range(len(rewards))):
delta = rewards[t] + gamma * next_values[t] - values[t]
carry = delta + gamma * lam * (~done[t].bool()) * carry
advantage[t] = carry
return advantage, advantage + values
def ppo_update(model, optimizer, data, epochs=2, minibatch=128):
if type(epochs) is not int or type(minibatch) is not int or min(epochs, minibatch) < 1:
raise ValueError("PPO epochs/minibatch must be positive integers")
advantage, returns = compute_gae(data["rewards"], data["values"], data["next_values"], data["done"])
tensors = [*data["observation"].values(), data["actions"], data["log_prob"]]
if (
data["log_prob"].shape != data["rewards"].shape
or any(value.shape[:2] != data["rewards"].shape for value in tensors)
or any(not torch.isfinite(value).all() for value in tensors)
):
raise ValueError("PPO requires finite, consistently shaped [T,B,...] rollout tensors")
batch = {k: v.detach().flatten(0, 1) for k, v in data["observation"].items()}
actions = data["actions"].detach().flatten(0, 1)
old_log = data["log_prob"].detach().flatten(0, 1)
advantage = advantage.flatten()
advantage = (advantage - advantage.mean()) / (advantage.std(unbiased=False) + 1e-8)
returns = returns.flatten()
rows = []
for _ in range(epochs):
indices = torch.randperm(len(actions), device=actions.device)
for start in range(0, len(actions), minibatch):
ids = indices[start : start + minibatch]
current = {k: v[ids] for k, v in batch.items()}
log_prob, entropy, value = model.evaluate(current, actions[ids])
ratio = (log_prob - old_log[ids]).exp()
if not torch.isfinite(ratio).all():
raise RuntimeError("Non-finite PPO probability ratio")
surrogate = torch.minimum(ratio * advantage[ids], ratio.clamp(0.8, 1.2) * advantage[ids])
actor = -surrogate.mean()
critic = (value - returns[ids]).square().mean()
loss = actor + 0.5 * critic - 0.01 * entropy.mean()
if not torch.isfinite(loss):
raise RuntimeError("Non-finite PPO loss")
optimizer.zero_grad(set_to_none=True)
loss.backward()
norm = nn.utils.clip_grad_norm_(model.parameters(), 1.0, error_if_nonfinite=True)
optimizer.step()
rows.append(torch.stack((actor.detach(), critic.detach(), entropy.detach().mean(), norm.detach())))
means = torch.stack(rows).mean(dim=0).cpu().tolist()
return dict(zip(("actor_loss", "value_loss", "entropy", "gradient_norm"), means))
@@ -0,0 +1,144 @@
"""Versioned declarative single-leg import contract (SI units)."""
from __future__ import annotations
import json
import math
import re
from dataclasses import asdict, dataclass
from pathlib import Path
@dataclass(frozen=True)
class JointSpec:
name: str
q_default: float
action_scale: float
kp: float
kd: float
def __post_init__(self):
values = (self.q_default, self.action_scale, self.kp, self.kd)
if not isinstance(self.name, str) or not self.name.strip() or not all(math.isfinite(v) for v in values):
raise ValueError("Joint names and finite control parameters are required")
if self.action_scale <= 0 or self.kp <= 0 or self.kd < 0:
raise ValueError("Require action_scale>0, kp>0, kd>=0")
@dataclass(frozen=True)
class LegSpec:
schema_version: int
part_id: str
source_root: Path
urdf: str
package_name: str
license_file: str
license_id: str
mount_joint: str
auxiliary_root_joints: tuple[str, ...]
joints: tuple[JointSpec, ...]
foot_body: str
foot_collision_index: int = 0
soft_limit_factor: float = 0.9
physics_dt: float = 0.002
decimation: int = 5
def __post_init__(self):
if (
type(self.schema_version) is not int
or self.schema_version != 1
or not re.fullmatch(r"[a-z][a-z0-9_]*", self.part_id)
):
raise ValueError("Expected schema_version=1 and a safe lowercase part_id")
names = [joint.name for joint in self.joints]
if not names or len(set(names)) != len(names) or self.mount_joint not in names:
raise ValueError("Require unique actuated joints including mount_joint")
if len(set(self.auxiliary_root_joints)) != len(self.auxiliary_root_joints) or set(
self.auxiliary_root_joints
) & set(names):
raise ValueError("Duplicate/actuated auxiliary roots")
if not self.foot_body or not self.package_name or not self.license_id or not self.license_file:
raise ValueError("Foot, source package, and license provenance are required")
if type(self.foot_collision_index) is not int or self.foot_collision_index < 0:
raise ValueError("foot_collision_index must be a nonnegative integer")
if not math.isfinite(self.soft_limit_factor) or not 0 < self.soft_limit_factor <= 1:
raise ValueError("Invalid soft_limit_factor")
if not math.isfinite(self.physics_dt) or self.physics_dt <= 0:
raise ValueError("Invalid physics_dt")
if type(self.decimation) is not int or self.decimation < 1:
raise ValueError("decimation must be a positive integer")
path = (self.source_root / self.urdf).resolve()
if not path.is_relative_to(self.source_root.resolve()):
raise ValueError("URDF must be inside source_root")
@classmethod
def load(cls, path: str | Path, source_root: str | Path | None = None) -> LegSpec:
return cls.from_dict(json.loads(Path(path).read_text()), source_root)
@classmethod
def from_dict(cls, data: dict, source_root: str | Path | None = None) -> LegSpec:
values = dict(data)
values["source_root"] = Path(source_root or values["source_root"]).expanduser().resolve()
values["joints"] = tuple(JointSpec(**item) for item in values["joints"])
values["auxiliary_root_joints"] = tuple(values.get("auxiliary_root_joints", []))
return cls(**values)
def to_dict(self) -> dict:
data = asdict(self)
data["source_root"] = str(self.source_root)
return data
def bundle_file(directory: Path, relative: str) -> Path:
"""Resolve a bundle-owned file, rejecting traversal and escaping symlinks."""
path = (directory / relative).resolve()
if not path.is_relative_to(directory):
raise ValueError(f"Unsafe bundle path: {relative}")
if not path.is_file():
raise FileNotFoundError(path)
return path
def load_bundle(directory: str | Path, require_usd: bool = False) -> dict:
"""Refuse stale URDF/manifest/converted USD combinations, before using physics."""
import hashlib
directory = Path(directory).resolve()
manifest_path = bundle_file(directory, "manifest.json")
bundle = json.loads(manifest_path.read_text())
if bundle.get("schema_version") != 2 or bundle.get("kind") != "single_leg":
raise ValueError("Expected single_leg asset manifest v2")
LegSpec.from_dict(bundle["spec"])
if any(not math.isfinite(bundle[key]) or bundle[key] <= 0 for key in ("length_scale_m", "known_mass_scale_kg")):
raise ValueError("Require positive finite bundle scales")
if hashlib.sha256(bundle_file(directory, "leg.urdf").read_bytes()).hexdigest() != bundle["derived_urdf_sha256"]:
raise ValueError("Derived URDF hash mismatch; rebuild the leg")
if hashlib.sha256(bundle_file(directory, "LICENSE").read_bytes()).hexdigest() != bundle["source"]["license_sha256"]:
raise ValueError("License provenance hash mismatch")
declared = [joint["name"] for joint in bundle["spec"]["joints"]]
if bundle["joint_order"] != declared or [joint["name"] for joint in bundle["joints"]] != declared:
raise ValueError("Manifest actuator order mismatch")
for relative, expected in bundle["resources"].items():
resource = bundle_file(directory, relative)
if hashlib.sha256(resource.read_bytes()).hexdigest() != expected:
raise ValueError(f"Resource hash/path mismatch: {relative}")
if require_usd:
conversion = json.loads(bundle_file(directory, "conversion.json").read_text())
if conversion["manifest_sha256"] != hashlib.sha256(manifest_path.read_bytes()).hexdigest():
raise ValueError("Stale USD conversion; rerun scripts/assets/convert_leg.py")
usd = bundle_file(directory, conversion["usd_path"])
layers = {bundle_file(directory, relative) for relative in conversion["used_layers"]}
if usd not in layers or conversion.get("fixed_to_world") is not True:
raise ValueError("USD entry layer/fixed support contract mismatch")
expected_bodies = set(bundle["links"]) | {bundle["anchor_body"]}
for key, expected in (("body_paths", expected_bodies), ("joint_paths", set(declared))):
mapping = conversion[key]
if set(mapping) != expected or len(set(mapping.values())) != len(mapping):
raise ValueError(f"USD {key} mapping mismatch")
if any(
not path.startswith("/") or any(p in ("", ".", "..") for p in path[1:].split("/"))
for path in mapping.values()
):
raise ValueError(f"Unsafe USD {key} prim path")
bundle["conversion"] = conversion
return bundle
@@ -0,0 +1,232 @@
"""Simulator-independent token schema v1 and batched arbitrary-axis FK."""
from __future__ import annotations
import math
import torch
from .urdf import forward, multiply, pose
FEATURE_SCHEMA = {
"version": 1,
"joint_description": [
"pivot_x/L",
"pivot_y/L",
"pivot_z/L",
"axis_x",
"axis_y",
"axis_z",
"q_nominal/pi",
"control_min/pi",
"control_max/pi",
"effort/Emax",
"velocity/Vmax",
"kp*pi/Emax",
"kd*Vmax/Emax",
"action_scale/pi",
"child_mass/Mknown",
"child_Ixx/ML2",
"child_Ixy/ML2",
"child_Ixz/ML2",
"child_Iyy/ML2",
"child_Iyz/ML2",
"child_Izz/ML2",
"child_com_x/L",
"child_com_y/L",
"child_com_z/L",
"inertial_valid",
],
"joint_observation": ["(q-q_nominal)/action_scale", "dq/velocity_limit", "last_applied_action"],
"effector_description": [
"nominal_x/L",
"nominal_y/L",
"nominal_z/L",
"collision_x/L",
"collision_y/L",
"collision_z/L",
"collision_qx",
"collision_qy",
"collision_qz",
"collision_qw",
"sphere_radius/L",
"box_x/L",
"box_y/L",
"box_z/L",
"is_sphere",
"is_box",
],
"effector_observation": [
"link_x/L",
"link_y/L",
"link_z/L",
"link_vx/(L/s)",
"link_vy/(L/s)",
"link_vz/(L/s)",
"normal_Fx/(Mknown*g)",
"normal_Fy/(Mknown*g)",
"normal_Fz/(Mknown*g)",
"contact",
"selected_collision_gap/L",
],
"context": [
"gravity_x",
"gravity_y",
"gravity_z",
"control_dt_s",
"ground_z_relative_to_anchor/L",
"length_scale_m",
"known_mass_scale_kg",
"max_effort/(Mknown*g*L)",
"max_joint_velocity_rad_per_s",
"has_missing_source_inertia",
],
"goal": ["target_link_x/L", "target_link_y/L", "target_link_z/L"],
"force_semantics": "normal contact force only; no friction wrench",
"inertia_semantics": (
"Source child COM inertia and relative COM offset rotated into anchor nominal frame; "
"not fixed-subtree aggregated"
),
"mass_semantics": "Known source mass excludes synthetic anchor; not a prediction of support force",
}
class LegKinematics:
def __init__(self, bundle, device="cpu"):
self.bundle = bundle
self.device = torch.empty(0, device=device, dtype=torch.float32).device
self.names = bundle["joint_order"]
self.indices = {name: i for i, name in enumerate(self.names)}
self.eye = torch.eye(4, device=self.device, dtype=torch.float32)
self.nodes = []
for node in bundle["tree"]:
static = self.eye.new_tensor(pose(node["xyz"], node["rpy"]))
axis = self.eye.new_tensor(node["axis"])
x, y, z = node["axis"]
skew = self.eye.new_tensor([[0, -z, y], [z, 0, -x], [-y, x, 0]])
self.nodes.append((node, static, axis[:, None] * axis[None, :], skew))
def forward(self, q):
if q.ndim != 2 or q.shape[1] != len(self.names) or q.device != self.device or q.dtype != torch.float32:
raise ValueError("FK requires float32 [B, declared_joint_count] on the configured device")
b = len(q)
poses = {self.bundle["anchor_body"]: self.eye.expand(b, -1, -1)}
for node, static, outer, skew in self.nodes:
frame = poses[node["parent"]] @ static
if node["type"] == "revolute":
angle = q[:, self.indices[node["name"]]]
c, s = angle.cos()[:, None, None], angle.sin()[:, None, None]
rot = c * self.eye[:3, :3] + (1 - c) * outer + s * skew
local = self.eye.expand(b, -1, -1).clone()
local[:, :3, :3] = rot
frame = frame @ local
poses[node["child"]] = frame
return poses
def foot_position(self, q):
return self.forward(q)[self.bundle["foot"]["body"]][:, :3, 3]
def static_tokens(bundle, max_joints, device="cpu"):
n = len(bundle["joints"])
if type(max_joints) is not int or max_joints < n:
raise ValueError(f"max_joints must be >= {n}")
length, mass = bundle["length_scale_m"], bundle["known_mass_scale_kg"]
effort = max(j["limits"]["effort"] for j in bundle["joints"])
velocity = max(j["limits"]["velocity"] for j in bundle["joints"])
rows = []
nominal_poses, _ = forward(bundle["tree"], {j["name"]: j["q_default"] for j in bundle["spec"]["joints"]})
for record, control in zip(bundle["joints"], bundle["spec"]["joints"]):
child = bundle["links"][record["child"]]
nominal, scale = control["q_default"], control["action_scale"]
inertia, com = [0.0] * 6, [0.0] * 3
if child is not None:
frame = multiply(nominal_poses[record["child"]], pose(child["xyz"], child["rpy"]))
a, b, c, d, e, f = child["tensor6"]
tensor = [[a, b, c], [b, d, e], [c, e, f]]
rotated = [
[
sum(frame[i][k] * tensor[k][column] * frame[j][column] for k in range(3) for column in range(3))
for j in range(3)
]
for i in range(3)
]
inertia = [rotated[i][j] for i, j in ((0, 0), (0, 1), (0, 2), (1, 1), (1, 2), (2, 2))]
com = [frame[i][3] - record["nominal_position"][i] for i in range(3)]
low = max(record["soft_limits"][0], nominal - scale)
high = min(record["soft_limits"][1], nominal + scale)
rows.append(
[
*(v / length for v in record["nominal_position"]),
*record["nominal_axis"],
nominal / math.pi,
low / math.pi,
high / math.pi,
record["limits"]["effort"] / effort,
record["limits"]["velocity"] / velocity,
control["kp"] * math.pi / effort,
control["kd"] * velocity / effort,
scale / math.pi,
child["mass"] / mass if child else 0.0,
*(v / (mass * length**2) for v in inertia),
*(v / length for v in com),
float(child is not None),
]
)
joint = torch.zeros(1, max_joints, len(FEATURE_SCHEMA["joint_description"]), device=device, dtype=torch.float32)
joint[0, :n] = torch.tensor(rows, device=device, dtype=torch.float32)
mask = torch.arange(max_joints, device=device)[None, :] < n
foot = bundle["foot"]
r, p, y = (v / 2 for v in foot["rpy"])
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
quat = [
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy,
]
values = [
*(v / length for v in foot["nominal_position"]),
*(v / length for v in foot["xyz"]),
*quat,
foot.get("radius", 0.0) / length,
*(v / length for v in foot.get("size", [0.0] * 3)),
float(foot["type"] == "sphere"),
float(foot["type"] == "box"),
]
return {
"joint_description": joint,
"joint_mask": mask,
"effector_description": torch.tensor(values, device=device, dtype=torch.float32)[None, None, :],
"effector_mask": torch.ones(1, 1, device=device, dtype=torch.bool),
}
def pad_joint_observations(q, dq, previous_action, bundle, max_joints):
n = len(bundle["joint_order"])
if any(value.dtype != torch.float32 or value.device != q.device for value in (q, dq, previous_action)):
raise ValueError("Joint states/actions require float32 on one device")
if q.shape != dq.shape or q.shape != previous_action.shape or q.ndim != 2 or q.shape[1] != n or max_joints < n:
raise ValueError("Joint state/action shape mismatch")
controls = bundle["spec"]["joints"]
nominal = q.new_tensor([j["q_default"] for j in controls])
scale = q.new_tensor([j["action_scale"] for j in controls])
limit = q.new_tensor([j["limits"]["velocity"] for j in bundle["joints"]])
result = q.new_zeros(len(q), max_joints, 3)
result[:, :n] = torch.stack(((q - nominal) / scale, dq / limit, previous_action), dim=-1)
return result
def native_actions(actions, num_envs, num_joints, max_joints, device):
if not 0 < num_joints <= max_joints or num_envs < 1:
raise ValueError("Require B>=1 and 1<=native_DoF<=Jmax")
if (
actions.shape != (num_envs, max_joints)
or actions.dtype != torch.float32
or actions.device != torch.device(device)
):
raise ValueError("Expected float32 [B,Jmax] actions on the environment device")
result = actions[:, :num_joints]
if not torch.isfinite(result).all():
raise ValueError("Non-finite active action")
return result
@@ -0,0 +1,411 @@
"""Pure-stdlib tree extraction and arbitrary-axis FK; never loads a simulator."""
from __future__ import annotations
import copy
import hashlib
import json
import math
import shutil
import subprocess
import xml.etree.ElementTree as ET
from pathlib import Path
from .spec import LegSpec
ANCHOR = "anchor_base"
def digest(path):
return hashlib.sha256(Path(path).read_bytes()).hexdigest()
def vector(text, length=3):
values = [float(v) for v in text.split()]
if len(values) != length or not all(math.isfinite(v) for v in values):
raise ValueError(f"Expected {length} finite values: {text}")
return values
def origin(element):
child = element.find("origin")
return {key: vector(child.get(key, "0 0 0") if child is not None else "0 0 0") for key in ("xyz", "rpy")}
def identity():
return [[float(i == j) for j in range(4)] for i in range(4)]
def multiply(a, b):
return [[sum(a[i][k] * b[k][j] for k in range(4)) for j in range(4)] for i in range(4)]
def pose(xyz, rpy):
r, p, y = rpy
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
return [
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, xyz[0]],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, xyz[1]],
[-sp, cp * sr, cp * cr, xyz[2]],
[0.0, 0.0, 0.0, 1.0],
]
def inverse(t):
result = identity()
for i in range(3):
for j in range(3):
result[i][j] = t[j][i]
result[i][3] = -sum(t[k][i] * t[k][3] for k in range(3))
return result
def rotation(axis, angle):
x, y, z = axis
cross = [[0, -z, y], [z, 0, -x], [-y, x, 0]]
result = identity()
c, s = math.cos(angle), math.sin(angle)
for i in range(3):
for j in range(3):
result[i][j] = c * float(i == j) + (1 - c) * axis[i] * axis[j] + s * cross[i][j]
return result
def inertial(link):
item = link.find("inertial")
if item is None:
return None
mass = float(item.find("mass").get("value"))
values = [float(item.find("inertia").get(k, "0")) for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")]
a, b, c, d, e, f = values
det = a * (d * f - e * e) - b * (b * f - c * e) + c * (b * e - c * d)
if not all(math.isfinite(v) for v in [mass, *values]) or mass <= 0 or min(a, a * d - b * b, det) <= 0:
raise ValueError(f"Invalid complete inertia: {link.get('name')}")
return {"mass": mass, "tensor6": values, **origin(item)}
def forward(tree, q, anchor=ANCHOR):
links = {anchor: identity()}
pivots = {}
for node in tree:
pivot = multiply(links[node["parent"]], pose(node["xyz"], node["rpy"]))
pivots[node["name"]] = pivot
links[node["child"]] = (
multiply(pivot, rotation(node["axis"], q[node["name"]])) if node["type"] == "revolute" else pivot
)
return links, pivots
def _inside(path, root):
path = path.resolve()
if not path.is_relative_to(root):
raise ValueError(f"Resource escapes source_root: {path}")
if not path.is_file():
raise FileNotFoundError(path)
return path
def _source_tree(original):
link_nodes = original.findall("link")
joint_nodes = original.findall("joint")
links = {item.get("name"): item for item in link_nodes}
joints = {item.get("name"): item for item in joint_nodes}
if (
not all(links)
or not all(joints)
or len(links) != len(link_nodes)
or len(joints) != len(joint_nodes)
or ANCHOR in links
):
raise ValueError("Duplicate names or reserved anchor_base link")
parent_of = {}
for joint in joint_nodes:
if joint.find("parent") is None or joint.find("child") is None:
raise ValueError(f"Missing parent/child: {joint.get('name')}")
parent, child = joint.find("parent").get("link"), joint.find("child").get("link")
if parent not in links or child not in links or child in parent_of:
raise ValueError("Source must be a valid tree (missing link/multiple parents)")
parent_of[child] = parent
for name in links:
seen = set()
while name in parent_of:
if name in seen:
raise ValueError("Cyclic URDF")
seen.add(name)
name = parent_of[name]
return link_nodes, joint_nodes, links, joints
def _foot_geometry(spec, links, nominal_links):
collisions = links[spec.foot_body].findall("collision")
if spec.foot_collision_index >= len(collisions):
raise ValueError("Missing selected foot collision")
collision = collisions[spec.foot_collision_index]
geometry = collision.find("geometry")
if geometry is None:
raise ValueError("Missing foot collision geometry")
sphere, box = geometry.find("sphere"), geometry.find("box")
if sphere is not None:
shape = {"type": "sphere", "radius": float(sphere.get("radius"))}
extent = shape["radius"]
elif box is not None:
shape = {"type": "box", "size": vector(box.get("size"))}
extent = math.sqrt(sum(v * v for v in shape["size"])) / 2
else:
raise ValueError("v1 foot gap requires a sphere or box collision (no mesh approximation)")
if not math.isfinite(extent) or extent <= 0 or (box is not None and min(shape["size"]) <= 0):
raise ValueError("Invalid foot collision dimensions")
foot = {
"body": spec.foot_body,
"collision_index": spec.foot_collision_index,
**shape,
**origin(collision),
"nominal_position": [nominal_links[spec.foot_body][i][3] for i in range(3)],
}
return foot, extent
def _resources(robot, spec, root, source):
resources = {}
for item in [*robot.findall(".//mesh"), *robot.findall(".//texture")]:
uri = item.get("filename")
prefix = f"package://{spec.package_name}/"
if uri.startswith("package://"):
if not uri.startswith(prefix):
raise ValueError(f"Undeclared resource package: {uri}")
src = root / uri[len(prefix) :]
elif "://" in uri:
raise ValueError(f"Unsupported resource URI: {uri}")
else:
src = source.parent / uri
src = _inside(src, root)
relative = src.relative_to(root)
resources[relative.as_posix()] = src
item.set("filename", relative.as_posix())
if src.suffix.lower() == ".dae":
for image in ET.parse(src).getroot().findall(".//{*}image/{*}init_from"):
image_src = _inside(src.parent / image.text.strip(), root)
resources[image_src.relative_to(root).as_posix()] = image_src
if src.suffix.lower() == ".obj" and "mtllib " in src.read_text(errors="replace"):
raise ValueError("Textured OBJ dependencies not yet supported; use DAE/STL")
return resources
def _output_directory(output, source_root, resources, protected):
output = Path(output).resolve()
if source_root.is_relative_to(output):
raise ValueError("Output must not overwrite the source root or its ancestors")
reserved = {"leg.urdf", "manifest.json", "conversion.json", "LICENSE"}
if reserved & resources.keys():
raise ValueError("Resource collides with a reserved bundle file")
for relative in (*resources, *reserved):
target = (output / relative).resolve()
if not target.is_relative_to(output):
raise ValueError(f"Output symlink escapes bundle: {relative}")
if target in protected:
raise ValueError(f"Output would overwrite source file: {target}")
return output
def _joint_limits(item, control, soft_factor):
name = item.get("name")
limit = item.find("limit")
keys = ("lower", "upper", "effort", "velocity")
if limit is None or any(limit.get(key) is None for key in keys):
raise ValueError(f"Missing bounded joint limits: {name}")
limits = {key: float(limit.get(key)) for key in keys}
if (
not all(math.isfinite(v) for v in limits.values())
or limits["lower"] >= limits["upper"]
or min(limits["effort"], limits["velocity"]) <= 0
):
raise ValueError(f"Invalid joint limits: {name}")
mid = (limits["lower"] + limits["upper"]) / 2
soft = [mid + soft_factor * (limits[key] - mid) for key in ("lower", "upper")]
if not soft[0] <= control.q_default <= soft[1]:
raise ValueError(f"Default outside soft joint range: {name}")
return limits, soft
def extract_leg(spec: LegSpec, output: str | Path) -> dict:
root = spec.source_root.resolve()
source = _inside(root / spec.urdf, root)
original = ET.parse(source).getroot()
link_nodes, joint_nodes, links, joints = _source_tree(original)
required = (spec.mount_joint, *spec.auxiliary_root_joints)
if any(name not in joints for name in required):
raise ValueError("Missing mount/auxiliary root joint")
source_parent = joints[spec.mount_joint].find("parent").get("link")
if any(joints[name].get("type") != "fixed" for name in spec.auxiliary_root_joints):
raise ValueError("Auxiliary roots must be fixed")
if any(joints[name].find("parent").get("link") != source_parent for name in required):
raise ValueError("Auxiliary roots must share the mount's parent frame")
keep = {joints[name].find("child").get("link") for name in required}
while True:
expanded = keep | {
item.find("child").get("link") for item in joint_nodes if item.find("parent").get("link") in keep
}
if expanded == keep:
break
keep = expanded
if spec.foot_body not in keep or source_parent in keep:
raise ValueError("Foot must belong to the isolated leg")
selected = [copy.deepcopy(item) for item in joint_nodes if item.find("child").get("link") in keep]
names = [item.name for item in spec.joints]
moving = [item.get("name") for item in selected if item.get("type") != "fixed"]
if set(moving) != set(names) or len(moving) != len(names):
raise ValueError(f"Uncontrolled/missing moving joints: {moving}, declared: {names}")
mount = origin(joints[spec.mount_joint])
mounting = pose(mount["xyz"], mount["rpy"])
records = {}
tree = []
for item in selected:
name = item.get("name")
if item.get("type") not in ("fixed", "revolute") or item.find("mimic") is not None:
raise ValueError(f"Only independent bounded revolute trees supported: {name}")
if item.find("parent").get("link") not in keep:
if name not in required:
raise ValueError(f"Unexpected external connection: {name}")
transformed = multiply(inverse(mounting), pose(**origin(item)))
xyz = [transformed[i][3] for i in range(3)]
pitch = math.asin(max(-1.0, min(1.0, -transformed[2][0])))
if abs(math.cos(pitch)) > 1e-8:
roll, yaw = (
math.atan2(transformed[2][1], transformed[2][2]),
math.atan2(transformed[1][0], transformed[0][0]),
)
else:
roll, yaw = math.atan2(-transformed[1][2], transformed[1][1]), 0.0
child = item.find("origin")
if child is None:
child = ET.SubElement(item, "origin")
child.set("xyz", " ".join(f"{v:.12g}" for v in xyz))
child.set("rpy", " ".join(f"{v:.12g}" for v in [roll, pitch, yaw]))
item.find("parent").set("link", ANCHOR)
axis = [1.0, 0.0, 0.0]
if item.get("type") == "revolute":
axis_node = item.find("axis")
axis = vector(axis_node.get("xyz") if axis_node is not None else "1 0 0")
norm = math.sqrt(sum(v * v for v in axis))
if norm < 1e-8:
raise ValueError("Zero joint axis")
axis = [v / norm for v in axis]
if axis_node is None:
axis_node = ET.SubElement(item, "axis")
axis_node.set("xyz", " ".join(f"{v:.12g}" for v in axis))
control = next(value for value in spec.joints if value.name == name)
limits, soft = _joint_limits(item, control, spec.soft_limit_factor)
records[name] = {
"name": name,
"child": item.find("child").get("link"),
"axis": axis,
"limits": limits,
"soft_limits": soft,
}
else:
item.set("dont_collapse", "true")
tree.append(
{
"name": name,
"type": item.get("type"),
"parent": item.find("parent").get("link"),
"child": item.find("child").get("link"),
"axis": axis,
**origin(item),
}
)
# Topological order is separate from declared actuator order.
ordered, available = [], {ANCHOR}
pending = list(tree)
while pending:
ready = [node for node in pending if node["parent"] in available]
if not ready:
raise ValueError("Disconnected/cyclic extracted leg")
ordered.extend(ready)
available.update(node["child"] for node in ready)
pending = [node for node in pending if node not in ready]
tree = ordered
link_records = {name: inertial(links[name]) for name in sorted(keep)}
nominal = {joint.name: joint.q_default for joint in spec.joints}
nominal_links, pivots = forward(tree, nominal)
for name, record in records.items():
frame = pivots[name]
record["nominal_position"] = [frame[i][3] for i in range(3)]
record["nominal_axis"] = [sum(frame[i][k] * record["axis"][k] for k in range(3)) for i in range(3)]
foot, extent = _foot_geometry(spec, links, nominal_links)
# Conservative path length; guarantees air-mode ground clearance for any joint pose.
by_child = {node["child"]: node for node in tree}
cursor = spec.foot_body
length = extent + math.sqrt(sum(v * v for v in foot["xyz"]))
while cursor != ANCHOR:
node = by_child[cursor]
length += math.sqrt(sum(v * v for v in node["xyz"]))
cursor = node["parent"]
mass = sum(value["mass"] for value in link_records.values() if value is not None)
if mass <= 0:
raise ValueError("Require known positive physical link mass")
robot = ET.Element("robot", name=spec.part_id + "_anchor")
for material in original.findall("material"):
robot.append(copy.deepcopy(material))
anchor = ET.SubElement(robot, "link", name=ANCHOR)
anchor.append(
ET.fromstring(
'<inertial><mass value="1"/>'
'<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/></inertial>'
)
)
for item in link_nodes:
if item.get("name") in keep:
robot.append(copy.deepcopy(item))
robot.extend(selected)
resources = _resources(robot, spec, root, source)
license_path = (root / spec.license_file).resolve()
if not license_path.is_file():
raise FileNotFoundError(license_path)
try:
commit = subprocess.check_output(
["git", "-C", str(root), "rev-parse", "HEAD"], text=True, stderr=subprocess.DEVNULL
).strip()
except (OSError, subprocess.CalledProcessError):
commit = None
output = _output_directory(output, root, resources, {source, license_path, *resources.values()})
output.mkdir(parents=True, exist_ok=True)
for relative, src in sorted(resources.items()):
dst = output / relative
dst.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(src, dst)
shutil.copy2(license_path, output / "LICENSE")
ET.indent(robot)
ET.ElementTree(robot).write(output / "leg.urdf", encoding="utf-8", xml_declaration=True)
bundle = {
"schema_version": 2,
"kind": "single_leg",
"spec": spec.to_dict(),
"source": {
"urdf": str(source),
"sha256": digest(source),
"commit": commit,
"license_id": spec.license_id,
"license_sha256": digest(license_path),
},
"derived_urdf_sha256": digest(output / "leg.urdf"),
"resources": {relative: digest(src) for relative, src in sorted(resources.items())},
"joint_order": names,
"joints": [records[name] for name in names],
"tree": tree,
"links": link_records,
"missing_inertial_links": [name for name, value in link_records.items() if value is None],
"anchor_body": ANCHOR,
"foot": foot,
"length_scale_m": length,
"known_mass_scale_kg": mass,
"mount": {
"source_parent": source_parent,
"source_parent_T_anchor": mount,
"anchor_frame": "Mount-joint zero frame, not the source parent/body frame",
},
"mass_semantics": "Sum of known source inertials, excludes synthetic anchor; not a foot force prediction",
"skill": {"weights": None, "latent": None, "trained": False, "reaction_wrench": None},
}
(output / "manifest.json").write_text(json.dumps(bundle, indent=2, sort_keys=True) + "\n")
return bundle
@@ -0,0 +1,11 @@
"""Generic, descriptor-driven single-leg reach training task."""
import gymnasium as gym
gym.register(
id="SkillFactory-LegSkill-v0",
entry_point=f"{__name__}.leg_env:LegSkillEnv",
disable_env_checker=True,
kwargs={"env_cfg_entry_point":f"{__name__}.leg_env_cfg:LegEnvCfg"},
)
@@ -0,0 +1,61 @@
"""Expose pre-reset terminal tokens for correct PPO timeout bootstrapping."""
import gymnasium as gym
import numpy as np
import torch
from isaaclab.envs import ManagerBasedRLEnv
from .mdp import state
class LegSkillEnv(ManagerBasedRLEnv):
def _configure_gym_env_spaces(self):
super()._configure_gym_env_spaces()
# The standard manager assumes float observations; masks are boolean.
policy = self.single_observation_space["policy"]
for name in ("joint_mask", "effector_mask"):
policy[name] = gym.spaces.Box(0, 1, shape=policy[name].shape, dtype=np.bool_)
self.observation_space = gym.vector.utils.batch_space(self.single_observation_space, self.num_envs)
def reset(self, seed=None, env_ids=None, options=None):
if env_ids is not None:
ids = torch.as_tensor(env_ids, device=self.device)
if ids.ndim != 1:
raise ValueError("env_ids must be one-dimensional")
if ids.numel() == 0:
if seed is not None:
raise ValueError("An empty reset cannot reseed the environment")
return self.observation_manager.compute(update_history=False), {}
if ids.dtype not in (torch.int32, torch.int64):
raise ValueError("env_ids must be integer indices")
if (ids < 0).any() or (ids >= self.num_envs).any() or len(ids.unique()) != len(ids):
raise ValueError("Invalid/duplicate env_ids")
env_ids = ids
return super().reset(seed=seed, env_ids=env_ids, options=options)
def step(self, action):
# Validate before ActionManager overwrites current/previous actions.
if (
not isinstance(action, torch.Tensor)
or action.shape != (self.num_envs, self.action_manager.total_action_dim)
or action.dtype != torch.float32
or not torch.isfinite(action).all()
):
raise ValueError("Expected finite float32 [B, native_DoF] actions")
self.extras["terminal_tokens"] = None
self.extras["terminal_ids"] = None
self._in_leg_step = True
try:
return super().step(action)
finally:
self._in_leg_step = False
def _reset_idx(self, env_ids):
if getattr(self, "_in_leg_step", False):
ids = torch.as_tensor(env_ids, device=self.device, dtype=torch.long)
self.extras["terminal_ids"] = ids.clone()
self.extras["terminal_tokens"] = {
name: value[ids].clone() for name, value in state(self).snapshot().items()
}
super()._reset_idx(env_ids)
@@ -0,0 +1,162 @@
"""One task config for any validated LegSpec bundle; Kit must already be launched."""
import os
import re
from pathlib import Path
from isaaclab_physx.physics import PhysxCfg
import isaaclab.sim as sim_utils
from isaaclab.actuators import IdealPDActuatorCfg
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.envs import mdp as base_mdp
from isaaclab.managers import EventTermCfg, ObservationGroupCfg, ObservationTermCfg, RewardTermCfg, TerminationTermCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg
from isaaclab.utils.configclass import configclass
from SkillFactory.legs.spec import load_bundle
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.actions import BoundedJointPositionActionCfg
from . import mdp
ROOT = Path(__file__).resolve().parents[6]
@configclass
class LegSceneCfg(InteractiveSceneCfg):
ground = AssetBaseCfg(
prim_path="/World/ground",
spawn=sim_utils.UsdFileCfg(usd_path=str(Path(__file__).resolve().parents[3] / "legs/resources/ground.usda")),
)
light = AssetBaseCfg(prim_path="/World/light", spawn=sim_utils.DomeLightCfg(intensity=2000.0))
robot: ArticulationCfg = None
foot_contact: ContactSensorCfg = None
@configclass
class ActionsCfg:
joint_position: BoundedJointPositionActionCfg = None
@configclass
class CommandsCfg:
reach = mdp.ReachCommandCfg()
@configclass
class ObservationsCfg:
@configclass
class PolicyCfg(ObservationGroupCfg):
joint_description = ObservationTermCfg(func=mdp.observation, params={"name": "joint_description"})
joint_observation = ObservationTermCfg(func=mdp.observation, params={"name": "joint_observation"})
joint_mask = ObservationTermCfg(func=mdp.observation, params={"name": "joint_mask"})
effector_description = ObservationTermCfg(func=mdp.observation, params={"name": "effector_description"})
effector_observation = ObservationTermCfg(func=mdp.observation, params={"name": "effector_observation"})
effector_mask = ObservationTermCfg(func=mdp.observation, params={"name": "effector_mask"})
context = ObservationTermCfg(func=mdp.observation, params={"name": "context"})
goal = ObservationTermCfg(func=mdp.observation, params={"name": "goal"})
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
policy = PolicyCfg()
@configclass
class EventsCfg:
reset_state = EventTermCfg(func=base_mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True})
@configclass
class RewardsCfg:
reach = RewardTermCfg(func=mdp.tracking_reward, weight=1.0)
effort = RewardTermCfg(func=mdp.normalized_effort, weight=-0.01)
action_rate = RewardTermCfg(func=mdp.action_rate, weight=-0.005)
@configclass
class TerminationsCfg:
time_out = TerminationTermCfg(func=base_mdp.time_out, time_out=True)
invalid = TerminationTermCfg(func=mdp.invalid_state)
@configclass
class LegEnvCfg(ManagerBasedRLEnvCfg):
bundle_dir: str = os.environ.get("SKILLFACTORY_LEG_BUNDLE", str(ROOT / "assets/generated/legs/go2_fl"))
max_joints: int = 8
anchor_height: float | None = None # default suspended, geometrically conservative
leg_bundle: dict | None = None
scene = LegSceneCfg(
num_envs=64, env_spacing=2.0, replicate_physics=True, filter_collisions=True, lazy_sensor_update=False
)
actions = ActionsCfg()
commands = CommandsCfg()
observations = ObservationsCfg()
events = EventsCfg()
rewards = RewardsCfg()
terminations = TerminationsCfg()
def __post_init__(self):
bundle = load_bundle(self.bundle_dir, require_usd=True)
self.leg_bundle = bundle
controls = bundle["spec"]["joints"]
names = [re.escape(value["name"]) for value in controls]
if type(self.max_joints) is not int or self.max_joints < len(names):
raise ValueError("max_joints smaller than asset DoF")
conversion = bundle["conversion"]
effort = {re.escape(value["name"]): value["limits"]["effort"] for value in bundle["joints"]}
velocity = {re.escape(value["name"]): value["limits"]["velocity"] for value in bundle["joints"]}
height = bundle["length_scale_m"] + 0.1 if self.anchor_height is None else self.anchor_height
self.scene.robot = ArticulationCfg(
prim_path="{ENV_REGEX_NS}/Robot",
spawn=sim_utils.UsdFileCfg(
usd_path=str(Path(self.bundle_dir).resolve() / conversion["usd_path"]),
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False, max_depenetration_velocity=1.0),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4
),
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, height),
joint_pos={re.escape(value["name"]): value["q_default"] for value in controls},
joint_vel={".*": 0.0},
),
soft_joint_pos_limit_factor=bundle["spec"]["soft_limit_factor"],
actuators={
"leg": IdealPDActuatorCfg(
joint_names_expr=names,
stiffness={re.escape(value["name"]): value["kp"] for value in controls},
damping={re.escape(value["name"]): value["kd"] for value in controls},
effort_limit=effort,
effort_limit_sim=effort,
velocity_limit=velocity,
velocity_limit_sim=velocity,
)
},
)
self.scene.foot_contact = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Robot" + conversion["body_paths"][bundle["foot"]["body"]],
update_period=0.0,
history_length=bundle["spec"]["decimation"],
filter_prim_paths_expr=["/World/ground/GroundPlane/CollisionPlane"],
track_air_time=True,
force_threshold=1.0,
)
self.actions.joint_position = BoundedJointPositionActionCfg(
asset_name="robot",
joint_names=names,
scale={re.escape(value["name"]): value["action_scale"] for value in controls},
preserve_order=True,
use_default_offset=True,
)
self.decimation = bundle["spec"]["decimation"]
self.sim.dt = bundle["spec"]["physics_dt"]
self.sim.render_interval = 20
self.sim.physics = PhysxCfg(enable_external_forces_every_iteration=True)
self.episode_length_s = 5.0
self.viewer.eye = (1.2, 1.2, 1.0)
self.viewer.lookat = (0.0, 0.0, height / 2)
@@ -0,0 +1,214 @@
"""Runtime adapter, reachable FK goals, and normalized single-leg rewards."""
import re
import torch
from isaaclab.managers import CommandTerm, CommandTermCfg
from isaaclab.utils.configclass import configclass
from isaaclab.utils.math import quat_apply, quat_apply_inverse, quat_mul
from SkillFactory.legs.tokens import LegKinematics, native_actions, pad_joint_observations, static_tokens
class LegState:
def __init__(self, env):
self.env = env
self.bundle = env.cfg.leg_bundle
self.robot = env.scene["robot"]
self.joint_ids, _ = self.robot.find_joints(
[re.escape(n) for n in self.bundle["joint_order"]], preserve_order=True
)
if len(self.joint_ids) != len(self.bundle["joint_order"]):
raise RuntimeError("Actuator mapping mismatch")
foot, _ = self.robot.find_bodies(re.escape(self.bundle["foot"]["body"]))
anchor, _ = self.robot.find_bodies(re.escape(self.bundle["anchor_body"]))
if len(foot) != 1 or len(anchor) != 1:
raise RuntimeError("Foot/anchor mapping must be unique")
self.foot, self.anchor = foot[0], anchor[0]
self.static = static_tokens(self.bundle, env.cfg.max_joints, env.device)
self.fk = LegKinematics(self.bundle, env.device)
self.offset = torch.tensor(self.bundle["foot"]["xyz"], device=env.device, dtype=torch.float32)
self.collision_quat = self.static["effector_description"][0, 0, 6:10]
self.half_size = (
torch.tensor(self.bundle["foot"].get("size", [0.0, 0.0, 0.0]), device=env.device, dtype=torch.float32) / 2
)
controls = self.bundle["spec"]["joints"]
self.nominal = torch.tensor([v["q_default"] for v in controls], device=env.device, dtype=torch.float32)
self.scale = torch.tensor([v["action_scale"] for v in controls], device=env.device, dtype=torch.float32)
self.soft = torch.tensor(
[v["soft_limits"] for v in self.bundle["joints"]], device=env.device, dtype=torch.float32
)
self.effort = torch.tensor(
[v["limits"]["effort"] for v in self.bundle["joints"]], device=env.device, dtype=torch.float32
)
self.length = self.bundle["length_scale_m"]
self.mass = self.bundle["known_mass_scale_kg"]
# Keep absolute scales and actuator/gravity authority: per-asset ratios
# alone would erase physically important differences between legs.
maximum_effort = max(j["limits"]["effort"] for j in self.bundle["joints"])
maximum_velocity = max(j["limits"]["velocity"] for j in self.bundle["joints"])
self.up = torch.tensor([0.0, 0.0, 1.0], device=env.device, dtype=torch.float32).expand(env.num_envs, -1)
self.control_dt = torch.full((env.num_envs, 1), env.step_dt, device=env.device, dtype=torch.float32)
self.physical_context = torch.tensor(
[
self.length,
self.mass,
maximum_effort / (self.mass * 9.81 * self.length),
maximum_velocity,
float(bool(self.bundle["missing_inertial_links"])),
],
device=env.device,
dtype=torch.float32,
)[None, :]
def q(self):
return self.robot.data.joint_pos.torch[:, self.joint_ids]
def foot_position(self):
data = self.robot.data
return quat_apply_inverse(
data.body_link_quat_w.torch[:, self.anchor],
data.body_link_pos_w.torch[:, self.foot] - data.body_link_pos_w.torch[:, self.anchor],
)
def gap(self):
data = self.robot.data
quat = data.body_link_quat_w.torch[:, self.foot]
center = data.body_link_pos_w.torch[:, self.foot] + quat_apply(quat, self.offset.expand(self.env.num_envs, -1))
if self.bundle["foot"]["type"] == "sphere":
extent = self.bundle["foot"]["radius"]
else:
collision = quat_mul(quat, self.collision_quat.expand(self.env.num_envs, -1))
extent = (quat_apply_inverse(collision, self.up).abs() * self.half_size).sum(dim=-1)
return center[:, 2] - extent # ground plane world z=0
def joint_observation(self):
return pad_joint_observations(
self.q(),
self.robot.data.joint_vel.torch[:, self.joint_ids],
self.env.action_manager.action,
self.bundle,
self.env.cfg.max_joints,
)
def effector_observation(self):
env, data = self.env, self.robot.data
anchor_quat = data.body_link_quat_w.torch[:, self.anchor]
velocity = quat_apply_inverse(
anchor_quat, data.body_link_lin_vel_w.torch[:, self.foot] - data.body_link_lin_vel_w.torch[:, self.anchor]
)
force_w = env.scene["foot_contact"].data.force_matrix_w.torch[:, 0].sum(dim=1)
force = quat_apply_inverse(anchor_quat, force_w)
return torch.cat(
(
self.foot_position() / self.length,
velocity / self.length,
force / (self.mass * 9.81),
(force.norm(dim=-1, keepdim=True) > 1.0).float(),
self.gap()[:, None] / self.length,
),
dim=-1,
)[:, None, :]
def context(self):
data = self.robot.data
gravity = quat_apply_inverse(data.body_link_quat_w.torch[:, self.anchor], -self.up)
return torch.cat(
(
gravity,
self.control_dt,
-data.body_link_pos_w.torch[:, self.anchor, 2, None] / self.length,
self.physical_context.expand(self.env.num_envs, -1),
),
dim=-1,
)
def goal(self):
return self.env.command_manager.get_command("reach") / self.length
def token(self, name):
if name in self.static:
value = self.static[name]
return value.expand(self.env.num_envs, *value.shape[1:])
if name not in ("joint_observation", "effector_observation", "context", "goal"):
raise KeyError(name)
return getattr(self, name)()
def snapshot(self):
"""Fresh tokens: no step/reset cache that could retain terminal or old goals."""
return {
name: self.token(name)
for name in (*self.static, "joint_observation", "effector_observation", "context", "goal")
}
def actions(self, actions):
return native_actions(actions, self.env.num_envs, len(self.joint_ids), self.env.cfg.max_joints, self.env.device)
def state(env):
if not hasattr(env, "_leg_state"):
env._leg_state = LegState(env)
return env._leg_state
def observation(env, name):
# ObservationManager calls each term separately. Compute only the requested
# component, rather than rebuilding the entire snapshot eight times.
return state(env).token(name)
class ReachCommand(CommandTerm):
def __init__(self, cfg, env):
super().__init__(cfg, env)
self._command = torch.zeros(self.num_envs, 3, device=self.device, dtype=torch.float32)
self.goal_q = torch.zeros(
self.num_envs, len(env.cfg.leg_bundle["joints"]), device=self.device, dtype=torch.float32
)
self.metrics["position_error_m"] = torch.zeros(self.num_envs, device=self.device, dtype=torch.float32)
@property
def command(self):
return self._command
def _resample_command(self, env_ids):
adapter = state(self._env)
actions = adapter.nominal.new_empty((len(env_ids), len(adapter.joint_ids))).uniform_(-0.75, 0.75)
q = (adapter.nominal + actions * adapter.scale).clamp(min=adapter.soft[:, 0], max=adapter.soft[:, 1])
self.goal_q[env_ids] = q
self._command[env_ids] = adapter.fk.foot_position(q)
def _update_metrics(self):
self.metrics["position_error_m"] += (state(self._env).foot_position() - self.command).norm(
dim=-1
) / self._env.max_episode_length
def _update_command(self):
pass
@configclass
class ReachCommandCfg(CommandTermCfg):
class_type = ReachCommand
resampling_time_range = (1.0, 2.0)
debug_vis = False
def tracking_reward(env):
adapter = state(env)
error = (adapter.foot_position() - env.command_manager.get_command("reach")) / adapter.length
return torch.exp(-error.square().sum(dim=-1) / 0.06**2)
def normalized_effort(env):
adapter = state(env)
return (adapter.robot.data.applied_torque.torch[:, adapter.joint_ids] / adapter.effort).square().mean(dim=-1)
def action_rate(env):
return (env.action_manager.action - env.action_manager.prev_action).square().mean(dim=-1)
def invalid_state(env):
adapter = state(env)
return ~(torch.isfinite(adapter.q()).all(dim=-1) & torch.isfinite(adapter.robot.data.joint_vel.torch).all(dim=-1))
@@ -0,0 +1,5 @@
"""Register the isolated Go2 single-leg benchmark independently of Cartpole."""
import gymnasium as gym
gym.register(id='SkillFactory-SingleLegAnchor-Go2-v0', entry_point='isaaclab.envs:ManagerBasedRLEnv',
disable_env_checker=True, kwargs={'env_cfg_entry_point':f'{__name__}.anchor_env_cfg:AnchorEnvCfg'})
@@ -0,0 +1,98 @@
"""Week-one deterministic Go2 FL anchor benchmark; no RL training reward design."""
from isaaclab_physx.physics import PhysxCfg
import isaaclab.sim as sim_utils
from isaaclab.assets import AssetBaseCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.managers import EventTermCfg, ObservationGroupCfg, ObservationTermCfg, RewardTermCfg, TerminationTermCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg
from isaaclab.utils.configclass import configclass
from SkillFactory.assets.go2_leg import FOOT_PATH, GO2_LEG_CFG, JOINT_NAMES
from . import mdp
@configclass
class AnchorSceneCfg(InteractiveSceneCfg):
ground = AssetBaseCfg(
prim_path="/World/ground",
spawn=sim_utils.GroundPlaneCfg(
physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.8, dynamic_friction=0.6, restitution=0.0)
),
)
robot = GO2_LEG_CFG.copy()
foot_contact = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Robot" + FOOT_PATH,
update_period=0.0,
history_length=5,
filter_prim_paths_expr=["/World/ground/GroundPlane/CollisionPlane"],
track_air_time=True,
force_threshold=1.0,
)
light = AssetBaseCfg(prim_path="/World/light", spawn=sim_utils.DomeLightCfg(intensity=2000.0))
@configclass
class ActionsCfg:
joint_position = mdp.BoundedJointPositionActionCfg(
asset_name="robot", joint_names=JOINT_NAMES, scale=0.25, preserve_order=True, use_default_offset=True
)
@configclass
class ObservationsCfg:
@configclass
class PolicyCfg(ObservationGroupCfg):
q_rel = ObservationTermCfg(func=mdp.joint_pos_rel)
dq = ObservationTermCfg(func=mdp.joint_vel)
foot_position = ObservationTermCfg(func=mdp.foot_pos_anchor)
foot_velocity = ObservationTermCfg(func=mdp.foot_vel_anchor)
normal_force = ObservationTermCfg(func=mdp.normal_force_anchor)
contact = ObservationTermCfg(func=mdp.contact)
last_action = ObservationTermCfg(func=mdp.previous_action)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = True
policy = PolicyCfg()
@configclass
class EventsCfg:
reset_state = EventTermCfg(func=mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True})
@configclass
class RewardsCfg:
# Placeholder required by RL environment; not a pretrained skill reward.
alive = RewardTermCfg(func=mdp.is_alive, weight=1.0)
@configclass
class TerminationsCfg:
time_out = TerminationTermCfg(func=mdp.time_out, time_out=True)
@configclass
class AnchorEnvCfg(ManagerBasedRLEnvCfg):
scene = AnchorSceneCfg(
num_envs=64, env_spacing=2.0, replicate_physics=True, filter_collisions=True, lazy_sensor_update=False
)
actions = ActionsCfg()
observations = ObservationsCfg()
events = EventsCfg()
rewards = RewardsCfg()
terminations = TerminationsCfg()
def __post_init__(self):
self.decimation = 5
self.episode_length_s = 10.0
self.sim.dt = 0.002
self.sim.render_interval = 20
self.sim.physics = PhysxCfg(enable_external_forces_every_iteration=True)
self.viewer.eye = (1.0, 1.0, 0.8)
self.viewer.lookat = (0.0, 0.0, 0.3)
@@ -0,0 +1,3 @@
from isaaclab.envs.mdp import * # noqa: F403
from .actions import BoundedJointPositionActionCfg
from .observations import * # noqa: F403
@@ -0,0 +1,40 @@
"""Thin normalized-input guard over Isaac Lab's position action term."""
import torch
from isaaclab.envs.mdp.actions import JointPositionAction, JointPositionActionCfg
from isaaclab.utils.configclass import configclass
class BoundedJointPositionAction(JointPositionAction):
"""Finite normalized actions; absolute position targets respect soft limits [rad]."""
def process_actions(self, actions: torch.Tensor):
if actions.shape != (self.num_envs, self.action_dim) or actions.dtype != torch.float32:
self._env.action_manager.action[:] = self.raw_actions
raise ValueError(f"Expected float32 [num_envs, {self.action_dim}] actions")
if not torch.isfinite(actions).all():
# ActionManager stores input before invoking terms; restore its
# current command so a rejected input cannot poison the next step.
self._env.action_manager.action[:] = self.raw_actions
raise ValueError("Non-finite action rejected before physical stepping")
bounded = actions.clamp(-1.0, 1.0)
super().process_actions(bounded)
limits = self._asset.data.soft_joint_pos_limits.torch[:, self._joint_ids]
self._processed_actions.clamp_(min=limits[:, :, 0], max=limits[:, :, 1])
# The standard manager buffers feed last-action observations. Store the
# effective normalized command, not out-of-range caller input.
self._env.action_manager.action[:] = bounded
def reset(self, env_ids=None):
ids = slice(None) if env_ids is None else env_ids
super().reset(ids)
if isinstance(self._offset, torch.Tensor):
self._processed_actions[ids] = self._offset[ids]
else:
self._processed_actions[ids] = self._offset
@configclass
class BoundedJointPositionActionCfg(JointPositionActionCfg):
class_type = BoundedJointPositionAction
@@ -0,0 +1,95 @@
"""Local observations and diagnostic views. SI units, xyzw quaternions."""
import torch
from isaaclab.utils.math import quat_apply, quat_apply_inverse
def _ids(env):
robot = env.scene["robot"]
if not hasattr(env, "_anchor_body_ids"):
foot, _ = robot.find_bodies("FL_foot")
anchor, _ = robot.find_bodies("anchor_base")
if len(foot) != 1 or len(anchor) != 1:
raise RuntimeError("Expected unique foot and anchor body mapping")
env._anchor_body_ids = foot[0], anchor[0]
return env._anchor_body_ids
def joint_pos_rel(env):
data = env.scene["robot"].data
return data.joint_pos.torch - data.default_joint_pos.torch
def joint_vel(env):
return env.scene["robot"].data.joint_vel.torch
def foot_pos_anchor(env):
foot, anchor = _ids(env)
data = env.scene["robot"].data
p = data.body_link_pos_w.torch
q = data.body_link_quat_w.torch[:, anchor]
return quat_apply_inverse(q, p[:, foot] - p[:, anchor])
def foot_vel_anchor(env):
foot, anchor = _ids(env)
data = env.scene["robot"].data
# Anchor is fixed, but subtract its velocity for explicit frame semantics.
v = data.body_link_lin_vel_w.torch
return quat_apply_inverse(data.body_link_quat_w.torch[:, anchor], v[:, foot] - v[:, anchor])
def normal_force_w(env):
return env.scene["foot_contact"].data.force_matrix_w.torch[:, 0].sum(dim=1)
def normal_force_anchor(env):
_, anchor = _ids(env)
return quat_apply_inverse(env.scene["robot"].data.body_link_quat_w.torch[:, anchor], normal_force_w(env))
def contact(env):
return (normal_force_w(env).norm(dim=-1, keepdim=True) > 1.0).to(torch.float32)
def previous_action(env):
return env.action_manager.prev_action
def foot_gap(env):
"""Sphere bottom height relative to z=0 ground [m], not foot-frame height."""
foot, _ = _ids(env)
data = env.scene["robot"].data
offset = torch.tensor([-0.002, 0.0, 0.0], device=env.device).expand(env.num_envs, -1)
center = data.body_link_pos_w.torch[:, foot] + quat_apply(data.body_link_quat_w.torch[:, foot], offset)
return center[:, 2] - 0.022
def diagnostics(env):
"""Diagnostic tensor views; clone if retaining values across stepping/reset."""
robot = env.scene["robot"]
data = robot.data
foot, anchor = _ids(env)
return {
"q": data.joint_pos.torch,
"dq": data.joint_vel.torch,
"q_target": data.joint_pos_target.torch,
"computed_torque": data.computed_torque.torch,
"applied_torque": data.applied_torque.torch,
"foot_pos_w": data.body_link_pos_w.torch[:, foot],
"foot_vel_w": data.body_link_lin_vel_w.torch[:, foot],
"anchor_pose_w": data.body_link_pose_w.torch[:, anchor],
"foot_pos_anchor": foot_pos_anchor(env),
"foot_vel_anchor": foot_vel_anchor(env),
"normal_force_w": normal_force_w(env),
"gap": foot_gap(env),
"force_history_w": env.scene["foot_contact"].data.net_forces_w_history.torch,
"episode_step": env.episode_length_buf,
"env_ids": torch.arange(env.num_envs, device=env.device),
"joint_mask": torch.ones((env.num_envs, 3), dtype=torch.bool, device=env.device),
"sim_time_s": env._sim_step_counter * env.physics_dt,
"reaction_wrench": None,
"reaction_wrench_valid": False,
}
@@ -38,4 +38,4 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
lam=0.95,
desired_kl=0.01,
max_grad_norm=1.0,
)
)
@@ -177,4 +177,4 @@ class SkillfactoryEnvCfg(ManagerBasedRLEnvCfg):
self.viewer.eye = (8.0, 0.0, 5.0)
# simulation settings
self.sim.dt = 1 / 120
self.sim.render_interval = self.decimation
self.sim.render_interval = self.decimation
@@ -44,4 +44,4 @@ class ExampleExtension(omni.ext.IExt):
omni.ui.Button("Reset", clicked_fn=on_reset)
def on_shutdown(self):
print("[SkillFactory] shutdown")
print("[SkillFactory] shutdown")
+5 -5
View File
@@ -1,18 +1,18 @@
[package]
# Semantic Versioning is used: https://semver.org/
version = "0.1.0"
version = "0.0.1"
# Description
category = "isaaclab"
readme = "README.md"
title = "Extension Template"
title = "SkillFactory"
author = "Isaac Lab Project Developers"
maintainer = "Isaac Lab Project Developers"
description="Extension Template for Isaac Lab"
repository = "https://github.com/isaac-sim/IsaacLab.git"
keywords = ["extension", "template", "isaaclab"]
description="Single-leg asset import, fixed-anchor simulation, and shared-policy training for Isaac Lab"
repository = "https://gitea.robotquan.com/chenlin/SkillFactory"
keywords = ["isaaclab", "robotics", "single-leg", "shared-policy"]
[dependencies]
"isaaclab" = {}
+9 -5
View File
@@ -1,10 +1,14 @@
Changelog
---------
0.1.0 (2026-10-08)
~~~~~~~~~~~~~~~~~~
0.0.1
~~~~~
Added
^^^^^
* Initial SkillFactory release based on the Isaac Lab project template.
* Verified Go2 fixed-anchor benchmark with contact sensing and indexed reset.
* Declarative Go2/Go1 leg import, variable-DoF tokens and shared-policy PPO smoke runner.
* Mask, kinematics, reset, bootstrap, checkpoint and failure-report regression tests.
* Consolidated run commands and interface/acceptance documentation.
* Created an initial template for building an extension or project based on Isaac Lab
Limitations: no converged or frozen Skill, learned modular latent, held-out morphology
validation, whole-body transfer or production-throughput sampler.
+4 -3
View File
@@ -8,7 +8,7 @@
import os
import toml
from setuptools import setup
from setuptools import find_packages, setup
# Obtain the extension data from the extension.toml file
EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__))
@@ -24,7 +24,7 @@ INSTALL_REQUIRES = [
# Installation operation
setup(
name="SkillFactory",
packages=["SkillFactory"],
packages=find_packages(),
author=EXTENSION_TOML_DATA["package"]["author"],
maintainer=EXTENSION_TOML_DATA["package"]["maintainer"],
url=EXTENSION_TOML_DATA["package"]["repository"],
@@ -34,6 +34,7 @@ setup(
install_requires=INSTALL_REQUIRES,
license="Apache-2.0",
include_package_data=True,
package_data={"SkillFactory.legs": ["resources/*.usda"]},
python_requires=">=3.12",
classifiers=[
"Natural Language :: English",
@@ -41,4 +42,4 @@ setup(
"Isaac Sim :: 6.0.0",
],
zip_safe=False,
)
)
+87
View File
@@ -0,0 +1,87 @@
"""Contract tests; opt into fresh Kit integration runs with ANCHOR_RUNTIME_TESTS=1."""
import importlib.util
import json
import os
import runpy
import subprocess
import sys
from pathlib import Path
import pytest
ROOT = Path(__file__).parents[1]
def test_metadata_schema(tmp_path):
spec = importlib.util.spec_from_file_location("extract_schema", ROOT / "scripts/assets/extract_go2_leg.py")
extractor = importlib.util.module_from_spec(spec)
spec.loader.exec_module(extractor)
manifest = extractor.extract(extractor.DEFAULT_SOURCE, tmp_path / "asset")
assert manifest["schema_version"] == 1
assert manifest["interface"]["policy_shape"] == ["B", 19]
assert manifest["interface"]["action_shape"] == ["B", 3]
assert manifest["interface"]["quaternion_order"] == "xyzw"
assert manifest["control"]["physics_dt"] == 0.002
assert manifest["control"]["decimation"] == 5
assert [j["limits"]["effort"] for j in manifest["joints"]] == [23.7, 23.7, 45.43]
assert [j["limits"]["velocity"] for j in manifest["joints"]] == [30.1, 30.1, 15.7]
assert all(len(v["origin"]["xyz"]) == 3 for v in manifest["links"].values() if v is not None)
def test_task_registration_preserves_template():
import gymnasium as gym
import SkillFactory.tasks # noqa: F401
ids = ["Template-Skillfactory-v0", "SkillFactory-SingleLegAnchor-Go2-v0"]
for task in ids:
spec = gym.spec(task)
assert spec.entry_point == "isaaclab.envs:ManagerBasedRLEnv"
assert spec.kwargs["env_cfg_entry_point"].startswith("SkillFactory.")
generic = gym.spec("SkillFactory-LegSkill-v0")
assert generic.entry_point == "SkillFactory.tasks.manager_based.leg_skill.leg_env:LegSkillEnv"
def test_packaging_discovers_task_subpackages(monkeypatch):
import setuptools
captured = {}
monkeypatch.setattr(setuptools, "setup", lambda **kwargs: captured.update(kwargs))
monkeypatch.chdir(ROOT / "source/SkillFactory")
runpy.run_path(str(ROOT / "source/SkillFactory/setup.py"))
prefix = "SkillFactory.tasks.manager_based.single_leg_anchor"
assert prefix in captured["packages"]
assert prefix + ".mdp" in captured["packages"]
assert "SkillFactory.assets" in captured["packages"]
assert "SkillFactory.legs" in captured["packages"]
assert "SkillFactory.tasks.manager_based.leg_skill" in captured["packages"]
assert captured["package_data"]["SkillFactory.legs"] == ["resources/*.usda"]
assert (ROOT / "source/SkillFactory/SkillFactory/legs/resources/ground.usda").is_file()
@pytest.mark.skipif(os.environ.get("ANCHOR_RUNTIME_TESTS") != "1", reason="Opt-in Kit/CUDA integration")
@pytest.mark.parametrize("num_envs", [1, 16, 64])
def test_cuda_contract(num_envs, tmp_path):
env = dict(os.environ)
env["PYTHONPATH"] = str(ROOT / "source/SkillFactory") + os.pathsep + env.get("PYTHONPATH", "")
with (tmp_path / "runtime.log").open("w") as log:
result = subprocess.run(
[
sys.executable,
str(ROOT / "scripts/verify_anchor.py"),
"--suite",
"contract",
"--num_envs",
str(num_envs),
"--output",
str(tmp_path / "report"),
"--headless",
],
cwd=ROOT,
env=env,
stdout=log,
stderr=subprocess.STDOUT,
)
assert result.returncode == 0, (tmp_path / "runtime.log").read_text()[-4000:]
report = json.loads((tmp_path / "report/result.json").read_text())
assert report["passed"] and report["contract"]["policy_shape"] == [num_envs, 19]
+71
View File
@@ -0,0 +1,71 @@
"""Pure XML tests: no Kit, Isaac Lab, network or GPU required."""
import importlib.util
import shutil
import xml.etree.ElementTree as ET
from pathlib import Path
import pytest
SPEC = importlib.util.spec_from_file_location(
"extract_leg", Path(__file__).parents[1] / "scripts/assets/extract_go2_leg.py"
)
extractor = importlib.util.module_from_spec(SPEC)
SPEC.loader.exec_module(extractor)
def test_repeatable_and_real_subtree(tmp_path):
a, b = tmp_path / "a", tmp_path / "b"
manifest = extractor.extract(extractor.DEFAULT_SOURCE, a)
extractor.extract(extractor.DEFAULT_SOURCE, b)
assert (a / "go2_fl.urdf").read_bytes() == (b / "go2_fl.urdf").read_bytes()
assert (a / "manifest.json").read_bytes() == (b / "manifest.json").read_bytes()
robot = ET.parse(a / "go2_fl.urdf").getroot()
assert manifest["joint_order"] == list(extractor.JOINTS)
assert len([j for j in robot.findall("joint") if j.get("type") == "revolute"]) == 3
assert {link.get("name") for link in robot.findall("link")} == {
"anchor_base",
"FL_hip",
"FL_thigh",
"FL_calf",
"FL_foot",
"FL_hip_rotor",
"FL_thigh_rotor",
"FL_calf_rotor",
"FL_calflower",
"FL_calflower1",
}
assert robot.find("joint[@name='FL_foot_joint']").get("dont_collapse") == "true"
assert robot.find("joint[@name='FL_hip_joint']/origin").get("xyz") == "0 0 0"
assert robot.find("link[@name='anchor_base']/collision") is None
for mesh in robot.findall(".//mesh"):
assert (a / mesh.get("filename")).is_file()
assert manifest["skill"]["valid"] is False
assert (a / "LICENSE").is_file()
assert manifest["missing_inertial_fixed_links"] == ["FL_calflower", "FL_calflower1"]
center = extractor.foot_center(robot)
assert abs(center[2] + manifest["anchor"]["default_height"] - 0.022 - 0.01) < 1e-12
original = ET.parse(extractor.DEFAULT_SOURCE / "urdf/go2_description.urdf").getroot()
old = extractor.foot_center(original)
assert all(
abs(new + shift - old_value) < 1e-10
for new, shift, old_value in zip(center, manifest["anchor"]["original_mount_translation"], old)
)
def test_missing_reference_fails(tmp_path):
source = tmp_path / "unitree/robots/go2_description"
shutil.copytree(extractor.DEFAULT_SOURCE, source)
(source / "dae/hip.dae").unlink()
with pytest.raises(FileNotFoundError):
extractor.extract(source, tmp_path / "out")
def test_invalid_inertia_fails(tmp_path):
source = tmp_path / "unitree/robots/go2_description"
shutil.copytree(extractor.DEFAULT_SOURCE, source)
tree = ET.parse(source / "urdf/go2_description.urdf")
tree.find("link[@name='FL_thigh']/inertial/inertia").set("ixy", "100")
tree.write(source / "urdf/go2_description.urdf")
with pytest.raises(ValueError, match="Invalid inertia"):
extractor.extract(source, tmp_path / "out")
+214
View File
@@ -0,0 +1,214 @@
"""Declarative extraction tests, including non-Go2 names and arbitrary axes."""
import dataclasses
import json
import math
import xml.etree.ElementTree as ET
from pathlib import Path
import pytest
from SkillFactory.legs import JointSpec, LegSpec
from SkillFactory.legs.spec import load_bundle
from SkillFactory.legs.urdf import extract_leg, forward, multiply, pose
ROOT = Path(__file__).parents[1]
def fixture_spec(tmp_path, n=2, box=False):
robot = ET.Element("robot", name="fixture")
for name in ["mount", *(f"segment{i}" for i in range(n)), "toe"]:
link = ET.SubElement(robot, "link", name=name)
link.append(
ET.fromstring('<inertial><mass value="0.2"/><inertia ixx="0.001" iyy="0.001" izz="0.001"/></inertial>')
)
if name == "toe":
link.append(
ET.fromstring(
'<collision><origin xyz="0.01 0 0" rpy="0 0.2 0"/><geometry>'
+ ('<box size="0.03 0.04 0.05"/>' if box else '<sphere radius="0.02"/>')
+ "</geometry></collision>"
)
)
for index in range(n):
joint = ET.SubElement(robot, "joint", name=f"pivot{index}", type="revolute")
ET.SubElement(joint, "parent", link="mount" if index == 0 else f"segment{index - 1}")
ET.SubElement(joint, "child", link=f"segment{index}")
ET.SubElement(
joint,
"origin",
xyz="0.2 0.1 -0.3" if index == 0 else "0.1 0 -0.15",
rpy="0.1 0.2 0.3" if index == 0 else "0 0 0",
)
ET.SubElement(joint, "axis", xyz="0.2 0.7 -0.3")
ET.SubElement(joint, "limit", lower="-1", upper="1", effort="10", velocity="5")
joint = ET.SubElement(robot, "joint", name="toe_mount", type="fixed")
ET.SubElement(joint, "parent", link=f"segment{n - 1}")
ET.SubElement(joint, "child", link="toe")
ET.SubElement(joint, "origin", xyz="0 0 -0.1", rpy="0 0 0")
ET.ElementTree(robot).write(tmp_path / "robot.urdf")
(tmp_path / "LICENSE").write_text("Fixture license\n")
return LegSpec(
1,
"fixture",
tmp_path,
"robot.urdf",
"fixture",
"LICENSE",
"test",
"pivot0",
(),
tuple(JointSpec(f"pivot{i}", 0.0, 0.1, 20.0, 0.5) for i in range(n)),
"toe",
)
@pytest.mark.parametrize("preset", ["go2_fl", "go1_fl"])
def test_real_legs_deterministic_and_complete(tmp_path, preset):
spec = LegSpec.load(ROOT / f"configs/legs/{preset}.json")
first = extract_leg(spec, tmp_path / "one")
second = extract_leg(spec, tmp_path / "two")
assert first == second
assert (tmp_path / "one/leg.urdf").read_bytes() == (tmp_path / "two/leg.urdf").read_bytes()
assert first["joint_order"] == [item.name for item in spec.joints]
assert spec.foot_body in first["links"] and "FL_hip_rotor" in first["links"]
assert all(name.startswith("FL_") for name in first["links"])
assert first["skill"]["trained"] is False
assert load_bundle(tmp_path / "one")["source"]["sha256"] == first["source"]["sha256"]
@pytest.mark.parametrize("n", [1, 2, 4])
def test_arbitrary_names_axes_and_mount_frame(tmp_path, n):
spec = fixture_spec(tmp_path, n)
bundle = extract_leg(spec, tmp_path / "out")
q = {joint.name: 0.3 for joint in spec.joints}
links, _ = forward(bundle["tree"], q)
expected = pose([0.2, 0.1, -0.3], [0.1, 0.2, 0.3])
original = ET.parse(tmp_path / "robot.urdf").getroot()
from SkillFactory.legs.urdf import origin, rotation, vector
for joint in original.findall("joint"):
expected = multiply(expected, pose(**origin(joint))) if joint.get("name") != "pivot0" else expected
if joint.get("type") == "revolute":
axis = vector(joint.find("axis").get("xyz"))
norm = math.sqrt(sum(v * v for v in axis))
expected = multiply(expected, rotation([v / norm for v in axis], 0.3))
actual = multiply(pose([0.2, 0.1, -0.3], [0.1, 0.2, 0.3]), links["toe"])
assert max(abs(a - b) for row_a, row_b in zip(actual, expected) for a, b in zip(row_a, row_b)) < 1e-10
assert len(bundle["joints"]) == n
def test_box_foot_and_declared_actuator_order(tmp_path):
spec = fixture_spec(tmp_path, 2, box=True)
spec = dataclasses.replace(spec, joints=tuple(reversed(spec.joints)))
bundle = extract_leg(spec, tmp_path / "out")
assert bundle["foot"]["type"] == "box"
assert bundle["joint_order"] == ["pivot1", "pivot0"]
assert bundle["tree"][0]["name"] == "pivot0"
def test_invalid_uncontrolled_and_stale_inputs(tmp_path):
spec = fixture_spec(tmp_path)
with pytest.raises(ValueError, match="moving joints"):
extract_leg(dataclasses.replace(spec, joints=spec.joints[:1]), tmp_path / "bad")
with pytest.raises(ValueError):
JointSpec("a", 0.0, 0.0, 20.0, 0.5)
with pytest.raises(ValueError):
dataclasses.replace(spec, decimation=0)
extract_leg(spec, tmp_path / "out")
with (tmp_path / "out/leg.urdf").open("a") as stream:
stream.write("<!-- tampered -->")
with pytest.raises(ValueError, match="hash mismatch"):
load_bundle(tmp_path / "out")
def test_missing_license_and_unsafe_resource(tmp_path):
spec = fixture_spec(tmp_path)
with pytest.raises(FileNotFoundError):
extract_leg(dataclasses.replace(spec, license_file="missing"), tmp_path / "bad")
robot = ET.parse(tmp_path / "robot.urdf")
visual = ET.SubElement(robot.getroot().find("link[@name='toe']"), "visual")
ET.SubElement(ET.SubElement(visual, "geometry"), "mesh", filename="../../outside.stl")
robot.write(tmp_path / "robot.urdf")
with pytest.raises(ValueError, match="escapes"):
extract_leg(spec, tmp_path / "bad")
def test_output_cannot_overwrite_sources_or_escape_through_symlink(tmp_path):
spec = fixture_spec(tmp_path)
before = (tmp_path / "robot.urdf").read_bytes()
with pytest.raises(ValueError, match="source root"):
extract_leg(spec, tmp_path)
assert (tmp_path / "robot.urdf").read_bytes() == before
output = tmp_path / "out"
output.mkdir()
(output / "leg.urdf").symlink_to(tmp_path / "robot.urdf")
with pytest.raises(ValueError, match="symlink"):
extract_leg(spec, output)
assert (tmp_path / "robot.urdf").read_bytes() == before
def test_nested_output_does_not_overwrite_an_existing_source_leg(tmp_path):
source = tmp_path / "nested"
source.mkdir()
spec = fixture_spec(source)
(source / "robot.urdf").rename(source / "leg.urdf")
spec = dataclasses.replace(spec, source_root=tmp_path, urdf="nested/leg.urdf", license_file="nested/LICENSE")
before = (source / "leg.urdf").read_bytes()
with pytest.raises(ValueError, match="overwrite source file"):
extract_leg(spec, source)
assert (source / "leg.urdf").read_bytes() == before
@pytest.mark.parametrize("change", ["missing_parent", "missing_limit", "missing_geometry"])
def test_malformed_urdf_fails_with_actionable_error(tmp_path, change):
spec = fixture_spec(tmp_path)
tree = ET.parse(tmp_path / "robot.urdf")
root = tree.getroot()
if change == "missing_geometry":
item = root.find("link[@name='toe']/collision")
item.remove(item.find("geometry"))
else:
item = root.find("joint[@name='pivot0']")
item.remove(item.find("parent" if change == "missing_parent" else "limit"))
tree.write(tmp_path / "robot.urdf")
with pytest.raises(ValueError, match="Missing"):
extract_leg(spec, tmp_path / "out")
@pytest.mark.parametrize("invalid", ["scale", "mapping", "layer", "support"])
def test_stale_or_invalid_conversion_rejected_before_kit(tmp_path, invalid):
from SkillFactory.legs.urdf import digest
bundle = extract_leg(fixture_spec(tmp_path), tmp_path / "out")
directory = tmp_path / "out"
(directory / "leg.usda").write_text("#usda 1.0\\n")
conversion = {
"usd_path": "leg.usda",
"manifest_sha256": digest(directory / "manifest.json"),
"used_layers": ["leg.usda"],
"fixed_to_world": True,
"body_paths": {name: "/Geometry/" + name for name in (*bundle["links"], bundle["anchor_body"])},
"joint_paths": {name: "/Joints/" + name for name in bundle["joint_order"]},
}
if invalid == "scale":
bundle["length_scale_m"] = 0
(directory / "manifest.json").write_text(json.dumps(bundle))
conversion["manifest_sha256"] = digest(directory / "manifest.json")
elif invalid == "mapping":
conversion["body_paths"]["toe"] = "/Geometry/" + bundle["anchor_body"]
elif invalid == "layer":
conversion["used_layers"] = []
else:
conversion["fixed_to_world"] = False
(directory / "conversion.json").write_text(json.dumps(conversion))
with pytest.raises(ValueError):
load_bundle(directory, require_usd=True)
def test_unknown_spec_fields_rejected(tmp_path):
data = LegSpec.load(ROOT / "configs/legs/go2_fl.json").to_dict()
data["typo"] = True
path = tmp_path / "spec.json"
path.write_text(json.dumps(data))
with pytest.raises(TypeError):
LegSpec.load(path)
+183
View File
@@ -0,0 +1,183 @@
"""Masking, variable DoF, equivalent token permutations, FK, and PPO semantics."""
import copy
import pytest
import torch
from SkillFactory.legs.policy import LegActorCritic, compute_gae, load_policy, ppo_update, save_policy
from SkillFactory.legs.tokens import FEATURE_SCHEMA, LegKinematics, pad_joint_observations, static_tokens
from SkillFactory.legs.urdf import extract_leg, forward
from test_leg_assets import fixture_spec
def batch(b=3, j=6, e=1):
result = {
name: torch.randn(b, j if name.startswith("joint") else e, len(FEATURE_SCHEMA[name]))
for name in ("joint_description", "joint_observation", "effector_description", "effector_observation")
}
result.update(
joint_mask=torch.tensor([[True, True, False, False, False, False]]).expand(b, -1).clone(),
effector_mask=torch.ones(b, e, dtype=torch.bool),
context=torch.randn(b, len(FEATURE_SCHEMA["context"])),
goal=torch.randn(b, 3),
)
return result
def test_padding_and_missing_feet_do_not_affect_active_policy():
torch.manual_seed(3)
model = LegActorCritic()
original = batch()
action, logp, value = model.act(original, deterministic=True)
changed = copy.deepcopy(original)
changed["joint_description"][:, 2:] = torch.nan
changed["joint_observation"][:, 2:] = torch.nan
other, other_log, other_value = model.act(changed, deterministic=True)
assert torch.equal(action, other) and torch.equal(logp, other_log) and torch.equal(value, other_value)
assert (action[:, 2:] == 0).all()
original["effector_mask"][:] = False
original["effector_description"][:] = torch.nan
original["effector_observation"][:] = torch.nan
assert all(torch.isfinite(t).all() for t in model.act(original))
def test_permutation_equivariance_and_no_inactive_gradients():
torch.manual_seed(42)
model = LegActorCritic()
original = batch()
original["joint_description"].requires_grad_()
original["joint_observation"].requires_grad_()
permutation = torch.tensor([3, 1, 5, 0, 4, 2])
shuffled = {k: v[:, permutation] if k.startswith("joint") else v for k, v in original.items()}
a, _, v = model.act(original, deterministic=True)
b, _, w = model.act(shuffled, deterministic=True)
torch.testing.assert_close(b, a[:, permutation], atol=1e-6, rtol=1e-6)
torch.testing.assert_close(v, w, atol=1e-6, rtol=1e-6)
(a.sum() + v.sum()).backward()
assert original["joint_description"].grad[:, 2:].abs().max() == 0
assert original["joint_observation"].grad[:, 2:].abs().max() == 0
assert original["joint_observation"].grad[:, :2].abs().sum() > 0
distribution, _ = model.distribution_value(original)
_, entropy, _ = model.evaluate(original, a)
torch.testing.assert_close(entropy, distribution.entropy()[:, :2].sum(dim=-1))
@pytest.mark.parametrize("n", [1, 2, 4])
def test_actual_fk_and_shared_network_across_declared_dof(tmp_path, n):
spec = fixture_spec(tmp_path, n)
bundle = extract_leg(spec, tmp_path / "asset")
static = static_tokens(bundle, 6)
q = torch.linspace(-0.2, 0.3, n, dtype=torch.float32)[None, :].expand(3, -1)
fk = LegKinematics(bundle)
actual = fk.foot_position(q)[0]
expected, _ = forward(bundle["tree"], dict(zip(bundle["joint_order"], q[0].tolist())))
torch.testing.assert_close(actual, torch.tensor([expected["toe"][i][3] for i in range(3)]), atol=1e-6, rtol=1e-6)
tokens = {k: v.expand(3, *v.shape[1:]) for k, v in static.items()}
tokens.update(
joint_observation=pad_joint_observations(q, q * 0, q * 0, bundle, 6),
effector_observation=torch.zeros(3, 1, 11),
context=torch.zeros(3, len(FEATURE_SCHEMA["context"])),
goal=torch.zeros(3, 3),
)
model = LegActorCritic()
action, _, _ = model.act(tokens)
assert action.shape == (3, 6) and (action[:, n:] == 0).all()
assert static["joint_mask"].sum() == n
with pytest.raises(ValueError):
static_tokens(bundle, n - 1)
def test_inertia_and_com_are_in_anchor_nominal_frame(tmp_path):
import xml.etree.ElementTree as ET
spec = fixture_spec(tmp_path, 1)
tree = ET.parse(tmp_path / "robot.urdf")
item = tree.getroot().find("link[@name='segment0']/inertial")
ET.SubElement(item, "origin", xyz="0.01 0 0", rpy="0 0 1.5707963267948966")
item.find("inertia").set("iyy", "0.002")
item.find("inertia").set("izz", "0.003")
tree.write(tmp_path / "robot.urdf")
bundle = extract_leg(spec, tmp_path / "asset")
description = static_tokens(bundle, 3)["joint_description"][0, 0]
scale = bundle["known_mass_scale_kg"] * bundle["length_scale_m"] ** 2
torch.testing.assert_close(
description[[15, 18, 20]] * scale, torch.tensor([0.002, 0.001, 0.003]), atol=1e-7, rtol=1e-6
)
torch.testing.assert_close(
description[21:24] * bundle["length_scale_m"], torch.tensor([0.01, 0.0, 0.0]), atol=1e-7, rtol=1e-6
)
def test_checkpoint_reload_is_exact(tmp_path):
model = LegActorCritic().eval()
observation = batch()
save_policy(model, tmp_path / "policy.pt", {"test": True})
loaded = load_policy(tmp_path / "policy.pt").eval()
assert all(torch.equal(a, b) for a, b in zip(model.act(observation, True), loaded.act(observation, True)))
def test_gae_bootstraps_timeout_not_reset_state():
rewards = torch.tensor([[1.0, 1.0], [1.0, 1.0]])
values = torch.zeros_like(rewards)
# terminal first env bootstrap=0, timeout second env terminal critic=10
next_values = torch.tensor([[0.0, 0.0], [0.0, 10.0]])
done = torch.tensor([[False, False], [True, True]])
advantage, returns = compute_gae(rewards, values, next_values, done, gamma=0.9, lam=1.0)
torch.testing.assert_close(returns, torch.tensor([[1.9, 10.0], [1.0, 10.0]]))
assert torch.equal(returns, advantage)
def test_ppo_mixed_masks_updates_actor_without_rollout_gradients():
torch.manual_seed(1)
model = LegActorCritic(hidden=16, latent=8)
observation = batch(b=4)
observation["joint_mask"][2:, 2] = True
action, logp, value = model.act(observation)
data = {
"observation": {key: item[None].repeat(3, *([1] * item.ndim)) for key, item in observation.items()},
"actions": action[None].expand(3, -1, -1),
"log_prob": logp[None].expand(3, -1),
"values": value[None].expand(3, -1),
"rewards": torch.randn(3, 4),
"next_values": value[None].expand(3, -1),
"done": torch.zeros(3, 4, dtype=torch.bool),
}
before = model.mean[-1].weight.detach().clone()
stats = ppo_update(model, torch.optim.Adam(model.parameters(), lr=1e-3), data, epochs=2, minibatch=5)
assert not torch.equal(before, model.mean[-1].weight)
assert all(torch.isfinite(torch.tensor(v)) for v in stats.values())
advantage, _ = compute_gae(data["rewards"], data["values"], data["next_values"], data["done"])
assert not advantage.requires_grad
with pytest.raises(ValueError, match="positive"):
ppo_update(model, torch.optim.Adam(model.parameters()), data, epochs=0)
data["log_prob"] = torch.full((3, 4), torch.nan)
frozen = {key: item.clone() for key, item in model.state_dict().items()}
with pytest.raises(ValueError, match="finite"):
ppo_update(model, torch.optim.Adam(model.parameters()), data)
assert all(torch.equal(model.state_dict()[key], item) for key, item in frozen.items())
@pytest.mark.parametrize("empty", [True, False])
def test_gae_rejects_empty_or_nonfinite_inputs(empty):
reward = torch.zeros(0 if empty else 1, 2)
if not empty:
reward[0, 0] = torch.nan
with pytest.raises(ValueError):
compute_gae(reward, reward, reward, torch.zeros_like(reward, dtype=torch.bool))
def test_float32_contract_ignores_global_default_dtype(tmp_path):
spec = fixture_spec(tmp_path, 2)
bundle = extract_leg(spec, tmp_path / "asset")
old = torch.get_default_dtype()
try:
torch.set_default_dtype(torch.float64)
static = static_tokens(bundle, 4)
q = torch.zeros(2, 2, dtype=torch.float32)
assert static["joint_description"].dtype == torch.float32
assert static["effector_description"].dtype == torch.float32
assert LegKinematics(bundle).foot_position(q).dtype == torch.float32
assert pad_joint_observations(q, q, q, bundle, 4).dtype == torch.float32
assert all(p.dtype == torch.float32 for p in LegActorCritic().parameters())
finally:
torch.set_default_dtype(old)
+55
View File
@@ -0,0 +1,55 @@
"""Reporting must never turn validation/shutdown failures into success."""
import importlib.util
import json
from pathlib import Path
from types import SimpleNamespace
import pytest
ROOT = Path(__file__).resolve().parents[1]
SPEC = importlib.util.spec_from_file_location("runtime_common", ROOT / "scripts/runtime_common.py")
runtime = importlib.util.module_from_spec(SPEC)
SPEC.loader.exec_module(runtime)
def test_atomic_report_rejects_nan_without_overwriting(tmp_path):
path = tmp_path / "result.json"
runtime.write_json(path, {"passed": False})
with pytest.raises(ValueError):
runtime.write_json(path, {"passed": True, "value": float("nan")})
assert json.loads(path.read_text()) == {"passed": False}
assert list(tmp_path.iterdir()) == [path]
@pytest.mark.parametrize("close_fails", [False, True])
def test_success_only_after_environment_close(tmp_path, close_fails):
events = []
def close():
events.append("env")
assert not (tmp_path / "result.json").exists()
if close_fails:
raise RuntimeError("shutdown failed")
def close_app(exit_code):
events.append("app")
report = json.loads((tmp_path / "result.json").read_text())
assert report["passed"] is (not close_fails)
assert exit_code == int(close_fails)
if close_fails:
assert report["milestone_complete"] is False
runtime.finish_run(
SimpleNamespace(close=close_app),
SimpleNamespace(close=close),
tmp_path / "result.json",
{"passed": True, "milestone_complete": True},
)
assert events == ["env", "app"]
def test_critical_log_scan_includes_buffer_overflow(tmp_path):
path = tmp_path / "worker.log"
path.write_text("[Warning] harmless\n[error] physics\ncontact buffer overflow\n[Fatal] stopped\n")
assert runtime.critical_log_lines(path) == ["[error] physics", "contact buffer overflow", "[Fatal] stopped"]