From 587d0146833dcfcfd90ddc5e721ade6cf92e76e4 Mon Sep 17 00:00:00 2001 From: wty-yy <993660140@qq.com> Date: Mon, 29 Dec 2025 23:41:09 +0800 Subject: [PATCH] v1.0.0 --- CMD.md | 6 +- UPDATE.md | 2 +- .../stairs_up_1.xml => stairs/stairs_1.xml} | 4 +- .../stairs_up_10.xml => stairs/stairs_10.xml} | 4 +- .../stairs_up_2.xml => stairs/stairs_2.xml} | 4 +- .../stairs_up_3.xml => stairs/stairs_3.xml} | 4 +- .../stairs_up_4.xml => stairs/stairs_4.xml} | 4 +- .../stairs_up_5.xml => stairs/stairs_5.xml} | 4 +- .../stairs_up_6.xml => stairs/stairs_6.xml} | 4 +- .../stairs_up_7.xml => stairs/stairs_7.xml} | 4 +- .../stairs_up_8.xml => stairs/stairs_8.xml} | 4 +- .../stairs_up_9.xml => stairs/stairs_9.xml} | 4 +- .../terrains/stairs_down/stairs_down_1.xml | 51 ------- .../terrains/stairs_down/stairs_down_10.xml | 51 ------- .../terrains/stairs_down/stairs_down_2.xml | 51 ------- .../terrains/stairs_down/stairs_down_3.xml | 51 ------- .../terrains/stairs_down/stairs_down_4.xml | 51 ------- .../terrains/stairs_down/stairs_down_5.xml | 51 ------- .../terrains/stairs_down/stairs_down_6.xml | 51 ------- .../terrains/stairs_down/stairs_down_7.xml | 51 ------- .../terrains/stairs_down/stairs_down_8.xml | 51 ------- .../terrains/stairs_down/stairs_down_9.xml | 51 ------- robogauge/scripts/server.py | 2 +- robogauge/tasks/__init__.py | 14 +- robogauge/tasks/custom/go2/__init__.py | 5 +- robogauge/tasks/custom/go2/go2_slope_task.py | 15 +- .../tasks/custom/go2/go2_stairs_down_task.py | 19 --- robogauge/tasks/custom/go2/go2_stairs_task.py | 25 ++++ .../tasks/custom/go2/go2_stairs_up_task.py | 18 --- robogauge/tasks/gauge/__init__.py | 5 +- robogauge/tasks/gauge/base_gauge.py | 2 +- robogauge/tasks/gauge/base_gauge_config.py | 1 + .../gauge/gauge_configs/slope_gauge_config.py | 13 +- .../gauge_configs/stairs_down_gauge_config.py | 33 ----- ...gauge_config.py => stairs_gauge_config.py} | 14 +- .../gauge_configs/terrain_levels_config.py | 128 +++++++++--------- robogauge/tasks/gauge/goals/velocity_goals.py | 8 +- robogauge/tasks/pipeline/base_pipeline.py | 3 +- robogauge/tasks/pipeline/stress_pipeline.py | 12 +- robogauge/tasks/simulator/mujoco_simulator.py | 11 ++ robogauge/utils/helpers.py | 4 +- robogauge/utils/task_register.py | 6 +- .../utils/visualize/plot_terrain_levels.py | 2 +- run.bash | 12 +- 44 files changed, 196 insertions(+), 714 deletions(-) rename resources/terrains/{stairs_up/stairs_up_1.xml => stairs/stairs_1.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_10.xml => stairs/stairs_10.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_2.xml => stairs/stairs_2.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_3.xml => stairs/stairs_3.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_4.xml => stairs/stairs_4.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_5.xml => stairs/stairs_5.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_6.xml => stairs/stairs_6.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_7.xml => stairs/stairs_7.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_8.xml => stairs/stairs_8.xml} (97%) rename resources/terrains/{stairs_up/stairs_up_9.xml => stairs/stairs_9.xml} (97%) delete mode 100644 resources/terrains/stairs_down/stairs_down_1.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_10.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_2.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_3.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_4.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_5.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_6.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_7.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_8.xml delete mode 100644 resources/terrains/stairs_down/stairs_down_9.xml delete mode 100644 robogauge/tasks/custom/go2/go2_stairs_down_task.py create mode 100644 robogauge/tasks/custom/go2/go2_stairs_task.py delete mode 100644 robogauge/tasks/custom/go2/go2_stairs_up_task.py delete mode 100644 robogauge/tasks/gauge/gauge_configs/stairs_down_gauge_config.py rename robogauge/tasks/gauge/gauge_configs/{stairs_up_gauge_config.py => stairs_gauge_config.py} (73%) diff --git a/CMD.md b/CMD.md index 7612592..d3fc8fa 100644 --- a/CMD.md +++ b/CMD.md @@ -7,7 +7,7 @@ python robogauge/scripts/run.py \ --headless # To evaluate metrics for level terrains, add `--goals` and `--spawn-type` -# level terrains: wave, slope, stairs up, stairs down, obstacle (default is target_pos) +# level terrains: wave, slope_fb, slope_bd, stairs_fb, stairs_bd, obstacle (default is target_pos) python robogauge/scripts/run.py \ --task go2_moe.obstacle \ --experiment-name debug \ @@ -31,7 +31,7 @@ python robogauge/scripts/run.py \ # Terrain with Level, need specify goals (default is target_pos) python robogauge/scripts/run.py \ - --task go2_moe.stairs_up \ + --task go2_moe.stairs_bd \ --experiment-name debug \ --multi \ --num-processes 5 \ @@ -63,7 +63,7 @@ python robogauge/scripts/run.py \ --task go2_moe \ --experiment-name debug \ --stress-benchmark \ - --stress-terrain-names flat slope stairs_up stairs_down wave obstacle \ + --stress-terrain-names flat slope_fb slope_bd stairs_fb stairs_bd wave obstacle \ --num-processes 50 \ --seeds 0 1 2 3 4 \ --frictions 0.5 0.75 1.0 1.25 1.5 1.75 2.0 2.25 2.5 \ diff --git a/UPDATE.md b/UPDATE.md index 6801f32..f78e97c 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -4,7 +4,7 @@ 1. 完成stress pipeline的客户端, 服务端代码, 支持异步推理 2. 加入MultiPipeline和LevelPipeline分开的种子数量`seeds, search_seeds`, Level 5个, Multi 3个, 这样可以显著提高速度, 并且保证评估精度不掉 (两种评估总分前三位都是相同的), 速度提升`3m 32s -> 2m 47s` 3. 删除stairs_down任务, 将stairs_up重命名为stairs -4. 对于slope, stairs再分化为两个任务, slope_fd, slope_bd, stairs_fd, stairs_bd, 分别表示正向/背向朝更高处的初始化, target_pos_velocity中指令同时根据是否backward反转 +4. 对于slope, stairs再分化为两个任务, slope_fd, slope_bd, stairs_fd, stairs_bd, 分别表示正向/背向朝更高处的初始化, target_pos_velocity中指令同时根据是否backward反转, 分别为slope, stairs写初始化位置, 原来6个地形换成现在7种, 测试用时变为`2m 59s` ## 20251228 ### v0.1.18 1. 将final_score改为merge_metrics, 并在multi_pipeline中也进行该评估 diff --git a/resources/terrains/stairs_up/stairs_up_1.xml b/resources/terrains/stairs/stairs_1.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_1.xml rename to resources/terrains/stairs/stairs_1.xml index 71c1230..ac9409f 100644 --- a/resources/terrains/stairs_up/stairs_up_1.xml +++ b/resources/terrains/stairs/stairs_1.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_10.xml b/resources/terrains/stairs/stairs_10.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_10.xml rename to resources/terrains/stairs/stairs_10.xml index b7d9f50..4c47f67 100644 --- a/resources/terrains/stairs_up/stairs_up_10.xml +++ b/resources/terrains/stairs/stairs_10.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_2.xml b/resources/terrains/stairs/stairs_2.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_2.xml rename to resources/terrains/stairs/stairs_2.xml index badcb3a..2b4f89b 100644 --- a/resources/terrains/stairs_up/stairs_up_2.xml +++ b/resources/terrains/stairs/stairs_2.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_3.xml b/resources/terrains/stairs/stairs_3.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_3.xml rename to resources/terrains/stairs/stairs_3.xml index 5b69914..f0e6aac 100644 --- a/resources/terrains/stairs_up/stairs_up_3.xml +++ b/resources/terrains/stairs/stairs_3.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_4.xml b/resources/terrains/stairs/stairs_4.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_4.xml rename to resources/terrains/stairs/stairs_4.xml index b39461b..ab08763 100644 --- a/resources/terrains/stairs_up/stairs_up_4.xml +++ b/resources/terrains/stairs/stairs_4.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_5.xml b/resources/terrains/stairs/stairs_5.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_5.xml rename to resources/terrains/stairs/stairs_5.xml index fc7dc62..9062da5 100644 --- a/resources/terrains/stairs_up/stairs_up_5.xml +++ b/resources/terrains/stairs/stairs_5.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_6.xml b/resources/terrains/stairs/stairs_6.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_6.xml rename to resources/terrains/stairs/stairs_6.xml index 0fe8990..b103418 100644 --- a/resources/terrains/stairs_up/stairs_up_6.xml +++ b/resources/terrains/stairs/stairs_6.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_7.xml b/resources/terrains/stairs/stairs_7.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_7.xml rename to resources/terrains/stairs/stairs_7.xml index 7451e9f..7c61295 100644 --- a/resources/terrains/stairs_up/stairs_up_7.xml +++ b/resources/terrains/stairs/stairs_7.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_8.xml b/resources/terrains/stairs/stairs_8.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_8.xml rename to resources/terrains/stairs/stairs_8.xml index 593035f..91eba23 100644 --- a/resources/terrains/stairs_up/stairs_up_8.xml +++ b/resources/terrains/stairs/stairs_8.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_up/stairs_up_9.xml b/resources/terrains/stairs/stairs_9.xml similarity index 97% rename from resources/terrains/stairs_up/stairs_up_9.xml rename to resources/terrains/stairs/stairs_9.xml index db33eeb..3197cc8 100644 --- a/resources/terrains/stairs_up/stairs_up_9.xml +++ b/resources/terrains/stairs/stairs_9.xml @@ -1,5 +1,5 @@ - - + + diff --git a/resources/terrains/stairs_down/stairs_down_1.xml b/resources/terrains/stairs_down/stairs_down_1.xml deleted file mode 100644 index 88830b5..0000000 --- a/resources/terrains/stairs_down/stairs_down_1.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_10.xml b/resources/terrains/stairs_down/stairs_down_10.xml deleted file mode 100644 index 0c801c2..0000000 --- a/resources/terrains/stairs_down/stairs_down_10.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_2.xml b/resources/terrains/stairs_down/stairs_down_2.xml deleted file mode 100644 index 1640ee6..0000000 --- a/resources/terrains/stairs_down/stairs_down_2.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_3.xml b/resources/terrains/stairs_down/stairs_down_3.xml deleted file mode 100644 index 1f6e4a7..0000000 --- a/resources/terrains/stairs_down/stairs_down_3.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_4.xml b/resources/terrains/stairs_down/stairs_down_4.xml deleted file mode 100644 index 7dbac2f..0000000 --- a/resources/terrains/stairs_down/stairs_down_4.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_5.xml b/resources/terrains/stairs_down/stairs_down_5.xml deleted file mode 100644 index 636fbd4..0000000 --- a/resources/terrains/stairs_down/stairs_down_5.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_6.xml b/resources/terrains/stairs_down/stairs_down_6.xml deleted file mode 100644 index 2c43342..0000000 --- a/resources/terrains/stairs_down/stairs_down_6.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_7.xml b/resources/terrains/stairs_down/stairs_down_7.xml deleted file mode 100644 index 8303c39..0000000 --- a/resources/terrains/stairs_down/stairs_down_7.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_8.xml b/resources/terrains/stairs_down/stairs_down_8.xml deleted file mode 100644 index 0b29551..0000000 --- a/resources/terrains/stairs_down/stairs_down_8.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/resources/terrains/stairs_down/stairs_down_9.xml b/resources/terrains/stairs_down/stairs_down_9.xml deleted file mode 100644 index 425d97c..0000000 --- a/resources/terrains/stairs_down/stairs_down_9.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robogauge/scripts/server.py b/robogauge/scripts/server.py index 2685664..00dbde3 100644 --- a/robogauge/scripts/server.py +++ b/robogauge/scripts/server.py @@ -28,7 +28,7 @@ from pprint import pprint default_args_list = [ '--stress-benchmark', - # '--stress-terrain-names', 'flat', 'wave', 'slope', 'stairs_up', 'stairs_down', 'obstacle', + # '--stress-terrain-names', 'flat', 'wave', 'slope_fd', 'slope_bd', 'stairs_fd', 'stairs_bd', 'obstacle', '--stress-terrain-names', 'flat', 'wave', '--num-processes', '50', '--seeds', '0', '1', '2', diff --git a/robogauge/tasks/__init__.py b/robogauge/tasks/__init__.py index d2f7242..9482d6a 100644 --- a/robogauge/tasks/__init__.py +++ b/robogauge/tasks/__init__.py @@ -11,16 +11,18 @@ task_register.register('base', BasePipeline, MujocoConfig, BaseGaugeConfig, Robo # Go2 MLP task_register.register('go2.flat', BasePipeline, MujocoConfig, Go2FlatGaugeConfig, Go2Config) -task_register.register('go2.slope', BasePipeline, MujocoConfig, Go2SlopeGaugeConfig, Go2TerrainConfig) +task_register.register('go2.slope_fd', BasePipeline, MujocoConfig, Go2SlopeForwardGaugeConfig, Go2TerrainConfig) +task_register.register('go2.slope_bd', BasePipeline, MujocoConfig, Go2SlopeBackwardGaugeConfig, Go2TerrainConfig) task_register.register('go2.wave', BasePipeline, MujocoConfig, Go2WaveGaugeConfig, Go2TerrainConfig) -task_register.register('go2.stairs_up', BasePipeline, MujocoConfig, Go2StairsUpGaugeConfig, Go2TerrainConfig) -task_register.register('go2.stairs_down', BasePipeline, MujocoConfig, Go2StairsDownGaugeConfig, Go2TerrainConfig) +task_register.register('go2.stairs_fd', BasePipeline, MujocoConfig, Go2StairsForwardGaugeConfig, Go2TerrainConfig) +task_register.register('go2.stairs_bd', BasePipeline, MujocoConfig, Go2StairsBackwardGaugeConfig, Go2TerrainConfig) task_register.register('go2.obstacle', BasePipeline, MujocoConfig, Go2ObstacleGaugeConfig, Go2TerrainConfig) # Go2 MoE task_register.register('go2_moe.flat', BasePipeline, MujocoConfig, Go2FlatGaugeConfig, Go2MoEConfig) -task_register.register('go2_moe.slope', BasePipeline, MujocoConfig, Go2SlopeGaugeConfig, Go2MoETerrainConfig) +task_register.register('go2_moe.slope_fd', BasePipeline, MujocoConfig, Go2SlopeForwardGaugeConfig, Go2MoETerrainConfig) +task_register.register('go2_moe.slope_bd', BasePipeline, MujocoConfig, Go2SlopeBackwardGaugeConfig, Go2MoETerrainConfig) task_register.register('go2_moe.wave', BasePipeline, MujocoConfig, Go2WaveGaugeConfig, Go2MoETerrainConfig) -task_register.register('go2_moe.stairs_up', BasePipeline, MujocoConfig, Go2StairsUpGaugeConfig, Go2MoETerrainConfig) -task_register.register('go2_moe.stairs_down', BasePipeline, MujocoConfig, Go2StairsDownGaugeConfig, Go2MoETerrainConfig) +task_register.register('go2_moe.stairs_fd', BasePipeline, MujocoConfig, Go2StairsForwardGaugeConfig, Go2MoETerrainConfig) +task_register.register('go2_moe.stairs_bd', BasePipeline, MujocoConfig, Go2StairsBackwardGaugeConfig, Go2MoETerrainConfig) task_register.register('go2_moe.obstacle', BasePipeline, MujocoConfig, Go2ObstacleGaugeConfig, Go2MoETerrainConfig) diff --git a/robogauge/tasks/custom/go2/__init__.py b/robogauge/tasks/custom/go2/__init__.py index 8cd836d..fcf3380 100644 --- a/robogauge/tasks/custom/go2/__init__.py +++ b/robogauge/tasks/custom/go2/__init__.py @@ -7,8 +7,7 @@ @Blog : https://wty-yy.github.io/ ''' from .go2_flat_task import Go2FlatGaugeConfig -from .go2_slope_task import Go2SlopeGaugeConfig +from .go2_slope_task import Go2SlopeForwardGaugeConfig, Go2SlopeBackwardGaugeConfig from .go2_wave_task import Go2WaveGaugeConfig -from .go2_stairs_up_task import Go2StairsUpGaugeConfig -from .go2_stairs_down_task import Go2StairsDownGaugeConfig +from .go2_stairs_task import Go2StairsForwardGaugeConfig, Go2StairsBackwardGaugeConfig from .go2_obstacle_task import Go2ObstacleGaugeConfig diff --git a/robogauge/tasks/custom/go2/go2_slope_task.py b/robogauge/tasks/custom/go2/go2_slope_task.py index 55bb7cf..3b1169b 100644 --- a/robogauge/tasks/custom/go2/go2_slope_task.py +++ b/robogauge/tasks/custom/go2/go2_slope_task.py @@ -8,12 +8,19 @@ @Desc : Go2 Slope Task Configuration ''' from robogauge.tasks.robots import Go2Config, Go2MoEConfig -from robogauge.tasks.gauge import SlopeGaugeConfig +from robogauge.tasks.gauge import SlopeForwardGaugeConfig, SlopeBackwardGaugeConfig from robogauge.tasks.simulator.mujoco_config import MujocoConfig -class Go2SlopeGaugeConfig(SlopeGaugeConfig): - class metrics(SlopeGaugeConfig.metrics): - class dof_limits(SlopeGaugeConfig.metrics.dof_limits): +class Go2SlopeForwardGaugeConfig(SlopeForwardGaugeConfig): + class metrics(SlopeForwardGaugeConfig.metrics): + class dof_limits(SlopeForwardGaugeConfig.metrics.dof_limits): + enabled = True + soft_dof_limit_ratio = 0.7 + dof_names = ['hip', 'thigh'] # List of DOF names to monitor, None for all + +class Go2SlopeBackwardGaugeConfig(SlopeBackwardGaugeConfig): + class metrics(SlopeBackwardGaugeConfig.metrics): + class dof_limits(SlopeBackwardGaugeConfig.metrics.dof_limits): enabled = True soft_dof_limit_ratio = 0.7 dof_names = ['hip', 'thigh'] # List of DOF names to monitor, None for all diff --git a/robogauge/tasks/custom/go2/go2_stairs_down_task.py b/robogauge/tasks/custom/go2/go2_stairs_down_task.py deleted file mode 100644 index 6896d67..0000000 --- a/robogauge/tasks/custom/go2/go2_stairs_down_task.py +++ /dev/null @@ -1,19 +0,0 @@ -# -*- coding: utf-8 -*- -''' -@File : go2_stairs_down_task.py -@Time : 2025/12/25 16:36:18 -@Author : wty-yy -@Version : 1.0 -@Blog : https://wty-yy.github.io/ -@Desc : Go2 Stairs Down Task Configuration -''' -from robogauge.tasks.robots import Go2Config, Go2MoEConfig -from robogauge.tasks.gauge import StairsDownGaugeConfig -from robogauge.tasks.simulator.mujoco_config import MujocoConfig - -class Go2StairsDownGaugeConfig(StairsDownGaugeConfig): - class metrics(StairsDownGaugeConfig.metrics): - class dof_limits(StairsDownGaugeConfig.metrics.dof_limits): - enabled = True - soft_dof_limit_ratio = 0.7 - dof_names = ['hip', 'thigh'] # List of DOF names to monitor, None for all diff --git a/robogauge/tasks/custom/go2/go2_stairs_task.py b/robogauge/tasks/custom/go2/go2_stairs_task.py new file mode 100644 index 0000000..fc82678 --- /dev/null +++ b/robogauge/tasks/custom/go2/go2_stairs_task.py @@ -0,0 +1,25 @@ +# -*- coding: utf-8 -*- +''' +@File : go2_wave_task.py +@Time : 2025/12/22 11:02:23 +@Author : wty-yy +@Version : 1.0 +@Blog : https://wty-yy.github.io/ +@Desc : Go2 Wave Task Configuration +''' +from robogauge.tasks.gauge import StairsForwardGaugeConfig, StairsBackwardGaugeConfig +from robogauge.tasks.simulator.mujoco_config import MujocoConfig + +class Go2StairsForwardGaugeConfig(StairsForwardGaugeConfig): + class metrics(StairsForwardGaugeConfig.metrics): + class dof_limits(StairsForwardGaugeConfig.metrics.dof_limits): + enabled = True + soft_dof_limit_ratio = 0.7 + dof_names = ['hip', 'thigh'] # List of DOF names to monitor, None for all + +class Go2StairsBackwardGaugeConfig(StairsBackwardGaugeConfig): + class metrics(StairsBackwardGaugeConfig.metrics): + class dof_limits(StairsBackwardGaugeConfig.metrics.dof_limits): + enabled = True + soft_dof_limit_ratio = 0.7 + dof_names = ['hip', 'thigh'] # List of DOF names to monitor, None for all diff --git a/robogauge/tasks/custom/go2/go2_stairs_up_task.py b/robogauge/tasks/custom/go2/go2_stairs_up_task.py deleted file mode 100644 index 6639e85..0000000 --- a/robogauge/tasks/custom/go2/go2_stairs_up_task.py +++ /dev/null @@ -1,18 +0,0 @@ -# -*- coding: utf-8 -*- -''' -@File : go2_wave_task.py -@Time : 2025/12/22 11:02:23 -@Author : wty-yy -@Version : 1.0 -@Blog : https://wty-yy.github.io/ -@Desc : Go2 Wave Task Configuration -''' -from robogauge.tasks.gauge import StairsUpGaugeConfig -from robogauge.tasks.simulator.mujoco_config import MujocoConfig - -class Go2StairsUpGaugeConfig(StairsUpGaugeConfig): - class metrics(StairsUpGaugeConfig.metrics): - class dof_limits(StairsUpGaugeConfig.metrics.dof_limits): - enabled = True - soft_dof_limit_ratio = 0.7 - dof_names = ['hip', 'thigh'] # List of DOF names to monitor, None for all diff --git a/robogauge/tasks/gauge/__init__.py b/robogauge/tasks/gauge/__init__.py index 4fb00cd..9a03467 100644 --- 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 diff --git a/robogauge/tasks/gauge/base_gauge.py b/robogauge/tasks/gauge/base_gauge.py index dbe9b2c..0128ef4 100644 --- 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.") diff --git a/robogauge/tasks/gauge/base_gauge_config.py b/robogauge/tasks/gauge/base_gauge_config.py index b60fcbc..32612a9 100644 --- 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" diff --git a/robogauge/tasks/gauge/gauge_configs/slope_gauge_config.py b/robogauge/tasks/gauge/gauge_configs/slope_gauge_config.py index 1478f70..f60293c 100644 --- 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 - \ No newline at end of file + +class SlopeBackwardGaugeConfig(SlopeForwardGaugeConfig): + backward = True + + class assets(SlopeForwardGaugeConfig.assets): + terrain_name = "slope_bd" diff --git a/robogauge/tasks/gauge/gauge_configs/stairs_down_gauge_config.py b/robogauge/tasks/gauge/gauge_configs/stairs_down_gauge_config.py deleted file mode 100644 index 0afbab2..0000000 --- a/robogauge/tasks/gauge/gauge_configs/stairs_down_gauge_config.py +++ /dev/null @@ -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 diff --git a/robogauge/tasks/gauge/gauge_configs/stairs_up_gauge_config.py b/robogauge/tasks/gauge/gauge_configs/stairs_gauge_config.py similarity index 73% rename from robogauge/tasks/gauge/gauge_configs/stairs_up_gauge_config.py rename to robogauge/tasks/gauge/gauge_configs/stairs_gauge_config.py index 4bb07a6..09adadd 100644 --- a/robogauge/tasks/gauge/gauge_configs/stairs_up_gauge_config.py +++ b/robogauge/tasks/gauge/gauge_configs/stairs_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" diff --git a/robogauge/tasks/gauge/gauge_configs/terrain_levels_config.py b/robogauge/tasks/gauge/gauge_configs/terrain_levels_config.py index 678728c..acd10e4 100644 --- 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) diff --git a/robogauge/tasks/gauge/goals/velocity_goals.py b/robogauge/tasks/gauge/goals/velocity_goals.py index e02a210..9c39c26 100644 --- 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 ) diff --git a/robogauge/tasks/pipeline/base_pipeline.py b/robogauge/tasks/pipeline/base_pipeline.py index 989cf69..7cd5914 100644 --- a/robogauge/tasks/pipeline/base_pipeline.py +++ b/robogauge/tasks/pipeline/base_pipeline.py @@ -56,7 +56,8 @@ class BasePipeline: self.gauge_cfg.assets.terrain_xmls, self.robot_cfg.assets.robot_xml, self.gauge_cfg.assets.terrain_spawn_pos, - self.robot_cfg.control.default_dof_pos + self.robot_cfg.control.default_dof_pos, + self.gauge_cfg.backward, ) def run(self): diff --git a/robogauge/tasks/pipeline/stress_pipeline.py b/robogauge/tasks/pipeline/stress_pipeline.py index d475a40..94b99b4 100644 --- a/robogauge/tasks/pipeline/stress_pipeline.py +++ b/robogauge/tasks/pipeline/stress_pipeline.py @@ -192,11 +192,10 @@ class StressPipeline: continue summary[key] = result['results'] - if terrain_name != 'stairs_down': # skip stairs_down for metrics means calculation - for metric, means in result['results']['terrain_weighted_summary'].items(): - for mean_name, value_str in means.items(): - value = float(value_str.split(' ± ')[0]) - metric_collections[metric][mean_name].append(value) + for metric, means in result['results']['terrain_weighted_summary'].items(): + for mean_name, value_str in means.items(): + value = float(value_str.split(' ± ')[0]) + metric_collections[metric][mean_name].append(value) for mean_name, value in result['results']['terrain_quality_score'].items(): terrain_collections[terrain_name][mean_name].append(value) @@ -214,8 +213,7 @@ class StressPipeline: values.extend([0.0] * zero_terrain_count[terrain_name]) # include zero terrains robust_score[terrain_name][mean_name] = float(np.mean(values)) scores[terrain_name] = robust_score[terrain_name]['mean@50'] - if terrain_name != 'stairs_down': # skip stairs_down for benchmark score calculation - robust_scores.append(robust_score[terrain_name]['mean@50']) + robust_scores.append(robust_score[terrain_name]['mean@50']) summary['robust_score'] = dict(robust_score) summary['benchmark_score'] = float(np.mean(robust_scores)) scores['benchmark'] = summary['benchmark_score'] diff --git a/robogauge/tasks/simulator/mujoco_simulator.py b/robogauge/tasks/simulator/mujoco_simulator.py index 666b7c3..b562a48 100644 --- a/robogauge/tasks/simulator/mujoco_simulator.py +++ b/robogauge/tasks/simulator/mujoco_simulator.py @@ -35,6 +35,7 @@ class MujocoSimulator: self.terrain_spawn_pos = None self.robot_spawn_height = None self.default_dof_pos = None + self.invert_yaw = None self.viewer = None self.offscreen_cam = mujoco.MjvCamera() self.renderer = None @@ -52,6 +53,7 @@ class MujocoSimulator: robot_xml: str = None, terrain_spawn_pos: list = None, default_dof_pos: list = None, + invert_yaw: bool = None, ): """ Load terrain and robot into the simulator, support re-loading. """ if terrain_xmls is not None: @@ -62,6 +64,8 @@ class MujocoSimulator: self.terrain_spawn_pos = terrain_spawn_pos if default_dof_pos is not None: self.default_dof_pos = default_dof_pos + if invert_yaw is not None: + self.invert_yaw = invert_yaw terrain_xmls = self.terrain_xmls robot_xml = self.robot_xml @@ -91,6 +95,9 @@ class MujocoSimulator: self.mj_data = self.mj_physics.data.ptr self.mj_model.opt.timestep = self.cfg.physics.simulation_dt self.sim_dt = self.cfg.physics.simulation_dt + if self.invert_yaw: + self.mj_data.qpos[3] = 0.0 + self.mj_data.qpos[6] = 1.0 self.mj_data.qpos[7:] = default_dof_pos # Domain randomization: base mass @@ -265,6 +272,7 @@ class MujocoSimulator: proprio=proprio ) + # input("DEBUG") self.n_step += 1 self.sim_time = self.n_step * self.sim_dt self.check_truncation(sim_data) @@ -305,6 +313,9 @@ class MujocoSimulator: def reset(self): """ Reset the simulator to initial state. """ self.mj_physics.reset() + if self.invert_yaw: + self.mj_data.qpos[3] = 0.0 + self.mj_data.qpos[6] = 1.0 self.mj_data.qpos[7:] = self.default_dof_pos mujoco.mj_forward(self.mj_model, self.mj_data) diff --git a/robogauge/utils/helpers.py b/robogauge/utils/helpers.py index 09fd03a..0cc2e76 100644 --- a/robogauge/utils/helpers.py +++ b/robogauge/utils/helpers.py @@ -76,7 +76,7 @@ def parse_args(args_list=None): {"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)."}, - {"name": "--spawn-type", "type": str, "choices": ["level_eval", "level_search"], "help": "Spawn type for the robot."}, + {"name": "--spawn-type", "type": str, "default": "level_search", "choices": ["level_eval", "level_search"], "help": "Spawn type for the robot when specify level (Default is level_search)."}, {"name": "--goals", "type": str, "nargs": "+", "help": "List of goal names to evaluate."}, # Multiprocessing parameters, with different seeds @@ -91,7 +91,7 @@ def parse_args(args_list=None): # Stress pipeline parameters {"name": "--stress-benchmark", "action": "store_true", "default": False, "help": "Use stress pipeline to benchmark model robustness."}, - {"name": "--stress-terrain-names", "type": str, "nargs": "+", "default": ["flat", "slope", "wave", "stairs_up", "stairs_down"], "help": "List of terrain names for stress benchmark."}, + {"name": "--stress-terrain-names", "type": str, "nargs": "+", "default": ["flat", "slope_fd", "slope_bd", "wave", "stairs_fd", "stairs_bd"], "help": "List of terrain names for stress benchmark."}, # Common parameters {"name": "--num-processes", "type": int, "default": 2, "help": "Number of parallel processes for Multi or Stress benchmark."}, diff --git a/robogauge/utils/task_register.py b/robogauge/utils/task_register.py index 3e12851..c012570 100644 --- a/robogauge/utils/task_register.py +++ b/robogauge/utils/task_register.py @@ -10,7 +10,7 @@ from robogauge import ROBOGAUGE_ROOT_DIR from robogauge.utils.logger import logger from robogauge.utils.helpers import parse_args, set_seed, class_to_dict -from robogauge.tasks.gauge.gauge_configs.terrain_levels_config import TerrainSearchLevelsConfig, TerrainEvalLevelsConfig +from robogauge.tasks.gauge.gauge_configs.terrain_levels_config import TerrainSearchLevelsConfig, TerrainEvalLevelsConfig, TERRAIN_NAME2_XML_NAME class TaskRegister(): def __init__(self): @@ -82,10 +82,10 @@ class TaskRegister(): gauger_cfg.goals.target_pos_velocity.target_pos = search_cfg.targets[search_cfg.levels.index(args.level)] if hasattr(search_cfg, 'spawns') and args.spawn_type == "level_search": gauger_cfg.assets.terrain_spawn_pos = search_cfg.spawns[search_cfg.levels.index(args.level)] - elif hasattr(eval_cfg, 'spawns') and args.spawn_type == "level_eval": + if hasattr(eval_cfg, 'spawns') and args.spawn_type == "level_eval": gauger_cfg.assets.terrain_spawn_pos = eval_cfg.spawns[eval_cfg.levels.index(args.level)] xml = gauger_cfg.assets.terrain_xmls[0] - xml = xml.rsplit('/', 1)[0] + f"/{gauger_cfg.assets.terrain_name}_{args.level}.xml" + xml = xml.rsplit('/', 1)[0] + f"/{TERRAIN_NAME2_XML_NAME[gauger_cfg.assets.terrain_name]}_{args.level}.xml" gauger_cfg.assets.terrain_xmls[0] = xml if args.goals is not None: keys = class_to_dict(gauger_cfg.goals).keys() diff --git a/robogauge/utils/visualize/plot_terrain_levels.py b/robogauge/utils/visualize/plot_terrain_levels.py index 2bb11d4..8a8ec62 100644 --- a/robogauge/utils/visualize/plot_terrain_levels.py +++ b/robogauge/utils/visualize/plot_terrain_levels.py @@ -33,7 +33,7 @@ def load_terrain_data(file_paths): Returns: model_data: {model_name: {terrain_type: {friction: level}}} """ - target_terrains = ['wave', 'slope', 'stairs_up', 'stairs_down', 'obstacle'] + target_terrains = ['wave', 'slope_fd', 'slope_bd', 'stairs_fd', 'stairs_bd', 'obstacle'] model_data = {} # 用于从键名中提取 friction 的正则 (例如 friction1.25) diff --git a/run.bash b/run.bash index 0b01872..7708b72 100755 --- a/run.bash +++ b/run.bash @@ -6,7 +6,7 @@ python robogauge/scripts/run.py \ --task-name go2_moe \ --experiment-name debug \ --stress-benchmark \ - --stress-terrain-names flat slope stairs_up stairs_down wave obstacle \ + --stress-terrain-names flat slope_fd slope_bd stairs_fd stairs_bd wave obstacle \ --num-processes 62 \ --seeds 0 1 2 \ --search-seeds 0 1 2 3 4 \ @@ -19,7 +19,7 @@ python robogauge/scripts/run.py \ --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_hard_terrain_141k.pt \ --experiment-name debug \ --stress-benchmark \ - --stress-terrain-names flat slope stairs_up stairs_down wave obstacle \ + --stress-terrain-names flat slope_fd slope_bd stairs_fd stairs_bd wave obstacle \ --num-processes 62 \ --seeds 0 1 2 \ --search-seeds 0 1 2 3 4 \ @@ -32,7 +32,7 @@ python robogauge/scripts/run.py \ --model-path /root/Coding/RoboGauge/mytest/go2_cts_hard_terrain_141k.pt \ --experiment-name debug \ --stress-benchmark \ - --stress-terrain-names flat slope stairs_up stairs_down wave obstacle \ + --stress-terrain-names flat slope_fd slope_bd stairs_fd stairs_bd wave obstacle \ --num-processes 62 \ --seeds 0 1 2 \ --search-seeds 0 1 2 3 4 \ @@ -44,7 +44,7 @@ python robogauge/scripts/run.py \ --task-name go2 \ --experiment-name debug \ --stress-benchmark \ - --stress-terrain-names flat slope stairs_up stairs_down wave obstacle \ + --stress-terrain-names flat slope_fd slope_bd stairs_fd stairs_bd wave obstacle \ --num-processes 62 \ --seeds 0 1 2 \ --search-seeds 0 1 2 3 4 \ @@ -57,7 +57,7 @@ python robogauge/scripts/run.py \ --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_hard_terrain_100k.pt \ --experiment-name debug \ --stress-benchmark \ - --stress-terrain-names flat slope stairs_up stairs_down wave obstacle \ + --stress-terrain-names flat slope_fd slope_bd stairs_fd stairs_bd wave obstacle \ --num-processes 62 \ --seeds 0 1 2 \ --search-seeds 0 1 2 3 4 \ @@ -70,7 +70,7 @@ python robogauge/scripts/run.py \ --model-path /root/Coding/RoboGauge/mytest/go2_cts_hard_terrain_100k.pt \ --experiment-name debug \ --stress-benchmark \ - --stress-terrain-names flat slope stairs_up stairs_down wave obstacle \ + --stress-terrain-names flat slope_fd slope_bd stairs_fd stairs_bd wave obstacle \ --num-processes 62 \ --seeds 0 1 2 \ --search-seeds 0 1 2 3 4 \