v0.1.13; add 2.5 rad trunctation; add LevelPipeline binary search maximum terrain levels

UPDATE.md

@@ -1,7 +1,11 @@
 # UPDATE
+TODO: 处理LevelPipeline返回的result信息
 ## 20251222
 ### v0.1.13
 1. 即使模型崩溃也要继续测完后续的goals, 但是跳过当前的sub goals
+2. 加入当翻滚角>2.5rad时认为崩溃, 自动切换为下一个goal
+3. 初步完成二分查找level等级, 细节如下
+    LevelPipeline: 一种二分查找当前域随机化(friction, base_mass)下的最大成功terrain等级, 每次测试中采用三个seeds, 调用MultiPipeline同时测试三个seeds, 如果同时全部成功, 则判定为通过
 ## 20251221
 ### v0.1.12
 1. 在模型崩溃时也记录下最后的gauge信息, 修改single/multi pipeline逻辑

resources/terrains/slope/slope_1.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.072718512065541 0.0 -0.2006837555478992" type="box" size="2.0214935072861353 5.0 0.5" quat="0.9973383355451144 0.0 -0.07291258089041118 0.0" />
+    <geom pos="5.072718512065541 0.0 0.09331624445210085" type="box" size="4.042987014572271 3.0 0.5" quat="0.9973383355451144 0.0 -0.07291258089041118 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_10.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.247601522004908 0.0 0.7056113649036695" type="box" size="2.3020860105565126 5.0 0.5" quat="0.9666377993314406 0.0 -0.25614715478347516 0.0" />
+    <geom pos="5.247601522004908 0.0 1.8456113649036694" type="box" size="4.604172021113025 3.0 0.5" quat="0.9666377993314406 0.0 -0.25614715478347516 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_2.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.0952246135495556 0.0 -0.10284852345131673" type="box" size="2.037288393919722 5.0 0.5" quat="0.9954137448575425 0.0 -0.09566335008080827 0.0" />
+    <geom pos="5.095224613549555 0.0 0.2851514765486833" type="box" size="4.074576787839444 3.0 0.5" quat="0.9954137448575425 0.0 -0.09566335008080827 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_3.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.1171460337138486 0.0 -0.004083127443355927" type="box" size="2.057261286273574 5.0 0.5" quat="0.9930171838610629 0.0 -0.11796979510300111 0.0" />
+    <geom pos="5.117146033713849 0.0 0.47791687255664406" type="box" size="4.114522572547148 3.0 0.5" quat="0.9930171838610629 0.0 -0.11796979510300111 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_4.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.138375592151506 0.0 0.09552919391838177" type="box" size="2.081291906485008 5.0 0.5" quat="0.9901872580889023 0.0 -0.13974689233890575 0.0" />
+    <geom pos="5.138375592151506 0.0 0.6715291939183818" type="box" size="4.162583812970016 3.0 0.5" quat="0.9901872580889023 0.0 -0.13974689233890575 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_5.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.1588248614041845 0.0 0.19589593610691164" type="box" size="2.1092415698539604 5.0 0.5" quat="0.9869671037542681 0.0 -0.16092214299751198 0.0" />
+    <geom pos="5.1588248614041845 0.0 0.8658959361069116" type="box" size="4.218483139707921 3.0 0.5" quat="0.9869671037542681 0.0 -0.16092214299751198 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_6.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.1784248990036357 0.0 0.29691911255592746" type="box" size="2.1409567954538455 5.0 0.5" quat="0.9834027086824972 0.0 -0.18143624928863422 0.0" />
+    <geom pos="5.178424899003636 0.0 1.0609191125559274" type="box" size="4.281913590907691 3.0 0.5" quat="0.9834027086824972 0.0 -0.18143624928863422 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_7.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.197126007501461 0.0 0.39849881701291123" type="box" size="2.1762729608208615 5.0 0.5" quat="0.9795413125474028 0.0 -0.20124317879846595 0.0" />
+    <geom pos="5.197126007501461 0.0 1.2564988170129112" type="box" size="4.352545921641723 3.0 0.5" quat="0.9795413125474028 0.0 -0.20124317879846595 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_8.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.2148966897390405 0.0 0.5005363660944522" type="box" size="2.2150178328853247 5.0 0.5" quat="0.9754299738605267 0.0 -0.22030970494840266 0.0" />
+    <geom pos="5.2148966897390405 0.0 1.4525363660944521" type="box" size="4.4300356657706494 3.0 0.5" quat="0.9754299738605267 0.0 -0.22030970494840266 0.0" />
   </worldbody>
 </mujoco>

resources/terrains/slope/slope_9.xml

@@ -14,6 +14,6 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
-    <geom pos="3.2317220029368436 0.0 0.6029368968702795" type="box" size="2.257014842662759 5.0 0.5" quat="0.9711143615093542 0.0 -0.2386145361671822 0.0" />
+    <geom pos="5.231722002936844 0.0 1.6489368968702796" type="box" size="4.514029685325518 3.0 0.5" quat="0.9711143615093542 0.0 -0.2386145361671822 0.0" />
   </worldbody>
 </mujoco>

robogauge/scripts/run.py

@@ -14,6 +14,7 @@ os.environ["MKL_NUM_THREADS"] = "1"
 
 from robogauge.tasks import *
 from robogauge.tasks.pipeline.multi_pipeline import MultiPipeline
+from robogauge.tasks.pipeline.level_pipeline import LevelPipeline
 
 from robogauge.utils.task_register import task_register
 from robogauge.utils.helpers import parse_args
@@ -22,9 +23,12 @@ from robogauge.utils.logger import logger
 
 if __name__ == '__main__':
     args = parse_args()
-    if not args.multi:
+    if args.multi:
-        pipeline: BasePipeline = task_register.make_pipeline(args=args)
-        pipeline.run()
-    else:
         multi_pipeline = MultiPipeline(args)
         multi_pipeline.run()
+    elif args.search_max_level:
+        level_pipeline = LevelPipeline(args)
+        level_pipeline.run()
+    else:
+        pipeline: BasePipeline = task_register.make_pipeline(args=args)
+        pipeline.run()

robogauge/tasks/gauge/base_gauge.py

@@ -152,7 +152,9 @@ class BaseGauge:
                 self.results['summary'][metric_name][quantile] = f"{mean:.4f} ± {std:.4f}"
 
         save_path = Path(logger.log_dir) / "results.yaml"
-        self.results["terrain"] = f"{self.cfg.assets.terrain_name}"
+        self.results["terrain_name"] = self.cfg.assets.terrain_name
+        self.results["terrain_level"] = self.cfg.assets.terrain_level
+
         with open(save_path, 'w') as file:
             yaml.dump(self.results, file, allow_unicode=True, sort_keys=False)
             yaml_str = yaml.dump(self.results, allow_unicode=True, sort_keys=False)

robogauge/tasks/gauge/base_gauge_config.py

@@ -14,7 +14,8 @@ class BaseGaugeConfig(Config):
     write_tensorboard = False  # Whether to write tensorboard logs
 
     class assets:
-        terrain_name = "flat_0"  # {type}_{level}
+        terrain_name = "flat"
+        terrain_level = 0
         terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
         terrain_spawn_pos = [0, 0, 0]  # x y z [m], robot freejoint spawn position on the terrain
     

robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py

@@ -13,6 +13,7 @@ class FlatGaugeConfig(BaseGaugeConfig):
     gauge_class = 'BaseGauge'
 
     class assets(BaseGaugeConfig.assets):
-        terrain_name = "flat_0"  # {type}_{level}
+        terrain_name = "flat"
+        terrain_level = 0
         terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
         terrain_spawn_pos = [0, 0, 0]  # x y z [m], robot freejoint spawn position on the terrain

robogauge/tasks/gauge/gauge_configs/slope_gauge_config.py

@@ -13,14 +13,15 @@ class SlopeGaugeConfig(BaseGaugeConfig):
     gauge_class = 'BaseGauge'
 
     class assets(BaseGaugeConfig.assets):
-        terrain_name = "slope_5"  # {type}_{level}
+        terrain_name = "slope"
+        terrain_level = 10  # 1-10
         terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/slope/slope_10.xml'
         terrain_spawn_pos = [0, 0, 0]  # x y z [m], robot freejoint spawn position on the terrain
     
-    class goals:
+    class goals(BaseGaugeConfig.goals):
         class target_pos_velocity:  # goal to reach a target position by velocity command
             enabled = True
-            target_pos = [4, 0, 2.0]  # x y z [m], target position in the environment, used for target position goal
+            target_pos = [5, 0, 2.28]  # x y z [m], target position in the environment, used for target position goal
             lin_vel_x = 1.0  # +/- m/s
             lin_vel_y = 1.0  # +/- m/s
             ang_vel_yaw = 1.5  # +/- rad/s

robogauge/tasks/gauge/gauge_configs/terrain_levels_config.py

@@ -0,0 +1,26 @@
+from robogauge.utils.config import Config
+
+class TerrainLevelsConfig(Config):
+    class flat:
+        levels = [0]
+        targets = [[4, 0, 0]]  # target positions for each level, if target goal is used
+    
+    class slope:
+        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
+        targets = [
+            [5, 0, 0.588],
+            [5, 0, 0.776],
+            [5, 0, 0.964],
+            [5, 0, 1.152],
+            [5, 0, 1.340],
+            [5, 0, 1.528],
+            [5, 0, 1.716],
+            [5, 0, 1.904],
+            [5, 0, 2.092],
+            [5, 0, 2.280],
+        ]
+
+    class wave:
+        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
+        targets = []
+

robogauge/tasks/gauge/gauge_configs/wave_gauge_config.py

@@ -13,7 +13,8 @@ class WaveGaugeConfig(BaseGaugeConfig):
     gauge_class = 'BaseGauge'
 
     class assets(BaseGaugeConfig.assets):
-        terrain_name = "wave_1"  # {type}_{level}
+        terrain_name = "wave"
+        terrain_level = 1  # 1-10
         terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/wave/wave_1.xml'
         terrain_spawn_pos = [1.5, 0, 1]  # x y z [m], robot freejoint spawn position on the terrain
     

robogauge/tasks/pipeline/level_pipeline.py

@@ -0,0 +1,54 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : level_pipeline.py
+@Time    : 2025/12/22 20:38:11
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Level Pipeline for Robogauge
+'''
+import yaml
+
+from robogauge.tasks.pipeline.multi_pipeline import MultiPipeline
+from robogauge.utils.logger import logger
+
+class LevelPipeline:
+    def __init__(self, args):
+        self.args = args
+        self.seeds = args.seeds
+        self.model_path = None
+        logger.create(args.experiment_name+'_level', args.run_name)
+    
+    def run(self):
+        logger.info(f"🚀 Starting Level Seacher for '{self.args.experiment_name}'.")
+        logger.info(f"🔢 Seeds: {self.seeds}")
+
+        # binary search levels
+        l, r = 0, 10
+        while l < r:
+            level = (l + r + 1) // 2
+            if self.test_level(level):
+                l = level
+            else:
+                r = level - 1
+        if l >= 1:
+            logger.info(f"🏆 Found maximum level: {l}")
+        else:
+            logger.info(f"❌ No valid level found [1-10].")
+    
+    def test_level(self, level: int) -> bool:
+        logger.info(f"🔍 Testing level {level}...")
+        self.args.level = level
+        multi_pipeline = MultiPipeline(self.args)
+        log_dir = multi_pipeline.run()
+        # load results.yaml
+        with open(log_dir / "aggregated_results.yaml", 'r') as f:
+            results = yaml.safe_load(f)
+        self.model_path = results['model_path']
+        success_mean = float(results['success']['mean'].split(' ')[0])
+        all_success = success_mean == 1.0
+        if all_success:
+            logger.info(f"✅ Level {level} passed all tests.")
+        else:
+            logger.info(f"❌ Level {level} failed some tests.")
+        return all_success

robogauge/tasks/pipeline/multi_pipeline.py

@@ -90,6 +90,7 @@ class MultiPipeline:
 
         logger.info("✅ Multi-Process Evaluation Completed.")
         self.aggregate_results(result_log_dirs, success_flags, workers_data)
+        return logger.log_dir
     
     def aggregate_results(self, log_dirs, success_flags, workers_data):
         """ Process results.yaml from each log_dir """

robogauge/tasks/simulator/mujoco_config.py

@@ -45,3 +45,7 @@ class MujocoConfig(Config):
         ang_vel = 0.8       # [rad/s]
         joint_pos = 0.01    # [rad]
         joint_vel = 3.0    # [rad/s]
+
+    class truncation:
+        enabled = True
+        projected_gravity_rad = 2.5  # [rad], if gravity projection angle exceeds this value, truncate episode

robogauge/tasks/simulator/mujoco_simulator.py

@@ -20,6 +20,7 @@ from typing import Literal
 
 from robogauge.utils.logger import logger
 from robogauge.utils.helpers import parse_path
+from robogauge.utils.math_utils import get_projected_gravity
 from robogauge.tasks.simulator.mujoco_config import MujocoConfig
 from robogauge.tasks.simulator.sim_data import (
     SimData,
@@ -261,8 +262,15 @@ class MujocoSimulator:
 
         self.n_step += 1
         self.sim_time = self.n_step * self.sim_dt
+        self.check_truncation(sim_data)
         return sim_data
     
+    def check_truncation(self, sim_data: SimData):
+        if self.cfg.truncation.enabled:
+            projected_gravity = get_projected_gravity(sim_data.proprio.base.quat)
+            if -projected_gravity[2] < np.cos(self.cfg.truncation.projected_gravity_rad):
+                raise RuntimeError(f"Episode truncated due to excessive projected gravity, angle: {np.arccos(-projected_gravity[2]):.3f} rad, projected: {projected_gravity}")
+    
     def reset(self):
         """ Reset the simulator to initial state. """
         self.mj_physics.reset()

robogauge/utils/helpers.py

@@ -75,13 +75,17 @@ def parse_args():
         {"name": "--plot-radar", "action": "store_true", "default": False, "help": "Plot radar charts for metrics."},
         {"name": "--base-mass", "type": float, "default": 0.0, "help": "Set the base mass of the robot."},
         {"name": "--friction", "type": float, "default": 1.0, "help": "Set the ground friction coefficient."},
+        {"name": "--level", "type": int, "help": "Set the difficulty level of the environment, range 1-10 (flat is 0)."},
 
         # Multiprocessing parameters, with different seeds
         {"name": "--multi", "action": "store_true", "default": False, "help": "Enable multiprocessing."},
         {"name": "--num-processes", "type": int, "default": 2, "help": "Number of parallel processes."},
         {"name": "--seeds", "type": int, "nargs": "+", "default": [0], "help": "List of random seeds for multiple runs."},
         {"name": "--base-masses", "type": float, "nargs": "+", "default": [0], "help": "List of base masses for the model."},
-        {"name": "--frictions", "type": float, "nargs": "+", "default": [0.4, 0.7, 1.0, 1.3, 1.6], "help": "List of friction coefficients for the model."}
+        {"name": "--frictions", "type": float, "nargs": "+", "default": [0.4, 0.7, 1.0, 1.3, 1.6], "help": "List of friction coefficients for the model."},
+
+        # Level pipeline parameters
+        {"name": "--search-max-level", "action": "store_true", "default": False, "help": "Use level pipeline to search maximum level."},
     ]
     for param in parameters:
         parser.add_argument(param['name'], **{k: v for k, v in param.items() if k != 'name'})

robogauge/utils/task_register.py

@@ -10,6 +10,7 @@
 from robogauge import ROBOGAUGE_ROOT_DIR
 from robogauge.utils.logger import logger
 from robogauge.utils.helpers import parse_args, set_seed
+from robogauge.tasks.gauge.gauge_configs.terrain_levels_config import TerrainLevelsConfig
 
 class TaskRegister():
     def __init__(self):
@@ -69,5 +70,15 @@ class TaskRegister():
             sim_cfg.domain_rand.friction = args.friction
         if args.base_mass is not None:
             sim_cfg.domain_rand.base_mass = args.base_mass
+        if args.level is not None:
+            gauger_cfg.assets.terrain_level = args.level
+            levels_cfg = TerrainLevelsConfig()
+            cfg = getattr(levels_cfg, gauger_cfg.assets.terrain_name)
+            assert cfg is not None, f"Level {args.level} configuration not found in TerrainLevelsConfig."
+            assert args.level in cfg.levels, f"Level must be in {cfg.levels}."
+            gauger_cfg.goals.target_pos_velocity.target_pos = cfg.targets[cfg.levels.index(args.level)]
+            xml = gauger_cfg.assets.terrain_xml
+            xml = xml.rsplit('/', 1)[0] + f"/{gauger_cfg.assets.terrain_name}_{args.level}.xml"
+            gauger_cfg.assets.terrain_xml = xml
 
 task_register = TaskRegister()