From f713f7056175647e681e44083061b48f01d300e7 Mon Sep 17 00:00:00 2001 From: wty-yy <993660140@qq.com> Date: Mon, 23 Mar 2026 21:13:41 +0800 Subject: [PATCH] v1.1.5; Add friction margin --- .vscode/launch.json | 21 +++- README.md | 3 +- README_zh.md | 3 +- UPDATE.md | 4 + robogauge/tasks/gauge/base_gauge_config.py | 5 + robogauge/tasks/gauge/metrics/__init__.py | 2 +- .../tasks/gauge/metrics/stable_metric.py | 111 ++++++++++++++++++ robogauge/tasks/pipeline/stress_pipeline.py | 9 +- robogauge/tasks/robots/base_robot_config.py | 1 + robogauge/tasks/robots/go2/go2_config.py | 1 + robogauge/tasks/simulator/mujoco_simulator.py | 16 +++ robogauge/tasks/simulator/sim_data.py | 3 + setup.py | 2 +- 13 files changed, 172 insertions(+), 9 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index ce37e73..00b0853 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,21 @@ // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "name": "go2 joystick debug", + "type": "debugpy", + "request": "launch", + "program": "${workspaceFolder}/robogauge/scripts/run.py", + "args": [ + "--task", "go2_moe.flat", + "--experiment-name", "debug", + "--seed", "0", + "--friction", "0.6", + "--goals", "joystick", + "--write-tensorboard", + ], + "console": "integratedTerminal" + }, { "name": "go2 moe flat single pipeline", "type": "debugpy", @@ -13,7 +28,7 @@ "--task", "go2_moe.flat", "--experiment-name", "debug", "--seed", "0", - "--friction", "1.6", + "--friction", "0.6", "--headless" ], "console": "integratedTerminal" @@ -29,7 +44,7 @@ "--seed", "0", // "--search-max-level", // "--seeds", "0", "1", "2", - "--frictions", "1", + "--frictions", "0.4", // "--headless" ], "console": "integratedTerminal" @@ -45,7 +60,7 @@ "--experiment-name", "mcp_debug", "--seed", "3", "--level", "8", - "--frictions", "0.5", + "--frictions", "0.2", ], "env": {"DISPLAY": ":1"}, "console": "integratedTerminal" diff --git a/README.md b/README.md index 0bf50b6..02f201b 100644 --- a/README.md +++ b/README.md @@ -133,7 +133,8 @@ Supported per-step metrics (measured after each `env.step`). **All metrics are n | 4 | `dof_power` | Motor energy consumption | Scaling factor | 100 | `1-x` | | 5 | `orientation_stability` | Body orientation stability (Roll) | NA | NA | `1-x` | | 6 | `torque_smoothness` | Torque smoothness | Scaling factor | 30 | `1-x` | -| 7 | `zmp_margin` | ZMP margin | NA | Default diagonal foot distance | `1-x` | +| 7 | `friction_margin` | Foot friction margin | Foot contact geom names | $\mu f_{\text{normal}}$ | `1-x` | +| 8 | `zmp_margin` | ZMP margin | NA | Default diagonal foot distance | `1-x` | ### Velocity-Tracking Targets diff --git a/README_zh.md b/README_zh.md index 9815f7d..7a1ee25 100644 --- a/README_zh.md +++ b/README_zh.md @@ -119,7 +119,8 @@ while True: | 4 | `dof_power` | 电机耗能 | 缩放系数 | 100 | `1-x` | | 5 | `orientation_stability` | 机身姿态稳定性 (Roll) | NA | NA | `1-x` | | 6 | `torque_smoothness` | 力矩平滑度 | 缩放系数 | 30 | `1-x` | -| 7 | `zmp_margin` | ZMP裕度 | NA | 默认状态下对角足端间距 | `1-x` | +| 7 | `friction_margin` | 足端摩擦裕度 | 足端接触 geom 名称 | $\mu f_{\text{normal}}$ | `1-x` | +| 8 | `zmp_margin` | ZMP裕度 | NA | 默认状态下对角足端间距 | `1-x` | ### 速度追踪目标 针对在虚实迁移中发现的问题, 整理指标 (metrics) 内容如下: diff --git a/UPDATE.md b/UPDATE.md index ea32e9a..db9c4b2 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -1,4 +1,8 @@ # UPDATE +## 20260323 +### v1.1.5 +1. 添加friction margin指标，计算足端切向力与法向力与摩擦系数比例，用法向力加权平均 +2. stress_pipeline中加入每个地形的得分方差 ## 20260322 ### v1.1.4 1. 添加ZMP裕度指标 diff --git a/robogauge/tasks/gauge/base_gauge_config.py b/robogauge/tasks/gauge/base_gauge_config.py index b7b04e0..7f7ce8a 100644 --- a/robogauge/tasks/gauge/base_gauge_config.py +++ b/robogauge/tasks/gauge/base_gauge_config.py @@ -16,6 +16,7 @@ QUALITY_WEIGHTS = { # Weights for geometric average, to calculate quality scor 'dof_power': 1, 'orientation_stability': 1, 'torque_smoothness': 1, + 'friction_margin': 1, 'zmp_margin': 1, } @@ -83,6 +84,10 @@ class BaseGaugeConfig(Config): enabled = True scaling_factor = 30.0 # [Nm] scaling factor for torque smoothness metric + class friction_margin: + enabled = True + force_threshold = 5.0 # [N] skip feet with too small accumulated normal force + class zmp_margin: enabled = True contact_threshold = 1e-3 # [m] contact.dist <= threshold is treated as support contact diff --git a/robogauge/tasks/gauge/metrics/__init__.py b/robogauge/tasks/gauge/metrics/__init__.py index 8428b30..7c1791c 100644 --- a/robogauge/tasks/gauge/metrics/__init__.py +++ b/robogauge/tasks/gauge/metrics/__init__.py @@ -2,4 +2,4 @@ from .base_metric import BaseMetric from .dof_metrics import DofLimitsMetric, DofPowerMetric from .visualization import VisualizationMetric from .vel_metrics import LinVelErrMetric, AngVelErrMetric -from .stable_metric import OrientationStabilityMetric, TorqueSmoothnessMetric, ZmpMarginMetric +from .stable_metric import OrientationStabilityMetric, TorqueSmoothnessMetric, FrictionMarginMetric, ZmpMarginMetric diff --git a/robogauge/tasks/gauge/metrics/stable_metric.py b/robogauge/tasks/gauge/metrics/stable_metric.py index 920b569..49e0ccf 100644 --- a/robogauge/tasks/gauge/metrics/stable_metric.py +++ b/robogauge/tasks/gauge/metrics/stable_metric.py @@ -62,6 +62,117 @@ class TorqueSmoothnessMetric(BaseMetric): return metric_value +def _normalize_name(name: str) -> str: + if name is None: + return "" + return name.rsplit('/', 1)[-1] + + +class FrictionMarginMetric(BaseMetric): + """ Metric to log the friction margin of the contacting feet. """ + name = 'friction_margin_metric' + + def __init__(self, + robot_cfg: RobotConfig, + foot_geom_names: list = None, + force_threshold: float = 1e-6, + **kwargs + ): + super().__init__(robot_cfg) + if foot_geom_names is None: + foot_geom_names = getattr(robot_cfg.assets, 'foot_geom_names', None) + if foot_geom_names is None or len(foot_geom_names) == 0: + raise ValueError( + "[FrictionMarginMetric] foot_geom_names is required. " + "Please configure robot_cfg.assets.foot_geom_names." + ) + self.foot_geom_names = {_normalize_name(name) for name in foot_geom_names} + self.force_threshold = force_threshold + + def __call__(self, sim_data: SimData, goal_data: GoalData) -> float: + dynamics = sim_data.dynamics + if dynamics is None or dynamics.contacts is None: + raise RuntimeError("Friction margin metric requires sim_data.dynamics.contacts, but got None.") + + contacts = dynamics.contacts + if contacts.positions.shape[0] == 0: + logger.log(1.0, 'stable_metric/friction_margin', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_foot_count', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_contact_count', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_worst_utilization', step=sim_data.n_step) + return 1.0 + + foot_force_map = {} + valid_contact_count = 0 + for idx, geom_name in enumerate(contacts.robot_geom_names): + normalized_geom_name = _normalize_name(geom_name) + if normalized_geom_name not in self.foot_geom_names: + continue + + if normalized_geom_name not in foot_force_map: + foot_force_map[normalized_geom_name] = { + 'normal': 0.0, + 'tangent': 0.0, + 'friction_limit': 0.0, + } + + normal_force = float(contacts.normal_forces[idx]) + tangent_force = float(contacts.tangent_forces[idx]) + friction_coeff = float(contacts.friction_coefficients[idx]) + foot_force_map[normalized_geom_name]['normal'] += normal_force + foot_force_map[normalized_geom_name]['tangent'] += tangent_force + foot_force_map[normalized_geom_name]['friction_limit'] += friction_coeff * normal_force + valid_contact_count += 1 + + if len(foot_force_map) == 0: + logger.warning( + f"Friction margin metric found no matching foot contacts using " + f"foot_geom_names={sorted(self.foot_geom_names)}." + ) + logger.log(1.0, 'stable_metric/friction_margin', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_foot_count', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_contact_count', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_worst_utilization', step=sim_data.n_step) + return 1.0 + + foot_margins = [] + foot_normal_forces = [] + utilization_values = [] + for foot_name, force_data in foot_force_map.items(): + if force_data['normal'] <= self.force_threshold: + continue + if force_data['friction_limit'] <= self.force_threshold: + logger.warning( + f"Friction margin metric got too small friction limit on foot {foot_name}, returning 0 for this foot." + ) + foot_margins.append(0.0) + foot_normal_forces.append(force_data['normal']) + utilization_values.append(float('inf')) + continue + + utilization = force_data['tangent'] / force_data['friction_limit'] + foot_margins.append(max(0.0, 1.0 - utilization)) + foot_normal_forces.append(force_data['normal']) + utilization_values.append(utilization) + + if len(foot_margins) == 0: + logger.log(1.0, 'stable_metric/friction_margin', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_foot_count', step=sim_data.n_step) + logger.log(float(valid_contact_count), 'stable_metric/friction_margin_contact_count', step=sim_data.n_step) + logger.log(0.0, 'stable_metric/friction_margin_worst_utilization', step=sim_data.n_step) + return 1.0 + + metric_value = float(np.average(foot_margins, weights=np.array(foot_normal_forces, dtype=np.float32))) + worst_utilization = max(utilization_values) + if not np.isfinite(worst_utilization): + worst_utilization = 0.0 + logger.log(metric_value, 'stable_metric/friction_margin', step=sim_data.n_step) + logger.log(float(len(foot_margins)), 'stable_metric/friction_margin_foot_count', step=sim_data.n_step) + logger.log(float(valid_contact_count), 'stable_metric/friction_margin_contact_count', step=sim_data.n_step) + logger.log(float(worst_utilization), 'stable_metric/friction_margin_worst_utilization', step=sim_data.n_step) + return metric_value + + def _cross_2d(o: np.ndarray, a: np.ndarray, b: np.ndarray) -> float: oa = a - o ob = b - o diff --git a/robogauge/tasks/pipeline/stress_pipeline.py b/robogauge/tasks/pipeline/stress_pipeline.py index b200fcb..d9b9931 100644 --- a/robogauge/tasks/pipeline/stress_pipeline.py +++ b/robogauge/tasks/pipeline/stress_pipeline.py @@ -223,10 +223,15 @@ class StressPipeline: summary['summary'][metric][mean_name] = f"{float(np.mean(values)):.4f} ± {float(np.std(values)):.4f}" for terrain_name, means in terrain_collections.items(): + terrain_mean_at_50 = 0.0 for mean_name, values in means.items(): 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'] + mean_value = float(np.mean(values)) + variance_value = float(np.var(values)) + robust_score[terrain_name][mean_name] = f"{mean_value:.4f} ± {variance_value:.4f}" + if mean_name == 'mean@50': + terrain_mean_at_50 = mean_value + scores[terrain_name] = terrain_mean_at_50 for terrain_name in robust_score: if len(robust_score[terrain_name]) == 0: robust_score[terrain_name] = None diff --git a/robogauge/tasks/robots/base_robot_config.py b/robogauge/tasks/robots/base_robot_config.py index b781077..cbe5a86 100644 --- a/robogauge/tasks/robots/base_robot_config.py +++ b/robogauge/tasks/robots/base_robot_config.py @@ -17,6 +17,7 @@ class RobotConfig(Config): class assets: robot_xml = "{ROBOGAUGE_ROOT_DIR}/resources/robots/go2/go2.xml" + foot_geom_names = [] # List of foot contact geom names (without Mujoco attach prefix) class control: device = 'cpu' diff --git a/robogauge/tasks/robots/go2/go2_config.py b/robogauge/tasks/robots/go2/go2_config.py index ed4571d..60c6eca 100644 --- a/robogauge/tasks/robots/go2/go2_config.py +++ b/robogauge/tasks/robots/go2/go2_config.py @@ -17,6 +17,7 @@ class Go2Config(RobotConfig): class assets: robot_xml = "{ROBOGAUGE_ROOT_DIR}/resources/robots/go2/go2.xml" robot_spawn_height = 0.1 # z [m] + foot_geom_names = ['FL', 'FR', 'RL', 'RR'] class control(RobotConfig.control): device = 'cpu' diff --git a/robogauge/tasks/simulator/mujoco_simulator.py b/robogauge/tasks/simulator/mujoco_simulator.py index 198e8d2..96a842b 100644 --- a/robogauge/tasks/simulator/mujoco_simulator.py +++ b/robogauge/tasks/simulator/mujoco_simulator.py @@ -755,6 +755,9 @@ class MujocoSimulator: def get_ground_contact_state(self) -> GroundContactState: positions = [] distances = [] + normal_forces = [] + tangent_forces = [] + friction_coefficients = [] robot_geom_names = [] other_geom_names = [] robot_body_names = [] @@ -776,8 +779,15 @@ class MujocoSimulator: robot_body = body1 if is_robot1 else body2 other_body = body2 if is_robot1 else body1 + contact_force = np.zeros(6, dtype=np.float64) + mujoco.mj_contactForce(self.mj_model, self.mj_data, i, contact_force) + friction = np.array(contact.friction, dtype=np.float32).reshape(-1) + positions.append(np.array(contact.pos, dtype=np.float32)) distances.append(float(contact.dist)) + normal_forces.append(float(abs(contact_force[0]))) + tangent_forces.append(float(np.linalg.norm(contact_force[1:3]))) + friction_coefficients.append(float(friction[0]) if friction.size > 0 else 0.0) robot_geom_names.append(self._get_geom_name(robot_geom)) other_geom_names.append(self._get_geom_name(other_geom)) robot_body_names.append(self._get_body_name(robot_body)) @@ -785,9 +795,15 @@ class MujocoSimulator: positions = np.array(positions, dtype=np.float32).reshape(-1, 3) distances = np.array(distances, dtype=np.float32) + normal_forces = np.array(normal_forces, dtype=np.float32) + tangent_forces = np.array(tangent_forces, dtype=np.float32) + friction_coefficients = np.array(friction_coefficients, dtype=np.float32) return GroundContactState( positions=positions, distances=distances, + normal_forces=normal_forces, + tangent_forces=tangent_forces, + friction_coefficients=friction_coefficients, robot_geom_names=robot_geom_names, other_geom_names=other_geom_names, robot_body_names=robot_body_names, diff --git a/robogauge/tasks/simulator/sim_data.py b/robogauge/tasks/simulator/sim_data.py index 0407508..26f1b05 100644 --- a/robogauge/tasks/simulator/sim_data.py +++ b/robogauge/tasks/simulator/sim_data.py @@ -45,6 +45,9 @@ class RigidBodyDynamics: class GroundContactState: positions: np.ndarray # [m] world frame, shape (n_contact, 3) distances: np.ndarray # [m] shape (n_contact,) + normal_forces: np.ndarray # [N] contact-frame normal force magnitude, shape (n_contact,) + tangent_forces: np.ndarray # [N] contact-frame tangential force magnitude, shape (n_contact,) + friction_coefficients: np.ndarray # [-] translational friction coefficient, shape (n_contact,) robot_geom_names: list # list of robot geom names other_geom_names: list # list of non-robot geom names robot_body_names: list # list of robot body names diff --git a/setup.py b/setup.py index aa6af74..865c9ff 100644 --- a/setup.py +++ b/setup.py @@ -2,7 +2,7 @@ from setuptools import setup, find_packages setup( name="robogauge", - version="1.1.4", + version="1.1.5", author="Wu Tianyang", author_email="993660140@qq.com", description="A generic robot RL model evaluation library based on MuJoCo",