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:
+6
-2
@@ -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_*
|
||||
|
||||
@@ -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 学习和冻结调用;固定支座结果不证明整机迁移。
|
||||
|
||||
@@ -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"
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
@@ -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)。本项目是固定支座单腿工程改编,不是原论文全身模仿或零样本迁移复现。
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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
@@ -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.
Binary file not shown.
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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))
|
||||
Binary file not shown.
@@ -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'})
|
||||
+98
@@ -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
|
||||
+95
@@ -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,
|
||||
}
|
||||
+1
-1
@@ -38,4 +38,4 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
|
||||
lam=0.95,
|
||||
desired_kl=0.01,
|
||||
max_grad_norm=1.0,
|
||||
)
|
||||
)
|
||||
|
||||
+1
-1
@@ -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")
|
||||
|
||||
@@ -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" = {}
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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,
|
||||
)
|
||||
)
|
||||
|
||||
@@ -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]
|
||||
@@ -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")
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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"]
|
||||
Reference in New Issue
Block a user