diff --git a/.gitignore b/.gitignore index 85bf635..a046f21 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,16 @@ .venv/ *.vscode/ build/ +dist/ *.egg-info/ -*__pycache__/ +__pycache__/ +*.py[cod] dataset/ models/ + +# 训练与回放产物不属于源码。已被 Git 跟踪的历史文件不会因本规则自动删除。 +logs/ +outputs/ +*.pt +*.onnx +events.out.tfevents.* diff --git a/MANIFEST.in b/MANIFEST.in index 5e6cc75..851f38b 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -1 +1,3 @@ recursive-include source/engineai_lab/assets * +global-exclude *.pyc *.pyo +prune source/engineai_lab/assets/__pycache__ diff --git a/README.md b/README.md index 7acaf4b..8c978f1 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,9 @@ engineai-lab └── engineai_lab ├── algorithms ├── assets + │ ├── gen2 + │ │ ├── meshes + │ │ └── urdf │ └── pm01 │ ├── meshes │ └── urdf @@ -26,6 +29,10 @@ engineai-lab ├── tasks │ └── velocity │ ├── config + │ │ ├── common + │ │ ├── gen2 + │ │ │ ├── agents + │ │ │ └── stages │ │ └── pm01 │ └── mdp └── utils @@ -58,7 +65,7 @@ Create and activate a new environment with Python 3.11: git clone https://github.com/isaac-sim/IsaacLab.git cd IsaacLab git checkout 4df6560e - ./isaaclab -i rsl_rl # Install rsl-rl dependency + ./isaaclab.sh -i rsl_rl # Install rsl-rl dependency ``` We highly recommend using the main branch`(4df6560e)` of Isaac Lab, as it can support rsl-rl-lib >= 5.0 and Isaac Sim >= 5.0 . @@ -76,6 +83,11 @@ pip install -e . For a step-by-step Chinese guide to importing and training a new humanoid robot, see [EngineAI Lab 新人形机器人行走训练攻略](docs/humanoid_locomotion_onboarding_guide.md). +Gen2 的阶段划分、训练/续训/迁移命令和配置修改约定见 +[Gen2 分阶段训练说明](source/engineai_lab/tasks/velocity/config/gen2/README.md)； +URDF、mesh 与简化碰撞体约定见 +[Gen2 资产说明](source/engineai_lab/assets/gen2/README.md)。 + ### Supported Robots This repository currently supports the following environments from the EngineAI Robots family: @@ -84,6 +96,12 @@ This repository currently supports the following environments from the EngineAI |--------|--------|--------| PM01|`Flat-PM01-v0`|Basic flat-terrain locomotion PM01|`Flat-AMP-PM01-v0`|AMP-based motion imitation on flat terrain +Gen2|`Flat-Gen2-v0`|基础低速行走，包含站立指令采样 +Gen2|`Flat-Gen2-Speed-v0`|平地速度与稳定性巩固 +Gen2|`Flat-Gen2-Natural-v0`|自然对侧摆臂 +Gen2|`Flat-Gen2-Fast-v0`|课程提升至 1.6 m/s +Gen2|`Flat-Gen2-Sprint-v0`|课程提升至 3.0 m/s +Gen2|`Flat-Gen2-NaturalRun-v0`|3.0 m/s 自然跑姿微调 *More robots and environments are coming soon!* @@ -94,6 +112,39 @@ PM01|`Flat-AMP-PM01-v0`|AMP-based motion imitation on flat terrain python scripts/play.py --task=Flat-PM01-v0 --num_envs 128 --load_run ``` +### Gen2 Staged Training + +基础阶段从头训练： + +```bash +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ + --task Flat-Gen2-v0 \ + --num_envs 4096 \ + --max_iterations 1500 \ + --run_name gen2_walk_v0 \ + --device cuda:0 \ + --rl_device cuda:0 \ + --headless +``` + +跨阶段只迁移策略权重，例如 Fast -> Sprint： + +```bash +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ + --task Flat-Gen2-Sprint-v0 \ + --num_envs 4096 \ + --resume True \ + --load_mode finetune \ + --load_run '' \ + --checkpoint model_.pt \ + --run_name gen2_sprint_v1 \ + --device cuda:0 \ + --rl_device cuda:0 \ + --headless +``` + +同一 Task 中断续训才使用 `--load_mode resume`。所有阶段和对应 Play Task 的完整表格见上面的 Gen2 分阶段训练说明。 + ### Evaluating a Policy ``` @@ -142,7 +193,7 @@ If you have any questions about using this repository, we're here to help! ## License -EngineAI-Lab is released under [BSD-3 License](LICENSE). +This checkout does not currently contain a `LICENSE` file, while its package metadata and the upstream README use different license labels. Add a license file and align the metadata before redistribution. ## Acknowledgement diff --git a/docs/humanoid_locomotion_onboarding_guide.md b/docs/humanoid_locomotion_onboarding_guide.md index 9c74f75..81300f0 100644 --- a/docs/humanoid_locomotion_onboarding_guide.md +++ b/docs/humanoid_locomotion_onboarding_guide.md @@ -29,10 +29,11 @@ | 模块 | 典型路径 | 职责 | |---|---|---| -| 机器人资产 | `source/gen2_lab/assets/` | URDF、mesh、简化碰撞体和审计结果 | +| 机器人资产 | `source/engineai_lab/assets/gen2/` | URDF、mesh 和简化碰撞体；审计结果写入 `outputs/` | | 机器人配置 | `source/engineai_lab/robots/gen2.py` | q0、初始高度、关节顺序、执行器参数和资产加载 | -| 环境配置 | `source/engineai_lab/tasks/velocity/config/gen2/flat_env_cfg.py` | scene、action、observation、command、reward、reset、termination、curriculum | -| PPO 配置 | `source/engineai_lab/tasks/velocity/config/gen2/agents/rsl_rl_ppo_cfg.py` | 网络、熵系数、迭代数和实验目录 | +| 环境共享配置 | `source/engineai_lab/tasks/velocity/config/gen2/common_env_cfg.py` | scene、action、observation、reset、termination 和 Play 公共设置 | +| 分阶段环境 | `source/engineai_lab/tasks/velocity/config/gen2/stages/` | 每阶段独立的 reward、command、curriculum 和覆盖参数 | +| 分阶段 PPO | `source/engineai_lab/tasks/velocity/config/gen2/agents/` | 每阶段独立的迭代数、熵系数和学习率 | | MDP 函数 | `source/engineai_lab/tasks/velocity/mdp/` | 自定义命令、观测、奖励和终止逻辑 | | 任务注册 | `source/engineai_lab/tasks/velocity/config/gen2/__init__.py` | Gym task ID 到环境/PPO 配置的映射 | | 训练入口 | `scripts/train.py` | 创建环境、加载 checkpoint、运行 RSL-RL | @@ -45,10 +46,17 @@ source/engineai_lab/robots/.py source/engineai_lab/tasks/velocity/config// ├── __init__.py -├── flat_env_cfg.py +├── registry.py +├── common_env_cfg.py +├── stages/ +│ ├── walk.py +│ ├── speed.py +│ └── natural.py └── agents/ ├── __init__.py - └── rsl_rl_ppo_cfg.py + ├── walk_ppo_cfg.py + ├── speed_ppo_cfg.py + └── natural_ppo_cfg.py ``` ## 3. 总体推进路线和阶段门 @@ -151,13 +159,13 @@ Gen2 实例中，PM01 参考腿长约 `0.82 m`，Gen2 有效腿长约 `0.786 m` 可以把 Gen2 的静态检查脚本复制并参数化： ```bash -cd /home/xtkuang/Projects/cmvr/RL/engineai_amp +cd /home/xtkuang/Projects/cmvr/RL/cmvr_ai_lab /home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \ scripts/gen2_check_rl_readiness.py ``` -该脚本检查质量、惯量、关节限位、碰撞重叠、脚底高度和 reset z。用于新机器人时，必须修改其中的 URDF、foot link、robot cfg 和高度常量路径。当前脚本仍按旧名称 `GEN2_BASE_HEIGHT_TARGET` 搜索高度；复制时应改为新机器人的 `ROBOT_PELVIS_HEIGHT_TARGET`/`ROBOT_TORSO_HEIGHT_TARGET`，任何 `UNKNOWN` 都不能当作通过。 +该脚本检查质量、惯量、关节限位、碰撞重叠、脚底高度、reset z，以及 pelvis/torso 高度目标。用于新机器人时，必须修改其中的 URDF、foot link、robot cfg 和高度常量路径；任何 `unknown` 都会使检查失败，不能当作通过。 ### 5.2 视觉 mesh 不适合直接作为训练碰撞体 @@ -530,10 +538,9 @@ gym.register( ### 10.1 静态检查 ```bash -/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python -m py_compile \ +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python -m compileall \ source/engineai_lab/robots/.py \ - source/engineai_lab/tasks/velocity/config//flat_env_cfg.py \ - source/engineai_lab/tasks/velocity/config//agents/rsl_rl_ppo_cfg.py + source/engineai_lab/tasks/velocity/config// ``` ### 10.2 环境加载检查 @@ -761,7 +768,7 @@ Gen2 在这一步才把 `0.4 m/s` 固定命令下的实际速度从约 `0.23` 下列 `<...>` 是需要替换的占位符，不能原样执行。 ```bash -cd /home/xtkuang/Projects/cmvr/RL/engineai_amp +cd /home/xtkuang/Projects/cmvr/RL/cmvr_ai_lab /home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ --task Flat--v0 \ @@ -774,7 +781,9 @@ cd /home/xtkuang/Projects/cmvr/RL/engineai_amp --headless ``` -### 14.2 从 checkpoint 续训 +### 14.2 同一 Task 从 checkpoint 完整续训 + +同一任务中断后继续时使用 `resume`，它会恢复 optimizer、iteration 和 curriculum： ```bash /home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ @@ -783,6 +792,7 @@ cd /home/xtkuang/Projects/cmvr/RL/engineai_amp --seed 42 \ --max_iterations \ --resume True \ + --load_mode resume \ --load_run \ --checkpoint model_.pt \ --run_name \ @@ -793,7 +803,29 @@ cd /home/xtkuang/Projects/cmvr/RL/engineai_amp 当前 CLI 的 `--resume` 使用布尔值参数，应写成 `--resume True`，不能只写一个裸 `--resume`。 -### 14.3 TensorBoard +### 14.3 跨阶段迁移 checkpoint + +从一个 Task 进入下一阶段时必须使用 `finetune`。它只加载 actor/critic，使用新阶段的 optimizer 和 curriculum： + +```bash +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ + --task Flat---v0 \ + --num_envs 4096 \ + --seed 42 \ + --max_iterations \ + --resume True \ + --load_mode finetune \ + --load_run \ + --checkpoint model_.pt \ + --run_name \ + --device cuda:0 \ + --rl_device cuda:0 \ + --headless +``` + +checkpoint 会记录原 Task ID；跨 Task 使用完整 `resume` 会被训练入口拒绝，这是为了防止错误恢复旧 curriculum。 + +### 14.4 TensorBoard ```bash /home/xtkuang/App/anaconda3/envs/engineai_lab/bin/tensorboard \ @@ -1136,8 +1168,10 @@ fixed-speed benchmark 当前仓库中可以直接参考： - [`robots/gen2.py`](../source/engineai_lab/robots/gen2.py) -- [`config/gen2/flat_env_cfg.py`](../source/engineai_lab/tasks/velocity/config/gen2/flat_env_cfg.py) -- [`config/gen2/agents/rsl_rl_ppo_cfg.py`](../source/engineai_lab/tasks/velocity/config/gen2/agents/rsl_rl_ppo_cfg.py) +- [`config/gen2/README.md`](../source/engineai_lab/tasks/velocity/config/gen2/README.md) +- [`config/gen2/common_env_cfg.py`](../source/engineai_lab/tasks/velocity/config/gen2/common_env_cfg.py) +- [`config/gen2/stages/`](../source/engineai_lab/tasks/velocity/config/gen2/stages) +- [`config/gen2/agents/`](../source/engineai_lab/tasks/velocity/config/gen2/agents) - [`mdp/commands.py`](../source/engineai_lab/tasks/velocity/mdp/commands.py) - [`mdp/rewards.py`](../source/engineai_lab/tasks/velocity/mdp/rewards.py) - [`scripts/train.py`](../scripts/train.py) diff --git a/scripts/cli_args.py b/scripts/cli_args.py index b4a3257..b469206 100644 --- a/scripts/cli_args.py +++ b/scripts/cli_args.py @@ -24,6 +24,15 @@ def add_rsl_rl_args(parser: argparse.ArgumentParser): arg_group.add_argument("--resume", type=bool, default=None, help="Whether to resume from a checkpoint.") arg_group.add_argument("--load_run", type=str, default=None, help="Name of the run folder to resume from.") arg_group.add_argument("--checkpoint", type=str, default=None, help="Checkpoint file to resume from.") + arg_group.add_argument( + "--load_mode", + choices=("resume", "finetune"), + default="resume", + help=( + "resume restores optimizer, iteration, and curriculum state; " + "finetune loads actor/critic weights into a fresh optimizer and curriculum." + ), + ) # -- logger arguments arg_group.add_argument( "--logger", type=str, default=None, choices={"wandb", "tensorboard", "neptune"}, help="Logger module to use." diff --git a/scripts/gen2_check_rl_readiness.py b/scripts/gen2_check_rl_readiness.py index d06a59e..05e683e 100644 --- a/scripts/gen2_check_rl_readiness.py +++ b/scripts/gen2_check_rl_readiness.py @@ -14,9 +14,26 @@ import numpy as np REPO_ROOT = Path(__file__).resolve().parents[1] -URDF_PATH = REPO_ROOT / "source" / "gen2_lab" / "assets" / "robot_simplified_collision.urdf" +URDF_PATH = ( + REPO_ROOT + / "source" + / "engineai_lab" + / "assets" + / "gen2" + / "urdf" + / "robot_simplified_collision.urdf" +) GEN2_ROBOT_CFG = REPO_ROOT / "source" / "engineai_lab" / "robots" / "gen2.py" -GEN2_ENV_CFG = REPO_ROOT / "source" / "engineai_lab" / "tasks" / "velocity" / "config" / "gen2" / "flat_env_cfg.py" +GEN2_ENV_CFG = ( + REPO_ROOT + / "source" + / "engineai_lab" + / "tasks" + / "velocity" + / "config" + / "gen2" + / "common_env_cfg.py" +) FOOT_LINKS = ("left_leg_link_6", "right_leg_link_6") @@ -91,14 +108,16 @@ def _load_base_init_z() -> float | None: return float(match.group(1).split(",")[2].strip()) -def _load_base_height_target() -> float | None: +def _load_height_targets() -> dict[str, float]: if not GEN2_ENV_CFG.exists(): - return None + return {} text = GEN2_ENV_CFG.read_text(encoding="utf-8") - match = re.search(r"GEN2_BASE_HEIGHT_TARGET\s*=\s*([0-9.]+)", text) - if match is None: - return None - return float(match.group(1)) + targets = {} + for name in ("GEN2_PELVIS_HEIGHT_TARGET", "GEN2_TORSO_HEIGHT_TARGET"): + match = re.search(rf"{name}\s*=\s*([0-9.]+)", text) + if match is not None: + targets[name] = float(match.group(1)) + return targets def _link_tree(root: ET.Element): @@ -282,7 +301,7 @@ def main() -> None: non_adjacent_overlaps.sort(reverse=True) base_z = _load_base_init_z() - base_target = _load_base_height_target() + height_targets = _load_height_targets() foot_min_z = {box.name: float(box.vertices[:, 2].min()) for box in boxes if box.name in FOOT_LINKS} lowest_foot_z = min(foot_min_z.values()) suggested_base_z = -lowest_foot_z + 0.01 @@ -307,10 +326,21 @@ def main() -> None: world_min = min_z + base_z if base_z is not None else float("nan") print(f" {name}: relative={min_z:.6f} world_at_init={world_min:.6f}") print(f"base_init_z={base_z:.6f}" if base_z is not None else "base_init_z=unknown") - print(f"base_height_target={base_target:.6f}" if base_target is not None else "base_height_target=unknown") + for target_name in ("GEN2_PELVIS_HEIGHT_TARGET", "GEN2_TORSO_HEIGHT_TARGET"): + target = height_targets.get(target_name) + print(f"{target_name.lower()}={target:.6f}" if target is not None else f"{target_name.lower()}=unknown") print(f"suggested_base_z_for_1cm_foot_clearance={suggested_base_z:.6f}") - hard_fail = bool(missing_inertial or bad_mass or bad_inertia or missing_limits or bad_limits or non_adjacent_overlaps) + missing_height_targets = len(height_targets) != 2 + hard_fail = bool( + missing_inertial + or bad_mass + or bad_inertia + or missing_limits + or bad_limits + or non_adjacent_overlaps + or missing_height_targets + ) ground_penetration = base_z is not None and lowest_foot_z + base_z < -0.005 if hard_fail: print("RESULT=FAIL") diff --git a/scripts/gen2_generate_simplified_collisions.py b/scripts/gen2_generate_simplified_collisions.py index 4fe95ed..8951ba4 100644 --- a/scripts/gen2_generate_simplified_collisions.py +++ b/scripts/gen2_generate_simplified_collisions.py @@ -3,10 +3,10 @@ The script does not modify the original robot.urdf. It writes: -- source/gen2_lab/assets/collision_simplified/*.stl -- source/gen2_lab/assets/robot_simplified_collision.urdf -- source/gen2_lab/assets/robot_simplified_collision_mesh.urdf -- source/gen2_lab/assets/collision_simplified_audit.csv +- source/engineai_lab/assets/gen2/meshes/collision_simplified/*.stl +- source/engineai_lab/assets/gen2/urdf/robot_simplified_collision.urdf +- source/engineai_lab/assets/gen2/urdf/robot_simplified_collision_mesh.urdf +- outputs/gen2_asset_audit/collision_simplified_audit.csv The recommended URDF is robot_simplified_collision.urdf. It uses URDF box primitives for collision. The generated STL files and mesh URDF are useful for @@ -24,12 +24,14 @@ from xml.dom import minidom REPO_ROOT = Path(__file__).resolve().parents[1] -ASSET_DIR = REPO_ROOT / "source" / "gen2_lab" / "assets" -INPUT_URDF = ASSET_DIR / "robot.urdf" -OUTPUT_DIR = ASSET_DIR / "collision_simplified" -OUTPUT_PRIMITIVE_URDF = ASSET_DIR / "robot_simplified_collision.urdf" -OUTPUT_MESH_URDF = ASSET_DIR / "robot_simplified_collision_mesh.urdf" -AUDIT_CSV = ASSET_DIR / "collision_simplified_audit.csv" +ASSET_DIR = REPO_ROOT / "source" / "engineai_lab" / "assets" / "gen2" +URDF_DIR = ASSET_DIR / "urdf" +MESH_DIR = ASSET_DIR / "meshes" +INPUT_URDF = URDF_DIR / "robot.urdf" +OUTPUT_DIR = MESH_DIR / "collision_simplified" +OUTPUT_PRIMITIVE_URDF = URDF_DIR / "robot_simplified_collision.urdf" +OUTPUT_MESH_URDF = URDF_DIR / "robot_simplified_collision_mesh.urdf" +AUDIT_CSV = REPO_ROOT / "outputs" / "gen2_asset_audit" / "collision_simplified_audit.csv" def _parse_floats(text: str | None, default: tuple[float, float, float]) -> tuple[float, float, float]: @@ -235,7 +237,9 @@ def main() -> None: continue mesh_file = mesh.attrib["filename"] - extent, center, tri_count = _binary_stl_bbox(ASSET_DIR / mesh_file) + # URDF 中只允许相对路径；从 URDF 所在目录解析，避免依赖当前工作目录。 + source_mesh_path = (INPUT_URDF.parent / mesh_file).resolve() + extent, center, tri_count = _binary_stl_bbox(source_mesh_path) proxy_center, size, scale = _proxy_box(name, extent, center) original_xyz = _parse_floats(origin.attrib.get("xyz"), (0.0, 0.0, 0.0)) @@ -244,7 +248,7 @@ def main() -> None: proxy_origin_xyz = tuple(original_xyz[i] + rotated_center[i] for i in range(3)) stl_name = f"{name}_box_collision.stl" - stl_rel_path = f"collision_simplified/{stl_name}" + stl_rel_path = f"../meshes/collision_simplified/{stl_name}" _write_binary_stl(OUTPUT_DIR / stl_name, _box_triangles(proxy_center, size)) _replace_collision_with_box(collision, proxy_origin_xyz, original_rpy, size) @@ -277,6 +281,7 @@ def main() -> None: _write_pretty_xml(primitive_tree, OUTPUT_PRIMITIVE_URDF) _write_pretty_xml(mesh_tree, OUTPUT_MESH_URDF) + AUDIT_CSV.parent.mkdir(parents=True, exist_ok=True) with AUDIT_CSV.open("w", newline="", encoding="utf-8") as file: writer = csv.DictWriter(file, fieldnames=list(audit_rows[0].keys())) writer.writeheader() diff --git a/scripts/gen2_validate_registry.py b/scripts/gen2_validate_registry.py new file mode 100644 index 0000000..78acb0d --- /dev/null +++ b/scripts/gen2_validate_registry.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +"""启动 Isaac Sim 后验证全部 Gen2 Task 注册和 Train/Play 契约。""" + +from __future__ import annotations + +import importlib +import sys +from pathlib import Path + + +REPO_ROOT = Path(__file__).resolve().parents[1] +SOURCE_ROOT = REPO_ROOT / "source" +if str(SOURCE_ROOT) not in sys.path: + sys.path.insert(0, str(SOURCE_ROOT)) + +from isaaclab.app import AppLauncher + + +# Isaac Lab 配置模块依赖 pxr，必须先启动 App，再导入 engineai_lab.tasks。 +app_launcher = AppLauncher(headless=True) +simulation_app = app_launcher.app + + +def _load_class(entry_point: str): + module_name, class_name = entry_point.rsplit(":", 1) + return getattr(importlib.import_module(module_name), class_name) + + +def main() -> None: + import gymnasium as gym + + import engineai_lab.tasks # noqa: F401 + from engineai_lab.tasks.velocity.config.gen2.agents.rsl_rl_ppo_cfg import ( + Gen2FastPPORunnerCfg as LegacyFastRunnerCfg, + ) + from engineai_lab.tasks.velocity.config.gen2.agents.fast_ppo_cfg import Gen2FastPPORunnerCfg + from engineai_lab.tasks.velocity.config.gen2.flat_env_cfg import Gen2FastEnvCfg as LegacyFastEnvCfg + from engineai_lab.tasks.velocity.config.gen2.registry import GEN2_TASK_SPECS + from engineai_lab.tasks.velocity.config.gen2.stages.fast import Gen2FastEnvCfg + + # 旧模块现在是兼容层；类对象必须与新阶段模块完全相同。 + assert LegacyFastEnvCfg is Gen2FastEnvCfg + assert LegacyFastRunnerCfg is Gen2FastPPORunnerCfg + + configs = {} + for task_id, _, _ in GEN2_TASK_SPECS: + spec = gym.spec(task_id) + env_cfg_cls = _load_class(spec.kwargs["env_cfg_entry_point"]) + runner_cfg_cls = _load_class(spec.kwargs["rsl_rl_cfg_entry_point"]) + env_cfg = env_cfg_cls() + runner_cfg = runner_cfg_cls() + configs[task_id] = env_cfg + + assert runner_cfg.experiment_name == "velocity_flat_terrain_gen2" + assert len(env_cfg.actions.joint_pos.joint_names) == 28 + assert env_cfg.observations.policy.joint_pos.history_length == 15 + print(f"OK {task_id}: {env_cfg_cls.__module__}.{env_cfg_cls.__name__}", flush=True) + + train_play_pairs = ( + ("Flat-Gen2-v0", "Flat-Gen2-Play-v0"), + ("Flat-Gen2-Speed-v0", "Flat-Gen2-Speed-Play-v0"), + ("Flat-Gen2-Natural-v0", "Flat-Gen2-Natural-Play-v0"), + ("Flat-Gen2-Fast-v0", "Flat-Gen2-Fast-Play-v0"), + ("Flat-Gen2-Sprint-v0", "Flat-Gen2-Sprint-Play-v0"), + ("Flat-Gen2-NaturalRun-v0", "Flat-Gen2-NaturalRun-Play-v0"), + ) + for train_id, play_id in train_play_pairs: + train_cfg = configs[train_id] + play_cfg = configs[play_id] + assert train_cfg.actions.joint_pos.joint_names == play_cfg.actions.joint_pos.joint_names + assert train_cfg.actions.joint_pos.scale == play_cfg.actions.joint_pos.scale + + print(f"RESULT=PASS tasks={len(configs)} train_play_pairs={len(train_play_pairs)}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/scripts/gen2_visualize_collisions.py b/scripts/gen2_visualize_collisions.py index e483fcb..f421e9e 100644 --- a/scripts/gen2_visualize_collisions.py +++ b/scripts/gen2_visualize_collisions.py @@ -19,8 +19,17 @@ from mpl_toolkits.mplot3d.art3d import Poly3DCollection REPO_ROOT = Path(__file__).resolve().parents[1] -DEFAULT_URDF = REPO_ROOT / "source" / "gen2_lab" / "assets" / "robot_simplified_collision.urdf" -DEFAULT_OUTPUT = REPO_ROOT / "source" / "gen2_lab" / "assets" / "gen2_collision_visualization.png" +DEFAULT_URDF = ( + REPO_ROOT + / "source" + / "engineai_lab" + / "assets" + / "gen2" + / "urdf" + / "robot_simplified_collision.urdf" +) +# 生成图属于检查产物，不写回运行时资产包。 +DEFAULT_OUTPUT = REPO_ROOT / "outputs" / "gen2_asset_audit" / "gen2_collision_visualization.png" GEN2_CFG = REPO_ROOT / "source" / "engineai_lab" / "robots" / "gen2.py" diff --git a/scripts/play.py b/scripts/play.py index 1187ff6..d824bdf 100644 --- a/scripts/play.py +++ b/scripts/play.py @@ -11,10 +11,11 @@ from isaaclab.app import AppLauncher # local imports import cli_args # isort: skip -# ensure repository root is on Python path for Hydra registry imports -REPO_ROOT = Path(__file__).resolve().parents[2] -if str(REPO_ROOT) not in sys.path: - sys.path.append(str(REPO_ROOT)) +# Prefer this checkout over any other editable ``engineai_lab`` installation. +REPO_ROOT = Path(__file__).resolve().parents[1] +SOURCE_ROOT = REPO_ROOT / "source" +if str(SOURCE_ROOT) not in sys.path: + sys.path.insert(0, str(SOURCE_ROOT)) # add argparse arguments parser = argparse.ArgumentParser(description="Play an RSL-RL policy checkpoint.") @@ -171,6 +172,13 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # set the log directory for the environment (works for all environment types) env_cfg.log_dir = log_dir + # Fixed/keyboard replay already applies its own step or ramp controller. + # Disable the training-side ramp to avoid applying acceleration limits twice. + if args_cli.command_source != "random" and hasattr( + env_cfg.commands.base_velocity, "command_ramp_rates" + ): + env_cfg.commands.base_velocity.command_ramp_rates = None + # create isaac environment env = gym.make(args_cli.task, cfg=env_cfg, render_mode=None) diff --git a/scripts/train.py b/scripts/train.py index 6f6ffc4..caa4986 100644 --- a/scripts/train.py +++ b/scripts/train.py @@ -11,10 +11,11 @@ from isaaclab.app import AppLauncher # local imports import cli_args # isort: skip -# ensure repository root is on the Python path for Hydra registry imports -REPO_ROOT = Path(__file__).resolve().parents[2] -if str(REPO_ROOT) not in sys.path: - sys.path.append(str(REPO_ROOT)) +# Prefer this checkout over any other editable ``engineai_lab`` installation. +REPO_ROOT = Path(__file__).resolve().parents[1] +SOURCE_ROOT = REPO_ROOT / "source" +if str(SOURCE_ROOT) not in sys.path: + sys.path.insert(0, str(SOURCE_ROOT)) # add argparse arguments parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") @@ -70,6 +71,49 @@ torch.backends.cudnn.deterministic = False torch.backends.cudnn.benchmark = False +def _restore_training_progress(env, runner, agent_cfg, checkpoint_infos): + """Restore environment-side curriculum state after a full RSL-RL resume.""" + base_env = env.unwrapped + engineai_info = checkpoint_infos.get("engineai_lab", {}) if isinstance(checkpoint_infos, dict) else {} + saved_task = engineai_info.get("task") + if saved_task is not None and saved_task != args_cli.task: + raise ValueError( + f"Checkpoint belongs to task {saved_task!r}, not {args_cli.task!r}. " + "Use --load_mode finetune for cross-task weight transfer." + ) + + if "common_step_counter" in engineai_info: + common_step_counter = int(engineai_info["common_step_counter"]) + state_source = "checkpoint metadata" + else: + common_step_counter = (runner.current_learning_iteration + 1) * agent_cfg.num_steps_per_env + state_source = "legacy checkpoint iteration estimate" + + base_env.common_step_counter = common_step_counter + base_env.curriculum_manager.compute(env_ids=None) + base_env.command_manager.reset(env_ids=None) + print(f"[INFO] Restored common_step_counter={common_step_counter} from {state_source}.") + command_term = base_env.command_manager.get_term("base_velocity") + if hasattr(command_term.cfg.ranges, "lin_vel_x"): + print(f"[INFO] Restored forward command range: {command_term.cfg.ranges.lin_vel_x}") + + +def _attach_training_state_to_checkpoints(env, runner): + """Make RSL-RL's periodic saves include the environment curriculum counter.""" + base_env = env.unwrapped + original_save = runner.save + + def save_with_training_state(path, infos=None): + checkpoint_infos = dict(infos) if isinstance(infos, dict) else {} + checkpoint_infos["engineai_lab"] = { + "task": args_cli.task, + "common_step_counter": int(base_env.common_step_counter), + } + original_save(path, infos=checkpoint_infos) + + runner.save = save_with_training_state + + @hydra_task_config(args_cli.task, "rsl_rl_cfg_entry_point") def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlOnPolicyRunnerCfg): """Train with RSL-RL agent.""" @@ -133,8 +177,25 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen # get path to previous checkpoint resume_path = get_checkpoint_path(log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) print(f"[INFO]: Loading model checkpoint from: {resume_path}") - # load previously trained model - runner.load(resume_path) + if args_cli.load_mode == "finetune": + runner.load( + resume_path, + load_cfg={ + "actor": True, + "critic": True, + "optimizer": False, + "iteration": False, + "rnd": False, + }, + ) + print("[INFO] Loaded actor/critic weights with a fresh optimizer and curriculum.") + else: + checkpoint_infos = runner.load(resume_path) + _restore_training_progress(env, runner, agent_cfg, checkpoint_infos) + + print(f"[INFO] Optimizer learning rate after load: {runner.alg.learning_rate:.6g}") + + _attach_training_state_to_checkpoints(env, runner) # dump the configuration into log-directory dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) diff --git a/setup.py b/setup.py index 649fae8..127c11b 100644 --- a/setup.py +++ b/setup.py @@ -30,7 +30,18 @@ setup( keywords=EXTENSION_TOML_DATA["package"]["keywords"], install_requires=INSTALL_REQUIRES, license="MIT", - include_package_data=True, + # 运行时资产显式列出，避免 setuptools 把 mesh/urdf 目录误判为 namespace package。 + include_package_data=False, + package_data={ + "engineai_lab.assets": [ + "pm01/meshes/*", + "pm01/urdf/*", + "gen2/README.md", + "gen2/urdf/*", + "gen2/meshes/*", + "gen2/meshes/collision_simplified/*", + ] + }, python_requires=">=3.10", classifiers=[ "Natural Language :: English", diff --git a/source/engineai_lab/assets/__init__.py b/source/engineai_lab/assets/__init__.py index 31e09f0..7512c45 100644 --- a/source/engineai_lab/assets/__init__.py +++ b/source/engineai_lab/assets/__init__.py @@ -1,4 +1,28 @@ -import os +"""运行时机器人资产路径。 -# Conveniences to other module directories via relative paths -ASSET_DIR = os.path.abspath(os.path.dirname(__file__)) +所有会被 Python 包加载的资产都必须位于 ``engineai_lab/assets`` 内， +这样 editable 安装和 wheel 安装使用相同的路径规则。 +""" + +from pathlib import Path + + +ASSET_ROOT = Path(__file__).resolve().parent +# 保留 PM01 现有的字符串拼接接口。 +ASSET_DIR = str(ASSET_ROOT) + +GEN2_ASSET_DIR = ASSET_ROOT / "gen2" +GEN2_URDF_DIR = GEN2_ASSET_DIR / "urdf" +GEN2_ORIGINAL_URDF_PATH = GEN2_URDF_DIR / "robot.urdf" +GEN2_SIMPLIFIED_COLLISION_URDF_PATH = GEN2_URDF_DIR / "robot_simplified_collision.urdf" +GEN2_SIMPLIFIED_COLLISION_MESH_URDF_PATH = GEN2_URDF_DIR / "robot_simplified_collision_mesh.urdf" + +__all__ = [ + "ASSET_DIR", + "ASSET_ROOT", + "GEN2_ASSET_DIR", + "GEN2_URDF_DIR", + "GEN2_ORIGINAL_URDF_PATH", + "GEN2_SIMPLIFIED_COLLISION_URDF_PATH", + "GEN2_SIMPLIFIED_COLLISION_MESH_URDF_PATH", +] diff --git a/source/engineai_lab/assets/gen2/README.md b/source/engineai_lab/assets/gen2/README.md new file mode 100644 index 0000000..84e9dce --- /dev/null +++ b/source/engineai_lab/assets/gen2/README.md @@ -0,0 +1,70 @@ +# Gen2 运行时资产 + +本目录是 `engineai_lab` Python 包内唯一的 Gen2 运行时资产位置。这样 editable 安装、wheel 安装和从其他工作目录启动时都会得到同一份 URDF 与 mesh。 + +```text +gen2/ +├── README.md +├── urdf/ +│ ├── robot.urdf +│ ├── robot_simplified_collision.urdf +│ └── robot_simplified_collision_mesh.urdf +└── meshes/ + ├── *_visual.stl + ├── *_collision.stl + └── collision_simplified/ + └── *_box_collision.stl +``` + +## 三份 URDF 的职责 + +| 文件 | 用途 | 是否用于训练 | +|---|---|---| +| `robot.urdf` | 原始高精度 visual/collision，作为生成源和人工核对基准 | 否 | +| `robot_simplified_collision.urdf` | visual mesh + URDF box collision，接触更稳定、计算更轻 | **是，唯一默认训练资产** | +| `robot_simplified_collision_mesh.urdf` | visual mesh + 简化 box STL，便于外部工具叠加检查 | 否 | + +`source/engineai_lab/robots/gen2.py` 明确加载 `robot_simplified_collision.urdf`。文件缺失时会直接报错，不会静默回退到原始高精度碰撞体，避免不同安装环境得到不同训练物理。 + +URDF 内 mesh 使用相对路径： + +```text +../meshes/_visual.stl +../meshes/_collision.stl +../meshes/collision_simplified/_box_collision.stl +``` + +不要写机器相关的绝对路径或旧的 `source/gen2_lab` 路径。 + +## 重新生成简化碰撞体 + +生成脚本不会覆盖 `robot.urdf`： + +```bash +cd /home/xtkuang/Projects/cmvr/RL/cmvr_ai_lab + +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \ + scripts/gen2_generate_simplified_collisions.py +``` + +它会更新两份 simplified URDF 和 `meshes/collision_simplified/`，审计 CSV 写入 `outputs/gen2_asset_audit/`。 + +生成后必须执行： + +```bash +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \ + scripts/gen2_check_rl_readiness.py + +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \ + scripts/gen2_visualize_collisions.py +``` + +静态检查期望 `RESULT=PASS`，且 pelvis/torso 高度目标不能显示 `unknown`。可视化 PNG 写入 `outputs/gen2_asset_audit/`，不写回资产包。 + +## 修改约束 + +1. `robot.urdf` 是生成源，先在这里确认 link、joint、limit、mass、inertia 和 mesh。 +2. 不要直接手改生成的 simplified URDF 后忘记同步生成脚本。 +3. 训练前检查 29 个 link、28 个 joint、全部 mesh 引用和 q0 脚底高度。 +4. 历史 `logs/**/params/env.yaml` 中的旧绝对路径只是训练快照，不要批量改写。 +5. 供应商原始压缩包放在仓库 `third_party/gen2/`，不进入运行时 wheel。 diff --git a/source/gen2_lab/assets/assets/body_link_collision.stl b/source/engineai_lab/assets/gen2/meshes/body_link_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/body_link_collision.stl rename to source/engineai_lab/assets/gen2/meshes/body_link_collision.stl diff --git a/source/gen2_lab/assets/assets/body_link_visual.stl b/source/engineai_lab/assets/gen2/meshes/body_link_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/body_link_visual.stl rename to source/engineai_lab/assets/gen2/meshes/body_link_visual.stl diff --git a/source/gen2_lab/assets/collision_simplified/body_link_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/body_link_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/body_link_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/body_link_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_arm_link_1_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_1_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_arm_link_1_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_1_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_arm_link_2_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_2_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_arm_link_2_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_2_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_arm_link_3_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_3_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_arm_link_3_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_3_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_arm_link_4_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_4_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_arm_link_4_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_4_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_arm_link_5_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_5_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_arm_link_5_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_5_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_arm_link_6_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_6_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_arm_link_6_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_6_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_arm_link_7_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_7_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_arm_link_7_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_arm_link_7_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_leg_link_1_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_1_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_leg_link_1_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_1_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_leg_link_2_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_2_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_leg_link_2_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_2_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_leg_link_3_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_3_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_leg_link_3_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_3_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_leg_link_4_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_4_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_leg_link_4_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_4_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_leg_link_5_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_5_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_leg_link_5_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_5_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/left_leg_link_6_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_6_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/left_leg_link_6_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/left_leg_link_6_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_arm_link_1_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_1_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_arm_link_1_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_1_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_arm_link_2_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_2_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_arm_link_2_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_2_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_arm_link_3_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_3_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_arm_link_3_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_3_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_arm_link_4_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_4_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_arm_link_4_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_4_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_arm_link_5_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_5_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_arm_link_5_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_5_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_arm_link_6_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_6_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_arm_link_6_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_6_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_arm_link_7_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_7_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_arm_link_7_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_arm_link_7_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_leg_link_1_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_1_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_leg_link_1_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_1_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_leg_link_2_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_2_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_leg_link_2_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_2_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_leg_link_3_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_3_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_leg_link_3_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_3_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_leg_link_4_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_4_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_leg_link_4_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_4_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_leg_link_5_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_5_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_leg_link_5_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_5_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/right_leg_link_6_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_6_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/right_leg_link_6_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/right_leg_link_6_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/waist_link_1_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/waist_link_1_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/waist_link_1_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/waist_link_1_box_collision.stl diff --git a/source/gen2_lab/assets/collision_simplified/waist_link_2_box_collision.stl b/source/engineai_lab/assets/gen2/meshes/collision_simplified/waist_link_2_box_collision.stl similarity index 100% rename from source/gen2_lab/assets/collision_simplified/waist_link_2_box_collision.stl rename to source/engineai_lab/assets/gen2/meshes/collision_simplified/waist_link_2_box_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_1_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_1_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_1_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_1_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_1_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_1_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_1_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_1_visual.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_2_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_2_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_2_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_2_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_2_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_2_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_2_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_2_visual.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_3_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_3_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_3_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_3_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_3_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_3_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_3_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_3_visual.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_4_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_4_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_4_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_4_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_4_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_4_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_4_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_4_visual.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_5_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_5_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_5_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_5_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_5_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_5_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_5_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_5_visual.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_6_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_6_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_6_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_6_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_6_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_6_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_6_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_6_visual.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_7_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_7_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_7_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_7_collision.stl diff --git a/source/gen2_lab/assets/assets/left_arm_link_7_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_arm_link_7_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_arm_link_7_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_arm_link_7_visual.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_1_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_1_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_1_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_1_collision.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_1_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_1_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_1_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_1_visual.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_2_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_2_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_2_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_2_collision.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_2_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_2_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_2_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_2_visual.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_3_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_3_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_3_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_3_collision.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_3_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_3_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_3_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_3_visual.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_4_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_4_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_4_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_4_collision.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_4_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_4_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_4_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_4_visual.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_5_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_5_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_5_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_5_collision.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_5_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_5_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_5_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_5_visual.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_6_collision.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_6_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_6_collision.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_6_collision.stl diff --git a/source/gen2_lab/assets/assets/left_leg_link_6_visual.stl b/source/engineai_lab/assets/gen2/meshes/left_leg_link_6_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/left_leg_link_6_visual.stl rename to source/engineai_lab/assets/gen2/meshes/left_leg_link_6_visual.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_1_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_1_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_1_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_1_collision.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_1_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_1_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_1_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_1_visual.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_2_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_2_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_2_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_2_collision.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_2_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_2_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_2_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_2_visual.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_3_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_3_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_3_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_3_collision.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_3_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_3_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_3_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_3_visual.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_4_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_4_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_4_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_4_collision.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_4_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_4_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_4_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_4_visual.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_5_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_5_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_5_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_5_collision.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_5_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_5_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_5_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_5_visual.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_6_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_6_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_6_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_6_collision.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_6_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_6_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_6_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_6_visual.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_7_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_7_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_7_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_7_collision.stl diff --git a/source/gen2_lab/assets/assets/right_arm_link_7_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_arm_link_7_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_arm_link_7_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_arm_link_7_visual.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_1_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_1_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_1_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_1_collision.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_1_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_1_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_1_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_1_visual.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_2_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_2_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_2_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_2_collision.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_2_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_2_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_2_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_2_visual.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_3_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_3_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_3_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_3_collision.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_3_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_3_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_3_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_3_visual.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_4_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_4_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_4_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_4_collision.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_4_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_4_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_4_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_4_visual.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_5_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_5_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_5_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_5_collision.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_5_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_5_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_5_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_5_visual.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_6_collision.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_6_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_6_collision.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_6_collision.stl diff --git a/source/gen2_lab/assets/assets/right_leg_link_6_visual.stl b/source/engineai_lab/assets/gen2/meshes/right_leg_link_6_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/right_leg_link_6_visual.stl rename to source/engineai_lab/assets/gen2/meshes/right_leg_link_6_visual.stl diff --git a/source/gen2_lab/assets/assets/waist_link_1_collision.stl b/source/engineai_lab/assets/gen2/meshes/waist_link_1_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/waist_link_1_collision.stl rename to source/engineai_lab/assets/gen2/meshes/waist_link_1_collision.stl diff --git a/source/gen2_lab/assets/assets/waist_link_1_visual.stl b/source/engineai_lab/assets/gen2/meshes/waist_link_1_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/waist_link_1_visual.stl rename to source/engineai_lab/assets/gen2/meshes/waist_link_1_visual.stl diff --git a/source/gen2_lab/assets/assets/waist_link_2_collision.stl b/source/engineai_lab/assets/gen2/meshes/waist_link_2_collision.stl similarity index 100% rename from source/gen2_lab/assets/assets/waist_link_2_collision.stl rename to source/engineai_lab/assets/gen2/meshes/waist_link_2_collision.stl diff --git a/source/gen2_lab/assets/assets/waist_link_2_visual.stl b/source/engineai_lab/assets/gen2/meshes/waist_link_2_visual.stl similarity index 100% rename from source/gen2_lab/assets/assets/waist_link_2_visual.stl rename to source/engineai_lab/assets/gen2/meshes/waist_link_2_visual.stl diff --git a/source/gen2_lab/assets/robot.urdf b/source/engineai_lab/assets/gen2/urdf/robot.urdf similarity index 89% rename from source/gen2_lab/assets/robot.urdf rename to source/engineai_lab/assets/gen2/urdf/robot.urdf index b072557..18580d2 100644 --- a/source/gen2_lab/assets/robot.urdf +++ b/source/engineai_lab/assets/gen2/urdf/robot.urdf @@ -13,7 +13,7 @@ - + @@ -22,7 +22,7 @@ - + @@ -37,7 +37,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -61,7 +61,7 @@ - + @@ -70,7 +70,7 @@ - + @@ -85,7 +85,7 @@ - + @@ -94,7 +94,7 @@ - + @@ -109,7 +109,7 @@ - + @@ -118,7 +118,7 @@ - + @@ -133,7 +133,7 @@ - + @@ -142,7 +142,7 @@ - + @@ -157,7 +157,7 @@ - + @@ -166,7 +166,7 @@ - + @@ -181,7 +181,7 @@ - + @@ -190,7 +190,7 @@ - + @@ -261,7 +261,7 @@ - + @@ -270,7 +270,7 @@ - + @@ -285,7 +285,7 @@ - + @@ -294,7 +294,7 @@ - + @@ -309,7 +309,7 @@ - + @@ -318,7 +318,7 @@ - + @@ -333,7 +333,7 @@ - + @@ -342,7 +342,7 @@ - + @@ -357,7 +357,7 @@ - + @@ -366,7 +366,7 @@ - + @@ -381,7 +381,7 @@ - + @@ -390,7 +390,7 @@ - + @@ -405,7 +405,7 @@ - + @@ -414,7 +414,7 @@ - + @@ -485,7 +485,7 @@ - + @@ -494,7 +494,7 @@ - + @@ -509,7 +509,7 @@ - + @@ -518,7 +518,7 @@ - + @@ -533,7 +533,7 @@ - + @@ -542,7 +542,7 @@ - + @@ -557,7 +557,7 @@ - + @@ -566,7 +566,7 @@ - + @@ -581,7 +581,7 @@ - + @@ -590,7 +590,7 @@ - + @@ -605,7 +605,7 @@ - + @@ -614,7 +614,7 @@ - + @@ -629,7 +629,7 @@ - + @@ -638,7 +638,7 @@ - + @@ -653,7 +653,7 @@ - + @@ -662,7 +662,7 @@ - + @@ -725,7 +725,7 @@ - + @@ -734,7 +734,7 @@ - + @@ -749,7 +749,7 @@ - + @@ -758,7 +758,7 @@ - + @@ -773,7 +773,7 @@ - + @@ -782,7 +782,7 @@ - + @@ -797,7 +797,7 @@ - + @@ -806,7 +806,7 @@ - + @@ -821,7 +821,7 @@ - + @@ -830,7 +830,7 @@ - + @@ -845,7 +845,7 @@ - + @@ -854,7 +854,7 @@ - + diff --git a/source/gen2_lab/assets/robot_simplified_collision.urdf b/source/engineai_lab/assets/gen2/urdf/robot_simplified_collision.urdf similarity index 94% rename from source/gen2_lab/assets/robot_simplified_collision.urdf rename to source/engineai_lab/assets/gen2/urdf/robot_simplified_collision.urdf index e2f01d5..c79bf72 100644 --- a/source/gen2_lab/assets/robot_simplified_collision.urdf +++ b/source/engineai_lab/assets/gen2/urdf/robot_simplified_collision.urdf @@ -9,7 +9,7 @@ - + @@ -31,7 +31,7 @@ - + @@ -53,7 +53,7 @@ - + @@ -75,7 +75,7 @@ - + @@ -97,7 +97,7 @@ - + @@ -119,7 +119,7 @@ - + @@ -141,7 +141,7 @@ - + @@ -163,7 +163,7 @@ - + @@ -234,7 +234,7 @@ - + @@ -256,7 +256,7 @@ - + @@ -278,7 +278,7 @@ - + @@ -300,7 +300,7 @@ - + @@ -322,7 +322,7 @@ - + @@ -344,7 +344,7 @@ - + @@ -366,7 +366,7 @@ - + @@ -437,7 +437,7 @@ - + @@ -459,7 +459,7 @@ - + @@ -481,7 +481,7 @@ - + @@ -503,7 +503,7 @@ - + @@ -525,7 +525,7 @@ - + @@ -547,7 +547,7 @@ - + @@ -569,7 +569,7 @@ - + @@ -591,7 +591,7 @@ - + @@ -655,7 +655,7 @@ - + @@ -677,7 +677,7 @@ - + @@ -699,7 +699,7 @@ - + @@ -721,7 +721,7 @@ - + @@ -743,7 +743,7 @@ - + @@ -765,7 +765,7 @@ - + diff --git a/source/gen2_lab/assets/robot_simplified_collision_mesh.urdf b/source/engineai_lab/assets/gen2/urdf/robot_simplified_collision_mesh.urdf similarity index 86% rename from source/gen2_lab/assets/robot_simplified_collision_mesh.urdf rename to source/engineai_lab/assets/gen2/urdf/robot_simplified_collision_mesh.urdf index 41580bf..e695572 100644 --- a/source/gen2_lab/assets/robot_simplified_collision_mesh.urdf +++ b/source/engineai_lab/assets/gen2/urdf/robot_simplified_collision_mesh.urdf @@ -9,7 +9,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -31,7 +31,7 @@ - + @@ -40,7 +40,7 @@ - + @@ -53,7 +53,7 @@ - + @@ -62,7 +62,7 @@ - + @@ -75,7 +75,7 @@ - + @@ -84,7 +84,7 @@ - + @@ -97,7 +97,7 @@ - + @@ -106,7 +106,7 @@ - + @@ -119,7 +119,7 @@ - + @@ -128,7 +128,7 @@ - + @@ -141,7 +141,7 @@ - + @@ -150,7 +150,7 @@ - + @@ -163,7 +163,7 @@ - + @@ -172,7 +172,7 @@ - + @@ -234,7 +234,7 @@ - + @@ -243,7 +243,7 @@ - + @@ -256,7 +256,7 @@ - + @@ -265,7 +265,7 @@ - + @@ -278,7 +278,7 @@ - + @@ -287,7 +287,7 @@ - + @@ -300,7 +300,7 @@ - + @@ -309,7 +309,7 @@ - + @@ -322,7 +322,7 @@ - + @@ -331,7 +331,7 @@ - + @@ -344,7 +344,7 @@ - + @@ -353,7 +353,7 @@ - + @@ -366,7 +366,7 @@ - + @@ -375,7 +375,7 @@ - + @@ -437,7 +437,7 @@ - + @@ -446,7 +446,7 @@ - + @@ -459,7 +459,7 @@ - + @@ -468,7 +468,7 @@ - + @@ -481,7 +481,7 @@ - + @@ -490,7 +490,7 @@ - + @@ -503,7 +503,7 @@ - + @@ -512,7 +512,7 @@ - + @@ -525,7 +525,7 @@ - + @@ -534,7 +534,7 @@ - + @@ -547,7 +547,7 @@ - + @@ -556,7 +556,7 @@ - + @@ -569,7 +569,7 @@ - + @@ -578,7 +578,7 @@ - + @@ -591,7 +591,7 @@ - + @@ -600,7 +600,7 @@ - + @@ -655,7 +655,7 @@ - + @@ -664,7 +664,7 @@ - + @@ -677,7 +677,7 @@ - + @@ -686,7 +686,7 @@ - + @@ -699,7 +699,7 @@ - + @@ -708,7 +708,7 @@ - + @@ -721,7 +721,7 @@ - + @@ -730,7 +730,7 @@ - + @@ -743,7 +743,7 @@ - + @@ -752,7 +752,7 @@ - + @@ -765,7 +765,7 @@ - + @@ -774,7 +774,7 @@ - + diff --git a/source/engineai_lab/robots/__init__.py b/source/engineai_lab/robots/__init__.py new file mode 100644 index 0000000..3288741 --- /dev/null +++ b/source/engineai_lab/robots/__init__.py @@ -0,0 +1,4 @@ +"""机器人 Articulation 配置包。 + +保持此文件轻量，不在包导入时自动创建全部机器人配置。 +""" diff --git a/source/engineai_lab/robots/gen2.py b/source/engineai_lab/robots/gen2.py index 4305867..e19d636 100644 --- a/source/engineai_lab/robots/gen2.py +++ b/source/engineai_lab/robots/gen2.py @@ -1,22 +1,25 @@ from __future__ import annotations -from pathlib import Path - import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.managers import SceneEntityCfg - -GEN2_ASSET_DIR = Path(__file__).resolve().parents[2] / "gen2_lab" / "assets" -GEN2_ORIGINAL_URDF_PATH = GEN2_ASSET_DIR / "robot.urdf" -GEN2_SIMPLIFIED_COLLISION_URDF_PATH = GEN2_ASSET_DIR / "robot_simplified_collision.urdf" -GEN2_URDF_PATH = ( - GEN2_SIMPLIFIED_COLLISION_URDF_PATH - if GEN2_SIMPLIFIED_COLLISION_URDF_PATH.exists() - else GEN2_ORIGINAL_URDF_PATH +from engineai_lab.assets import ( + GEN2_ASSET_DIR, + GEN2_ORIGINAL_URDF_PATH, + GEN2_SIMPLIFIED_COLLISION_URDF_PATH, ) +# 训练物理必须固定使用简化碰撞 URDF。禁止文件缺失时静默回退到高精度碰撞， +# 否则同一份配置会因安装方式不同而产生不同的接触行为。 +GEN2_URDF_PATH = GEN2_SIMPLIFIED_COLLISION_URDF_PATH +if not GEN2_URDF_PATH.is_file(): + raise FileNotFoundError( + f"Gen2 training URDF is missing: {GEN2_URDF_PATH}. " + "Reinstall the package with engineai_lab/assets/gen2 included." + ) + GEN2_LEG_JOINT_NAMES = [ "left_leg_J1", "left_leg_J2", diff --git a/source/engineai_lab/tasks/velocity/config/common/__init__.py b/source/engineai_lab/tasks/velocity/config/common/__init__.py new file mode 100644 index 0000000..d223df6 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/common/__init__.py @@ -0,0 +1,5 @@ +"""跨机器人复用的速度任务配置。""" + +from .terrain_cfg import ROUGH_TERRAIN_GENERATOR_CFG + +__all__ = ["ROUGH_TERRAIN_GENERATOR_CFG"] diff --git a/source/engineai_lab/tasks/velocity/config/common/rsl_rl_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/common/rsl_rl_ppo_cfg.py new file mode 100644 index 0000000..ecf67b0 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/common/rsl_rl_ppo_cfg.py @@ -0,0 +1,60 @@ +"""速度任务共用的 RSL-RL PPO 基础配置。""" + +from isaaclab.utils import configclass +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg + + +@configclass +class BasePPORunnerCfg(RslRlOnPolicyRunnerCfg): + """PM01 与 Gen2 共用的网络和 PPO 默认值。 + + 各阶段只覆盖真正不同的训练轮数、学习率和熵系数，避免整份配置复制。 + """ + + num_steps_per_env = 24 + max_iterations = 3000 + save_interval = 50 + experiment_name = "velocity_flat_terrain" + obs_groups = {"actor": ["policy"], "critic": ["policy"]} + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.008, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg( + init_std=1.0, + std_type="scalar", + ), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=True, + ) + + def __post_init__(self): + super().__post_init__() + + # rsl-rl >= 5.0 已移除这些旧字段；在基类统一清理，保证各阶段网络结构一致。 + deprecated_keys = {"stochastic", "init_noise_std", "noise_std_type", "state_dependent_std"} + + def _remove_deprecated_keys(cfg_obj): + if cfg_obj is None: + return None + return {key: value for key, value in vars(cfg_obj).items() if key not in deprecated_keys} + + self.actor = _remove_deprecated_keys(self.actor) + self.critic = _remove_deprecated_keys(self.critic) diff --git a/source/engineai_lab/tasks/velocity/config/common/terrain_cfg.py b/source/engineai_lab/tasks/velocity/config/common/terrain_cfg.py new file mode 100644 index 0000000..2094b07 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/common/terrain_cfg.py @@ -0,0 +1,56 @@ +"""PM01 与 Gen2 共用的地形生成配置。 + +把地形放在机器人目录之外，避免修改 PM01 时无意改变 Gen2，反之亦然。 +需要调整通用地形时，应在这里修改并同时回归两台机器人。 +""" + +from __future__ import annotations + +import math + +import isaaclab.terrains as terrain_gen +from isaaclab.terrains.terrain_generator_cfg import TerrainGeneratorCfg + + +ROUGH_TERRAIN_GENERATOR_CFG = TerrainGeneratorCfg( + size=(8.0, 8.0), + horizontal_scale=0.1, + vertical_scale=0.005, + border_width=25.0, + num_rows=10, + num_cols=20, + curriculum=True, + difficulty_range=(0.0, 1.0), + color_scheme="height", + slope_threshold=0.75, + sub_terrains={ + "flat": terrain_gen.HfPyramidSlopedTerrainCfg( + proportion=0.4, + slope_range=(0.0, 0.0), + platform_width=8.0, + ), + "slope_up": terrain_gen.HfPyramidSlopedTerrainCfg( + proportion=0.1, + slope_range=(0.0, math.radians(5)), + platform_width=2.0, + ), + "slope_down": terrain_gen.HfInvertedPyramidSlopedTerrainCfg( + proportion=0.1, + slope_range=(0.0, math.radians(5)), + platform_width=2.0, + ), + "obstacles": terrain_gen.HfDiscreteObstaclesTerrainCfg( + proportion=0.2, + obstacle_width_range=(1.0, 2.0), + obstacle_height_range=(0.01, 0.1), + num_obstacles=15, + platform_width=3.0, + ), + "rough_terrain": terrain_gen.HfRandomUniformTerrainCfg( + proportion=0.2, + noise_range=(-0.015, 0.015), + noise_step=0.005, + downsampled_scale=0.15, + ), + }, +) diff --git a/source/engineai_lab/tasks/velocity/config/gen2/README.md b/source/engineai_lab/tasks/velocity/config/gen2/README.md new file mode 100644 index 0000000..00a3daf --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/README.md @@ -0,0 +1,206 @@ +# Gen2 分阶段训练说明 + +本目录维护 Gen2 从基础低速行走到 3.0 m/s 跑步的完整训练链。重构后的原则是： + +- 共享的机器人语义、scene、action、observation、event 放在 `common_env_cfg.py`。 +- 每个训练阶段只在 `stages/` 的一个文件中定义该阶段新增或覆盖的 reward、command 和 curriculum。 +- 每个阶段的 PPO 差异放在 `agents/` 的独立文件中。 +- 保留原 Gym Task ID、策略输入输出和日志根，已有 checkpoint 可以继续使用。 + +> 当前仓库没有独立的 Gen2 Stand policy。`Flat-Gen2-v0` 是基础低速行走任务，其中 20% 环境采样站立指令。真正的“纯站立”任务若要增加，应单独设计和验证，不能把现有 Task ID 改名或改义。 + +## 目录结构 + +```text +gen2/ +├── README.md +├── __init__.py # 触发任务注册 +├── registry.py # Task ID -> EnvCfg/PPO 入口 +├── common_env_cfg.py # body 语义、scene、action、observation、event、Play helper +├── stages/ +│ ├── walk.py # 阶段 1：基础低速行走 +│ ├── speed.py # 阶段 2：平地速度巩固 +│ ├── natural.py # 阶段 3：自然摆臂 +│ ├── fast.py # 阶段 4：0.8 -> 1.6 m/s +│ ├── sprint.py # 阶段 5：1.6 -> 3.0 m/s +│ └── natural_run.py # 阶段 6：高速自然跑姿 +├── agents/ +│ ├── walk_ppo_cfg.py +│ ├── speed_ppo_cfg.py +│ ├── natural_ppo_cfg.py +│ ├── fast_ppo_cfg.py +│ ├── sprint_ppo_cfg.py +│ └── natural_run_ppo_cfg.py +├── flat_env_cfg.py # 旧导入路径兼容层 +├── sprint_env_cfg.py # 旧导入路径兼容层 +└── natural_run_env_cfg.py # 旧导入路径兼容层 +``` + +阶段依赖保持单向： + +```text +walk -> speed -> natural -> fast -> sprint -> natural_run +``` + +后面的阶段只继承前一阶段，不允许反向导入。这样查看某个文件时，可以直接看到“本阶段相对上一阶段改了什么”。 + +## 阶段总表 + +| 阶段 | 训练 Task | Play Task | 主要目标 | 默认前向速度 | PPO 迭代 | +|---|---|---|---|---:|---:| +| 1 基础低速行走 | `Flat-Gen2-v0` | `Flat-Gen2-Play-v0` | 站立采样、低速交替踏步、基础地形 | `-0.2~0.4 m/s` | 1500 | +| 2 平地速度巩固 | `Flat-Gen2-Speed-v0` | `Flat-Gen2-Speed-Play-v0` | plane、无 push、降低脚滑和落脚冲击 | `0.2~0.6 m/s` | 400 | +| 3 自然摆臂 | `Flat-Gen2-Natural-v0` | `Flat-Gen2-Natural-Play-v0` | 对侧摆臂、腕部稳定 | 继承阶段 2 | 300 | +| 4 Fast | `Flat-Gen2-Fast-v0` | `Flat-Gen2-Fast-Play-v0` | 课程提升至 1.6 m/s | `0.8 -> 1.6 m/s` | 750 | +| 5 Sprint | `Flat-Gen2-Sprint-v0` | `Flat-Gen2-Sprint-Play-v0` | 建立跑步腾空与 3.0 m/s 能力 | `1.6 -> 3.0 m/s` | 2250 | +| 6 NaturalRun | `Flat-Gen2-NaturalRun-v0` | `Flat-Gen2-NaturalRun-Play-v0` | 高速肩、肘、前臂协调 | `2.0 -> 3.0 m/s` | 1500 | + +所有阶段故意共用： + +- `experiment_name = velocity_flat_terrain_gen2` +- 28 个 action，关节顺序由 `GEN2_DFS_JOINT_NAMES` 固定 +- 15 帧 observation history +- actor/critic 隐藏层 `[512, 256, 128]` + +这些是 checkpoint 兼容契约。修改 observation 顺序、history、action 顺序或网络维度，会导致旧 checkpoint 不能加载。 + +Natural 及后续阶段把腕部 J5/J6/J7 action scale 从 `0.15` 缩小为 `0.06`，所以必须使用对应阶段的 Play Task。 + +## 三种加载方式 + +### 1. 从头训练阶段 1 + +```bash +cd /home/xtkuang/Projects/cmvr/RL/cmvr_ai_lab + +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ + --task Flat-Gen2-v0 \ + --num_envs 4096 \ + --seed 42 \ + --max_iterations 1500 \ + --run_name gen2_walk_v0 \ + --device cuda:0 \ + --rl_device cuda:0 \ + --headless +``` + +### 2. 同一 Task 中断后继续：`resume` + +`resume` 会恢复 actor、critic、optimizer、iteration 和 curriculum 计数。Task ID 必须和 checkpoint 中记录的一致。 + +```bash +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ + --task Flat-Gen2-Sprint-v0 \ + --num_envs 4096 \ + --max_iterations 200 \ + --resume True \ + --load_mode resume \ + --load_run '2026-07-13_15-39-32_gen2_sprint_3p0_curriculum_v1' \ + --checkpoint model_2249.pt \ + --run_name gen2_sprint_resume \ + --device cuda:0 \ + --rl_device cuda:0 \ + --headless +``` + +### 3. 从上一阶段迁移到下一阶段：`finetune` + +`finetune` 只加载 actor/critic 权重，使用新阶段的 optimizer 和 curriculum。跨 Task 迁移必须使用它，不能使用完整 `resume`。 + +下面以 Sprint -> NaturalRun 为例： + +```bash +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/train.py \ + --task Flat-Gen2-NaturalRun-v0 \ + --num_envs 4096 \ + --seed 42 \ + --max_iterations 1500 \ + --resume True \ + --load_mode finetune \ + --load_run '2026-07-13_15-39-32_gen2_sprint_3p0_curriculum_v1' \ + --checkpoint model_2249.pt \ + --run_name gen2_natural_run_v1 \ + --device cuda:0 \ + --rl_device cuda:0 \ + --headless +``` + +推荐迁移顺序： + +```text +Flat-Gen2-v0 + -> Flat-Gen2-Speed-v0 + -> Flat-Gen2-Natural-v0 + -> Flat-Gen2-Fast-v0 + -> Flat-Gen2-Sprint-v0 + -> Flat-Gen2-NaturalRun-v0 +``` + +每一步都从上一阶段视觉和固定速度测试最好的 checkpoint 开始，不要只按最后一次迭代选择。 + +## 固定指令回放 + +Sprint 3.0 m/s 示例： + +```bash +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python scripts/play.py \ + --task Flat-Gen2-Sprint-Play-v0 \ + --load_run '2026-07-13_15-39-32_gen2_sprint_3p0_curriculum_v1' \ + --checkpoint model_2249.pt \ + --num_envs 1 \ + --command_source fixed \ + --command_mode ramp \ + --linear_accel 0.6 \ + --vx 3.0 \ + --vy 0.0 \ + --wz 0.0 \ + --max_steps 4000 \ + --device cuda:0 \ + --rl_device cuda:0 +``` + +回放检查： + +1. pelvis 的真实前向速度是否接近命令，而不是只靠胸/腰扭动。 +2. 左右脚是否交替，接触脚是否明显滑动。 +3. Natural 之后是否为对侧摆臂，腕部是否高频乱动。 +4. Sprint 是否出现合理腾空，而不是单脚连续 pogo。 +5. 固定条件下比较 checkpoint，不用随机指令画面下结论。 + +## 修改某个阶段时 + +1. 只在对应 `stages/.py` 修改该阶段差异。 +2. 共享 body 名称、观测、动作或 reset 才修改 `common_env_cfg.py`。 +3. 修改 action scale 时同步验证该阶段 Play 配置。 +4. 保持 `super().__post_init__()` 先执行，再写本阶段覆盖。 +5. 不要顺手修改前一阶段已经验证的参数；新增能力应在后续阶段建立。 +6. 训练后以该 run 中的 `params/env.yaml` 和 `params/agent.yaml` 作为事实快照。 + +Fast 中有一个刻意保留的历史行为：`Gen2FastRewards` 声明速度跟踪权重为 `3.0`，但继承的 Speed `__post_init__` 最终将它设为 `2.5`；现有 Fast checkpoint 使用的就是 `2.5`。若要修正，应作为单独的训练行为改动，不应混在目录重构中。 + +## 快速检查 + +```bash +# Python 语法 +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python -m compileall \ + source/engineai_lab/tasks/velocity/config/gen2 + +# Gen2 URDF、惯量、关节限位和碰撞体 +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \ + scripts/gen2_check_rl_readiness.py + +# 不启动 Isaac Sim 的目录与 mesh 引用测试 +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python -m unittest \ + tests.test_gen2_structure + +# 启动 Isaac Sim，验证全部 Task 注册和 Train/Play action 契约 +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/python \ + scripts/gen2_validate_registry.py + +# 查看 TensorBoard +/home/xtkuang/App/anaconda3/envs/engineai_lab/bin/tensorboard \ + --logdir logs/rsl_rl/velocity_flat_terrain_gen2 \ + --port 6006 +``` + +资产目录和生成方式见 [Gen2 资产 README](../../../../assets/gen2/README.md)。完整的人形机器人接入方法见仓库根目录的 [`docs/humanoid_locomotion_onboarding_guide.md`](../../../../../../docs/humanoid_locomotion_onboarding_guide.md)。 diff --git a/source/engineai_lab/tasks/velocity/config/gen2/__init__.py b/source/engineai_lab/tasks/velocity/config/gen2/__init__.py index 47fb10e..60121af 100644 --- a/source/engineai_lab/tasks/velocity/config/gen2/__init__.py +++ b/source/engineai_lab/tasks/velocity/config/gen2/__init__.py @@ -1,58 +1,7 @@ -import gymnasium as gym +"""Gen2 速度任务包。""" -from . import agents +from .registry import register_gen2_tasks +register_gen2_tasks() -gym.register( - id="Flat-Gen2-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.flat_env_cfg:Gen2FlatEnvCfg", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:Gen2FlatPPORunnerCfg", - }, -) - - -gym.register( - id="Flat-Gen2-Play-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.flat_env_cfg:Gen2FlatEnvCfg_PLAY", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:Gen2FlatPPORunnerCfg", - }, -) - - -gym.register( - id="Flat-Gen2-Speed-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.flat_env_cfg:Gen2SpeedEnvCfg", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:Gen2SpeedPPORunnerCfg", - }, -) - - -gym.register( - id="Flat-Gen2-Natural-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.flat_env_cfg:Gen2NaturalEnvCfg", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:Gen2NaturalPPORunnerCfg", - }, -) - - -gym.register( - id="Flat-Gen2-Natural-Play-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.flat_env_cfg:Gen2NaturalEnvCfg_PLAY", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:Gen2NaturalPPORunnerCfg", - }, -) +__all__ = ["register_gen2_tasks"] diff --git a/source/engineai_lab/tasks/velocity/config/gen2/agents/fast_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/agents/fast_ppo_cfg.py new file mode 100644 index 0000000..21b32a9 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/agents/fast_ppo_cfg.py @@ -0,0 +1,10 @@ +"""阶段 4：1.6 m/s 快走 PPO 配置。""" + +from isaaclab.utils import configclass + +from .natural_ppo_cfg import Gen2NaturalPPORunnerCfg + + +@configclass +class Gen2FastPPORunnerCfg(Gen2NaturalPPORunnerCfg): + max_iterations = 750 diff --git a/source/engineai_lab/tasks/velocity/config/gen2/agents/natural_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/agents/natural_ppo_cfg.py new file mode 100644 index 0000000..0aa7101 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/agents/natural_ppo_cfg.py @@ -0,0 +1,17 @@ +"""阶段 3：自然摆臂 PPO 配置。""" + +from isaaclab.utils import configclass + +from .speed_ppo_cfg import Gen2SpeedPPORunnerCfg + + +@configclass +class Gen2NaturalPPORunnerCfg(Gen2SpeedPPORunnerCfg): + max_iterations = 300 + + def __post_init__(self): + super().__post_init__() + if isinstance(self.algorithm, dict): + self.algorithm["entropy_coef"] = 0.005 + else: + self.algorithm.entropy_coef = 0.005 diff --git a/source/engineai_lab/tasks/velocity/config/gen2/agents/natural_run_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/agents/natural_run_ppo_cfg.py new file mode 100644 index 0000000..8596e73 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/agents/natural_run_ppo_cfg.py @@ -0,0 +1,23 @@ +"""阶段 6：NaturalRun PPO 配置。""" + +from isaaclab.utils import configclass + +from .sprint_ppo_cfg import Gen2SprintPPORunnerCfg + + +@configclass +class Gen2NaturalRunPPORunnerCfg(Gen2SprintPPORunnerCfg): + """Sprint 到 NaturalRun 使用新优化器微调。""" + + max_iterations = 1500 + + def __post_init__(self): + super().__post_init__() + if isinstance(self.algorithm, dict): + self.algorithm["learning_rate"] = 3.0e-4 + self.algorithm["entropy_coef"] = 0.006 + self.algorithm["desired_kl"] = 0.01 + else: + self.algorithm.learning_rate = 3.0e-4 + self.algorithm.entropy_coef = 0.006 + self.algorithm.desired_kl = 0.01 diff --git a/source/engineai_lab/tasks/velocity/config/gen2/agents/rsl_rl_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/agents/rsl_rl_ppo_cfg.py index 312ea47..238297a 100644 --- a/source/engineai_lab/tasks/velocity/config/gen2/agents/rsl_rl_ppo_cfg.py +++ b/source/engineai_lab/tasks/velocity/config/gen2/agents/rsl_rl_ppo_cfg.py @@ -1,42 +1,18 @@ -from isaaclab.utils import configclass +"""旧 PPO 导入路径兼容层。 -from engineai_lab.tasks.velocity.config.pm01.agents.rsl_rl_ppo_cfg import PM01BasePPORunnerCfg +新代码应从对应的 ``*_ppo_cfg.py`` 导入；此文件保留已有脚本和外部代码的导入兼容。 +""" +from .fast_ppo_cfg import Gen2FastPPORunnerCfg +from .natural_ppo_cfg import Gen2NaturalPPORunnerCfg +from .speed_ppo_cfg import Gen2SpeedPPORunnerCfg +from .sprint_ppo_cfg import Gen2SprintPPORunnerCfg +from .walk_ppo_cfg import Gen2FlatPPORunnerCfg -@configclass -class Gen2FlatPPORunnerCfg(PM01BasePPORunnerCfg): - max_iterations = 1500 - experiment_name = "velocity_flat_terrain_gen2" - - def __post_init__(self): - super().__post_init__() - self.actor["hidden_dims"] = [512, 256, 128] - self.critic["hidden_dims"] = [512, 256, 128] - - -@configclass -class Gen2SpeedPPORunnerCfg(Gen2FlatPPORunnerCfg): - """Lower-exploration PPO settings for stage-two and stage-three fine-tuning.""" - - max_iterations = 400 - - def __post_init__(self): - super().__post_init__() - if isinstance(self.algorithm, dict): - self.algorithm["entropy_coef"] = 0.004 - else: - self.algorithm.entropy_coef = 0.004 - - -@configclass -class Gen2NaturalPPORunnerCfg(Gen2SpeedPPORunnerCfg): - """Fine-tuning settings with enough exploration to learn coordinated arm swing.""" - - max_iterations = 300 - - def __post_init__(self): - super().__post_init__() - if isinstance(self.algorithm, dict): - self.algorithm["entropy_coef"] = 0.005 - else: - self.algorithm.entropy_coef = 0.005 +__all__ = [ + "Gen2FlatPPORunnerCfg", + "Gen2SpeedPPORunnerCfg", + "Gen2NaturalPPORunnerCfg", + "Gen2FastPPORunnerCfg", + "Gen2SprintPPORunnerCfg", +] diff --git a/source/engineai_lab/tasks/velocity/config/gen2/agents/speed_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/agents/speed_ppo_cfg.py new file mode 100644 index 0000000..a959527 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/agents/speed_ppo_cfg.py @@ -0,0 +1,17 @@ +"""阶段 2：平地速度巩固 PPO 配置。""" + +from isaaclab.utils import configclass + +from .walk_ppo_cfg import Gen2FlatPPORunnerCfg + + +@configclass +class Gen2SpeedPPORunnerCfg(Gen2FlatPPORunnerCfg): + max_iterations = 400 + + def __post_init__(self): + super().__post_init__() + if isinstance(self.algorithm, dict): + self.algorithm["entropy_coef"] = 0.004 + else: + self.algorithm.entropy_coef = 0.004 diff --git a/source/engineai_lab/tasks/velocity/config/gen2/agents/sprint_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/agents/sprint_ppo_cfg.py new file mode 100644 index 0000000..e51bbd4 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/agents/sprint_ppo_cfg.py @@ -0,0 +1,19 @@ +"""阶段 5：3.0 m/s Sprint PPO 配置。""" + +from isaaclab.utils import configclass + +from .fast_ppo_cfg import Gen2FastPPORunnerCfg + + +@configclass +class Gen2SprintPPORunnerCfg(Gen2FastPPORunnerCfg): + max_iterations = 2250 + + def __post_init__(self): + super().__post_init__() + if isinstance(self.algorithm, dict): + self.algorithm["learning_rate"] = 5.0e-4 + self.algorithm["entropy_coef"] = 0.005 + else: + self.algorithm.learning_rate = 5.0e-4 + self.algorithm.entropy_coef = 0.005 diff --git a/source/engineai_lab/tasks/velocity/config/gen2/agents/walk_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/agents/walk_ppo_cfg.py new file mode 100644 index 0000000..406a443 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/agents/walk_ppo_cfg.py @@ -0,0 +1,17 @@ +"""阶段 1：基础低速行走 PPO 配置。""" + +from isaaclab.utils import configclass + +from engineai_lab.tasks.velocity.config.common.rsl_rl_ppo_cfg import BasePPORunnerCfg + + +@configclass +class Gen2FlatPPORunnerCfg(BasePPORunnerCfg): + max_iterations = 1500 + # 所有 Gen2 阶段保持同一日志根，跨阶段 finetune 才能按 load_run 找到 checkpoint。 + experiment_name = "velocity_flat_terrain_gen2" + + def __post_init__(self): + super().__post_init__() + self.actor["hidden_dims"] = [512, 256, 128] + self.critic["hidden_dims"] = [512, 256, 128] diff --git a/source/engineai_lab/tasks/velocity/config/gen2/common_env_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/common_env_cfg.py new file mode 100644 index 0000000..907b1db --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/common_env_cfg.py @@ -0,0 +1,332 @@ +from __future__ import annotations + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import ContactSensorCfg, RayCasterCfg, patterns +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +from engineai_lab.robots.actuator import DelayedImplicitActuatorCfg +from engineai_lab.robots.gen2 import ( + GEN2_CFG, + GEN2_DFS_JOINT_NAMES, + GEN2_DFS_JOINT_ORDER_ASSET_CFG, + GEN2_FEET_BODY_NAMES, +) +from engineai_lab.tasks.velocity import mdp +from engineai_lab.tasks.velocity.config.common import ROUGH_TERRAIN_GENERATOR_CFG + + +# 本文件只保存各阶段共同的机器人语义、场景、观测、动作和随机化。 +# reward、command、curriculum 以及阶段覆盖必须放在 stages/ 对应文件中。 + + +ACTUATOR_DELAY_RANGE = (2, 8) +GEN2_TORSO_BODY_NAME = "body_link" +GEN2_PELVIS_BODY_NAME = "waist_link_2" +GEN2_PELVIS_HEADING_YAW_OFFSET = 1.5708 +GEN2_TORSO_HEIGHT_TARGET = 1.27194 +GEN2_PELVIS_HEIGHT_TARGET = 0.85094 +GEN2_FOOT_SOLE_FRAME_OFFSETS_RPY = ( + (1.5708, 1.5708, 0.0), + (1.5708, 1.5708, 0.0), +) +GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME = ( + (0.096993, 0.120673, -0.785934), + (0.093994, -0.120677, -0.785934), +) +GEN2_ARM_SWING_JOINT_NAMES = ["left_arm_J1", "right_arm_J1", "left_arm_J4", "right_arm_J4"] +GEN2_ARM_PHASE_BODY_NAMES = ["left_arm_link_4", "right_arm_link_4"] +GEN2_WRIST_JOINT_NAMES = [ + "left_arm_J5", + "left_arm_J6", + "left_arm_J7", + "right_arm_J5", + "right_arm_J6", + "right_arm_J7", +] + +# Scale PM01-style gait targets by the two robots' approximate kinematic dimensions. +PM01_REFERENCE_LEG_LENGTH = 0.82 +PM01_REFERENCE_ARM_REACH = 0.575 +GEN2_LEG_LENGTH = abs(GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME[0][2]) +GEN2_ARM_REACH = 0.70 +GEN2_GAIT_PHASE_DISTANCE = 0.25 * GEN2_LEG_LENGTH / PM01_REFERENCE_LEG_LENGTH +GEN2_SHOULDER_SWING_AMPLITUDE = 0.20 * PM01_REFERENCE_ARM_REACH / GEN2_ARM_REACH +GEN2_ELBOW_FLEXION = 0.15 * PM01_REFERENCE_ARM_REACH / GEN2_ARM_REACH +GEN2_ARM_PHASE_AMPLITUDE = 0.10 + + +def _build_delayed_actuators(): + delayed_actuators = {} + for name, cfg in GEN2_CFG.actuators.items(): + delayed_actuators[name] = DelayedImplicitActuatorCfg( + joint_names_expr=cfg.joint_names_expr, + effort_limit=cfg.effort_limit, + effort_limit_sim=cfg.effort_limit_sim, + velocity_limit=cfg.velocity_limit, + velocity_limit_sim=cfg.velocity_limit_sim, + stiffness=cfg.stiffness, + damping=cfg.damping, + armature=cfg.armature, + friction=cfg.friction, + dynamic_friction=cfg.dynamic_friction, + viscous_friction=cfg.viscous_friction, + min_delay=ACTUATOR_DELAY_RANGE[0], + max_delay=ACTUATOR_DELAY_RANGE[1], + ) + return delayed_actuators + + +@configclass +class Gen2SceneCfg(InteractiveSceneCfg): + """Configuration for the terrain scene with the Gen2 robot.""" + + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=ROUGH_TERRAIN_GENERATOR_CFG, + max_init_terrain_level=5, + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + visual_material=sim_utils.MdlFileCfg( + mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/" + "TilesMarbleSpiderWhiteBrickBondHoned.mdl", + project_uvw=True, + texture_scale=(0.25, 0.25), + ), + debug_vis=False, + ) + + robot: ArticulationCfg = GEN2_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + actuators=_build_delayed_actuators(), + ) + + height_scanner = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot/body_link", + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), + ray_alignment="yaw", + pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), + debug_vis=False, + mesh_prim_paths=["/World/ground"], + ) + contact_forces = ContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True) + + sky_light = AssetBaseCfg( + prim_path="/World/skyLight", + spawn=sim_utils.DomeLightCfg( + intensity=750.0, + texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr", + ), + ) + + +@configclass +class Gen2Termination: + time_out = DoneTerm(func=mdp.time_out, time_out=True) + base_contact = DoneTerm( + func=mdp.illegal_contact, + params={ + "sensor_cfg": SceneEntityCfg( + "contact_forces", + body_names=["body_link", "waist_link_.*", ".*_leg_link_[1-4]", ".*_arm_link_.*"], + ), + "threshold": 1.0, + }, + ) + + +@configclass +class ActionsCfg: + joint_pos = mdp.JointPositionActionCfg( + asset_name="robot", + use_default_offset=True, + preserve_order=True, + joint_names=GEN2_DFS_JOINT_NAMES, + scale={ + ".*_leg_J1": 0.5, + ".*_leg_J2": 0.2, + ".*_leg_J3": 0.2, + ".*_leg_J4": 0.5, + ".*_leg_J5": 0.5, + ".*_leg_J6": 0.2, + "waist_J.*": 0.2, + ".*_arm_J1": 0.2, + ".*_arm_J2": 0.2, + ".*_arm_J3": 0.2, + ".*_arm_J4": 0.2, + ".*_arm_J5": 0.15, + ".*_arm_J6": 0.15, + ".*_arm_J7": 0.15, + }, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + noise=Unoise(n_min=-0.01, n_max=0.01), + params={"asset_cfg": GEN2_DFS_JOINT_ORDER_ASSET_CFG}, + history_length=15, + ) + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + noise=Unoise(n_min=-1.5, n_max=1.5), + params={"asset_cfg": GEN2_DFS_JOINT_ORDER_ASSET_CFG}, + history_length=15, + ) + actions = ObsTerm(func=mdp.last_action, history_length=15) + pelvis_ang_vel = ObsTerm( + func=mdp.body_ang_vel_yaw_frame, + noise=Unoise(n_min=-0.2, n_max=0.2), + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + }, + history_length=15, + ) + torso_projected_gravity = ObsTerm( + func=mdp.body_projected_gravity, + noise=Unoise(n_min=-0.05, n_max=0.05), + params={"asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME)}, + history_length=15, + ) + velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + @configclass + class CriticCfg(PolicyCfg): + pass + + policy: PolicyCfg = PolicyCfg() + critic: CriticCfg = CriticCfg() + + +VELOCITY_RANGE = { + "x": (-0.3, 0.3), + "y": (-0.3, 0.3), + "z": (-0.15, 0.15), + "roll": (-0.35, 0.35), + "pitch": (-0.35, 0.35), + "yaw": (-0.52, 0.52), +} + + +@configclass +class Gen2EventCfg: + """Gen2-specific randomizations.""" + + physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.3, 1.6), + "dynamic_friction_range": (0.3, 1.2), + "restitution_range": (0.0, 0.5), + "num_buckets": 64, + }, + ) + add_joint_default_pos = EventTerm( + func=mdp.randomize_joint_default_pos, + mode="startup", + params={ + "asset_cfg": GEN2_DFS_JOINT_ORDER_ASSET_CFG, + "pos_distribution_params": (-0.01, 0.01), + "operation": "add", + }, + ) + base_com = EventTerm( + func=mdp.randomize_rigid_body_com, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="body_link"), + "com_range": {"x": (-0.03, 0.03), "y": (-0.06, 0.06), "z": (-0.06, 0.06)}, + }, + ) + push_robot = EventTerm( + func=mdp.push_by_setting_velocity, + mode="interval", + interval_range_s=(1.0, 3.0), + params={"velocity_range": VELOCITY_RANGE}, + ) + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + }, + ) + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={"position_range": (0.8, 1.2), "velocity_range": (-0.5, 0.5)}, + ) + + +def configure_gen2_play_env(cfg): + """应用确定性回放设置，但不改变当前阶段的动作映射。""" + cfg.seed = 42 + cfg.scene.num_envs = 1 + cfg.scene.env_spacing = 2.5 + cfg.episode_length_s = 40.0 + + cfg.scene.terrain.terrain_type = "plane" + cfg.scene.terrain.terrain_generator = None + cfg.scene.terrain.max_init_terrain_level = None + cfg.scene.height_scanner = None + cfg.curriculum.terrain_levels = None + + cfg.observations.policy.enable_corruption = False + cfg.observations.critic.enable_corruption = False + + cfg.events.physics_material.params["static_friction_range"] = (1.0, 1.0) + cfg.events.physics_material.params["dynamic_friction_range"] = (1.0, 1.0) + cfg.events.physics_material.params["restitution_range"] = (0.0, 0.0) + cfg.events.add_joint_default_pos = None + cfg.events.base_com = None + cfg.events.push_robot = None + cfg.events.reset_base.params["pose_range"] = { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "yaw": (0.0, 0.0), + } + cfg.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + cfg.events.reset_robot_joints.params["velocity_range"] = (0.0, 0.0) + + for actuator_cfg in cfg.scene.robot.actuators.values(): + if hasattr(actuator_cfg, "min_delay") and hasattr(actuator_cfg, "max_delay"): + actuator_cfg.min_delay = 5 + actuator_cfg.max_delay = 5 + + cfg.commands.base_velocity.debug_vis = True diff --git a/source/engineai_lab/tasks/velocity/config/gen2/flat_env_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/flat_env_cfg.py index a636f33..d8deb97 100644 --- a/source/engineai_lab/tasks/velocity/config/gen2/flat_env_cfg.py +++ b/source/engineai_lab/tasks/velocity/config/gen2/flat_env_cfg.py @@ -1,822 +1,90 @@ -from __future__ import annotations +"""Gen2 旧环境配置导入路径兼容层。 -import isaaclab.sim as sim_utils -from isaaclab.assets import ArticulationCfg, AssetBaseCfg -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.managers import CurriculumTermCfg as CurrTerm -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import ObservationTermCfg as ObsTerm -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import SceneEntityCfg -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.sensors import ContactSensorCfg, RayCasterCfg, patterns -from isaaclab.terrains import TerrainImporterCfg -from isaaclab.utils import configclass -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR -from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise +阶段实现已经迁入 :mod:`.stages`。新代码请直接从对应阶段模块导入； +本文件仅用于兼容已有脚本、Hydra 字符串和用户代码。 +""" -from engineai_lab.robots.actuator import DelayedImplicitActuatorCfg -from engineai_lab.robots.gen2 import GEN2_CFG, GEN2_DFS_JOINT_NAMES, GEN2_DFS_JOINT_ORDER_ASSET_CFG -from engineai_lab.robots.gen2 import GEN2_FEET_BODY_NAMES -from engineai_lab.tasks.velocity import mdp -from engineai_lab.tasks.velocity.config.pm01.flat_env_cfg import terrain_generator - - -ACTUATOR_DELAY_RANGE = (2, 8) -GEN2_TORSO_BODY_NAME = "body_link" -GEN2_PELVIS_BODY_NAME = "waist_link_2" -GEN2_PELVIS_HEADING_YAW_OFFSET = 1.5708 -GEN2_TORSO_HEIGHT_TARGET = 1.27194 -GEN2_PELVIS_HEIGHT_TARGET = 0.85094 -GEN2_FOOT_SOLE_FRAME_OFFSETS_RPY = ( - (1.5708, 1.5708, 0.0), - (1.5708, 1.5708, 0.0), +from .common_env_cfg import ( + ACTUATOR_DELAY_RANGE, + GEN2_ARM_PHASE_AMPLITUDE, + GEN2_ARM_PHASE_BODY_NAMES, + GEN2_ARM_REACH, + GEN2_ARM_SWING_JOINT_NAMES, + GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME, + GEN2_ELBOW_FLEXION, + GEN2_FEET_BODY_NAMES, + GEN2_FOOT_SOLE_FRAME_OFFSETS_RPY, + GEN2_GAIT_PHASE_DISTANCE, + GEN2_LEG_LENGTH, + GEN2_PELVIS_BODY_NAME, + GEN2_PELVIS_HEADING_YAW_OFFSET, + GEN2_PELVIS_HEIGHT_TARGET, + GEN2_SHOULDER_SWING_AMPLITUDE, + GEN2_TORSO_BODY_NAME, + GEN2_TORSO_HEIGHT_TARGET, + GEN2_WRIST_JOINT_NAMES, + PM01_REFERENCE_ARM_REACH, + PM01_REFERENCE_LEG_LENGTH, + ActionsCfg, + Gen2EventCfg, + Gen2SceneCfg, + Gen2Termination, + ObservationsCfg, + VELOCITY_RANGE, + _build_delayed_actuators, + configure_gen2_play_env, ) -GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME = ( - (0.096993, 0.120673, -0.785934), - (0.093994, -0.120677, -0.785934), -) -GEN2_ARM_SWING_JOINT_NAMES = ["left_arm_J1", "right_arm_J1", "left_arm_J4", "right_arm_J4"] -GEN2_ARM_PHASE_BODY_NAMES = ["left_arm_link_4", "right_arm_link_4"] -GEN2_WRIST_JOINT_NAMES = [ - "left_arm_J5", - "left_arm_J6", - "left_arm_J7", - "right_arm_J5", - "right_arm_J6", - "right_arm_J7", +from .stages.fast import Gen2FastCurriculumCfg, Gen2FastEnvCfg, Gen2FastEnvCfg_PLAY, Gen2FastRewards +from .stages.natural import Gen2NaturalEnvCfg, Gen2NaturalEnvCfg_PLAY, Gen2NaturalRewards +from .stages.speed import Gen2SpeedEnvCfg, Gen2SpeedEnvCfg_PLAY, Gen2SpeedRewards +from .stages.walk import CurriculumCfg, Gen2Commands, Gen2FlatEnvCfg, Gen2FlatEnvCfg_PLAY, Gen2Rewards + +# 旧的 Sprint/NaturalRun 模块曾导入这个私有名称，继续保留别名。 +_configure_gen2_play_env = configure_gen2_play_env + +__all__ = [ + "ACTUATOR_DELAY_RANGE", + "ActionsCfg", + "CurriculumCfg", + "GEN2_ARM_PHASE_AMPLITUDE", + "GEN2_ARM_PHASE_BODY_NAMES", + "GEN2_ARM_REACH", + "GEN2_ARM_SWING_JOINT_NAMES", + "GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME", + "GEN2_ELBOW_FLEXION", + "GEN2_FEET_BODY_NAMES", + "GEN2_FOOT_SOLE_FRAME_OFFSETS_RPY", + "GEN2_GAIT_PHASE_DISTANCE", + "GEN2_LEG_LENGTH", + "GEN2_PELVIS_BODY_NAME", + "GEN2_PELVIS_HEADING_YAW_OFFSET", + "GEN2_PELVIS_HEIGHT_TARGET", + "GEN2_SHOULDER_SWING_AMPLITUDE", + "GEN2_TORSO_BODY_NAME", + "GEN2_TORSO_HEIGHT_TARGET", + "GEN2_WRIST_JOINT_NAMES", + "Gen2Commands", + "Gen2EventCfg", + "Gen2FastCurriculumCfg", + "Gen2FastEnvCfg", + "Gen2FastEnvCfg_PLAY", + "Gen2FastRewards", + "Gen2FlatEnvCfg", + "Gen2FlatEnvCfg_PLAY", + "Gen2NaturalEnvCfg", + "Gen2NaturalEnvCfg_PLAY", + "Gen2NaturalRewards", + "Gen2Rewards", + "Gen2SceneCfg", + "Gen2SpeedEnvCfg", + "Gen2SpeedEnvCfg_PLAY", + "Gen2SpeedRewards", + "Gen2Termination", + "ObservationsCfg", + "PM01_REFERENCE_ARM_REACH", + "PM01_REFERENCE_LEG_LENGTH", + "VELOCITY_RANGE", + "_build_delayed_actuators", + "_configure_gen2_play_env", + "configure_gen2_play_env", ] - -# Scale PM01-style gait targets by the two robots' approximate kinematic dimensions. -PM01_REFERENCE_LEG_LENGTH = 0.82 -PM01_REFERENCE_ARM_REACH = 0.575 -GEN2_LEG_LENGTH = abs(GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME[0][2]) -GEN2_ARM_REACH = 0.70 -GEN2_GAIT_PHASE_DISTANCE = 0.25 * GEN2_LEG_LENGTH / PM01_REFERENCE_LEG_LENGTH -GEN2_SHOULDER_SWING_AMPLITUDE = 0.20 * PM01_REFERENCE_ARM_REACH / GEN2_ARM_REACH -GEN2_ELBOW_FLEXION = 0.15 * PM01_REFERENCE_ARM_REACH / GEN2_ARM_REACH -GEN2_ARM_PHASE_AMPLITUDE = 0.10 - - -def _build_delayed_actuators(): - delayed_actuators = {} - for name, cfg in GEN2_CFG.actuators.items(): - delayed_actuators[name] = DelayedImplicitActuatorCfg( - joint_names_expr=cfg.joint_names_expr, - effort_limit=cfg.effort_limit, - effort_limit_sim=cfg.effort_limit_sim, - velocity_limit=cfg.velocity_limit, - velocity_limit_sim=cfg.velocity_limit_sim, - stiffness=cfg.stiffness, - damping=cfg.damping, - armature=cfg.armature, - friction=cfg.friction, - dynamic_friction=cfg.dynamic_friction, - viscous_friction=cfg.viscous_friction, - min_delay=ACTUATOR_DELAY_RANGE[0], - max_delay=ACTUATOR_DELAY_RANGE[1], - ) - return delayed_actuators - - -@configclass -class Gen2SceneCfg(InteractiveSceneCfg): - """Configuration for the terrain scene with the Gen2 robot.""" - - terrain = TerrainImporterCfg( - prim_path="/World/ground", - terrain_type="generator", - terrain_generator=terrain_generator, - max_init_terrain_level=5, - collision_group=-1, - physics_material=sim_utils.RigidBodyMaterialCfg( - friction_combine_mode="multiply", - restitution_combine_mode="multiply", - static_friction=1.0, - dynamic_friction=1.0, - ), - visual_material=sim_utils.MdlFileCfg( - mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/" - "TilesMarbleSpiderWhiteBrickBondHoned.mdl", - project_uvw=True, - texture_scale=(0.25, 0.25), - ), - debug_vis=False, - ) - - robot: ArticulationCfg = GEN2_CFG.replace( - prim_path="{ENV_REGEX_NS}/Robot", - actuators=_build_delayed_actuators(), - ) - - height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/body_link", - offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), - ray_alignment="yaw", - pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), - debug_vis=False, - mesh_prim_paths=["/World/ground"], - ) - contact_forces = ContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True) - - sky_light = AssetBaseCfg( - prim_path="/World/skyLight", - spawn=sim_utils.DomeLightCfg( - intensity=750.0, - texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr", - ), - ) - - -@configclass -class Gen2Rewards: - """Reward terms adapted from PM01, with Gen2 body names and geometry.""" - - pelvis_track_lin_vel_xy_exp = RewTerm( - func=mdp.track_lin_vel_xy_yaw_frame_exp_body, - weight=2.0, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "command_name": "base_velocity", - "sigma": 5, - "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, - }, - ) - whole_body_track_ang_vel_z_exp = RewTerm( - func=mdp.track_ang_vel_z_world_exp_bodies, - weight=2.5, - params={ - "asset_cfg": SceneEntityCfg( - "robot", body_names=[GEN2_PELVIS_BODY_NAME, GEN2_TORSO_BODY_NAME], preserve_order=True - ), - "command_name": "base_velocity", - "sigma": 5, - }, - ) - torso_orientation = RewTerm( - func=mdp.body_orientation, - weight=0.8, - params={"asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), "scale": 10.0}, - ) - pelvis_height = RewTerm( - func=mdp.body_height_tracking, - weight=0.4, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "target_height": GEN2_PELVIS_HEIGHT_TARGET, - "scale": 15.0, - }, - ) - torso_height = RewTerm( - func=mdp.body_height_tracking, - weight=0.1, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), - "target_height": GEN2_TORSO_HEIGHT_TARGET, - "scale": 15.0, - }, - ) - foot_position = RewTerm( - func=mdp.feet_position_relative_to_body, - weight=0.5, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), - "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "command_name": "base_velocity", - "stand_threshold": 0.1, - "desired_foot_positions": GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME, - "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, - }, - ) - feet_orientation = RewTerm( - func=mdp.feet_orientation_relative_to_body, - weight=0.25, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), - "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "command_name": "base_velocity", - "stand_threshold": 0.1, - "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, - "foot_frame_offsets_rpy": GEN2_FOOT_SOLE_FRAME_OFFSETS_RPY, - }, - ) - torso_pelvis_yaw_alignment = RewTerm( - func=mdp.body_yaw_alignment, - weight=0.3, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), - "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "reference_heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, - "scale": 4.0, - }, - ) - torso_pelvis_yaw_rate = RewTerm( - func=mdp.body_yaw_rate_difference_l2, - weight=-0.5, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), - "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - }, - ) - waist_pos = RewTerm( - func=mdp.joint_deviation_exp, - weight=0.45, - params={ - "asset_cfg": SceneEntityCfg("robot", joint_names=["waist_J1", "waist_J2"]), - "scale": 3.0, - "tolerance": 0.0, - }, - ) - waist_vel = RewTerm( - func=mdp.joint_vel_l2, - weight=-0.02, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=["waist_J1", "waist_J2"])}, - ) - leg_joint_position = RewTerm( - func=mdp.joint_deviation_exp, - weight=0.3, - params={ - "asset_cfg": SceneEntityCfg( - "robot", - joint_names=[".*_leg_J2", ".*_leg_J3", ".*_leg_J6"], - ), - "scale": 3.0, - }, - ) - arm_primary_position = RewTerm( - func=mdp.joint_deviation_exp, - weight=0.3, - params={ - "asset_cfg": SceneEntityCfg("robot", joint_names=[".*_arm_J1", ".*_arm_J2", ".*_arm_J3", ".*_arm_J4"]), - "scale": 3.0, - }, - ) - arm_distal_position = RewTerm( - func=mdp.joint_deviation_exp, - weight=0.3, - params={ - "asset_cfg": SceneEntityCfg("robot", joint_names=[".*_arm_J5", ".*_arm_J6", ".*_arm_J7"]), - "scale": 8.0, - }, - ) - feet_contact = RewTerm( - func=mdp.biped_contact_mode_reward, - weight=0.75, - params={ - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), - "command_name": "base_velocity", - "force_threshold": 5.0, - "linear_threshold": 0.1, - "angular_threshold": 0.1, - }, - ) - feet_air_time = RewTerm( - func=mdp.feet_air_time_positive_on_contact, - weight=2.0, - params={ - "command_name": "base_velocity", - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), - "min_air_time": 0.05, - "max_air_time": 0.25, - "linear_threshold": 0.1, - "angular_threshold": 0.1, - }, - ) - feet_air_time_dense = RewTerm( - func=mdp.feet_air_time_positive_biped, - weight=2.0, - params={ - "command_name": "base_velocity", - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), - "threshold": 0.25, - "linear_threshold": 0.1, - "angular_threshold": 0.1, - }, - ) - swing_foot_clearance = RewTerm( - func=mdp.swing_foot_clearance_reward, - weight=0.75, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), - "command_name": "base_velocity", - "target_height": 0.04, - "std": 0.05, - "force_threshold": 5.0, - "linear_threshold": 0.1, - "angular_threshold": 0.1, - }, - ) - foot_stumble = RewTerm( - func=mdp.feet_stumble, - weight=-1.0, - params={ - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), - "tangential_threshold": 2.0, - "normal_threshold": 1.0, - }, - ) - dof_pos_limits = RewTerm( - func=mdp.joint_pos_limits, - weight=-10.0, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")}, - ) - energy_cost = RewTerm( - func=mdp.energy_cost_with_curriculum, - weight=-0.004, - params={ - "asset_cfg": SceneEntityCfg("robot", joint_names=[".*"]), - "start_scale": 0.1, - "power": 0.8, - "interval_epochs": 200 * 24, - }, - ) - feet_slide = RewTerm( - func=mdp.feet_slide, - weight=-0.4, - params={ - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), - }, - ) - dof_vel = RewTerm( - func=mdp.joint_vel_l2, - weight=-1.0e-5, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, - ) - dof_acc = RewTerm( - func=mdp.joint_acc_l2, - weight=-1.25e-8, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, - ) - action_rate = RewTerm( - func=mdp.action_rate_with_curriculum, - weight=-0.06, - params={"start_scale": 0.1, "power": 0.8, "interval_epochs": 200 * 24}, - ) - action_smoothness = RewTerm( - func=mdp.action_smoothness_with_curriculum, - weight=-0.04, - params={"start_scale": 0.1, "power": 0.8, "interval_epochs": 200 * 24}, - ) - dof_torque = RewTerm( - func=mdp.joint_torques_l2, - weight=-1.0e-6, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, - ) - termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) - - -@configclass -class Gen2SpeedRewards(Gen2Rewards): - """Additional damping and landing terms for faster command tracking.""" - - feet_slide = RewTerm( - func=mdp.feet_slide, - weight=-0.7, - params={ - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), - }, - ) - - pelvis_vertical_velocity = RewTerm( - func=mdp.body_vertical_velocity_l2, - weight=-2.0, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "deadband": 0.05, - }, - ) - pelvis_roll_pitch_ang_vel = RewTerm( - func=mdp.body_roll_pitch_ang_vel_l2, - weight=-0.1, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "deadband": 0.1, - }, - ) - torso_roll_pitch_ang_vel = RewTerm( - func=mdp.body_roll_pitch_ang_vel_l2, - weight=-0.1, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), - "deadband": 0.1, - }, - ) - feet_landing_velocity = RewTerm( - func=mdp.feet_landing_velocity, - weight=-1.0, - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True), - "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True), - "velocity_threshold": 0.2, - "power": 2.0, - }, - ) - - -@configclass -class Gen2NaturalRewards(Gen2SpeedRewards): - """Whole-body walking terms with speed-scaled cross-body arm swing.""" - - arm_primary_position = RewTerm( - func=mdp.joint_deviation_exp, - weight=0.3, - params={ - "asset_cfg": SceneEntityCfg("robot", joint_names=[".*_arm_J2", ".*_arm_J3"]), - "tolerance": 0.05, - "scale": 4.0, - }, - ) - arm_distal_position = RewTerm( - func=mdp.joint_deviation_l1_with_deadband, - weight=-0.25, - params={ - "asset_cfg": SceneEntityCfg( - "robot", joint_names=GEN2_WRIST_JOINT_NAMES, preserve_order=True - ), - "deadband": 0.03, - }, - ) - cross_body_arm_swing = RewTerm( - func=mdp.cross_body_arm_swing_reward, - weight=0.8, - params={ - "arm_asset_cfg": SceneEntityCfg( - "robot", joint_names=GEN2_ARM_SWING_JOINT_NAMES, preserve_order=True - ), - "feet_asset_cfg": SceneEntityCfg( - "robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True - ), - "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "command_name": "base_velocity", - "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, - "min_forward_speed": 0.1, - "full_swing_speed": 0.6, - "min_swing_scale": 0.35, - "phase_distance": GEN2_GAIT_PHASE_DISTANCE, - "shoulder_amplitude": GEN2_SHOULDER_SWING_AMPLITUDE, - "elbow_flexion": GEN2_ELBOW_FLEXION, - # Gen2's left/right J1 axes are mirrored, so equal joint signs create opposite physical swing. - "shoulder_phase_signs": (-1.0, -1.0), - "elbow_flexion_signs": (1.0, -1.0), - "std": 0.12, - }, - ) - contralateral_arm_phase = RewTerm( - func=mdp.contralateral_arm_phase_reward, - weight=0.5, - params={ - "arm_body_cfg": SceneEntityCfg( - "robot", body_names=GEN2_ARM_PHASE_BODY_NAMES, preserve_order=True - ), - "feet_asset_cfg": SceneEntityCfg( - "robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True - ), - "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "command_name": "base_velocity", - "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, - "min_forward_speed": 0.1, - "full_swing_speed": 0.6, - "min_swing_scale": 0.35, - "phase_distance": GEN2_GAIT_PHASE_DISTANCE, - "arm_phase_amplitude": GEN2_ARM_PHASE_AMPLITUDE, - "std": 0.045, - }, - ) - arm_swing_velocity = RewTerm( - func=mdp.joint_vel_l2, - weight=-0.001, - params={ - "asset_cfg": SceneEntityCfg( - "robot", joint_names=GEN2_ARM_SWING_JOINT_NAMES, preserve_order=True - ) - }, - ) - wrist_velocity = RewTerm( - func=mdp.joint_vel_l2, - weight=-0.004, - params={ - "asset_cfg": SceneEntityCfg( - "robot", joint_names=GEN2_WRIST_JOINT_NAMES, preserve_order=True - ) - }, - ) - - -@configclass -class Gen2Termination: - time_out = DoneTerm(func=mdp.time_out, time_out=True) - base_contact = DoneTerm( - func=mdp.illegal_contact, - params={ - "sensor_cfg": SceneEntityCfg( - "contact_forces", - body_names=["body_link", "waist_link_.*", ".*_leg_link_[1-4]", ".*_arm_link_.*"], - ), - "threshold": 1.0, - }, - ) - - -@configclass -class ActionsCfg: - joint_pos = mdp.JointPositionActionCfg( - asset_name="robot", - use_default_offset=True, - preserve_order=True, - joint_names=GEN2_DFS_JOINT_NAMES, - scale={ - ".*_leg_J1": 0.5, - ".*_leg_J2": 0.2, - ".*_leg_J3": 0.2, - ".*_leg_J4": 0.5, - ".*_leg_J5": 0.5, - ".*_leg_J6": 0.2, - "waist_J.*": 0.2, - ".*_arm_J1": 0.2, - ".*_arm_J2": 0.2, - ".*_arm_J3": 0.2, - ".*_arm_J4": 0.2, - ".*_arm_J5": 0.15, - ".*_arm_J6": 0.15, - ".*_arm_J7": 0.15, - }, - ) - - -@configclass -class ObservationsCfg: - """Observation specifications for the MDP.""" - - @configclass - class PolicyCfg(ObsGroup): - joint_pos = ObsTerm( - func=mdp.joint_pos_rel, - noise=Unoise(n_min=-0.01, n_max=0.01), - params={"asset_cfg": GEN2_DFS_JOINT_ORDER_ASSET_CFG}, - history_length=15, - ) - joint_vel = ObsTerm( - func=mdp.joint_vel_rel, - noise=Unoise(n_min=-1.5, n_max=1.5), - params={"asset_cfg": GEN2_DFS_JOINT_ORDER_ASSET_CFG}, - history_length=15, - ) - actions = ObsTerm(func=mdp.last_action, history_length=15) - pelvis_ang_vel = ObsTerm( - func=mdp.body_ang_vel_yaw_frame, - noise=Unoise(n_min=-0.2, n_max=0.2), - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), - "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, - }, - history_length=15, - ) - torso_projected_gravity = ObsTerm( - func=mdp.body_projected_gravity, - noise=Unoise(n_min=-0.05, n_max=0.05), - params={"asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME)}, - history_length=15, - ) - velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) - - def __post_init__(self): - self.enable_corruption = True - self.concatenate_terms = True - - @configclass - class CriticCfg(PolicyCfg): - pass - - policy: PolicyCfg = PolicyCfg() - critic: CriticCfg = CriticCfg() - - -@configclass -class Gen2Commands: - """Conservative velocity commands for the first Gen2 walking stage.""" - - base_velocity = mdp.BodyVelocityCommandCfg( - asset_name="robot", - body_name=GEN2_PELVIS_BODY_NAME, - heading_yaw_offset=GEN2_PELVIS_HEADING_YAW_OFFSET, - resampling_time_range=(7.5, 7.5), - rel_standing_envs=0.2, - rel_heading_envs=0.0, - heading_command=False, - heading_control_stiffness=0.5, - debug_vis=False, - ranges=mdp.BodyVelocityCommandCfg.Ranges( - lin_vel_x=(-0.2, 0.4), - lin_vel_y=(-0.2, 0.2), - ang_vel_z=(-0.5, 0.5), - heading=None, - ), - ) - - -VELOCITY_RANGE = { - "x": (-0.3, 0.3), - "y": (-0.3, 0.3), - "z": (-0.15, 0.15), - "roll": (-0.35, 0.35), - "pitch": (-0.35, 0.35), - "yaw": (-0.52, 0.52), -} - - -@configclass -class Gen2EventCfg: - """Gen2-specific randomizations.""" - - physics_material = EventTerm( - func=mdp.randomize_rigid_body_material, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=".*"), - "static_friction_range": (0.3, 1.6), - "dynamic_friction_range": (0.3, 1.2), - "restitution_range": (0.0, 0.5), - "num_buckets": 64, - }, - ) - add_joint_default_pos = EventTerm( - func=mdp.randomize_joint_default_pos, - mode="startup", - params={ - "asset_cfg": GEN2_DFS_JOINT_ORDER_ASSET_CFG, - "pos_distribution_params": (-0.01, 0.01), - "operation": "add", - }, - ) - base_com = EventTerm( - func=mdp.randomize_rigid_body_com, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("robot", body_names="body_link"), - "com_range": {"x": (-0.03, 0.03), "y": (-0.06, 0.06), "z": (-0.06, 0.06)}, - }, - ) - push_robot = EventTerm( - func=mdp.push_by_setting_velocity, - mode="interval", - interval_range_s=(1.0, 3.0), - params={"velocity_range": VELOCITY_RANGE}, - ) - reset_base = EventTerm( - func=mdp.reset_root_state_uniform, - mode="reset", - params={ - "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, - "velocity_range": { - "x": (0.0, 0.0), - "y": (0.0, 0.0), - "z": (0.0, 0.0), - "roll": (0.0, 0.0), - "pitch": (0.0, 0.0), - "yaw": (0.0, 0.0), - }, - }, - ) - reset_robot_joints = EventTerm( - func=mdp.reset_joints_by_scale, - mode="reset", - params={"position_range": (0.8, 1.2), "velocity_range": (-0.5, 0.5)}, - ) - - -@configclass -class CurriculumCfg: - terrain_levels = CurrTerm(func=mdp.terrain_levels_vel) - - -@configclass -class Gen2FlatEnvCfg(ManagerBasedRLEnvCfg): - """Velocity-tracking RL environment for Gen2.""" - - scene: Gen2SceneCfg = Gen2SceneCfg(num_envs=4096, env_spacing=3.0) - observations: ObservationsCfg = ObservationsCfg() - actions: ActionsCfg = ActionsCfg() - commands: Gen2Commands = Gen2Commands() - rewards: Gen2Rewards = Gen2Rewards() - terminations: Gen2Termination = Gen2Termination() - events: Gen2EventCfg = Gen2EventCfg() - curriculum: CurriculumCfg = CurriculumCfg() - - def __post_init__(self): - self.decimation = 5 - self.episode_length_s = 20.0 - self.sim.dt = 0.002 - self.sim.render_interval = self.decimation - self.sim.physics_material = self.scene.terrain.physics_material - self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15 - if self.scene.height_scanner is not None: - self.scene.height_scanner.update_period = self.decimation * self.sim.dt - if self.scene.contact_forces is not None: - self.scene.contact_forces.update_period = 0.005 - - if getattr(self.curriculum, "terrain_levels", None) is not None: - if self.scene.terrain.terrain_generator is not None: - self.scene.terrain.terrain_generator.curriculum = True - else: - if self.scene.terrain.terrain_generator is not None: - self.scene.terrain.terrain_generator.curriculum = False - - -@configclass -class Gen2SpeedEnvCfg(Gen2FlatEnvCfg): - """Stage-two flat-ground consolidation task for faster and steadier tracking.""" - - rewards: Gen2SpeedRewards = Gen2SpeedRewards() - - def __post_init__(self): - super().__post_init__() - - self.commands.base_velocity.resampling_time_range = (3.0, 5.0) - self.commands.base_velocity.rel_standing_envs = 0.1 - self.commands.base_velocity.rel_straight_envs = 0.8 - self.commands.base_velocity.ranges.lin_vel_x = (0.2, 0.6) - self.commands.base_velocity.ranges.lin_vel_y = (-0.05, 0.05) - self.commands.base_velocity.ranges.ang_vel_z = (-0.2, 0.2) - self.rewards.pelvis_track_lin_vel_xy_exp.weight = 2.5 - - self.scene.terrain.terrain_type = "plane" - self.scene.terrain.terrain_generator = None - self.scene.terrain.max_init_terrain_level = None - self.scene.height_scanner = None - self.curriculum.terrain_levels = None - self.events.push_robot = None - - -@configclass -class Gen2NaturalEnvCfg(Gen2SpeedEnvCfg): - """Stage-three fine-tuning task that adds natural cross-body arm swing.""" - - rewards: Gen2NaturalRewards = Gen2NaturalRewards() - - def __post_init__(self): - super().__post_init__() - for joint_pattern in (".*_arm_J5", ".*_arm_J6", ".*_arm_J7"): - self.actions.joint_pos.scale[joint_pattern] = 0.06 - self.events.reset_robot_joints.params["position_range"] = (0.95, 1.05) - self.events.reset_robot_joints.params["velocity_range"] = (-0.2, 0.2) - - -def _configure_gen2_play_env(cfg): - """Apply deterministic replay settings without changing a task's action mapping.""" - cfg.seed = 42 - cfg.scene.num_envs = 1 - cfg.scene.env_spacing = 2.5 - cfg.episode_length_s = 40.0 - - cfg.scene.terrain.terrain_type = "plane" - cfg.scene.terrain.terrain_generator = None - cfg.scene.terrain.max_init_terrain_level = None - cfg.scene.height_scanner = None - cfg.curriculum.terrain_levels = None - - cfg.observations.policy.enable_corruption = False - cfg.observations.critic.enable_corruption = False - - cfg.events.physics_material.params["static_friction_range"] = (1.0, 1.0) - cfg.events.physics_material.params["dynamic_friction_range"] = (1.0, 1.0) - cfg.events.physics_material.params["restitution_range"] = (0.0, 0.0) - cfg.events.add_joint_default_pos = None - cfg.events.base_com = None - cfg.events.push_robot = None - cfg.events.reset_base.params["pose_range"] = { - "x": (0.0, 0.0), - "y": (0.0, 0.0), - "yaw": (0.0, 0.0), - } - cfg.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) - cfg.events.reset_robot_joints.params["velocity_range"] = (0.0, 0.0) - - for actuator_cfg in cfg.scene.robot.actuators.values(): - if hasattr(actuator_cfg, "min_delay") and hasattr(actuator_cfg, "max_delay"): - actuator_cfg.min_delay = 5 - actuator_cfg.max_delay = 5 - - cfg.commands.base_velocity.debug_vis = True - - -@configclass -class Gen2FlatEnvCfg_PLAY(Gen2FlatEnvCfg): - """Deterministic flat-ground configuration for policy replay.""" - - def __post_init__(self): - super().__post_init__() - _configure_gen2_play_env(self) - - -@configclass -class Gen2NaturalEnvCfg_PLAY(Gen2NaturalEnvCfg): - """Deterministic replay configuration preserving stage-three wrist action scales.""" - - def __post_init__(self): - super().__post_init__() - _configure_gen2_play_env(self) diff --git a/source/engineai_lab/tasks/velocity/config/gen2/natural_run_env_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/natural_run_env_cfg.py new file mode 100644 index 0000000..9509271 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/natural_run_env_cfg.py @@ -0,0 +1,20 @@ +"""NaturalRun 旧导入路径兼容层；实现位于 :mod:`.stages.natural_run`。""" + +from .stages.natural_run import ( + GEN2_ELBOW_JOINT_NAMES, + GEN2_FOREARM_PHASE_BODY_NAMES, + GEN2_NATURAL_RUN_FOREARM_PHASE_AMPLITUDE, + GEN2_NATURAL_RUN_MAX_ELBOW_FLEXION, + GEN2_NATURAL_RUN_MAX_ELBOW_PHASE, + GEN2_NATURAL_RUN_MIN_ELBOW_FLEXION, + GEN2_NATURAL_RUN_MIN_ELBOW_PHASE, + GEN2_NATURAL_RUN_SHOULDER_AMPLITUDE, + GEN2_NATURAL_RUN_UPPER_ARM_PHASE_AMPLITUDE, + GEN2_SPRINT_ELBOW_AT_1P6, + Gen2NaturalRunCurriculumCfg, + Gen2NaturalRunEnvCfg, + Gen2NaturalRunEnvCfg_PLAY, + Gen2NaturalRunRewards, +) + +__all__ = [name for name in globals() if name.startswith("GEN2_") or name.startswith("Gen2")] diff --git a/source/engineai_lab/tasks/velocity/config/gen2/registry.py b/source/engineai_lab/tasks/velocity/config/gen2/registry.py new file mode 100644 index 0000000..7dbacca --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/registry.py @@ -0,0 +1,76 @@ +"""Gen2 Gym 任务注册表。 + +环境和 PPO 入口集中在一张表中，新增阶段时只需要追加一条记录； +已有 Task ID 不得重命名，因为 resume checkpoint 会校验任务 ID。 +""" + +from __future__ import annotations + +import gymnasium as gym + + +_PACKAGE = "engineai_lab.tasks.velocity.config.gen2" + +GEN2_TASK_SPECS = ( + ("Flat-Gen2-v0", "stages.walk:Gen2FlatEnvCfg", "agents.walk_ppo_cfg:Gen2FlatPPORunnerCfg"), + ("Flat-Gen2-Play-v0", "stages.walk:Gen2FlatEnvCfg_PLAY", "agents.walk_ppo_cfg:Gen2FlatPPORunnerCfg"), + ("Flat-Gen2-Speed-v0", "stages.speed:Gen2SpeedEnvCfg", "agents.speed_ppo_cfg:Gen2SpeedPPORunnerCfg"), + ( + "Flat-Gen2-Speed-Play-v0", + "stages.speed:Gen2SpeedEnvCfg_PLAY", + "agents.speed_ppo_cfg:Gen2SpeedPPORunnerCfg", + ), + ( + "Flat-Gen2-Natural-v0", + "stages.natural:Gen2NaturalEnvCfg", + "agents.natural_ppo_cfg:Gen2NaturalPPORunnerCfg", + ), + ( + "Flat-Gen2-Natural-Play-v0", + "stages.natural:Gen2NaturalEnvCfg_PLAY", + "agents.natural_ppo_cfg:Gen2NaturalPPORunnerCfg", + ), + ("Flat-Gen2-Fast-v0", "stages.fast:Gen2FastEnvCfg", "agents.fast_ppo_cfg:Gen2FastPPORunnerCfg"), + ( + "Flat-Gen2-Fast-Play-v0", + "stages.fast:Gen2FastEnvCfg_PLAY", + "agents.fast_ppo_cfg:Gen2FastPPORunnerCfg", + ), + ( + "Flat-Gen2-Sprint-v0", + "stages.sprint:Gen2SprintEnvCfg", + "agents.sprint_ppo_cfg:Gen2SprintPPORunnerCfg", + ), + ( + "Flat-Gen2-Sprint-Play-v0", + "stages.sprint:Gen2SprintEnvCfg_PLAY", + "agents.sprint_ppo_cfg:Gen2SprintPPORunnerCfg", + ), + ( + "Flat-Gen2-NaturalRun-v0", + "stages.natural_run:Gen2NaturalRunEnvCfg", + "agents.natural_run_ppo_cfg:Gen2NaturalRunPPORunnerCfg", + ), + ( + "Flat-Gen2-NaturalRun-Play-v0", + "stages.natural_run:Gen2NaturalRunEnvCfg_PLAY", + "agents.natural_run_ppo_cfg:Gen2NaturalRunPPORunnerCfg", + ), +) + + +def register_gen2_tasks() -> None: + """幂等注册全部 Gen2 训练与回放任务。""" + + for task_id, env_entry, runner_entry in GEN2_TASK_SPECS: + if task_id in gym.registry: + continue + gym.register( + id=task_id, + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{_PACKAGE}.{env_entry}", + "rsl_rl_cfg_entry_point": f"{_PACKAGE}.{runner_entry}", + }, + ) diff --git a/source/engineai_lab/tasks/velocity/config/gen2/sprint_env_cfg.py b/source/engineai_lab/tasks/velocity/config/gen2/sprint_env_cfg.py new file mode 100644 index 0000000..57f65b9 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/sprint_env_cfg.py @@ -0,0 +1,10 @@ +"""Sprint 旧导入路径兼容层；实现位于 :mod:`.stages.sprint`。""" + +from .stages.sprint import Gen2SprintCurriculumCfg, Gen2SprintEnvCfg, Gen2SprintEnvCfg_PLAY, Gen2SprintRewards + +__all__ = [ + "Gen2SprintRewards", + "Gen2SprintCurriculumCfg", + "Gen2SprintEnvCfg", + "Gen2SprintEnvCfg_PLAY", +] diff --git a/source/engineai_lab/tasks/velocity/config/gen2/stages/__init__.py b/source/engineai_lab/tasks/velocity/config/gen2/stages/__init__.py new file mode 100644 index 0000000..303c175 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/stages/__init__.py @@ -0,0 +1 @@ +"""Gen2 分阶段训练配置；模块按 walk -> speed -> natural -> fast -> sprint -> natural_run 单向继承。""" diff --git a/source/engineai_lab/tasks/velocity/config/gen2/stages/fast.py b/source/engineai_lab/tasks/velocity/config/gen2/stages/fast.py new file mode 100644 index 0000000..ce2788c --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/stages/fast.py @@ -0,0 +1,130 @@ +from __future__ import annotations + +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from engineai_lab.tasks.velocity import mdp + +from ..common_env_cfg import ( + GEN2_FEET_BODY_NAMES, + GEN2_PELVIS_BODY_NAME, + GEN2_PELVIS_HEADING_YAW_OFFSET, + configure_gen2_play_env, +) +from .natural import Gen2NaturalEnvCfg, Gen2NaturalRewards + + +# 阶段 4：从 0.8 m/s 逐级提高到 1.6 m/s。 +# 注意：父类 Speed 的 __post_init__ 会把速度跟踪权重最终设为 2.5； +# 这是现有 Fast checkpoint 的训练语义，结构重构中刻意保留。 + + +@configclass +class Gen2FastRewards(Gen2NaturalRewards): + """High-speed terms that preserve stage-three upper-body coordination.""" + + pelvis_track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp_body, + weight=3.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "sigma": 2.5, + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + }, + ) + forward_speed_undertracking = RewTerm( + func=mdp.body_forward_velocity_undertracking_l1, + weight=-0.75, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "min_command_speed": 0.2, + }, + ) + feet_slide = RewTerm( + func=mdp.feet_slide, + weight=-0.8, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + }, + ) + pelvis_vertical_velocity = RewTerm( + func=mdp.body_vertical_velocity_l2, + weight=-1.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "deadband": 0.1, + }, + ) + feet_landing_velocity = RewTerm( + func=mdp.feet_landing_velocity, + weight=-0.8, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True), + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True), + "velocity_threshold": 0.3, + "power": 2.0, + }, + ) + swing_foot_clearance = RewTerm( + func=mdp.swing_foot_clearance_reward, + weight=1.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "command_name": "base_velocity", + "target_height": 0.055, + "std": 0.05, + "force_threshold": 5.0, + "linear_threshold": 0.1, + "angular_threshold": 0.1, + }, + ) + + +@configclass +class Gen2FastCurriculumCfg: + terrain_levels = None + forward_speed = CurrTerm( + func=mdp.forward_speed_range_curriculum, + params={ + "command_name": "base_velocity", + "min_forward_speed": 0.3, + "initial_max_speed": 0.8, + "final_max_speed": 1.6, + "speed_increment": 0.2, + "stage_steps": 150 * 24, + }, + ) + + +@configclass +class Gen2FastEnvCfg(Gen2NaturalEnvCfg): + """Progressive flat-ground task targeting up to 1.6 m/s forward speed.""" + + rewards: Gen2FastRewards = Gen2FastRewards() + curriculum: Gen2FastCurriculumCfg = Gen2FastCurriculumCfg() + + def __post_init__(self): + super().__post_init__() + self.commands.base_velocity.resampling_time_range = (3.0, 5.0) + self.commands.base_velocity.rel_standing_envs = 0.05 + self.commands.base_velocity.rel_straight_envs = 0.9 + self.commands.base_velocity.ranges.lin_vel_x = (0.3, 0.8) + self.commands.base_velocity.ranges.lin_vel_y = (-0.03, 0.03) + self.commands.base_velocity.ranges.ang_vel_z = (-0.15, 0.15) + + +@configclass +class Gen2FastEnvCfg_PLAY(Gen2FastEnvCfg): + """Deterministic replay configuration for the high-speed policy.""" + + def __post_init__(self): + super().__post_init__() + configure_gen2_play_env(self) + self.curriculum.forward_speed = None diff --git a/source/engineai_lab/tasks/velocity/config/gen2/stages/natural.py b/source/engineai_lab/tasks/velocity/config/gen2/stages/natural.py new file mode 100644 index 0000000..85c867e --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/stages/natural.py @@ -0,0 +1,137 @@ +from __future__ import annotations + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from engineai_lab.tasks.velocity import mdp + +from ..common_env_cfg import ( + GEN2_ARM_PHASE_AMPLITUDE, + GEN2_ARM_PHASE_BODY_NAMES, + GEN2_ARM_SWING_JOINT_NAMES, + GEN2_ELBOW_FLEXION, + GEN2_FEET_BODY_NAMES, + GEN2_GAIT_PHASE_DISTANCE, + GEN2_PELVIS_BODY_NAME, + GEN2_PELVIS_HEADING_YAW_OFFSET, + GEN2_SHOULDER_SWING_AMPLITUDE, + GEN2_WRIST_JOINT_NAMES, + configure_gen2_play_env, +) +from .speed import Gen2SpeedEnvCfg, Gen2SpeedRewards + + +# 阶段 3：保持阶段 2 的步态能力，同时学习对侧自然摆臂。 + + +@configclass +class Gen2NaturalRewards(Gen2SpeedRewards): + """Whole-body walking terms with speed-scaled cross-body arm swing.""" + + arm_primary_position = RewTerm( + func=mdp.joint_deviation_exp, + weight=0.3, + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=[".*_arm_J2", ".*_arm_J3"]), + "tolerance": 0.05, + "scale": 4.0, + }, + ) + arm_distal_position = RewTerm( + func=mdp.joint_deviation_l1_with_deadband, + weight=-0.25, + params={ + "asset_cfg": SceneEntityCfg( + "robot", joint_names=GEN2_WRIST_JOINT_NAMES, preserve_order=True + ), + "deadband": 0.03, + }, + ) + cross_body_arm_swing = RewTerm( + func=mdp.cross_body_arm_swing_reward, + weight=0.8, + params={ + "arm_asset_cfg": SceneEntityCfg( + "robot", joint_names=GEN2_ARM_SWING_JOINT_NAMES, preserve_order=True + ), + "feet_asset_cfg": SceneEntityCfg( + "robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True + ), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "min_forward_speed": 0.1, + "full_swing_speed": 0.6, + "min_swing_scale": 0.35, + "phase_distance": GEN2_GAIT_PHASE_DISTANCE, + "shoulder_amplitude": GEN2_SHOULDER_SWING_AMPLITUDE, + "elbow_flexion": GEN2_ELBOW_FLEXION, + # Gen2's left/right J1 axes are mirrored, so equal joint signs create opposite physical swing. + "shoulder_phase_signs": (-1.0, -1.0), + "elbow_flexion_signs": (1.0, -1.0), + "std": 0.12, + }, + ) + contralateral_arm_phase = RewTerm( + func=mdp.contralateral_arm_phase_reward, + weight=0.5, + params={ + "arm_body_cfg": SceneEntityCfg( + "robot", body_names=GEN2_ARM_PHASE_BODY_NAMES, preserve_order=True + ), + "feet_asset_cfg": SceneEntityCfg( + "robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True + ), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "min_forward_speed": 0.1, + "full_swing_speed": 0.6, + "min_swing_scale": 0.35, + "phase_distance": GEN2_GAIT_PHASE_DISTANCE, + "arm_phase_amplitude": GEN2_ARM_PHASE_AMPLITUDE, + "std": 0.045, + }, + ) + arm_swing_velocity = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.001, + params={ + "asset_cfg": SceneEntityCfg( + "robot", joint_names=GEN2_ARM_SWING_JOINT_NAMES, preserve_order=True + ) + }, + ) + wrist_velocity = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.004, + params={ + "asset_cfg": SceneEntityCfg( + "robot", joint_names=GEN2_WRIST_JOINT_NAMES, preserve_order=True + ) + }, + ) + + +@configclass +class Gen2NaturalEnvCfg(Gen2SpeedEnvCfg): + """Stage-three fine-tuning task that adds natural cross-body arm swing.""" + + rewards: Gen2NaturalRewards = Gen2NaturalRewards() + + def __post_init__(self): + super().__post_init__() + for joint_pattern in (".*_arm_J5", ".*_arm_J6", ".*_arm_J7"): + self.actions.joint_pos.scale[joint_pattern] = 0.06 + self.events.reset_robot_joints.params["position_range"] = (0.95, 1.05) + self.events.reset_robot_joints.params["velocity_range"] = (-0.2, 0.2) + + +@configclass +class Gen2NaturalEnvCfg_PLAY(Gen2NaturalEnvCfg): + """Deterministic replay configuration preserving stage-three wrist action scales.""" + + def __post_init__(self): + super().__post_init__() + configure_gen2_play_env(self) diff --git a/source/engineai_lab/tasks/velocity/config/gen2/stages/natural_run.py b/source/engineai_lab/tasks/velocity/config/gen2/stages/natural_run.py new file mode 100644 index 0000000..a4cb570 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/stages/natural_run.py @@ -0,0 +1,223 @@ +from __future__ import annotations + +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from engineai_lab.tasks.velocity import mdp + +from ..common_env_cfg import ( + GEN2_ARM_PHASE_AMPLITUDE, + GEN2_ELBOW_FLEXION, + GEN2_FEET_BODY_NAMES, + GEN2_GAIT_PHASE_DISTANCE, + GEN2_PELVIS_BODY_NAME, + GEN2_PELVIS_HEADING_YAW_OFFSET, + GEN2_SHOULDER_SWING_AMPLITUDE, + configure_gen2_play_env, +) +from .sprint import Gen2SprintEnvCfg, Gen2SprintRewards + + +# 阶段 6：在 Sprint 的速度能力上单独增加肩、肘和前臂协调课程。 + +GEN2_FOREARM_PHASE_BODY_NAMES = ["left_arm_link_5", "right_arm_link_5"] +GEN2_ELBOW_JOINT_NAMES = ["left_arm_J4", "right_arm_J4"] +GEN2_NATURAL_RUN_SHOULDER_AMPLITUDE = 0.24 +GEN2_NATURAL_RUN_UPPER_ARM_PHASE_AMPLITUDE = 0.145 +GEN2_NATURAL_RUN_FOREARM_PHASE_AMPLITUDE = 0.20 +GEN2_NATURAL_RUN_MIN_ELBOW_FLEXION = 0.35 +GEN2_NATURAL_RUN_MAX_ELBOW_FLEXION = 0.75 +GEN2_NATURAL_RUN_MIN_ELBOW_PHASE = 0.05 +GEN2_NATURAL_RUN_MAX_ELBOW_PHASE = 0.12 + +# Match the Sprint elbow target at 1.6 m/s during the initial warm-up. +GEN2_SPRINT_ELBOW_AT_1P6 = GEN2_ELBOW_FLEXION * ( + 0.35 + 0.65 * ((1.6 - 0.1) / (3.0 - 0.1)) +) + + +@configclass +class Gen2NaturalRunRewards(Gen2SprintRewards): + """Running rewards with separately supervised shoulder and forearm motion.""" + + elbow_flexion = RewTerm( + func=mdp.running_elbow_flexion_huber, + weight=-0.25, + params={ + "arm_asset_cfg": SceneEntityCfg( + "robot", joint_names=GEN2_ELBOW_JOINT_NAMES, preserve_order=True + ), + "feet_asset_cfg": SceneEntityCfg( + "robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True + ), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "run_transition_start": 1.6, + "full_run_speed": 3.0, + "min_elbow_flexion": GEN2_SPRINT_ELBOW_AT_1P6, + "max_elbow_flexion": GEN2_ELBOW_FLEXION, + "min_phase_modulation": 0.0, + "max_phase_modulation": 0.0, + "phase_distance": GEN2_GAIT_PHASE_DISTANCE, + "delta": 0.2, + "moving_threshold": 0.1, + }, + ) + contralateral_forearm_phase = RewTerm( + func=mdp.contralateral_arm_phase_reward, + weight=0.0, + params={ + "arm_body_cfg": SceneEntityCfg( + "robot", body_names=GEN2_FOREARM_PHASE_BODY_NAMES, preserve_order=True + ), + "feet_asset_cfg": SceneEntityCfg( + "robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True + ), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "min_forward_speed": 0.1, + "full_swing_speed": 3.0, + "min_swing_scale": 0.35, + "phase_distance": GEN2_GAIT_PHASE_DISTANCE, + "arm_phase_amplitude": GEN2_ARM_PHASE_AMPLITUDE, + "std": 0.06, + }, + ) + + +@configclass +class Gen2NaturalRunCurriculumCfg: + """Reintroduce 3.0 m/s and stronger arm motion in independent stages.""" + + terrain_levels = None + forward_speed = CurrTerm( + func=mdp.forward_speed_range_curriculum, + params={ + "command_name": "base_velocity", + "min_forward_speed": 2.0, + "initial_max_speed": 2.0, + "final_max_speed": 3.0, + "speed_increment": 0.2, + "stage_steps": 200 * 24, + }, + ) + arm_swing_targets = CurrTerm( + func=mdp.running_arm_target_curriculum, + params={ + "warmup_steps": 50 * 24, + "ramp_end_steps": 800 * 24, + "initial_shoulder_amplitude": GEN2_SHOULDER_SWING_AMPLITUDE, + "final_shoulder_amplitude": GEN2_NATURAL_RUN_SHOULDER_AMPLITUDE, + "initial_upper_arm_phase_amplitude": GEN2_ARM_PHASE_AMPLITUDE, + "final_upper_arm_phase_amplitude": GEN2_NATURAL_RUN_UPPER_ARM_PHASE_AMPLITUDE, + "initial_min_elbow_flexion": GEN2_SPRINT_ELBOW_AT_1P6, + "final_min_elbow_flexion": GEN2_NATURAL_RUN_MIN_ELBOW_FLEXION, + "initial_max_elbow_flexion": GEN2_ELBOW_FLEXION, + "final_max_elbow_flexion": GEN2_NATURAL_RUN_MAX_ELBOW_FLEXION, + "initial_min_phase_modulation": 0.0, + "final_min_phase_modulation": GEN2_NATURAL_RUN_MIN_ELBOW_PHASE, + "initial_max_phase_modulation": 0.0, + "final_max_phase_modulation": GEN2_NATURAL_RUN_MAX_ELBOW_PHASE, + "initial_forearm_phase_amplitude": GEN2_ARM_PHASE_AMPLITUDE, + "final_forearm_phase_amplitude": GEN2_NATURAL_RUN_FOREARM_PHASE_AMPLITUDE, + }, + ) + + +@configclass +class Gen2NaturalRunEnvCfg(Gen2SprintEnvCfg): + """Separate running policy with larger, phase-coupled shoulder and forearm motion.""" + + rewards: Gen2NaturalRunRewards = Gen2NaturalRunRewards() + curriculum: Gen2NaturalRunCurriculumCfg = Gen2NaturalRunCurriculumCfg() + + def __post_init__(self): + super().__post_init__() + + command = self.commands.base_velocity + command.resampling_time_range = (10.0, 14.0) + command.rel_standing_envs = 0.03 + command.rel_straight_envs = 0.97 + command.rel_high_speed_envs = 0.60 + command.rel_replay_speed_envs = 0.25 + command.replay_speed_range = (1.6, 2.2) + command.command_ramp_rates = (1.0, 0.5, 1.0) + command.ranges.lin_vel_x = (2.0, 2.0) + command.ranges.lin_vel_y = (-0.02, 0.02) + command.ranges.ang_vel_z = (-0.08, 0.08) + + # Keep the checkpoint's initial targets, then let the arm curriculum increase them. + self.rewards.cross_body_arm_swing.weight = 0.8 + self.rewards.cross_body_arm_swing.params["shoulder_amplitude"] = ( + GEN2_SHOULDER_SWING_AMPLITUDE + ) + self.rewards.cross_body_arm_swing.params["joint_error_weights"] = (1.0, 1.0, 0.0, 0.0) + self.rewards.contralateral_arm_phase.weight = 0.5 + self.rewards.contralateral_arm_phase.params["arm_phase_amplitude"] = ( + GEN2_ARM_PHASE_AMPLITUDE + ) + self.rewards.arm_swing_velocity.weight = -0.0008 + + # Recover some of the stability lost at 3 m/s without suppressing flight. + self.rewards.feet_contact.weight = 0.85 + self.rewards.feet_slide.weight = -1.0 + self.rewards.pelvis_roll_pitch_ang_vel.weight = -0.15 + self.rewards.torso_roll_pitch_ang_vel.weight = -0.15 + self.rewards.termination_penalty.weight = -225.0 + self.rewards.energy_cost.weight = -0.005 + self.rewards.energy_cost.params["start_scale"] = 0.45 + self.rewards.dof_torque.weight = -1.25e-6 + self.rewards.action_rate.params["start_scale"] = 0.55 + self.rewards.action_smoothness.params["start_scale"] = 0.55 + self.rewards.feet_landing_velocity.params["velocity_threshold"] = 0.4 + + +@configclass +class Gen2NaturalRunEnvCfg_PLAY(Gen2NaturalRunEnvCfg): + """Deterministic fixed-speed replay for the separate running policy.""" + + def __post_init__(self): + super().__post_init__() + configure_gen2_play_env(self) + self.curriculum.forward_speed = None + self.curriculum.arm_swing_targets = None + + self.rewards.cross_body_arm_swing.weight = 1.0 + self.rewards.cross_body_arm_swing.params["shoulder_amplitude"] = ( + GEN2_NATURAL_RUN_SHOULDER_AMPLITUDE + ) + self.rewards.contralateral_arm_phase.weight = 0.65 + self.rewards.contralateral_arm_phase.params["arm_phase_amplitude"] = ( + GEN2_NATURAL_RUN_UPPER_ARM_PHASE_AMPLITUDE + ) + self.rewards.elbow_flexion.weight = -0.5 + self.rewards.elbow_flexion.params["min_elbow_flexion"] = ( + GEN2_NATURAL_RUN_MIN_ELBOW_FLEXION + ) + self.rewards.elbow_flexion.params["max_elbow_flexion"] = ( + GEN2_NATURAL_RUN_MAX_ELBOW_FLEXION + ) + self.rewards.elbow_flexion.params["min_phase_modulation"] = ( + GEN2_NATURAL_RUN_MIN_ELBOW_PHASE + ) + self.rewards.elbow_flexion.params["max_phase_modulation"] = ( + GEN2_NATURAL_RUN_MAX_ELBOW_PHASE + ) + self.rewards.contralateral_forearm_phase.weight = 0.3 + self.rewards.contralateral_forearm_phase.params["arm_phase_amplitude"] = ( + GEN2_NATURAL_RUN_FOREARM_PHASE_AMPLITUDE + ) + + command = self.commands.base_velocity + command.rel_standing_envs = 0.0 + command.rel_straight_envs = 1.0 + command.rel_high_speed_envs = 0.0 + command.rel_replay_speed_envs = 0.0 + command.resampling_time_range = (40.0, 40.0) + command.ranges.lin_vel_x = (3.0, 3.0) + command.ranges.lin_vel_y = (0.0, 0.0) + command.ranges.ang_vel_z = (0.0, 0.0) diff --git a/source/engineai_lab/tasks/velocity/config/gen2/stages/speed.py b/source/engineai_lab/tasks/velocity/config/gen2/stages/speed.py new file mode 100644 index 0000000..3603b96 --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/stages/speed.py @@ -0,0 +1,101 @@ +from __future__ import annotations + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from engineai_lab.tasks.velocity import mdp + +from ..common_env_cfg import ( + GEN2_FEET_BODY_NAMES, + GEN2_PELVIS_BODY_NAME, + GEN2_TORSO_BODY_NAME, + configure_gen2_play_env, +) +from .walk import Gen2FlatEnvCfg, Gen2Rewards + + +# 阶段 2：在平地上巩固 0.2~0.6 m/s 速度跟踪和落脚稳定性。 + + +@configclass +class Gen2SpeedRewards(Gen2Rewards): + """Additional damping and landing terms for faster command tracking.""" + + feet_slide = RewTerm( + func=mdp.feet_slide, + weight=-0.7, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + }, + ) + + pelvis_vertical_velocity = RewTerm( + func=mdp.body_vertical_velocity_l2, + weight=-2.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "deadband": 0.05, + }, + ) + pelvis_roll_pitch_ang_vel = RewTerm( + func=mdp.body_roll_pitch_ang_vel_l2, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "deadband": 0.1, + }, + ) + torso_roll_pitch_ang_vel = RewTerm( + func=mdp.body_roll_pitch_ang_vel_l2, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), + "deadband": 0.1, + }, + ) + feet_landing_velocity = RewTerm( + func=mdp.feet_landing_velocity, + weight=-1.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True), + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True), + "velocity_threshold": 0.2, + "power": 2.0, + }, + ) + + +@configclass +class Gen2SpeedEnvCfg(Gen2FlatEnvCfg): + """Stage-two flat-ground consolidation task for faster and steadier tracking.""" + + rewards: Gen2SpeedRewards = Gen2SpeedRewards() + + def __post_init__(self): + super().__post_init__() + + self.commands.base_velocity.resampling_time_range = (3.0, 5.0) + self.commands.base_velocity.rel_standing_envs = 0.1 + self.commands.base_velocity.rel_straight_envs = 0.8 + self.commands.base_velocity.ranges.lin_vel_x = (0.2, 0.6) + self.commands.base_velocity.ranges.lin_vel_y = (-0.05, 0.05) + self.commands.base_velocity.ranges.ang_vel_z = (-0.2, 0.2) + self.rewards.pelvis_track_lin_vel_xy_exp.weight = 2.5 + + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + self.scene.terrain.max_init_terrain_level = None + self.scene.height_scanner = None + self.curriculum.terrain_levels = None + self.events.push_robot = None + + +@configclass +class Gen2SpeedEnvCfg_PLAY(Gen2SpeedEnvCfg): + """阶段 2 的确定性回放配置。""" + + def __post_init__(self): + super().__post_init__() + configure_gen2_play_env(self) diff --git a/source/engineai_lab/tasks/velocity/config/gen2/stages/sprint.py b/source/engineai_lab/tasks/velocity/config/gen2/stages/sprint.py new file mode 100644 index 0000000..2ddfbdf --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/stages/sprint.py @@ -0,0 +1,198 @@ +from __future__ import annotations + +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from engineai_lab.tasks.velocity import mdp + +from ..common_env_cfg import ( + GEN2_FEET_BODY_NAMES, + GEN2_PELVIS_BODY_NAME, + GEN2_PELVIS_HEADING_YAW_OFFSET, + configure_gen2_play_env, +) +from .fast import Gen2FastCurriculumCfg, Gen2FastEnvCfg, Gen2FastRewards + + +# 阶段 5：由 Fast checkpoint 迁移，建立 1.6~3.0 m/s 的跑步能力。 + + +@configclass +class Gen2SprintRewards(Gen2FastRewards): + """Running-oriented rewards for the 1.6 to 3.0 m/s curriculum.""" + + pelvis_track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp_body, + weight=3.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "sigma": 2.5, + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + }, + ) + pelvis_track_lin_vel_xy_wide_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp_body, + weight=1.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "sigma": 0.5, + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + }, + ) + forward_speed_undertracking = RewTerm( + func=mdp.body_forward_velocity_undertracking_huber, + weight=-1.25, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "min_command_speed": 0.2, + "delta": 0.25, + }, + ) + feet_contact = RewTerm( + func=mdp.speed_conditioned_biped_contact_flight, + weight=0.75, + params={ + "sensor_cfg": SceneEntityCfg( + "contact_forces", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True + ), + "command_name": "base_velocity", + "run_transition_start": 1.6, + "run_transition_end": 2.4, + "max_flight_time": 0.18, + "min_swing_lead_time": 0.05, + "invalid_flight_penalty": 1.0, + "excess_flight_penalty": 2.0, + "double_support_penalty": 0.25, + "linear_threshold": 0.1, + "angular_threshold": 0.1, + }, + ) + + # The former air-time terms can reward single-foot pogo and suppress flight. + feet_air_time = None + feet_air_time_dense = None + + pelvis_vertical_velocity = RewTerm( + func=mdp.body_vertical_velocity_l2, + weight=-0.6, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "deadband": 0.2, + }, + ) + feet_landing_velocity = RewTerm( + func=mdp.feet_landing_velocity, + weight=-0.8, + params={ + "asset_cfg": SceneEntityCfg( + "robot", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True + ), + "sensor_cfg": SceneEntityCfg( + "contact_forces", body_names=GEN2_FEET_BODY_NAMES, preserve_order=True + ), + "velocity_threshold": 0.45, + "power": 2.0, + }, + ) + swing_foot_clearance = RewTerm( + func=mdp.swing_foot_clearance_reward, + weight=1.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "command_name": "base_velocity", + "target_height": 0.07, + "std": 0.055, + "force_threshold": 5.0, + "linear_threshold": 0.1, + "angular_threshold": 0.1, + }, + ) + + +@configclass +class Gen2SprintCurriculumCfg(Gen2FastCurriculumCfg): + """Raise the command ceiling by 0.2 m/s every 250 PPO iterations.""" + + terrain_levels = None + forward_speed = CurrTerm( + func=mdp.forward_speed_range_curriculum, + params={ + "command_name": "base_velocity", + "min_forward_speed": 1.4, + "initial_max_speed": 1.6, + "final_max_speed": 3.0, + "speed_increment": 0.2, + "stage_steps": 250 * 24, + }, + ) + + +@configclass +class Gen2SprintEnvCfg(Gen2FastEnvCfg): + """Progressive flat-ground running task targeting 3.0 m/s.""" + + rewards: Gen2SprintRewards = Gen2SprintRewards() + curriculum: Gen2SprintCurriculumCfg = Gen2SprintCurriculumCfg() + + def __post_init__(self): + super().__post_init__() + + command = self.commands.base_velocity + command.resampling_time_range = (8.0, 12.0) + command.rel_standing_envs = 0.05 + command.rel_straight_envs = 0.95 + command.rel_high_speed_envs = 0.65 + command.high_speed_bandwidth = 0.3 + command.rel_replay_speed_envs = 0.25 + command.replay_speed_range = (0.3, 1.6) + command.command_ramp_rates = (1.0, 0.5, 1.5) + command.ranges.lin_vel_x = (1.4, 1.6) + command.ranges.lin_vel_y = (-0.02, 0.02) + command.ranges.ang_vel_z = (-0.1, 0.1) + + # Gen2SpeedEnvCfg overwrites this term, so restore Sprint values last. + self.rewards.pelvis_track_lin_vel_xy_exp.weight = 3.0 + self.rewards.pelvis_track_lin_vel_xy_exp.params["sigma"] = 2.5 + + self.rewards.cross_body_arm_swing.params["full_swing_speed"] = 3.0 + self.rewards.contralateral_arm_phase.params["full_swing_speed"] = 3.0 + self.rewards.action_rate.params["start_scale"] = 0.25 + self.rewards.action_smoothness.params["start_scale"] = 0.25 + self.rewards.energy_cost.params["start_scale"] = 0.15 + + self.events.physics_material.params["static_friction_range"] = (0.8, 1.2) + self.events.physics_material.params["dynamic_friction_range"] = (0.8, 1.2) + self.events.physics_material.params["restitution_range"] = (0.0, 0.1) + self.events.base_com.params["com_range"] = { + "x": (-0.02, 0.02), + "y": (-0.02, 0.02), + "z": (-0.02, 0.02), + } + self.events.push_robot = None + + +@configclass +class Gen2SprintEnvCfg_PLAY(Gen2SprintEnvCfg): + """Deterministic replay configuration for the Sprint policy.""" + + def __post_init__(self): + super().__post_init__() + configure_gen2_play_env(self) + self.curriculum.forward_speed = None + + command = self.commands.base_velocity + command.rel_standing_envs = 0.0 + command.rel_straight_envs = 1.0 + command.rel_high_speed_envs = 0.0 + command.rel_replay_speed_envs = 0.0 + command.resampling_time_range = (40.0, 40.0) + command.ranges.lin_vel_x = (3.0, 3.0) + command.ranges.lin_vel_y = (0.0, 0.0) + command.ranges.ang_vel_z = (0.0, 0.0) diff --git a/source/engineai_lab/tasks/velocity/config/gen2/stages/walk.py b/source/engineai_lab/tasks/velocity/config/gen2/stages/walk.py new file mode 100644 index 0000000..324180d --- /dev/null +++ b/source/engineai_lab/tasks/velocity/config/gen2/stages/walk.py @@ -0,0 +1,338 @@ +from __future__ import annotations + +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from engineai_lab.tasks.velocity import mdp + +from ..common_env_cfg import ( + ActionsCfg, + GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME, + GEN2_FEET_BODY_NAMES, + GEN2_FOOT_SOLE_FRAME_OFFSETS_RPY, + GEN2_PELVIS_BODY_NAME, + GEN2_PELVIS_HEADING_YAW_OFFSET, + GEN2_TORSO_BODY_NAME, + GEN2_TORSO_HEIGHT_TARGET, + GEN2_PELVIS_HEIGHT_TARGET, + Gen2EventCfg, + Gen2SceneCfg, + Gen2Termination, + ObservationsCfg, + configure_gen2_play_env, +) + + +# 阶段 1：基础低速行走。 +# 该任务包含 20% 的站立指令，但不是独立的“纯站立”策略。 + + +@configclass +class Gen2Rewards: + """Reward terms adapted from PM01, with Gen2 body names and geometry.""" + + pelvis_track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp_body, + weight=2.0, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "sigma": 5, + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + }, + ) + whole_body_track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_world_exp_bodies, + weight=2.5, + params={ + "asset_cfg": SceneEntityCfg( + "robot", body_names=[GEN2_PELVIS_BODY_NAME, GEN2_TORSO_BODY_NAME], preserve_order=True + ), + "command_name": "base_velocity", + "sigma": 5, + }, + ) + torso_orientation = RewTerm( + func=mdp.body_orientation, + weight=0.8, + params={"asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), "scale": 10.0}, + ) + pelvis_height = RewTerm( + func=mdp.body_height_tracking, + weight=0.4, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "target_height": GEN2_PELVIS_HEIGHT_TARGET, + "scale": 15.0, + }, + ) + torso_height = RewTerm( + func=mdp.body_height_tracking, + weight=0.1, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), + "target_height": GEN2_TORSO_HEIGHT_TARGET, + "scale": 15.0, + }, + ) + foot_position = RewTerm( + func=mdp.feet_position_relative_to_body, + weight=0.5, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "stand_threshold": 0.1, + "desired_foot_positions": GEN2_DEFAULT_FOOT_POS_PELVIS_FRAME, + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + }, + ) + feet_orientation = RewTerm( + func=mdp.feet_orientation_relative_to_body, + weight=0.25, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "command_name": "base_velocity", + "stand_threshold": 0.1, + "heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "foot_frame_offsets_rpy": GEN2_FOOT_SOLE_FRAME_OFFSETS_RPY, + }, + ) + torso_pelvis_yaw_alignment = RewTerm( + func=mdp.body_yaw_alignment, + weight=0.3, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + "reference_heading_yaw_offset": GEN2_PELVIS_HEADING_YAW_OFFSET, + "scale": 4.0, + }, + ) + torso_pelvis_yaw_rate = RewTerm( + func=mdp.body_yaw_rate_difference_l2, + weight=-0.5, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_TORSO_BODY_NAME), + "reference_body_cfg": SceneEntityCfg("robot", body_names=GEN2_PELVIS_BODY_NAME), + }, + ) + waist_pos = RewTerm( + func=mdp.joint_deviation_exp, + weight=0.45, + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=["waist_J1", "waist_J2"]), + "scale": 3.0, + "tolerance": 0.0, + }, + ) + waist_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.02, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["waist_J1", "waist_J2"])}, + ) + leg_joint_position = RewTerm( + func=mdp.joint_deviation_exp, + weight=0.3, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[".*_leg_J2", ".*_leg_J3", ".*_leg_J6"], + ), + "scale": 3.0, + }, + ) + arm_primary_position = RewTerm( + func=mdp.joint_deviation_exp, + weight=0.3, + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=[".*_arm_J1", ".*_arm_J2", ".*_arm_J3", ".*_arm_J4"]), + "scale": 3.0, + }, + ) + arm_distal_position = RewTerm( + func=mdp.joint_deviation_exp, + weight=0.3, + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=[".*_arm_J5", ".*_arm_J6", ".*_arm_J7"]), + "scale": 8.0, + }, + ) + feet_contact = RewTerm( + func=mdp.biped_contact_mode_reward, + weight=0.75, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "command_name": "base_velocity", + "force_threshold": 5.0, + "linear_threshold": 0.1, + "angular_threshold": 0.1, + }, + ) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_on_contact, + weight=2.0, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "min_air_time": 0.05, + "max_air_time": 0.25, + "linear_threshold": 0.1, + "angular_threshold": 0.1, + }, + ) + feet_air_time_dense = RewTerm( + func=mdp.feet_air_time_positive_biped, + weight=2.0, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "threshold": 0.25, + "linear_threshold": 0.1, + "angular_threshold": 0.1, + }, + ) + swing_foot_clearance = RewTerm( + func=mdp.swing_foot_clearance_reward, + weight=0.75, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "command_name": "base_velocity", + "target_height": 0.04, + "std": 0.05, + "force_threshold": 5.0, + "linear_threshold": 0.1, + "angular_threshold": 0.1, + }, + ) + foot_stumble = RewTerm( + func=mdp.feet_stumble, + weight=-1.0, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "tangential_threshold": 2.0, + "normal_threshold": 1.0, + }, + ) + dof_pos_limits = RewTerm( + func=mdp.joint_pos_limits, + weight=-10.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")}, + ) + energy_cost = RewTerm( + func=mdp.energy_cost_with_curriculum, + weight=-0.004, + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=[".*"]), + "start_scale": 0.1, + "power": 0.8, + "interval_epochs": 200 * 24, + }, + ) + feet_slide = RewTerm( + func=mdp.feet_slide, + weight=-0.4, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=GEN2_FEET_BODY_NAMES), + "asset_cfg": SceneEntityCfg("robot", body_names=GEN2_FEET_BODY_NAMES), + }, + ) + dof_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-1.0e-5, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, + ) + dof_acc = RewTerm( + func=mdp.joint_acc_l2, + weight=-1.25e-8, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, + ) + action_rate = RewTerm( + func=mdp.action_rate_with_curriculum, + weight=-0.06, + params={"start_scale": 0.1, "power": 0.8, "interval_epochs": 200 * 24}, + ) + action_smoothness = RewTerm( + func=mdp.action_smoothness_with_curriculum, + weight=-0.04, + params={"start_scale": 0.1, "power": 0.8, "interval_epochs": 200 * 24}, + ) + dof_torque = RewTerm( + func=mdp.joint_torques_l2, + weight=-1.0e-6, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, + ) + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + + +@configclass +class Gen2Commands: + """Conservative velocity commands for the first Gen2 walking stage.""" + + base_velocity = mdp.BodyVelocityCommandCfg( + asset_name="robot", + body_name=GEN2_PELVIS_BODY_NAME, + heading_yaw_offset=GEN2_PELVIS_HEADING_YAW_OFFSET, + resampling_time_range=(7.5, 7.5), + rel_standing_envs=0.2, + rel_heading_envs=0.0, + heading_command=False, + heading_control_stiffness=0.5, + debug_vis=False, + ranges=mdp.BodyVelocityCommandCfg.Ranges( + lin_vel_x=(-0.2, 0.4), + lin_vel_y=(-0.2, 0.2), + ang_vel_z=(-0.5, 0.5), + heading=None, + ), + ) + +@configclass +class CurriculumCfg: + terrain_levels = CurrTerm(func=mdp.terrain_levels_vel) + +@configclass +class Gen2FlatEnvCfg(ManagerBasedRLEnvCfg): + """Velocity-tracking RL environment for Gen2.""" + + scene: Gen2SceneCfg = Gen2SceneCfg(num_envs=4096, env_spacing=3.0) + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: Gen2Commands = Gen2Commands() + rewards: Gen2Rewards = Gen2Rewards() + terminations: Gen2Termination = Gen2Termination() + events: Gen2EventCfg = Gen2EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + self.decimation = 5 + self.episode_length_s = 20.0 + self.sim.dt = 0.002 + self.sim.render_interval = self.decimation + self.sim.physics_material = self.scene.terrain.physics_material + self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15 + if self.scene.height_scanner is not None: + self.scene.height_scanner.update_period = self.decimation * self.sim.dt + if self.scene.contact_forces is not None: + self.scene.contact_forces.update_period = 0.005 + + if getattr(self.curriculum, "terrain_levels", None) is not None: + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.curriculum = True + else: + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.curriculum = False + + +@configclass +class Gen2FlatEnvCfg_PLAY(Gen2FlatEnvCfg): + """Deterministic flat-ground configuration for policy replay.""" + + def __post_init__(self): + super().__post_init__() + configure_gen2_play_env(self) + diff --git a/source/engineai_lab/tasks/velocity/config/pm01/agents/rsl_rl_ppo_cfg.py b/source/engineai_lab/tasks/velocity/config/pm01/agents/rsl_rl_ppo_cfg.py index 6375569..db705e5 100644 --- a/source/engineai_lab/tasks/velocity/config/pm01/agents/rsl_rl_ppo_cfg.py +++ b/source/engineai_lab/tasks/velocity/config/pm01/agents/rsl_rl_ppo_cfg.py @@ -1,59 +1,16 @@ +"""PM01 的 RSL-RL 配置。""" + from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg,RslRlMLPModelCfg,RslRlPpoAlgorithmCfg +from engineai_lab.tasks.velocity.config.common.rsl_rl_ppo_cfg import BasePPORunnerCfg @configclass -class PM01BasePPORunnerCfg(RslRlOnPolicyRunnerCfg): - num_steps_per_env = 24 - max_iterations = 3000 - save_interval = 50 - experiment_name = "velocity_flat_terrain" - obs_groups = {"actor": ["policy"], "critic": ["policy"]} - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.2, - entropy_coef=0.008, - num_learning_epochs=5, - num_mini_batches=4, - learning_rate=1.0e-3, - schedule="adaptive", - gamma=0.99, - lam=0.95, - desired_kl=0.01, - max_grad_norm=1.0, - ) - actor = RslRlMLPModelCfg( - hidden_dims=[512, 256, 128], - activation="elu", - obs_normalization=True, - distribution_cfg= - RslRlMLPModelCfg.GaussianDistributionCfg( - init_std=1.0, - std_type="scalar" - ) - ) - critic = RslRlMLPModelCfg( - hidden_dims=[512, 256, 128], - activation="elu", - obs_normalization=True, - ) +class PM01BasePPORunnerCfg(BasePPORunnerCfg): + """旧名称兼容层；通用网络与 PPO 参数现在位于 config/common。""" + pass - def __post_init__(self): - super().__post_init__() - - # 2. deprecated key words for rsl-rl >= 5.0.0 - deprecated_keys = {"stochastic", "init_noise_std", "noise_std_type", "state_dependent_std"} - - def _remove_deprecated_keys(cfg_obj): - if cfg_obj is None: - return None - return {k: v for k, v in vars(cfg_obj).items() if k not in deprecated_keys} - - self.actor = _remove_deprecated_keys(self.actor) - self.critic = _remove_deprecated_keys(self.critic) @configclass class PM01FlatPPORunnerCfg(PM01BasePPORunnerCfg): diff --git a/source/engineai_lab/tasks/velocity/config/pm01/flat_env_cfg.py b/source/engineai_lab/tasks/velocity/config/pm01/flat_env_cfg.py index 35e1af1..b6b2c09 100644 --- a/source/engineai_lab/tasks/velocity/config/pm01/flat_env_cfg.py +++ b/source/engineai_lab/tasks/velocity/config/pm01/flat_env_cfg.py @@ -22,11 +22,8 @@ from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise from engineai_lab.tasks.velocity import mdp from engineai_lab.robots.pm01 import PM01_CFG, PM_WAIST_DFS_JOINT_NAMES, PM01_DFS_JOINT_ORDER_ASSET_CFG -import isaaclab.terrains as terrain_gen -import math -from isaaclab.terrains.terrain_generator_cfg import TerrainGeneratorCfg - from engineai_lab.robots.actuator import DelayedImplicitActuatorCfg +from engineai_lab.tasks.velocity.config.common import ROUGH_TERRAIN_GENERATOR_CFG ACTUATOR_DELAY_RANGE = (2, 8) @@ -51,48 +48,8 @@ def _build_delayed_actuators(): return delayed_actuators -terrain_generator=TerrainGeneratorCfg( - size=(8.0, 8.0), - horizontal_scale=0.1, - vertical_scale=0.005, - border_width=25.0, - num_rows=10, - num_cols=20, - curriculum=True, - difficulty_range=(0.0, 1.0), - color_scheme="height", - slope_threshold=0.75, - sub_terrains={ - "flat": terrain_gen.HfPyramidSlopedTerrainCfg( - proportion=0.4, - slope_range=(0.0, 0.0), - platform_width=8.0, - ), - "slope_up": terrain_gen.HfPyramidSlopedTerrainCfg( - proportion=0.1, - slope_range=(0.0, math.radians(5)), - platform_width=2.0, - ), - "slope_down": terrain_gen.HfInvertedPyramidSlopedTerrainCfg( - proportion=0.1, - slope_range=(0.0, math.radians(5)), - platform_width=2.0, - ), - "obstacles": terrain_gen.HfDiscreteObstaclesTerrainCfg( - proportion=0.2, - obstacle_width_range=(1.0, 2.0), - obstacle_height_range=(0.01, 0.1), - num_obstacles=15, - platform_width=3.0, - ), - "rough_terrain": terrain_gen.HfRandomUniformTerrainCfg( - proportion=0.2, - noise_range=(-0.015, 0.015), - noise_step=0.005, - downsampled_scale=0.15, - ), - }, - ) +# 保留旧变量名，避免外部代码导入 ``terrain_generator`` 时失效。 +terrain_generator = ROUGH_TERRAIN_GENERATOR_CFG @configclass class PM01SceneCfg(InteractiveSceneCfg): diff --git a/source/engineai_lab/tasks/velocity/mdp/commands.py b/source/engineai_lab/tasks/velocity/mdp/commands.py index 5440216..85aa5ac 100644 --- a/source/engineai_lab/tasks/velocity/mdp/commands.py +++ b/source/engineai_lab/tasks/velocity/mdp/commands.py @@ -18,6 +18,25 @@ class BodyVelocityCommand(UniformVelocityCommand): def __init__(self, cfg: BodyVelocityCommandCfg, env): if not 0.0 <= cfg.rel_straight_envs <= 1.0: raise ValueError(f"rel_straight_envs must be in [0, 1], got {cfg.rel_straight_envs}.") + if not 0.0 <= cfg.rel_high_speed_envs <= 1.0: + raise ValueError(f"rel_high_speed_envs must be in [0, 1], got {cfg.rel_high_speed_envs}.") + if not 0.0 <= cfg.rel_replay_speed_envs <= 1.0: + raise ValueError(f"rel_replay_speed_envs must be in [0, 1], got {cfg.rel_replay_speed_envs}.") + if cfg.rel_high_speed_envs + cfg.rel_replay_speed_envs > 1.0: + raise ValueError("rel_high_speed_envs + rel_replay_speed_envs must not exceed 1.") + if cfg.rel_high_speed_envs > 0.0 and cfg.high_speed_bandwidth <= 0.0: + raise ValueError("high_speed_bandwidth must be positive when high-speed sampling is enabled.") + if cfg.rel_replay_speed_envs > 0.0: + if cfg.replay_speed_range is None: + raise ValueError("replay_speed_range is required when replay-speed sampling is enabled.") + if cfg.replay_speed_range[0] > cfg.replay_speed_range[1]: + raise ValueError("replay_speed_range must be ordered from low to high.") + if cfg.command_ramp_rates is not None: + if len(cfg.command_ramp_rates) != 3: + raise ValueError("command_ramp_rates must contain vx, vy, and yaw rates.") + if any(rate <= 0.0 for rate in cfg.command_ramp_rates): + raise ValueError("All command_ramp_rates must be positive.") + super().__init__(cfg, env) body_ids, body_names = self.robot.find_bodies(cfg.body_name, preserve_order=True) if len(body_ids) != 1: @@ -27,11 +46,65 @@ class BodyVelocityCommand(UniformVelocityCommand): ) self.body_id = body_ids[0] + self._command_ramp_rates = None + self._ramped_vel_command_b = None + if cfg.command_ramp_rates is not None: + self._command_ramp_rates = torch.tensor( + cfg.command_ramp_rates, device=self.device, dtype=self.vel_command_b.dtype + ).view(1, 3) + self._ramped_vel_command_b = torch.zeros_like(self.vel_command_b) + + @property + def command(self) -> torch.Tensor: + """Velocity command exposed to observations and rewards.""" + ramped_command = getattr(self, "_ramped_vel_command_b", None) + return self.vel_command_b if ramped_command is None else ramped_command + + def _resolve_env_ids(self, env_ids: Sequence[int] | slice | None) -> torch.Tensor: + if env_ids is None: + return torch.arange(self.num_envs, device=self.device) + if isinstance(env_ids, slice): + return torch.arange(self.num_envs, device=self.device)[env_ids] + return torch.as_tensor(env_ids, device=self.device, dtype=torch.long) + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + """Reset raw targets and restart enabled command ramps from zero.""" + resolved_env_ids = self._resolve_env_ids(env_ids) + extras = super().reset(resolved_env_ids) + if self._ramped_vel_command_b is not None: + self._ramped_vel_command_b[resolved_env_ids] = 0.0 + return extras + def _resample_command(self, env_ids: Sequence[int]): - """Sample commands, with a configurable fraction reserved for straight walking.""" - env_ids = torch.as_tensor(env_ids, device=self.device, dtype=torch.long) + """Sample uniform, high-speed, and old-speed replay command buckets.""" + env_ids = self._resolve_env_ids(env_ids) super()._resample_command(env_ids) + speed_bucket_probability = self.cfg.rel_high_speed_envs + self.cfg.rel_replay_speed_envs + if speed_bucket_probability > 0.0 and env_ids.numel() > 0: + moving_env_ids = env_ids[~self.is_standing_env[env_ids]] + if moving_env_ids.numel() > 0: + selector = torch.rand(moving_env_ids.numel(), device=self.device) + + high_mask = selector < self.cfg.rel_high_speed_envs + high_env_ids = moving_env_ids[high_mask] + if high_env_ids.numel() > 0: + current_low, current_high = self.cfg.ranges.lin_vel_x + high_low = max(current_low, current_high - self.cfg.high_speed_bandwidth) + high_samples = torch.empty(high_env_ids.numel(), device=self.device) + self.vel_command_b[high_env_ids, 0] = high_samples.uniform_(high_low, current_high) + + replay_mask = (selector >= self.cfg.rel_high_speed_envs) & ( + selector < speed_bucket_probability + ) + replay_env_ids = moving_env_ids[replay_mask] + if replay_env_ids.numel() > 0: + replay_low, configured_replay_high = self.cfg.replay_speed_range + replay_high = min(configured_replay_high, self.cfg.ranges.lin_vel_x[1]) + if replay_low <= replay_high: + replay_samples = torch.empty(replay_env_ids.numel(), device=self.device) + self.vel_command_b[replay_env_ids, 0] = replay_samples.uniform_(replay_low, replay_high) + if self.cfg.rel_straight_envs <= 0.0 or env_ids.numel() == 0: return @@ -55,11 +128,12 @@ class BodyVelocityCommand(UniformVelocityCommand): max_command_step = max_command_time / self._env.step_dt body_lin_vel_heading = self._body_lin_vel_heading() body_yaw_rate = self.robot.data.body_ang_vel_w[:, self.body_id, 2] + command = self.command self.metrics["error_vel_xy"] += ( - torch.norm(self.vel_command_b[:, :2] - body_lin_vel_heading[:, :2], dim=-1) / max_command_step + torch.norm(command[:, :2] - body_lin_vel_heading[:, :2], dim=-1) / max_command_step ) self.metrics["error_vel_yaw"] += ( - torch.abs(self.vel_command_b[:, 2] - body_yaw_rate) / max_command_step + torch.abs(command[:, 2] - body_yaw_rate) / max_command_step ) def _update_command(self): @@ -79,6 +153,11 @@ class BodyVelocityCommand(UniformVelocityCommand): standing_env_ids = self.is_standing_env.nonzero(as_tuple=False).flatten() self.vel_command_b[standing_env_ids, :] = 0.0 + if self._ramped_vel_command_b is not None: + max_delta = self._command_ramp_rates * self._env.step_dt + delta = self.vel_command_b - self._ramped_vel_command_b + self._ramped_vel_command_b += torch.maximum(torch.minimum(delta, max_delta), -max_delta) + def _debug_vis_callback(self, _event): if not self.robot.is_initialized: return @@ -106,6 +185,30 @@ class BodyVelocityCommand(UniformVelocityCommand): return arrow_scale, arrow_quat +def forward_speed_range_curriculum( + env, + env_ids: Sequence[int], + command_name: str, + min_forward_speed: float, + initial_max_speed: float, + final_max_speed: float, + speed_increment: float, + stage_steps: int, +) -> dict[str, float]: + """Increase the sampled forward-speed ceiling in fixed training stages.""" + del env_ids + if not min_forward_speed <= initial_max_speed <= final_max_speed: + raise ValueError("Expected min_forward_speed <= initial_max_speed <= final_max_speed.") + if speed_increment <= 0.0 or stage_steps <= 0: + raise ValueError("speed_increment and stage_steps must be positive.") + + stage = env.common_step_counter // stage_steps + max_forward_speed = min(initial_max_speed + stage * speed_increment, final_max_speed) + command_term = env.command_manager.get_term(command_name) + command_term.cfg.ranges.lin_vel_x = (min_forward_speed, max_forward_speed) + return {"stage": float(stage), "max_forward_speed": float(max_forward_speed)} + + @configclass class BodyVelocityCommandCfg(UniformVelocityCommandCfg): """Configuration for body-referenced planar velocity commands.""" @@ -121,6 +224,21 @@ class BodyVelocityCommandCfg(UniformVelocityCommandCfg): rel_straight_envs: float = 0.0 """Fraction of sampled environments with zero lateral and yaw commands.""" + rel_high_speed_envs: float = 0.0 + """Fraction of moving environments sampled from the upper forward-speed band.""" + + high_speed_bandwidth: float = 0.3 + """Width of the upper forward-speed sampling band in m/s.""" + + rel_replay_speed_envs: float = 0.0 + """Fraction of moving environments sampled from the old-speed replay range.""" + + replay_speed_range: tuple[float, float] | None = None + """Independent forward-speed range retained to prevent low-speed forgetting.""" + + command_ramp_rates: tuple[float, float, float] | None = None + """Per-axis vx, vy, and yaw slew rates in m/s^2, m/s^2, and rad/s^2.""" + marker_height_offset: float = 0.35 """Vertical marker offset from the configured body origin in meters.""" diff --git a/source/engineai_lab/tasks/velocity/mdp/rewards.py b/source/engineai_lab/tasks/velocity/mdp/rewards.py index 23df72b..e9f3990 100644 --- a/source/engineai_lab/tasks/velocity/mdp/rewards.py +++ b/source/engineai_lab/tasks/velocity/mdp/rewards.py @@ -395,6 +395,78 @@ def biped_contact_mode_reward( return torch.where(moving, contact_count == 1, contact_count == 2).float() +def speed_conditioned_biped_contact_flight( + env: ManagerBasedRLEnv, + sensor_cfg: SceneEntityCfg, + command_name: str, + run_transition_start: float = 1.6, + run_transition_end: float = 2.4, + max_flight_time: float = 0.18, + min_swing_lead_time: float = 0.05, + invalid_flight_penalty: float = 1.0, + excess_flight_penalty: float = 2.0, + double_support_penalty: float = 0.25, + linear_threshold: float = 0.1, + angular_threshold: float = 0.1, +) -> torch.Tensor: + """Blend walking contact rules into a conservative running flight phase. + + A short flight phase becomes neutral at high speed, but is never rewarded every + frame. This avoids suppressing a true running gait without incentivizing jumping. + """ + if run_transition_end <= run_transition_start: + raise ValueError("run_transition_end must exceed run_transition_start.") + if max_flight_time <= 0.0: + raise ValueError("max_flight_time must be positive.") + + body_ids = sensor_cfg.body_ids + if body_ids is None or isinstance(body_ids, (int, slice)) or len(body_ids) != 2: + raise ValueError("speed_conditioned_biped_contact_flight requires two ordered foot body ids.") + + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + if not contact_sensor.cfg.track_air_time: + raise RuntimeError("ContactSensorCfg.track_air_time must be enabled for running contact rewards.") + + air_time = _to_env_device(env, contact_sensor.data.current_air_time[:, body_ids]) + contact_time = _to_env_device(env, contact_sensor.data.current_contact_time[:, body_ids]) + in_contact = contact_time > 0.0 + contact_count = torch.sum(in_contact.int(), dim=1) + single_support = contact_count == 1 + double_support = contact_count == 2 + in_flight = contact_count == 0 + + commands = env.command_manager.get_command(command_name) + moving = (torch.linalg.norm(commands[:, :2], dim=1) > linear_threshold) | ( + torch.abs(commands[:, 2]) > angular_threshold + ) + run_mix = torch.clamp( + (commands[:, 0] - run_transition_start) / (run_transition_end - run_transition_start), + min=0.0, + max=1.0, + ) + + # min() measures the time for which both feet have simultaneously been airborne. + flight_time = torch.min(air_time, dim=1).values + swing_lead_time = torch.abs(air_time[:, 0] - air_time[:, 1]) + valid_short_flight = ( + in_flight & (flight_time <= max_flight_time) & (swing_lead_time >= min_swing_lead_time) + ) + + # Walking penalizes any flight; running makes only short alternating flight neutral. + flight_invalidity = in_flight.float() * (1.0 - run_mix) + flight_invalidity += (in_flight & ~valid_short_flight).float() * run_mix + excess_ratio = torch.clamp( + (flight_time - max_flight_time) / max_flight_time, min=0.0, max=2.0 + ) + + moving_reward = single_support.float() + moving_reward -= invalid_flight_penalty * flight_invalidity + moving_reward -= excess_flight_penalty * excess_ratio * in_flight.float() + moving_reward -= double_support_penalty * run_mix * double_support.float() + + return torch.where(moving, moving_reward, double_support.float()) + + def swing_foot_clearance_reward( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, @@ -658,6 +730,54 @@ def body_roll_pitch_ang_vel_l2( return torch.square(torch.clamp(horizontal_ang_speed - deadband, min=0.0)) +def body_forward_velocity_undertracking_l1( + env, + command_name: str, + asset_cfg: SceneEntityCfg, + heading_yaw_offset: float = 0.0, + min_command_speed: float = 0.1, +) -> torch.Tensor: + """Penalize only forward-speed shortfall to retain a dense signal at high commands.""" + body_id = _single_body_id(asset_cfg, "body_forward_velocity_undertracking_l1") + asset = env.scene[asset_cfg.name] + commands = env.command_manager.get_command(command_name) + body_quat_w = asset.data.body_quat_w[:, body_id, :] + body_lin_vel_w = asset.data.body_lin_vel_w[:, body_id, :] + body_lin_vel_heading = quat_apply_inverse( + _heading_quat_with_offset(body_quat_w, heading_yaw_offset), body_lin_vel_w + ) + speed_shortfall = torch.clamp(commands[:, 0] - body_lin_vel_heading[:, 0], min=0.0) + return speed_shortfall * (commands[:, 0] >= min_command_speed) + + +def body_forward_velocity_undertracking_huber( + env, + command_name: str, + asset_cfg: SceneEntityCfg, + heading_yaw_offset: float = 0.0, + min_command_speed: float = 0.1, + delta: float = 0.25, +) -> torch.Tensor: + """Penalize forward-speed shortfall with a normalized smooth-L1 loss.""" + if delta <= 0.0: + raise ValueError("delta must be positive.") + + body_id = _single_body_id(asset_cfg, "body_forward_velocity_undertracking_huber") + asset = env.scene[asset_cfg.name] + commands = env.command_manager.get_command(command_name) + body_lin_vel_heading = quat_apply_inverse( + _heading_quat_with_offset(asset.data.body_quat_w[:, body_id, :], heading_yaw_offset), + asset.data.body_lin_vel_w[:, body_id, :], + ) + speed_shortfall = torch.clamp(commands[:, 0] - body_lin_vel_heading[:, 0], min=0.0) + loss = torch.where( + speed_shortfall <= delta, + 0.5 * torch.square(speed_shortfall) / delta, + speed_shortfall - 0.5 * delta, + ) + return loss * (commands[:, 0] >= min_command_speed) + + def joint_deviation_l1_with_deadband( env, asset_cfg: SceneEntityCfg, @@ -712,6 +832,7 @@ def cross_body_arm_swing_reward( shoulder_phase_signs: tuple[float, float] = (-1.0, -1.0), elbow_flexion_signs: tuple[float, float] = (1.0, -1.0), std: float = 0.2, + joint_error_weights: tuple[float, float, float, float] | None = None, ) -> torch.Tensor: """Track speed-scaled shoulder and elbow targets that oppose the leg phase. @@ -765,10 +886,210 @@ def cross_body_arm_swing_reward( ).unsqueeze(1) * shoulder_signs.unsqueeze(0) target_pos[:, 2:] += (elbow_flexion * speed_scale).unsqueeze(1) * elbow_signs.unsqueeze(0) - mean_square_error = torch.mean(torch.square(joint_pos - target_pos), dim=1) + square_error = torch.square(joint_pos - target_pos) + if joint_error_weights is None: + mean_square_error = torch.mean(square_error, dim=1) + else: + if any(weight < 0.0 for weight in joint_error_weights) or sum(joint_error_weights) <= 0.0: + raise ValueError("joint_error_weights must be non-negative with a positive sum.") + error_weights = joint_pos.new_tensor(joint_error_weights) + mean_square_error = torch.sum(square_error * error_weights.unsqueeze(0), dim=1) / sum( + joint_error_weights + ) return torch.exp(-mean_square_error / (std * std)) +def running_elbow_flexion_huber( + env, + arm_asset_cfg: SceneEntityCfg, + feet_asset_cfg: SceneEntityCfg, + reference_body_cfg: SceneEntityCfg, + command_name: str, + heading_yaw_offset: float = 0.0, + run_transition_start: float = 1.6, + full_run_speed: float = 3.0, + min_elbow_flexion: float = 0.2, + max_elbow_flexion: float = 0.75, + min_phase_modulation: float = 0.05, + max_phase_modulation: float = 0.12, + phase_distance: float = 0.25, + delta: float = 0.2, + moving_threshold: float = 0.1, +) -> torch.Tensor: + """Track mirrored running elbow flexion with a dense Smooth-L1 loss. + + The two J4 joints must be ordered left then right. Positive left-J4 and + negative right-J4 angles represent the same physical elbow flexion on Gen2. + A small phase modulation bends the forward-swinging arm slightly more. + """ + if full_run_speed <= run_transition_start: + raise ValueError("full_run_speed must exceed run_transition_start.") + if not 0.0 <= min_elbow_flexion <= max_elbow_flexion: + raise ValueError("Expected 0 <= min_elbow_flexion <= max_elbow_flexion.") + if not 0.0 <= min_phase_modulation <= max_phase_modulation: + raise ValueError("Expected 0 <= min_phase_modulation <= max_phase_modulation.") + if phase_distance <= 0.0 or delta <= 0.0: + raise ValueError("phase_distance and delta must be positive.") + + joint_ids = arm_asset_cfg.joint_ids + foot_ids = feet_asset_cfg.body_ids + if joint_ids is None or isinstance(joint_ids, slice) or len(joint_ids) != 2: + raise ValueError("running_elbow_flexion_huber requires two ordered elbow joint ids.") + if foot_ids is None or isinstance(foot_ids, slice) or len(foot_ids) != 2: + raise ValueError("running_elbow_flexion_huber requires two ordered foot body ids.") + + reference_body_id = _single_body_id(reference_body_cfg, "running_elbow_flexion_huber") + asset = env.scene[arm_asset_cfg.name] + feet_asset = env.scene[feet_asset_cfg.name] + reference_asset = env.scene[reference_body_cfg.name] + + reference_pos_w = reference_asset.data.body_pos_w[:, reference_body_id, :] + reference_quat_w = reference_asset.data.body_quat_w[:, reference_body_id, :] + feet_pos_rel = feet_asset.data.body_pos_w[:, foot_ids, :] - reference_pos_w.unsqueeze(1) + num_envs = reference_pos_w.shape[0] + heading_quat = _heading_quat_with_offset(reference_quat_w, heading_yaw_offset) + heading_quat_pairs = heading_quat.unsqueeze(1).expand(-1, 2, -1).reshape(-1, 4) + feet_pos_heading = quat_apply_inverse(heading_quat_pairs, feet_pos_rel.reshape(-1, 3)).reshape( + num_envs, 2, 3 + ) + leg_phase = torch.clamp( + (feet_pos_heading[:, 0, 0] - feet_pos_heading[:, 1, 0]) / phase_distance, + min=-1.0, + max=1.0, + ) + + commands = env.command_manager.get_command(command_name) + forward_speed = torch.abs(commands[:, 0]) + run_progress = torch.clamp( + (forward_speed - run_transition_start) / (full_run_speed - run_transition_start), + min=0.0, + max=1.0, + ) + base_flexion = min_elbow_flexion + run_progress * ( + max_elbow_flexion - min_elbow_flexion + ) + phase_modulation = min_phase_modulation + run_progress * ( + max_phase_modulation - min_phase_modulation + ) + target_offset = torch.stack( + ( + base_flexion - phase_modulation * leg_phase, + -(base_flexion + phase_modulation * leg_phase), + ), + dim=1, + ) + moving = forward_speed >= moving_threshold + target_offset = torch.where(moving.unsqueeze(1), target_offset, torch.zeros_like(target_offset)) + target_pos = asset.data.default_joint_pos[:, joint_ids] + target_offset + + abs_error = torch.abs(asset.data.joint_pos[:, joint_ids] - target_pos) + huber = torch.where( + abs_error <= delta, + 0.5 * torch.square(abs_error) / delta, + abs_error - 0.5 * delta, + ) + return torch.mean(huber, dim=1) + + +def running_arm_target_curriculum( + env, + env_ids, + warmup_steps: int, + ramp_end_steps: int, + initial_shoulder_amplitude: float, + final_shoulder_amplitude: float, + initial_upper_arm_phase_amplitude: float, + final_upper_arm_phase_amplitude: float, + initial_min_elbow_flexion: float, + final_min_elbow_flexion: float, + initial_max_elbow_flexion: float, + final_max_elbow_flexion: float, + initial_min_phase_modulation: float, + final_min_phase_modulation: float, + initial_max_phase_modulation: float, + final_max_phase_modulation: float, + initial_forearm_phase_amplitude: float, + final_forearm_phase_amplitude: float, + initial_cross_body_weight: float = 0.8, + final_cross_body_weight: float = 1.0, + initial_upper_arm_phase_weight: float = 0.5, + final_upper_arm_phase_weight: float = 0.65, + initial_elbow_weight: float = -0.25, + final_elbow_weight: float = -0.5, + initial_forearm_phase_weight: float = 0.0, + final_forearm_phase_weight: float = 0.3, + cross_body_term_name: str = "cross_body_arm_swing", + upper_arm_phase_term_name: str = "contralateral_arm_phase", + elbow_term_name: str = "elbow_flexion", + forearm_phase_term_name: str = "contralateral_forearm_phase", +) -> dict[str, float]: + """Progressively introduce natural running arm targets after checkpoint transfer. + + Reward-manager configs are updated only when an environment resets. With randomized + episode lengths this produces a smooth population-level transition instead of an + abrupt reward change on the first fine-tuning iteration. + """ + del env_ids + if warmup_steps < 0 or ramp_end_steps <= warmup_steps: + raise ValueError("Expected 0 <= warmup_steps < ramp_end_steps.") + + progress = min( + max((env.common_step_counter - warmup_steps) / (ramp_end_steps - warmup_steps), 0.0), + 1.0, + ) + + def lerp(initial: float, final: float) -> float: + return float(initial + progress * (final - initial)) + + cross_body_cfg = env.reward_manager.get_term_cfg(cross_body_term_name) + cross_body_cfg.params["shoulder_amplitude"] = lerp( + initial_shoulder_amplitude, final_shoulder_amplitude + ) + cross_body_cfg.weight = lerp(initial_cross_body_weight, final_cross_body_weight) + env.reward_manager.set_term_cfg(cross_body_term_name, cross_body_cfg) + + upper_arm_phase_cfg = env.reward_manager.get_term_cfg(upper_arm_phase_term_name) + upper_arm_phase_cfg.params["arm_phase_amplitude"] = lerp( + initial_upper_arm_phase_amplitude, final_upper_arm_phase_amplitude + ) + upper_arm_phase_cfg.weight = lerp( + initial_upper_arm_phase_weight, final_upper_arm_phase_weight + ) + env.reward_manager.set_term_cfg(upper_arm_phase_term_name, upper_arm_phase_cfg) + + elbow_cfg = env.reward_manager.get_term_cfg(elbow_term_name) + elbow_cfg.params["min_elbow_flexion"] = lerp( + initial_min_elbow_flexion, final_min_elbow_flexion + ) + elbow_cfg.params["max_elbow_flexion"] = lerp( + initial_max_elbow_flexion, final_max_elbow_flexion + ) + elbow_cfg.params["min_phase_modulation"] = lerp( + initial_min_phase_modulation, final_min_phase_modulation + ) + elbow_cfg.params["max_phase_modulation"] = lerp( + initial_max_phase_modulation, final_max_phase_modulation + ) + elbow_cfg.weight = lerp(initial_elbow_weight, final_elbow_weight) + env.reward_manager.set_term_cfg(elbow_term_name, elbow_cfg) + + forearm_phase_cfg = env.reward_manager.get_term_cfg(forearm_phase_term_name) + forearm_phase_cfg.params["arm_phase_amplitude"] = lerp( + initial_forearm_phase_amplitude, final_forearm_phase_amplitude + ) + forearm_phase_cfg.weight = lerp( + initial_forearm_phase_weight, final_forearm_phase_weight + ) + env.reward_manager.set_term_cfg(forearm_phase_term_name, forearm_phase_cfg) + + return { + "progress": float(progress), + "shoulder_amplitude": cross_body_cfg.params["shoulder_amplitude"], + "max_elbow_flexion": elbow_cfg.params["max_elbow_flexion"], + "forearm_phase_amplitude": forearm_phase_cfg.params["arm_phase_amplitude"], + } + + def contralateral_arm_phase_reward( env, arm_body_cfg: SceneEntityCfg, diff --git a/source/gen2_lab/assets/gen2_urdf.zip b/source/gen2_lab/assets/gen2_urdf.zip deleted file mode 100644 index 5dc9a9e..0000000 Binary files a/source/gen2_lab/assets/gen2_urdf.zip and /dev/null differ