v1.1.5; Add friction margin

.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"

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
 

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) 内容如下:

UPDATE.md

@@ -1,4 +1,8 @@
 # UPDATE
+## 20260323
+### v1.1.5
+1. 添加friction margin指标，计算足端切向力与法向力与摩擦系数比例，用法向力加权平均
+2. stress_pipeline中加入每个地形的得分方差
 ## 20260322
 ### v1.1.4
 1. 添加ZMP裕度指标

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

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

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

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))
+                mean_value = float(np.mean(values))
-            scores[terrain_name] = robust_score[terrain_name]['mean@50']
+                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

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'

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'

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,

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

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",