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

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()