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

v1.1.5; Add friction margin

Browse Source
This commit is contained in:
wty-yy
2026-03-23 21:13:41 +08:00
parent fe35afea22
commit f713f70561
13 changed files with 172 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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

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

@@ -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

111
robogauge/tasks/gauge/metrics/stable_metric.py
View File

@@ -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

9
robogauge/tasks/pipeline/stress_pipeline.py
View File

@@ -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

1
robogauge/tasks/robots/base_robot_config.py
View File

@@ -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'

1
robogauge/tasks/robots/go2/go2_config.py
View File

@@ -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'

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

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

3
robogauge/tasks/simulator/sim_data.py
View File

@@ -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