diff --git a/.gitignore b/.gitignore index 60989ad..078d109 100644 --- a/.gitignore +++ b/.gitignore @@ -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_* diff --git a/README.md b/README.md index 9c4a6d9..d914100 100644 --- a/README.md +++ b/README.md @@ -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//train.py --task= - ``` - - - 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= - ``` - - 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= - ``` - -### 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": [ - "/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 -"/extscache/omni.anim.*" // Animation packages -"/extscache/omni.kit.*" // Kit UI tools -"/extscache/omni.graph.*" // Graph UI tools -"/extscache/omni.services.*" // Services tools -... -``` \ No newline at end of file +- [`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 学习和冻结调用;固定支座结果不证明整机迁移。 diff --git a/configs/legs/go1_fl.json b/configs/legs/go1_fl.json new file mode 100644 index 0000000..9a5466e --- /dev/null +++ b/configs/legs/go1_fl.json @@ -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" +} diff --git a/configs/legs/go2_fl.json b/configs/legs/go2_fl.json new file mode 100644 index 0000000..38f2ad2 --- /dev/null +++ b/configs/legs/go2_fl.json @@ -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" +} diff --git a/docs/leg_interface.md b/docs/leg_interface.md new file mode 100644 index 0000000..b112991 --- /dev/null +++ b/docs/leg_interface.md @@ -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)。本项目是固定支座单腿工程改编,不是原论文全身模仿或零样本迁移复现。 diff --git a/scripts/anchor_common.py b/scripts/anchor_common.py new file mode 100644 index 0000000..6e0c80b --- /dev/null +++ b/scripts/anchor_common.py @@ -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) diff --git a/scripts/anchor_smoke.py b/scripts/anchor_smoke.py new file mode 100644 index 0000000..24db119 --- /dev/null +++ b/scripts/anchor_smoke.py @@ -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) diff --git a/scripts/assets/convert_go2_leg.py b/scripts/assets/convert_go2_leg.py new file mode 100644 index 0000000..71112fb --- /dev/null +++ b/scripts/assets/convert_go2_leg.py @@ -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() diff --git a/scripts/assets/convert_leg.py b/scripts/assets/convert_leg.py new file mode 100644 index 0000000..4616516 --- /dev/null +++ b/scripts/assets/convert_leg.py @@ -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() diff --git a/scripts/assets/extract_go2_leg.py b/scripts/assets/extract_go2_leg.py new file mode 100644 index 0000000..dc6c83d --- /dev/null +++ b/scripts/assets/extract_go2_leg.py @@ -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( + '' + '' + ) + ) + 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() diff --git a/scripts/assets/extract_leg.py b/scripts/assets/extract_leg.py new file mode 100644 index 0000000..e6b6373 --- /dev/null +++ b/scripts/assets/extract_leg.py @@ -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() diff --git a/scripts/assets/preview_go2_leg.py b/scripts/assets/preview_go2_leg.py new file mode 100644 index 0000000..1c6a33a --- /dev/null +++ b/scripts/assets/preview_go2_leg.py @@ -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() diff --git a/scripts/leg_worker.py b/scripts/leg_worker.py new file mode 100644 index 0000000..dce2c7d --- /dev/null +++ b/scripts/leg_worker.py @@ -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() diff --git a/scripts/list_envs.py b/scripts/list_envs.py index 4589c3a..1621325 100644 --- a/scripts/list_envs.py +++ b/scripts/list_envs.py @@ -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) diff --git a/scripts/rsl_rl/__pycache__/cli_args.cpython-312.pyc b/scripts/rsl_rl/__pycache__/cli_args.cpython-312.pyc deleted file mode 100644 index b6e9cf1..0000000 Binary files a/scripts/rsl_rl/__pycache__/cli_args.cpython-312.pyc and /dev/null differ diff --git a/scripts/rsl_rl/__pycache__/train_rsl_rl.cpython-312.pyc b/scripts/rsl_rl/__pycache__/train_rsl_rl.cpython-312.pyc deleted file mode 100644 index e2e3052..0000000 Binary files a/scripts/rsl_rl/__pycache__/train_rsl_rl.cpython-312.pyc and /dev/null differ diff --git a/scripts/rsl_rl/play.py b/scripts/rsl_rl/play.py index 34d0374..a558597 100644 --- a/scripts/rsl_rl/play.py +++ b/scripts/rsl_rl/play.py @@ -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 diff --git a/scripts/rsl_rl/play_rsl_rl.py b/scripts/rsl_rl/play_rsl_rl.py index e1e69e8..9dc6a56 100644 --- a/scripts/rsl_rl/play_rsl_rl.py +++ b/scripts/rsl_rl/play_rsl_rl.py @@ -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 diff --git a/scripts/rsl_rl/train.py b/scripts/rsl_rl/train.py index 8a8cd18..3399b75 100644 --- a/scripts/rsl_rl/train.py +++ b/scripts/rsl_rl/train.py @@ -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 diff --git a/scripts/rsl_rl/train_rsl_rl.py b/scripts/rsl_rl/train_rsl_rl.py index 3b3d9d0..25b611d 100644 --- a/scripts/rsl_rl/train_rsl_rl.py +++ b/scripts/rsl_rl/train_rsl_rl.py @@ -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 diff --git a/scripts/runtime_common.py b/scripts/runtime_common.py new file mode 100644 index 0000000..6ad9a0e --- /dev/null +++ b/scripts/runtime_common.py @@ -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) diff --git a/scripts/train_leg_skill.py b/scripts/train_leg_skill.py new file mode 100644 index 0000000..a996c3e --- /dev/null +++ b/scripts/train_leg_skill.py @@ -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() diff --git a/scripts/verify_anchor.py b/scripts/verify_anchor.py new file mode 100644 index 0000000..0cf6674 --- /dev/null +++ b/scripts/verify_anchor.py @@ -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() diff --git a/scripts/zero_agent.py b/scripts/zero_agent.py index c6cc8e5..90b74aa 100644 --- a/scripts/zero_agent.py +++ b/scripts/zero_agent.py @@ -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 diff --git a/source/SkillFactory/SkillFactory/assets/__init__.py b/source/SkillFactory/SkillFactory/assets/__init__.py new file mode 100644 index 0000000..9fdbfd7 --- /dev/null +++ b/source/SkillFactory/SkillFactory/assets/__init__.py @@ -0,0 +1 @@ +"""Local, reproducibly generated simulation assets.""" diff --git a/source/SkillFactory/SkillFactory/assets/go2_leg.py b/source/SkillFactory/SkillFactory/assets/go2_leg.py new file mode 100644 index 0000000..54b930d --- /dev/null +++ b/source/SkillFactory/SkillFactory/assets/go2_leg.py @@ -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, + ) + }, +) diff --git a/source/SkillFactory/SkillFactory/legs/__init__.py b/source/SkillFactory/SkillFactory/legs/__init__.py new file mode 100644 index 0000000..8b0e0d6 --- /dev/null +++ b/source/SkillFactory/SkillFactory/legs/__init__.py @@ -0,0 +1,5 @@ +"""Single-leg asset and learning contracts; no Kit imports at module load.""" + +from .spec import JointSpec, LegSpec + +__all__ = ["JointSpec", "LegSpec"] diff --git a/source/SkillFactory/SkillFactory/legs/policy.py b/source/SkillFactory/SkillFactory/legs/policy.py new file mode 100644 index 0000000..5e07c88 --- /dev/null +++ b/source/SkillFactory/SkillFactory/legs/policy.py @@ -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)) diff --git a/source/SkillFactory/SkillFactory/legs/resources/ground.usda b/source/SkillFactory/SkillFactory/legs/resources/ground.usda new file mode 100644 index 0000000..2cb2e15 --- /dev/null +++ b/source/SkillFactory/SkillFactory/legs/resources/ground.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:26e7ef57327a36b59aacd0cc1ba0704ec4d86387450b4cab1f4020ae28bac70d +size 1151 diff --git a/source/SkillFactory/SkillFactory/legs/spec.py b/source/SkillFactory/SkillFactory/legs/spec.py new file mode 100644 index 0000000..ab44d5f --- /dev/null +++ b/source/SkillFactory/SkillFactory/legs/spec.py @@ -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 diff --git a/source/SkillFactory/SkillFactory/legs/tokens.py b/source/SkillFactory/SkillFactory/legs/tokens.py new file mode 100644 index 0000000..840da76 --- /dev/null +++ b/source/SkillFactory/SkillFactory/legs/tokens.py @@ -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 diff --git a/source/SkillFactory/SkillFactory/legs/urdf.py b/source/SkillFactory/SkillFactory/legs/urdf.py new file mode 100644 index 0000000..a3cc66e --- /dev/null +++ b/source/SkillFactory/SkillFactory/legs/urdf.py @@ -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( + '' + '' + ) + ) + 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 diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/__init__.py b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/__init__.py new file mode 100644 index 0000000..1a00519 --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/__init__.py @@ -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"}, +) diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/leg_env.py b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/leg_env.py new file mode 100644 index 0000000..aceaa7e --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/leg_env.py @@ -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) diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/leg_env_cfg.py b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/leg_env_cfg.py new file mode 100644 index 0000000..a439922 --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/leg_env_cfg.py @@ -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) diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/mdp.py b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/mdp.py new file mode 100644 index 0000000..22b3343 --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/leg_skill/mdp.py @@ -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)) diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/__init__.py b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/__init__.py new file mode 100644 index 0000000..cfd1bca --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/__init__.py @@ -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'}) diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/anchor_env_cfg.py b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/anchor_env_cfg.py new file mode 100644 index 0000000..9c527f4 --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/anchor_env_cfg.py @@ -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) diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/__init__.py b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/__init__.py new file mode 100644 index 0000000..ea09fc8 --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/__init__.py @@ -0,0 +1,3 @@ +from isaaclab.envs.mdp import * # noqa: F403 +from .actions import BoundedJointPositionActionCfg +from .observations import * # noqa: F403 diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/actions.py b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/actions.py new file mode 100644 index 0000000..2e32e65 --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/actions.py @@ -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 diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/observations.py b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/observations.py new file mode 100644 index 0000000..ef5c3c7 --- /dev/null +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/single_leg_anchor/mdp/observations.py @@ -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, + } diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/agents/rsl_rl_ppo_cfg.py b/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/agents/rsl_rl_ppo_cfg.py index 8b669a6..4e7d739 100644 --- a/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/agents/rsl_rl_ppo_cfg.py +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/agents/rsl_rl_ppo_cfg.py @@ -38,4 +38,4 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg): lam=0.95, desired_kl=0.01, max_grad_norm=1.0, - ) \ No newline at end of file + ) diff --git a/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/skillfactory_env_cfg.py b/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/skillfactory_env_cfg.py index 1a94663..3a262c0 100644 --- a/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/skillfactory_env_cfg.py +++ b/source/SkillFactory/SkillFactory/tasks/manager_based/skillfactory/skillfactory_env_cfg.py @@ -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 \ No newline at end of file + self.sim.render_interval = self.decimation diff --git a/source/SkillFactory/SkillFactory/ui_extension_example.py b/source/SkillFactory/SkillFactory/ui_extension_example.py index 01a51ed..2cf53c8 100644 --- a/source/SkillFactory/SkillFactory/ui_extension_example.py +++ b/source/SkillFactory/SkillFactory/ui_extension_example.py @@ -44,4 +44,4 @@ class ExampleExtension(omni.ext.IExt): omni.ui.Button("Reset", clicked_fn=on_reset) def on_shutdown(self): - print("[SkillFactory] shutdown") \ No newline at end of file + print("[SkillFactory] shutdown") diff --git a/source/SkillFactory/config/extension.toml b/source/SkillFactory/config/extension.toml index 589eba4..ad8c1c0 100644 --- a/source/SkillFactory/config/extension.toml +++ b/source/SkillFactory/config/extension.toml @@ -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" = {} diff --git a/source/SkillFactory/docs/CHANGELOG.rst b/source/SkillFactory/docs/CHANGELOG.rst index f738088..a6d3951 100644 --- a/source/SkillFactory/docs/CHANGELOG.rst +++ b/source/SkillFactory/docs/CHANGELOG.rst @@ -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 \ No newline at end of file +Limitations: no converged or frozen Skill, learned modular latent, held-out morphology +validation, whole-body transfer or production-throughput sampler. diff --git a/source/SkillFactory/setup.py b/source/SkillFactory/setup.py index f681f09..26bb271 100644 --- a/source/SkillFactory/setup.py +++ b/source/SkillFactory/setup.py @@ -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, -) \ No newline at end of file +) diff --git a/tests/test_anchor_contract.py b/tests/test_anchor_contract.py new file mode 100644 index 0000000..8591c03 --- /dev/null +++ b/tests/test_anchor_contract.py @@ -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] diff --git a/tests/test_go2_leg_extraction.py b/tests/test_go2_leg_extraction.py new file mode 100644 index 0000000..7dbe81a --- /dev/null +++ b/tests/test_go2_leg_extraction.py @@ -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") diff --git a/tests/test_leg_assets.py b/tests/test_leg_assets.py new file mode 100644 index 0000000..a95bb2b --- /dev/null +++ b/tests/test_leg_assets.py @@ -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('') + ) + if name == "toe": + link.append( + ET.fromstring( + '' + + ('' if box else '') + + "" + ) + ) + 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("") + 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) diff --git a/tests/test_leg_policy.py b/tests/test_leg_policy.py new file mode 100644 index 0000000..0eea9e9 --- /dev/null +++ b/tests/test_leg_policy.py @@ -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) diff --git a/tests/test_runtime_common.py b/tests/test_runtime_common.py new file mode 100644 index 0000000..a7754b9 --- /dev/null +++ b/tests/test_runtime_common.py @@ -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"]