v1.0.0

2025-12-29 23:41:09 +08:00
parent c28bb64b0a
commit 587d014683
44 changed files with 196 additions and 714 deletions
--- a/robogauge/tasks/gauge/init.py
+++ b/robogauge/tasks/gauge/init.py
@@ -1,8 +1,7 @@
 from .base_gauge import BaseGauge
 from .base_gauge_config import BaseGaugeConfig
 from .gauge_configs.flat_gauge_config import FlatGaugeConfig
-from .gauge_configs.slope_gauge_config import SlopeGaugeConfig
+from .gauge_configs.slope_gauge_config import SlopeForwardGaugeConfig, SlopeBackwardGaugeConfig
 from .gauge_configs.wave_gauge_config import WaveGaugeConfig
-from .gauge_configs.stairs_up_gauge_config import StairsUpGaugeConfig
-from .gauge_configs.stairs_down_gauge_config import StairsDownGaugeConfig
+from .gauge_configs.stairs_gauge_config import StairsForwardGaugeConfig, StairsBackwardGaugeConfig
 from .gauge_configs.obstacle_gauge_config import ObstacleGaugeConfig
--- a/robogauge/tasks/gauge/base_gauge.py
+++ b/robogauge/tasks/gauge/base_gauge.py
@@ -52,7 +52,7 @@ class BaseGauge:
                self.goals.append(DiagonalVelocityGoal(robot_cfg.control.control_dt, robot_cfg.commands, **kwargs))
                log_str += f"  - Diagonal Velocity Goal: {kwargs}\n"
            elif name == 'target_pos_velocity':
-                self.goals.append(TargetPosVelocityGoal(robot_cfg.control.control_dt,**kwargs))
+                self.goals.append(TargetPosVelocityGoal(robot_cfg.control.control_dt, cfg.backward, **kwargs))
                log_str += f"  - Target Position Velocity Goal: {kwargs}\n"
            else:
                raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.")
--- a/robogauge/tasks/gauge/base_gauge_config.py
+++ b/robogauge/tasks/gauge/base_gauge_config.py
@@ -12,6 +12,7 @@ from robogauge.utils.config import Config
 class BaseGaugeConfig(Config):
    gauge_class = 'BaseGauge'
    write_tensorboard = False  # Whether to write tensorboard logs
+    backward = False  # Whether to invert init yaw orient and backward move to target

    class assets:
        terrain_name = "flat"
--- a/robogauge/tasks/gauge/gauge_configs/slope_gauge_config.py
+++ b/robogauge/tasks/gauge/gauge_configs/slope_gauge_config.py
@@ -5,15 +5,15 @@
@Author  : wty-yy
@Version : 1.0
@Blog    : https://wty-yy.github.io/
-@Desc    : Slope Gauge Configuration
+@Desc    : Slope Forward(fd)/Backward(bd) Gauge Configuration
 '''
 from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig

-class SlopeGaugeConfig(BaseGaugeConfig):
+class SlopeForwardGaugeConfig(BaseGaugeConfig):
    gauge_class = 'BaseGauge'

    class assets(BaseGaugeConfig.assets):
-        terrain_name = "slope"
+        terrain_name = "slope_fd"
        terrain_level = 4  # 1-10
        terrain_xmls = [
            '{ROBOGAUGE_ROOT_DIR}/resources/terrains/slope/slope_4.xml',
@@ -30,4 +30,9 @@ class SlopeGaugeConfig(BaseGaugeConfig):
            ang_vel_yaw = 1.5  # +/- rad/s
            max_cmd_duration = 20.0  # [s] maximum duration to reach the target position
            reach_threshold = 0.1
-        
+
+class SlopeBackwardGaugeConfig(SlopeForwardGaugeConfig):
+    backward = True
+
+    class assets(SlopeForwardGaugeConfig.assets):
+        terrain_name = "slope_bd"
--- a/robogauge/tasks/gauge/gauge_configs/stairs_down_gauge_config.py
+++ b/robogauge/tasks/gauge/gauge_configs/stairs_down_gauge_config.py
@@ -1,33 +0,0 @@
-# -*- coding: utf-8 -*-
-'''
-@File    : stairs_down_gauge_config.py
-@Time    : 2025/12/25 14:12:59
-@Author  : wty-yy
-@Version : 1.0
-@Blog    : https://wty-yy.github.io/
-@Desc    : Stairs Down Gauge Configuration
-'''
-from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
-
-class StairsDownGaugeConfig(BaseGaugeConfig):
-    gauge_class = 'BaseGauge'
-
-    class assets(BaseGaugeConfig.assets):
-        terrain_name = "stairs_down"
-        terrain_level = 10  # 1-10
-        terrain_xmls = [
-            '{ROBOGAUGE_ROOT_DIR}/resources/terrains/stairs_down/stairs_down_10.xml',
-            '{ROBOGAUGE_ROOT_DIR}/resources/terrains/wall/10x10_wall.xml',
-        ]
-        # NOTE: Adjusted y=3.0 to avoid rolling down just pass the target
-        terrain_spawn_pos = [1.1, 3.0, 7]  # x y z [m], robot freejoint spawn position on the terrain
-    
-    class goals(BaseGaugeConfig.goals):
-        class target_pos_velocity:  # goal to reach a target position by velocity command
-            enabled = True
-            target_pos = [8.3, 0.0, 1.90]  # x y z [m], target position in the environment, used for target position goal
-            lin_vel_x = 0.8  # +/- m/s
-            lin_vel_y = 1.0  # +/- m/s
-            ang_vel_yaw = 1.5  # +/- rad/s
-            max_cmd_duration = 30.0  # [s] maximum duration to reach the target position
-            reach_threshold = 0.3
--- a/robogauge/tasks/gauge/gauge_configs/stairs_up_gauge_config.py
+++ b/robogauge/tasks/gauge/gauge_configs/stairs_up_gauge_config.py
@@ -5,18 +5,18 @@
@Author  : wty-yy
@Version : 1.0
@Blog    : https://wty-yy.github.io/
-@Desc    : Stairs Up Gauge Configuration
+@Desc    : Stairs Forward(fd)/Backward(bd) Gauge Configuration
 '''
 from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig

-class StairsUpGaugeConfig(BaseGaugeConfig):
+class StairsForwardGaugeConfig(BaseGaugeConfig):
    gauge_class = 'BaseGauge'

    class assets(BaseGaugeConfig.assets):
-        terrain_name = "stairs_up"
+        terrain_name = "stairs_fd"
        terrain_level = 10  # 1-10
        terrain_xmls = [
-            '{ROBOGAUGE_ROOT_DIR}/resources/terrains/stairs_up/stairs_up_10.xml',
+            '{ROBOGAUGE_ROOT_DIR}/resources/terrains/stairs/stairs_10.xml',
            '{ROBOGAUGE_ROOT_DIR}/resources/terrains/wall/10x10_wall.xml',
        ]
        terrain_spawn_pos = [1.0, 0.0, 1.5]  # x y z [m], robot freejoint spawn position on the terrain
@@ -30,3 +30,9 @@ class StairsUpGaugeConfig(BaseGaugeConfig):
            ang_vel_yaw = 1.5  # +/- rad/s
            max_cmd_duration = 30.0  # [s] maximum duration to reach the target position
            reach_threshold = 0.1
+
+class StairsBackwardGaugeConfig(StairsForwardGaugeConfig):
+    backward = True
+
+    class assets(StairsForwardGaugeConfig.assets):
+        terrain_name = "stairs_bd"
--- a/robogauge/tasks/gauge/gauge_configs/terrain_levels_config.py
+++ b/robogauge/tasks/gauge/gauge_configs/terrain_levels_config.py
@@ -9,9 +9,19 @@
 '''
 from robogauge.utils.config import Config

-SEARCH_LEVELS_TERRAINS = ['slope', 'wave', 'stairs_up', 'stairs_down', 'obstacle']
+SEARCH_LEVELS_TERRAINS = ['slope_fd', 'slope_bd', 'wave', 'stairs_fd', 'stairs_bd', 'obstacle']
+TERRAIN_NAME2_XML_NAME = {
+    'flat': 'flat',
+    'slope_fd': 'slope',
+    'slope_bd': 'slope',
+    'wave': 'wave',
+    'stairs_fd': 'stairs',
+    'stairs_bd': 'stairs',
+    'obstacle': 'obstacle',
+}

 class TerrainSearchLevelsConfig(Config):
+    """ Go2 robot init/target position """
    class flat:
        levels = [0]
        targets = [[4, 0, 0]]  # target positions for each level, if target goal is used
@@ -46,7 +56,7 @@ class TerrainSearchLevelsConfig(Config):
            [6.5, -2.65, 0.80],
        ]

-    class stairs_up:
+    class stairs:
        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
        targets = [
            [4.5, 0.0, 1.35],
@@ -61,36 +71,30 @@ class TerrainSearchLevelsConfig(Config):
            [4.5, 0.0, 3.60],
        ]

-    class stairs_down:
-        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
-        targets = [
-            [8.3, 0.0, 1.90 - 0.07 * 15],
-            [8.3, 0.0, 1.90 - 0.07 * 12],
-            [8.3, 0.0, 1.90 - 0.07 * 9],
-            [8.3, 0.0, 1.90 - 0.07 * 6],
-            [8.3, 0.0, 1.90 - 0.07 * 5],
-            [8.3, 0.0, 1.90 - 0.07 * 4],
-            [8.3, 0.0, 1.90 - 0.07 * 3],
-            [8.3, 0.0, 1.90 - 0.07 * 2],
-            [8.3, 0.0, 1.90 - 0.07],
-            [8.3, 0.0, 1.90],
-        ]
-
        spawns = [
-            [1.1, 3.0, 7 - 0.3 * 15],
-            [1.1, 3.0, 7 - 0.3 * 12],
-            [1.1, 3.0, 7 - 0.3 * 9],
-            [1.1, 3.0, 7 - 0.3 * 6],
-            [1.1, 3.0, 7 - 0.3 * 5],
-            [1.1, 3.0, 7 - 0.3 * 4],
-            [1.1, 3.0, 7 - 0.3 * 3],
-            [1.1, 3.0, 7 - 0.3 * 2],
-            [1.1, 3.0, 7 - 0.3],
-            [1.1, 3.0, 7],
+            [1.15, 0.0, 1.1 - 0.05 * 6 - 0.15 * 3],
+            [1.15, 0.0, 1.1 - 0.05 * 6 - 0.15 * 2],
+            [1.15, 0.0, 1.1 - 0.05 * 6 - 0.15],
+            [1.15, 0.0, 1.1 - 0.05 * 6],
+            [1.15, 0.0, 1.1 - 0.05 * 5],
+            [1.15, 0.0, 1.1 - 0.05 * 4],
+            [1.15, 0.0, 1.1 - 0.05 * 3],
+            [1.15, 0.0, 1.1 - 0.05 * 2],
+            [1.15, 0.0, 1.1 - 0.05 * 1],
+            [1.15, 0.0, 1.1],
        ]

    class obstacle:
        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
+    
+    class slope_fd(slope):  # forward direction
+        pass
+    class slope_bd(slope):  # backward direction
+        pass
+    class stairs_fd(stairs):  # forward direction
+        pass
+    class stairs_bd(stairs):  # backward direction
+        pass

 class TerrainEvalLevelsConfig(Config):
    class flat:
@@ -99,16 +103,16 @@ class TerrainEvalLevelsConfig(Config):
    class slope:
        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
        spawns = [
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 9)],
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 8)],
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 7)],
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 6)],
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 5)],
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 4)],
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 3)],
-            [5.0, 0, 4.2 * (0.57 - 0.047 * 2)],
-            [5.0, 0, 4.2 * (0.57 - 0.047)],
-            [5.0, 0, 4.2 * 0.57],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 9)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 8)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 7)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 6)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 5)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 4)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 3)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047 * 2)],
+            [5.0, 0, 0.03 + 4.2 * (0.57 - 0.047)],
+            [5.0, 0, 0.03 + 4.2 * 0.57],
        ]

    class wave:
@@ -126,34 +130,19 @@ class TerrainEvalLevelsConfig(Config):
            [4.55, 0, 0.65],
        ]

-    class stairs_up:
+    class stairs:
        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
        spawns = [
-            [5.0, 0.0, 1.35],
-            [5.0, 0.0, 1.80],
-            [5.0, 0.0, 2.25 + 0.03],
-            [5.0, 0.0, 2.70 + 0.08],
-            [5.0, 0.0, 2.85 + 0.1],
-            [5.0, 0.0, 3.00 + 0.12],
-            [5.0, 0.0, 3.15 + 0.14],
-            [5.0, 0.0, 3.30 + 0.16],
-            [5.0, 0.0, 3.45 + 0.18],
-            [5.0, 0.0, 3.60 + 0.2],
-        ]
-
-    class stairs_down:
-        levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
-        spawns = [
-            [5.1, 0.0, 1.35],
-            [5.1, 0.0, 1.80 + 0.08],
-            [5.1, 0.0, 2.25 + 0.15],
-            [5.1, 0.0, 2.70 + 0.25],
-            [5.1, 0.0, 2.85 + 0.27],
-            [5.1, 0.0, 3.00 + 0.30],
-            [5.1, 0.0, 3.15 + 0.33],
-            [5.1, 0.0, 3.30 + 0.36],
-            [5.1, 0.0, 3.45 + 0.39],
-            [5.1, 0.0, 3.60 + 0.42],
+            [4.85, 0.0, 0.05 + 1.35],
+            [4.85, 0.0, 0.05 + 1.80],
+            [4.85, 0.0, 0.05 + 2.25 + 0.03],
+            [4.85, 0.0, 0.05 + 2.70 + 0.08],
+            [4.85, 0.0, 0.05 + 2.85 + 0.1],
+            [4.85, 0.0, 0.05 + 3.00 + 0.12],
+            [4.85, 0.0, 0.05 + 3.15 + 0.14],
+            [4.85, 0.0, 0.05 + 3.30 + 0.16],
+            [4.85, 0.0, 0.05 + 3.45 + 0.18],
+            [4.85, 0.0, 0.05 + 3.60 + 0.2],
        ]

    class obstacle:
@@ -170,3 +159,16 @@ class TerrainEvalLevelsConfig(Config):
            [5, 0.0, 0.20 - 0.023 * 1],
            [5, 0.0, 0.20],
        ]
+
+    class slope_fd(slope):  # forward direction
+        pass
+    class slope_bd(slope):  # backward direction
+        pass
+    class stairs_fd(stairs):  # forward direction
+        pass
+    class stairs_bd(stairs):  # backward direction
+        pass
+
+if __name__ == '__main__':
+    terrain_search_cfg = TerrainSearchLevelsConfig()
+    print(terrain_search_cfg.stairs.spawns)
--- a/robogauge/tasks/gauge/goals/velocity_goals.py
+++ b/robogauge/tasks/gauge/goals/velocity_goals.py
@@ -171,6 +171,7 @@ class TargetPosVelocityGoal(BaseVelocityGoal):

    def __init__(self,
        control_dt: float,
+        backward: bool,
        target_pos: List[float],
        lin_vel_x: float,
        lin_vel_y: float,
@@ -186,6 +187,7 @@ class TargetPosVelocityGoal(BaseVelocityGoal):
        self.lin_vel_y = lin_vel_y
        self.ang_vel_yaw = ang_vel_yaw
        self.reach_threshold = reach_threshold
+        self.backward = backward

        kwargs.pop('enabled', None)
        if kwargs:
@@ -205,8 +207,12 @@ class TargetPosVelocityGoal(BaseVelocityGoal):
        self.update_runtime_count(sim_data)
        delta_pos, _, delta_ang = self.get_delta_info(sim_data)
        ang_vel_yaw = np.sign(delta_ang) * min(abs(delta_ang) * 2, self.ang_vel_yaw)
+        lin_vel_x = self.lin_vel_x if abs(delta_ang) < PI / 4 else 0.0
+        if self.backward:
+            ang_vel_yaw = -np.sign(delta_ang) * min((PI - abs(delta_ang)) * 2, self.ang_vel_yaw)
+            lin_vel_x = -self.lin_vel_x if abs(delta_ang) > PI * 3 / 4 else 0.0
        self.current_goal = VelocityGoal(
-            lin_vel_x=self.lin_vel_x if abs(delta_ang) < PI / 4 else 0.0,
+            lin_vel_x=lin_vel_x,
            lin_vel_y=np.sign(delta_pos[1]) * min(abs(delta_pos[1]), self.lin_vel_y) if abs(delta_ang) >= PI / 4 else 0.0,
            ang_vel_yaw=ang_vel_yaw
        )