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

4
robogauge/tasks/__init__.py
View File

@@ -13,3 +13,7 @@ task_register.register('go2_slope', BasePipeline, Go2SlopeMujocoConfig, Go2Slope
task_register.register('go2_moe_slope', BasePipeline, Go2SlopeMujocoConfig, Go2SlopeGaugeConfig, Go2MoEConfig)
task_register.register('go2_wave', BasePipeline, MujocoConfig, Go2WaveGaugeConfig, Go2Config)
task_register.register('go2_moe_wave', BasePipeline, MujocoConfig, Go2WaveGaugeConfig, Go2MoEConfig)
task_register.register('go2_stairs_up', BasePipeline, MujocoConfig, Go2StairsUpGaugeConfig, Go2Config)
task_register.register('go2_moe_stairs_up', BasePipeline, MujocoConfig, Go2StairsUpGaugeConfig, Go2MoEConfig)
task_register.register('go2_stairs_down', BasePipeline, MujocoConfig, Go2StairsDownGaugeConfig, Go2Config)
task_register.register('go2_moe_stairs_down', BasePipeline, MujocoConfig, Go2StairsDownGaugeConfig, Go2MoEConfig)

2
robogauge/tasks/custom/go2/__init__.py
View File

@@ -9,3 +9,5 @@
from .go2_flat_task import Go2FlatGaugeConfig
from .go2_slope_task import Go2SlopeGaugeConfig, Go2SlopeMujocoConfig
from .go2_wave_task import Go2WaveGaugeConfig
from .go2_stairs_up_task import Go2StairsUpGaugeConfig
from .go2_stairs_down_task import Go2StairsDownGaugeConfig

19
robogauge/tasks/custom/go2/go2_stairs_down_task.py Normal file
View File

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

19
robogauge/tasks/custom/go2/go2_stairs_up_task.py Normal file
View File

@@ -0,0 +1,19 @@
# -*- 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.robots import Go2Config, Go2MoEConfig
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

10
robogauge/tasks/custom/go2/go2_wave_task.py
View File

@@ -1,12 +1,4 @@
# -*- 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.robots import Go2Config, Go2MoEConfig
from robogauge.tasks.gauge import WaveGaugeConfig
from robogauge.tasks.simulator.mujoco_config import MujocoConfig

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)

25
robogauge/tasks/pipeline/base_pipeline.py
View File

@@ -53,7 +53,7 @@ class BasePipeline:
def load(self):
self.sim.load(
self.gauge_cfg.assets.terrain_xml,
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
@@ -68,7 +68,7 @@ class BasePipeline:
"Control dt must be multiple of simulation dt."
logger.info(f"Sim FPS: {1.0 / self.sim_cfg.physics.simulation_dt:.2f}, Control FPS: {1.0 / self.robot_cfg.control.control_dt:.2f}, Frame Skip: {frame_skip:d}")
logger.info("Running pipeline...")
error = None
warning, error = None, None
while not self.gauge.is_done():
try:
if self.first_reset: # wait for robot to be still
@@ -77,7 +77,10 @@ class BasePipeline:
velocity_goal=VelocityGoal(), # zero velocity
position_goal=PositionGoal(), # current position
)
if np.linalg.norm(sim_data.proprio.base.lin_vel) < 0.05 and sim_data.sim_time - self.last_reset_time > 0.1:
if (
(np.linalg.norm(sim_data.proprio.base.lin_vel) < 0.05 and sim_data.sim_time - self.last_reset_time > 0.1) or # robot is still
(sim_data.sim_time - self.last_reset_time) > 3.0 # wait max 3s
):
self.first_reset = False
else:
goal_data = self.gauge.get_goal(sim_data)
@@ -106,18 +109,24 @@ class BasePipeline:
if self.gauge.is_reset(sim_data):
sim_data = self.reset_sim_and_robot(sim_data)
except Exception as e:
error = e
logger.error(f"❌ Goal '{self.gauge.goal_str}' failed with error: {e},\n{traceback.format_exc()}")
self.gauge.switch_to_next_goal() # save current goal metrics
if str(e).startswith("[Penetration Error]"):
warning = e
logger.warning(f"⚠️ Penetration detected! Reset current goal and continue..., error: {e}")
self.gauge.reset_current_goal()
logger.info("⏩ Pipeline recovered from penetration and continued current goal 🎯.")
else:
error = e
logger.error(f"❌ Goal '{self.gauge.goal_str}' failed with error: {e},\n{traceback.format_exc()}")
self.gauge.switch_to_next_goal() # skip to next goal
logger.info("⏩ Pipeline recovered from error and continued next goal 🎯.")
sim_data = self.reset_sim_and_robot(sim_data)
logger.info("⏩ Pipeline recovered from error and continued next goal 🎯.")
self.sim.close_viewer()
self.sim.close_video_writer()
logger.info("✅ Pipeline execution finished.")
logger.info(f"📁 Logging saved at: {logger.log_dir}")
return self.gauge.results, error
return self.gauge.results, warning, error
def reset_sim_and_robot(self, sim_data: SimData):
self.sim.reset()

7
robogauge/tasks/pipeline/level_pipeline.py
View File

@@ -46,17 +46,16 @@ class LevelPipeline:
with open(logger.log_dir / "level_search_results.yaml", 'w') as f:
yaml.dump(level_results, f, allow_unicode=True, sort_keys=False)
return level, level_results
def test_level(self, level: int) -> bool:
logger.info(f"🔍 Testing level {level}...")
self.args.level = level
multi_pipeline = MultiPipeline(self.args)
aggregated_results = multi_pipeline.run()
success_mean = float(aggregated_results['success']['mean'].split(' ')[0])
all_success = success_mean == 1.0
all_success = success_mean >= 0.8
if all_success:
logger.info(f"✅ Level {level} passed all tests.")
else:
logger.info(f"❌ Level {level} failed some tests.")
return all_success, aggregated_results
return all_success, aggregated_results

4
robogauge/tasks/pipeline/multi_pipeline.py
View File

@@ -36,13 +36,15 @@ def run_single_process(args, data):
console_output=False
)
pipeline = task_register.make_pipeline(args=local_args, create_logger=False)
results, error = pipeline.run()
results, warning, error = pipeline.run()
if error is None:
ret = {
'status': 'success',
'results': results,
'model_path': pipeline.robot_cfg.control.model_path,
}
if warning is not None:
logger.warning(f"⚠️ Process with seed={seed}, base_mass={base_mass}, friction={friction} completed with warning: {warning}")
else:
logger.error(f"❌ Process with seed={seed}, base_mass={base_mass}, friction={friction} failed with error: {error}")
ret = {

1
robogauge/tasks/simulator/mujoco_config.py
View File

@@ -49,3 +49,4 @@ class MujocoConfig(Config):
class truncation:
enabled = True
projected_gravity_rad = 2.5 # [rad], if gravity projection angle exceeds this value, truncate episode
penetration_threshold = -0.05 # [m], if any contact penetration depth is below this threshold, truncate episode

34
robogauge/tasks/simulator/mujoco_simulator.py
View File

@@ -16,7 +16,7 @@ import time
import imageio
import numpy as np
from pathlib import Path
from typing import Literal
from typing import Literal, List
from robogauge.utils.logger import logger
from robogauge.utils.helpers import parse_path
@@ -30,7 +30,7 @@ from robogauge.tasks.simulator.sim_data import (
class MujocoSimulator:
def __init__(self, sim_cfg: MujocoConfig):
self.cfg = sim_cfg
self.terrain_xml = None
self.terrain_xmls = None
self.robot_xml = None
self.terrain_spawn_pos = None
self.robot_spawn_height = None
@@ -47,14 +47,14 @@ class MujocoSimulator:
def load(
self,
terrain_xml: str = None,
terrain_xmls: List[str] = None,
robot_xml: str = None,
terrain_spawn_pos: list = None,
default_dof_pos: list = None,
):
""" Load terrain and robot into the simulator, support re-loading. """
if terrain_xml is not None:
self.terrain_xml = parse_path(terrain_xml)
if terrain_xmls is not None:
self.terrain_xmls = [parse_path(xml) for xml in terrain_xmls]
if robot_xml is not None:
self.robot_xml = parse_path(robot_xml)
if terrain_spawn_pos is not None:
@@ -62,17 +62,20 @@ class MujocoSimulator:
if default_dof_pos is not None:
self.default_dof_pos = default_dof_pos
terrain_xml = self.terrain_xml
terrain_xmls = self.terrain_xmls
robot_xml = self.robot_xml
terrain_spawn_pos = self.terrain_spawn_pos
if terrain_xml is None or robot_xml is None:
if terrain_xmls is None or robot_xml is None:
raise ValueError("Terrain and robot XML paths must be provided.")
if default_dof_pos is None:
raise ValueError("Default DOF positions must be provided.")
# Create MJCF models
robot_mjcf = mjcf.from_path(robot_xml)
terrain_mjcf = mjcf.from_path(terrain_xml)
terrain_mjcf = mjcf.from_path(terrain_xmls[0])
for path in terrain_xmls[1:]:
next_terrain = mjcf.from_path(path)
terrain_mjcf.attach(next_terrain)
for j in robot_mjcf.find_all('joint'):
if j.tag == 'freejoint':
j.remove()
@@ -265,11 +268,24 @@ class MujocoSimulator:
self.check_truncation(sim_data)
return sim_data
def check_penetration(self, threshold: float = -0.02):
for i in range(self.mj_data.ncon):
contact = self.mj_data.contact[i]
if contact.dist < threshold:
geom1_name = mujoco.mj_id2name(self.mj_model, mujoco.mjtObj.mjOBJ_GEOM, contact.geom1)
geom2_name = mujoco.mj_id2name(self.mj_model, mujoco.mjtObj.mjOBJ_GEOM, contact.geom2)
return True, geom1_name, geom2_name, contact.dist
return False, None, None, None
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}")
raise RuntimeError(f"[Roll Error] Episode truncated due to excessive projected gravity, angle: {np.arccos(-projected_gravity[2]):.3f} rad, projected: {projected_gravity}")
# is_penetrated, geom1, geom2, dist = self.check_penetration(self.cfg.truncation.penetration_threshold)
# if is_penetrated:
# raise RuntimeError(f"[Penetration Error] Episode truncated: Penetration ({geom1} <-> {geom2}), distance: {dist}")
def reset(self):
""" Reset the simulator to initial state. """