cyy/RoboGauge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v0.1.15; add stairs_up/down

Browse Source
This commit is contained in:
wty-yy
2025-12-25 18:15:48 +08:00
parent 0dd3c10aba
commit 9d9d83ed9f
54 changed files with 1356 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
robogauge/tasks/gauge/__init__.py
View File

@@ -3,3 +3,5 @@ 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.wave_gauge_config import WaveGaugeConfig
from .gauge_configs.stairs_up_gauge_config import StairsUpGaugeConfig
from .gauge_configs.stairs_down_gauge_config import StairsDownGaugeConfig

7
robogauge/tasks/gauge/base_gauge.py
View File

@@ -112,6 +112,13 @@ class BaseGauge:
logger.info(f"New Goal [{self.goal_idx+1}/{len(self.goals)}] [{goal_obj.count+1}/{goal_obj.total}]: {self.goal_str}")
return goal
def reset_current_goal(self):
""" Reset the current goal """
if self.goal_idx >= len(self.goals):
return
goal_obj = self.goals[self.goal_idx]
goal_obj.reset_goal()
def switch_to_next_goal(self):
""" Switch to the next goal and log the metrics of the current goal. """
goal_obj = self.goals[self.goal_idx]

2
robogauge/tasks/gauge/base_gauge_config.py
View File

@@ -16,7 +16,7 @@ class BaseGaugeConfig(Config):
class assets:
terrain_name = "flat"
terrain_level = 0
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
terrain_xmls = ['{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml']
terrain_spawn_pos = [0, 0, 0] # x y z [m], robot freejoint spawn position on the terrain
class goals:

2
robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py
View File

@@ -15,5 +15,5 @@ class FlatGaugeConfig(BaseGaugeConfig):
class assets(BaseGaugeConfig.assets):
terrain_name = "flat"
terrain_level = 0
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
terrain_xmls = ['{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml']
terrain_spawn_pos = [0, 0, 0] # x y z [m], robot freejoint spawn position on the terrain

9
robogauge/tasks/gauge/gauge_configs/slope_gauge_config.py
View File

@@ -15,13 +15,16 @@ class SlopeGaugeConfig(BaseGaugeConfig):
class assets(BaseGaugeConfig.assets):
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
terrain_xmls = [
'{ROBOGAUGE_ROOT_DIR}/resources/terrains/slope/slope_10.xml',
'{ROBOGAUGE_ROOT_DIR}/resources/terrains/wall/10x10_wall.xml',
]
terrain_spawn_pos = [0.8, 0, 1] # 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 = [5, 0, 2.28] # x y z [m], target position in the environment, used for target position goal
target_pos = [4, 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

33
robogauge/tasks/gauge/gauge_configs/stairs_down_gauge_config.py Normal file
View File

@@ -0,0 +1,33 @@
# -*- 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

32
robogauge/tasks/gauge/gauge_configs/stairs_up_gauge_config.py Normal file
View File

@@ -0,0 +1,32 @@
# -*- coding: utf-8 -*-
'''
@File : stairs_up_gauge_config.py
@Time : 2025/12/25 11:26:47
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Stairs Up Gauge Configuration
'''
from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
class StairsUpGaugeConfig(BaseGaugeConfig):
gauge_class = 'BaseGauge'
class assets(BaseGaugeConfig.assets):
terrain_name = "stairs_up"
terrain_level = 10 # 1-10
terrain_xmls = [
'{ROBOGAUGE_ROOT_DIR}/resources/terrains/stairs_up/stairs_up_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
class goals(BaseGaugeConfig.goals):
class target_pos_velocity: # goal to reach a target position by velocity command
enabled = True
target_pos = [4.5, 0.0, 3.8] # 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.1

103
robogauge/tasks/gauge/gauge_configs/terrain_levels_config.py
View File

@@ -1,3 +1,12 @@
# -*- coding: utf-8 -*-
'''
@File : terrain_levels_config.py
@Time : 2025/12/25 11:27:05
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Terrain Levels Configuration
'''
from robogauge.utils.config import Config
class TerrainLevelsConfig(Config):
@@ -8,30 +17,84 @@ class TerrainLevelsConfig(Config):
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],
[4, 0, 0.588 + 0.1],
[4, 0, 0.776 + 0.1],
[4, 0, 0.964 + 0.1],
[4, 0, 1.152 + 0.1],
[4, 0, 1.340 + 0.1],
[4, 0, 1.528 + 0.1],
[4, 0, 1.716 + 0.1],
[4, 0, 1.904 + 0.1],
[4, 0, 2.092 + 0.1],
[4, 0, 2.280 + 0.1],
]
spawns = [
[0.8, 0, 0.55 - 0.037 * 9],
[0.8, 0, 0.55 - 0.037 * 8],
[0.8, 0, 0.55 - 0.037 * 7],
[0.8, 0, 0.55 - 0.037 * 6],
[0.8, 0, 0.55 - 0.037 * 5],
[0.8, 0, 0.55 - 0.037 * 4],
[0.8, 0, 0.55 - 0.037 * 3],
[0.8, 0, 0.55 - 0.037 * 2],
[0.8, 0, 0.55 - 0.037],
[0.8, 0, 0.55],
]
class wave:
levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
targets = [
[6.5, 0.0, 0.08],
[6.5, 0.0, 0.16],
[6.5, 0.0, 0.24],
[6.5, 0.0, 0.32],
[6.5, 0.0, 0.40],
[6.5, 0.0, 0.48],
[6.5, 0.0, 0.56],
[6.5, 0.0, 0.64],
[6.5, 0.0, 0.72],
[6.5, 0.0, 0.80],
[6.5, -2.65, 0.08],
[6.5, -2.65, 0.16],
[6.5, -2.65, 0.24],
[6.5, -2.65, 0.32],
[6.5, -2.65, 0.40],
[6.5, -2.65, 0.48],
[6.5, -2.65, 0.56],
[6.5, -2.65, 0.64],
[6.5, -2.65, 0.72],
[6.5, -2.65, 0.80],
]
class stairs_up:
levels = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
targets = [
[4.5, 0.0, 1.35],
[4.5, 0.0, 1.80],
[4.5, 0.0, 2.25],
[4.5, 0.0, 2.70],
[4.5, 0.0, 2.85],
[4.5, 0.0, 3.00],
[4.5, 0.0, 3.15],
[4.5, 0.0, 3.30],
[4.5, 0.0, 3.45],
[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],
]

7
robogauge/tasks/gauge/gauge_configs/wave_gauge_config.py
View File

@@ -15,13 +15,16 @@ class WaveGaugeConfig(BaseGaugeConfig):
class assets(BaseGaugeConfig.assets):
terrain_name = "wave"
terrain_level = 10 # 1-10
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/wave/wave_10.xml'
terrain_xmls = [
'{ROBOGAUGE_ROOT_DIR}/resources/terrains/wave/wave_10.xml',
'{ROBOGAUGE_ROOT_DIR}/resources/terrains/wall/10x10_wall.xml',
]
terrain_spawn_pos = [1.5, 1.25, 1.0] # x y z [m], robot freejoint spawn position on the terrain
class goals:
class target_pos_velocity: # goal to reach a target position by velocity command
enabled = True
target_pos = [6.5, 0.0, 0.8] # x y z [m], target position in the environment, used for target position goal
target_pos = [6.5, -2.65, 0.8] # 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

3
robogauge/tasks/gauge/goals/base_goal.py
View File

@@ -25,6 +25,9 @@ class BaseGoal:
def pre_get_goal(self) -> bool:
raise NotImplementedError
def reset_goal(self):
raise NotImplementedError
def is_reset(self, sim_data: SimData) -> bool:
raise NotImplementedError

12
robogauge/tasks/gauge/goals/velocity_goals.py
View File

@@ -33,10 +33,15 @@ class BaseVelocityGoal(BaseGoal):
super().__init__()
self.control_dt = control_dt
self.cmd_duration = cmd_duration
self.goals = []
self.goal_runtime = 0.0
self.first_goal_after_reset = True
self.last_reset_time = -1
def reset_goal(self):
self.goal_runtime = 0.0
self.first_goal_after_reset = True
self.last_reset_time = -1
self.goals = []
def is_reset(self, sim_data: SimData) -> bool:
if self.last_reset_time == -1:
@@ -190,6 +195,11 @@ class TargetPosVelocityGoal(BaseVelocityGoal):
self.total = 1 # only one task
self.done = False
self.success = False
def reset_goal(self):
super().reset_goal()
self.done = False
self.success = False
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
self.update_runtime_count(sim_data)