From 1ab6c58fd179862960e9752a3f3da6addaf63dfc Mon Sep 17 00:00:00 2001 From: wty-yy <993660140@qq.com> Date: Tue, 24 Mar 2026 21:55:53 +0800 Subject: [PATCH] v1.1.6; fix terrain_weighted_summary bug --- UPDATE.md | 3 + assets/docs/friction_margin.md | 76 +++++++++++++++++ assets/docs/zmp_margin.md | 26 ++++-- .../tasks/gauge/metrics/stable_metric.py | 6 +- robogauge/tasks/pipeline/multi_pipeline.py | 2 +- scripts/{run.bash => run.sh} | 12 +-- scripts/{run_models.bash => run_models.sh} | 0 ...un_save_latent.bash => run_save_latent.sh} | 18 ++-- scripts/run_save_video.sh | 85 +++++++++++++++++++ setup.py | 2 +- 10 files changed, 202 insertions(+), 28 deletions(-) create mode 100644 assets/docs/friction_margin.md rename scripts/{run.bash => run.sh} (86%) rename scripts/{run_models.bash => run_models.sh} (100%) rename scripts/{run_save_latent.bash => run_save_latent.sh} (90%) create mode 100755 scripts/run_save_video.sh diff --git a/UPDATE.md b/UPDATE.md index db9c4b2..16eb1ba 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -1,4 +1,7 @@ # UPDATE +## 20260324 +### v1.1.6 +1. 修复multi_pipeline中统计terrain_weighted_summary的地形等级加权分错误，但不影响stress_pipeline最终计算的benchmark_score，只影响metrics里面的mean和mean@50的计算 ## 20260323 ### v1.1.5 1. 添加friction margin指标，计算足端切向力与法向力与摩擦系数比例，用法向力加权平均 diff --git a/assets/docs/friction_margin.md b/assets/docs/friction_margin.md new file mode 100644 index 0000000..096005e --- /dev/null +++ b/assets/docs/friction_margin.md @@ -0,0 +1,76 @@ +# friction_margin + +## 基本定义 + +该指标用于度量当前足端接触是否仍然处于可接受的库仑摩擦锥内。实现上，它不是直接对每个原始接触点求平均，而是先按足端 `geom` 聚合，再对每只参与计算的脚求摩擦裕度，最后使用法向力进行加权平均。 + +## MuJoCo 中的实现约定 + +在代码中，仅统计机器人配置 `foot_geom_names` 中声明的足端 `geom`。这里使用的是 `dynamics.contacts` 中已经整理出的机器人与外界之间的接触信息，并不会像 ZMP 指标那样额外按接触距离阈值再筛选一次。 + +设当前共有 $N$ 只足端参与摩擦裕度计算。对于第 $i$ 只足端，其可能包含多个原始接触点。将这些接触点的接触力聚合后，定义： + +- 切向力总和：$f_i^{\text{tangent}}$ +- 法向力总和：$f_i^{\text{normal}}$ +- 摩擦极限：$f_i^{\text{limit}}$ + +其中 + +$$ +f_i^{\text{normal}} = \sum_{j \in \mathcal{C}_i} f_{ij}^{\text{normal}} +$$ + +$$ +f_i^{\text{tangent}} = \sum_{j \in \mathcal{C}_i} f_{ij}^{\text{tangent}} +$$ + +$$ +f_i^{\text{limit}} = \sum_{j \in \mathcal{C}_i} \mu_{ij} f_{ij}^{\text{normal}} +$$ + +这里 $\mathcal{C}_i$ 表示第 $i$ 只脚对应的原始接触点集合，$\mu_{ij}$ 是第 $j$ 个接触点的摩擦系数。注意，代码中使用的是各接触点切向力模长的逐点求和结果，而不是先做二维切向合力向量相加后再取范数。 + +## 单足摩擦裕度 + +对第 $i$ 只足端，定义其摩擦利用率为 + +$$ +\nu_i = \frac{f_i^{\text{tangent}}}{f_i^{\text{limit}}} +$$ + +则单足摩擦裕度为 + +$$ +m_i^{\text{friction}} = \max \left(0, 1 - \nu_i \right) +$$ + +当某只脚的摩擦极限 $f_i^{\text{limit}}$ 过小或数值异常时，代码会直接将该足端的摩擦裕度记为 $0$。当某只脚的总法向力 $f_i^{\text{normal}}$ 不超过阈值 `force_threshold` 时，该脚不会参与最终平均。 + +## 总体 friction_margin + +整体指标使用各足端法向力作为权重进行加权平均。令 + +$$ +w_i = \frac{f_i^{\text{normal}}}{\sum_{k=1}^{N} f_k^{\text{normal}}} +$$ + +则总的 `friction_margin` 定义为 + +$$ +m_{\text{friction margin}} = \sum_{i=1}^{N} w_i \, m_i^{\text{friction}} = \sum_{i=1}^{N} w_i \max \left(0, 1 - \frac{f_i^{\text{tangent}}}{f_i^{\text{limit}}} \right) +$$ + +等价地，也可写为 + +$$ +m_{\text{friction margin}} = \sum_{i=1}^{N} w_i \max \left(0, 1 - \frac{f_i^{\text{tangent}}}{\sum_{j \in \mathcal{C}_i} \mu_{ij} f_{ij}^{\text{normal}}} \right) +$$ + +## 与代码一致的边界情况 + +- 当没有任何接触点时，指标直接返回 $1.0$。 +- 当存在接触，但没有任何接触点匹配到 `foot_geom_names` 时，指标也返回 $1.0$。 +- 当某只脚的总法向力小于等于阈值 `force_threshold` 时，该脚不会参与平均。 +- 日志中的 `friction_margin_contact_count` 统计的是匹配到足端 `geom` 的原始接触点数量，不是足端数量。 +- 日志中的 `friction_margin_foot_count` 统计的是最终参与计算的足端数量。 +- 日志中的 `friction_margin_worst_utilization` 记录的是所有参与计算足端中最大的摩擦利用率。 diff --git a/assets/docs/zmp_margin.md b/assets/docs/zmp_margin.md index 9975816..aa6b8f5 100644 --- a/assets/docs/zmp_margin.md +++ b/assets/docs/zmp_margin.md @@ -4,24 +4,34 @@ ## 基本定义 -- Zero Moment Point（ZMP，零矩点）：机械系统的惯性力和重力的净力矩在该点沿水平轴分量为零。 -- Support Polygon（支撑多边形）：机械系统所有与地面接触点构成的凸包。 -- FZMP（fictitious ZMP，虚拟零矩点）：当 ZMP 落在支撑多边形外时，称 ZMP 为 FZMP，此时系统可能处于非平衡状态。 -- Virtual Horizontal Plane（虚拟水平投影面）：机械系统所有与地面接触点的几何中心记为 $O'$，将世界坐标系平移到 $O'$，得到的 $xy$ 平面。 +- Zero Moment Point（ZMP，零矩点）：机械系统的惯性力与重力对某点的合力矩在水平轴方向上的分量为零时，该点称为 ZMP。 +- Support Polygon（支撑多边形）：所有有效地面接触点在水平面上的投影构成的凸包。 +- FZMP（fictitious ZMP，虚拟零矩点）：当 ZMP 落在支撑多边形外时，称其为 FZMP，此时系统可能处于非平衡状态。 +- Virtual Horizontal Plane（虚拟水平投影面）：记所有有效接触点的三维几何中心为 $O'$，将世界坐标系平移到 $O'$ 后，其 $xy$ 平面即本文使用的参考平面。 ## MuJoCo 中的符号约定 -设机器人总共包含 $N$ 个刚体。对于第 $i$ 个刚体，在当前时刻具有如下量： +在实现中，首先选出满足接触距离阈值的有效接触点 $\{\boldsymbol{c}_k\}_{k=1}^{K}$，并定义其几何中心为 + +$$ +O' = \frac{1}{K} \sum_{k=1}^{K} \boldsymbol{c}_k +$$ + +这里的 $O'$ 就是代码中的 `support_center`。注意它是所有有效接触点的简单平均，不是按接触力加权的中心，也不是支撑多边形的面积质心。 + +随后以 $O'$ 为原点建立平移后的参考平面，其方向仍与世界坐标系保持一致。 + +设机器人总共包含 $N$ 个动态刚体。这里的“动态刚体”指质量 $m_i > 0$ 的刚体。对于第 $i$ 个刚体，在当前时刻具有如下量： - 质量：$m_i$ - 质心位置：$\boldsymbol{p}_i$ - 这里相对于当前 `base_link` 在世界坐标系 $z$ 轴方向上平移到虚拟水平投影面的点。 + 这里表示该刚体质心相对于 $O'$ 的位置。 - 质心线加速度：$\boldsymbol{\ddot{p}}_i$ - 角速度：$\boldsymbol{\omega}_i$ - 角加速度：$\boldsymbol{\dot{\omega}}_i$ -- 惯性张量：$\boldsymbol{I}_i$ +- 惯性张量：$\boldsymbol{I}_i$，并且需要是世界坐标系下的惯性张量。 -上述量均相对于世界坐标系，保持相对于惯性参考系表达；本体坐标系不一定是参考系。计算 $\boldsymbol{I}_i$ 时需要确保其已经转换到世界坐标系下。 +除 $\boldsymbol{p}_i$ 是相对于 $O'$ 的相对位置外，其余运动学与动力学量均在世界坐标系下表达，并保持相对于惯性参考系计算。 ## 总力与总力矩 diff --git a/robogauge/tasks/gauge/metrics/stable_metric.py b/robogauge/tasks/gauge/metrics/stable_metric.py index 49e0ccf..2999171 100644 --- a/robogauge/tasks/gauge/metrics/stable_metric.py +++ b/robogauge/tasks/gauge/metrics/stable_metric.py @@ -142,9 +142,9 @@ class FrictionMarginMetric(BaseMetric): 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." - ) + # 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')) diff --git a/robogauge/tasks/pipeline/multi_pipeline.py b/robogauge/tasks/pipeline/multi_pipeline.py index 6114549..52ff683 100644 --- a/robogauge/tasks/pipeline/multi_pipeline.py +++ b/robogauge/tasks/pipeline/multi_pipeline.py @@ -172,7 +172,7 @@ class MultiPipeline: for mean_name, values in means.items(): twv = float(np.mean(values)) if summary['terrain_name'] in SEARCH_LEVELS_TERRAINS: - twv = 0.09 * (summary['terrain_level'] - 1) + 0.19 * v + twv = 0.09 * (summary['terrain_level'] - 1) + 0.19 * twv summary['terrain_weighted_summary'][metric][mean_name] = f"{twv:.4f} ± {float(np.std(values)):.4f}" save_path = multi_logger.log_dir / "aggregated_results.yaml" diff --git a/scripts/run.bash b/scripts/run.sh similarity index 86% rename from scripts/run.bash rename to scripts/run.sh index 2f08d48..fa4b8ee 100755 --- a/scripts/run.bash +++ b/scripts/run.sh @@ -10,7 +10,7 @@ python robogauge/scripts/run.py \ --num-processes 35 \ --seeds 0 1 2 \ --search-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 \ + --frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \ --compress-logs \ --headless @@ -23,7 +23,7 @@ python robogauge/scripts/run.py \ --num-processes 35 \ --seeds 0 1 2 \ --search-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 \ + --frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \ --compress-logs \ --headless @@ -36,7 +36,7 @@ python robogauge/scripts/run.py \ --num-processes 35 \ --seeds 0 1 2 \ --search-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 \ + --frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \ --compress-logs \ --headless @@ -48,7 +48,7 @@ python robogauge/scripts/run.py \ --num-processes 35 \ --seeds 0 1 2 \ --search-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 \ + --frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \ --compress-logs \ --headless @@ -61,7 +61,7 @@ python robogauge/scripts/run.py \ --num-processes 35 \ --seeds 0 1 2 \ --search-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 \ + --frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \ --compress-logs \ --headless @@ -74,6 +74,6 @@ python robogauge/scripts/run.py \ --num-processes 35 \ --seeds 0 1 2 \ --search-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 \ + --frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \ --compress-logs \ --headless diff --git a/scripts/run_models.bash b/scripts/run_models.sh similarity index 100% rename from scripts/run_models.bash rename to scripts/run_models.sh diff --git a/scripts/run_save_latent.bash b/scripts/run_save_latent.sh similarity index 90% rename from scripts/run_save_latent.bash rename to scripts/run_save_latent.sh index 502ab51..f5a98b9 100755 --- a/scripts/run_save_latent.bash +++ b/scripts/run_save_latent.sh @@ -1,7 +1,7 @@ #!/bin/bash # Change robogauge/tasks/robots/go2/go2_moe_config.py `save_additional_output = True` -# This +# Add change max velocity to only x=1m/s source /root/Programs/miniforge3/bin/activate robot @@ -21,7 +21,7 @@ MODEL_PATH="/root/Coding/RoboGauge/mytest/go2_moe_cts_79k_0.6637.pt" # --num-processes 35 \ # --seeds 0 1 2 \ # --search-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 \ +# --frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \ # --compress-logs \ # --headless @@ -30,7 +30,7 @@ python robogauge/scripts/run.py \ --model-path ${MODEL_PATH} \ --experiment-name latent \ --seed 0 \ - --friction 1.5 \ + --friction 0.6 \ --goals max_velocity \ --headless @@ -40,7 +40,7 @@ python robogauge/scripts/run.py \ --model-path ${MODEL_PATH} \ --experiment-name latent \ --seed 0 \ - --friction 1.5 \ + --friction 0.6 \ --goals max_velocity \ --headless @@ -50,7 +50,7 @@ python robogauge/scripts/run.py \ --model-path ${MODEL_PATH} \ --experiment-name latent \ --seed 0 \ - --friction 1.5 \ + --friction 0.6 \ --goals max_velocity \ --headless @@ -60,7 +60,7 @@ python robogauge/scripts/run.py \ --model-path ${MODEL_PATH} \ --experiment-name latent \ --seed 0 \ - --friction 1.5 \ + --friction 0.6 \ --goals max_velocity \ --headless @@ -70,7 +70,7 @@ python robogauge/scripts/run.py \ --model-path ${MODEL_PATH} \ --experiment-name latent \ --seed 0 \ - --friction 1.5 \ + --friction 0.6 \ --goals max_velocity \ --headless @@ -80,7 +80,7 @@ python robogauge/scripts/run.py \ --model-path ${MODEL_PATH} \ --experiment-name latent \ --seed 0 \ - --friction 1.5 \ + --friction 0.6 \ --goals max_velocity \ --headless @@ -90,6 +90,6 @@ python robogauge/scripts/run.py \ --model-path ${MODEL_PATH} \ --experiment-name latent \ --seed 0 \ - --friction 1.5 \ + --friction 0.6 \ --goals max_velocity \ --headless diff --git a/scripts/run_save_video.sh b/scripts/run_save_video.sh new file mode 100755 index 0000000..b110204 --- /dev/null +++ b/scripts/run_save_video.sh @@ -0,0 +1,85 @@ +#!/bin/bash + +MODEL_PATH="resources/models/go2/go2_moe_cts_137k_0.6739.pt" + +python robogauge/scripts/run.py \ + --task-name go2_moe.flat \ + --model-path ${MODEL_PATH} \ + --experiment-name save-video \ + --seed 0 \ + --friction 0.6 \ + --goals max_velocity \ + --save-video \ + --headless + +python robogauge/scripts/run.py \ + --task-name go2_moe.obstacle \ + --level 5 \ + --spawn-type level_eval \ + --model-path ${MODEL_PATH} \ + --experiment-name save-video \ + --seed 0 \ + --friction 0.6 \ + --goals max_velocity \ + --save-video \ + --headless + +# python robogauge/scripts/run.py \ +# --task-name go2_moe.slope_bd \ +# --level 5 \ +# --spawn-type level_eval \ +# --model-path ${MODEL_PATH} \ +# --experiment-name save-video \ +# --seed 0 \ +# --friction 0.6 \ +# --goals max_velocity \ +# --save-video \ +# --headless + +# python robogauge/scripts/run.py \ +# --task-name go2_moe.slope_fd \ +# --level 5 \ +# --spawn-type level_eval \ +# --model-path ${MODEL_PATH} \ +# --experiment-name save-video \ +# --seed 0 \ +# --friction 0.6 \ +# --goals max_velocity \ +# --save-video \ +# --headless + +# python robogauge/scripts/run.py \ +# --task-name go2_moe.stairs_bd \ +# --level 5 \ +# --spawn-type level_eval \ +# --model-path ${MODEL_PATH} \ +# --experiment-name save-video \ +# --seed 0 \ +# --friction 0.6 \ +# --goals max_velocity \ +# --save-video \ +# --headless + +python robogauge/scripts/run.py \ + --task-name go2_moe.stairs_fd \ + --level 5 \ + --spawn-type level_eval \ + --model-path ${MODEL_PATH} \ + --experiment-name save-video \ + --seed 0 \ + --friction 0.6 \ + --goals max_velocity \ + --save-video \ + --headless + +python robogauge/scripts/run.py \ + --task-name go2_moe.wave \ + --level 5 \ + --spawn-type level_eval \ + --model-path ${MODEL_PATH} \ + --experiment-name save-video \ + --seed 0 \ + --friction 0.6 \ + --goals max_velocity \ + --save-video \ + --headless diff --git a/setup.py b/setup.py index 865c9ff..b49479c 100644 --- a/setup.py +++ b/setup.py @@ -2,7 +2,7 @@ from setuptools import setup, find_packages setup( name="robogauge", - version="1.1.5", + version="1.1.6", author="Wu Tianyang", author_email="993660140@qq.com", description="A generic robot RL model evaluation library based on MuJoCo",