v1.1.1-rc1; add visualize tools

UPDATE.md

@@ -1,4 +1,12 @@
 # UPDATE
+## 20260122
+### v1.1.1-rc1
+1. 加入可视化潜空间的绘图工具，使用方法：
+    1. 打开`robogauge/tasks/robots/go2/go2_moe_config.py`中的`save_additional_output = True`
+    2. `robogauge/tasks/gauge/goals/velocity_goals.py`中注释掉`self.current_goal = VelocityGoal()  # zero velocity`
+    3. 修改评估脚本`scripts/run_save_latent.bash`中模型为评估模型，开始评估
+    4. 将评估得到的`logs/*latent/`文件夹都移动到`logs_latent/{model_name}`目录下: `mkdir -p logs_latent/{model_name} && mv logs/*latent logs_latent/{model_name}/`
+    5. 修改`robogauge/utils/visualize/plot_latent_pca_cmd.py`或`robogauge/utils/visualize/plot_latent_pca_terrain.py`中的`data_root`为对应的`logs_latent/{model_name}`目录，运行脚本即可生成潜空间PCA可视化图
 ## 20260113
 ### v1.1.1
 1. 修复base lin vel计算错误，错误将世界坐标系下的速度作为了body坐标系下的速度，导致关键指标计算错误，重新评估

robogauge/utils/visualize/plot_latent_pca_cmd.py

@@ -1,3 +1,12 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : plot_latent_pca_cmd.py
+@Time    : 2026/01/22 23:32:20
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : PCA Visualization of Latent Space Grouped by Control Command (All Terrains Mixed)
+'''
 import numpy as np
 import matplotlib.pyplot as plt
 from sklearn.decomposition import PCA
@@ -5,7 +14,7 @@ import os
 import glob
 
 # ================= 配置区域 =================
-data_root = "/root/Coding/RoboGauge/logs_latent"
+data_root = "/root/Coding/RoboGauge/logs_latent/rem"
 
 # 地形列表 (我们需要遍历所有地形来收集同一个指令的数据)
 terrains = ['flat', 'wave', 'slope_fd', 'slope_bd', 'stairs_fd', 'stairs_bd', 'obstacle']
@@ -137,4 +146,4 @@ plt.grid(True, linestyle='--', alpha=0.4)
 save_path = 'latent_pca_by_command_mixed.png'
 plt.savefig(save_path)
 print(f"Done! Visualization saved to {save_path}")
-plt.show()
+# plt.show()

robogauge/utils/visualize/plot_latent_pca_terrain.py

@@ -0,0 +1,165 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : plot_latent_pca_terrain.py
+@Time    : 2026/01/22 23:31:55
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : PCA Visualization of Latent Space Grouped by Terrain for Command 0 (Forward)
+'''
+import numpy as np
+import matplotlib.pyplot as plt
+from sklearn.decomposition import PCA
+import os
+import glob
+from pathlib import Path
+
+# ================= 配置区域 =================
+data_root = "/root/Coding/RoboGauge/logs_latent/rem"
+# data_root = "/root/Coding/RoboGauge/logs_latent/cts"
+# data_root = "/root/Coding/RoboGauge/logs_latent/moe"
+# data_root = "/root/Coding/RoboGauge/logs_latent/rem_0.6357"
+# data_root = "/root/Coding/RoboGauge/logs_latent/moe_0.6637"
+
+# 地形列表
+# terrains = ['flat', 'wave', 'slope_fd', 'slope_bd', 'stairs_fd', 'stairs_bd', 'obstacle']
+terrains = ['flat', 'wave', 'stairs_fd', 'obstacle']
+
+# 我们只关注 指令 0 (Pos X)
+TARGET_CMD_ID = 0 
+TARGET_CMD_NAME = "Pos X"
+
+# 每个地形最大采样数 (防止绘图过慢或重叠严重)
+MAX_SAMPLES_PER_TERRAIN = 3000
+
+# 地形颜色映射 (使用 matplotlib 的 tab10 色板，确保区分度)
+# 为每个地形分配一个固定颜色
+terrain_colors = {
+    'flat': '#1f77b4',      # 蓝
+    'wave': '#ff7f0e',      # 橙
+    'slope_fd': '#2ca02c',  # 绿
+    'slope_bd': '#d62728',  # 红
+    'stairs_fd': '#9467bd', # 紫
+    'stairs_bd': '#8c564b', # 棕
+    'obstacle': '#e377c2'   # 粉
+}
+
+# ================= 数据加载函数 =================
+
+def load_cmd0_data_by_terrain(root_path, terrain_list, cmd_id):
+    """
+    加载指定 cmd_id 的数据，并按地形分类返回。
+    返回结构: { 'flat': np.array((N, dim)), 'wave': ... }
+    """
+    data_dict = {}
+    
+    filename = f"moe_info_{cmd_id}.npz"
+    print(f"[*] Loading data for Command {cmd_id} ({TARGET_CMD_NAME})...")
+
+    for t_name in terrain_list:
+        # 构建搜索路径: root/go2_moe_{terrain}_latent/*/moe_info_{id}.npz
+        # 注意：中间有个 "*" 匹配时间戳文件夹
+        search_pattern = os.path.join(root_path, f"go2_moe_{t_name}_latent", "*", filename)
+        files = glob.glob(search_pattern)
+        
+        terrain_vectors = []
+        
+        for f in files:
+            try:
+                raw = np.load(f)
+                if 'latent' in raw:
+                    vec = raw['latent']
+                    # 确保是二维数组 (N, D)
+                    if len(vec.shape) == 1:
+                        vec = vec.reshape(1, -1)
+                    terrain_vectors.append(vec)
+            except Exception as e:
+                print(f"Error loading {f}: {e}")
+
+        if terrain_vectors:
+            # 合并该地形下所有文件的向量
+            full_data = np.vstack(terrain_vectors)
+            
+            # --- 采样处理 ---
+            n_samples = full_data.shape[0]
+            if n_samples > MAX_SAMPLES_PER_TERRAIN:
+                # 随机采样索引
+                indices = np.random.choice(n_samples, MAX_SAMPLES_PER_TERRAIN, replace=False)
+                full_data = full_data[indices]
+            
+            data_dict[t_name] = full_data
+            print(f"    -> Terrain '{t_name}': Loaded {full_data.shape[0]} samples (Original: {n_samples})")
+        else:
+            print(f"    -> Terrain '{t_name}': No data found.")
+            
+    return data_dict
+
+# ================= 主程序 =================
+
+# 1. 加载数据
+terrain_data = load_cmd0_data_by_terrain(data_root, terrains, TARGET_CMD_ID)
+
+if not terrain_data:
+    print("Error: No data loaded. Please check the path.")
+    exit()
+
+# 2. 准备 PCA 数据
+#我们需要将所有数据堆叠在一起进行 PCA fit，以便它们处于同一个坐标系中
+all_vectors = []
+all_labels = [] # 用于记录每一行数据属于哪个地形
+
+for t_name, vectors in terrain_data.items():
+    all_vectors.append(vectors)
+    # 记录对应的标签，长度与 vectors 的行数相同
+    all_labels.extend([t_name] * vectors.shape[0])
+
+X = np.vstack(all_vectors)
+print(f"[*] Starting PCA on matrix shape: {X.shape} ...")
+
+# 3. 执行 PCA 降维
+pca = PCA(n_components=2)
+X_pca = pca.fit_transform(X)
+
+# 计算解释方差比 (Explained Variance Ratio)
+evr = pca.explained_variance_ratio_
+print(f"[*] PCA Done. Explained Variance: PC1={evr[0]:.2%}, PC2={evr[1]:.2%}")
+
+# 4. 绘图
+plt.figure(figsize=(10, 8), dpi=100)
+
+# 当前绘图的起止索引
+start_idx = 0
+
+for t_name in terrains:
+    if t_name not in terrain_data:
+        continue
+        
+    count = terrain_data[t_name].shape[0]
+    end_idx = start_idx + count
+    
+    # 提取该地形对应的 PCA 坐标
+    # X_pca 的行顺序与我们构建 all_vectors 的顺序一致
+    subset = X_pca[start_idx:end_idx]
+    
+    plt.scatter(
+        subset[:, 0], 
+        subset[:, 1], 
+        s=20, # 点的大小
+        alpha=0.6, # 透明度，防止重叠完全遮挡
+        c=terrain_colors.get(t_name, 'gray'), 
+        label=t_name
+    )
+    
+    start_idx = end_idx
+
+plt.title(f'PCA of Latent Space - Command {TARGET_CMD_ID}: {TARGET_CMD_NAME}\n(Colored by Terrain)', fontsize=14)
+plt.xlabel(f'Principal Component 1 ({evr[0]:.2%} variance)', fontsize=12)
+plt.ylabel(f'Principal Component 2 ({evr[1]:.2%} variance)', fontsize=12)
+plt.legend(title="Terrain", loc='best')
+plt.grid(True, linestyle='--', alpha=0.3)
+
+# 保存图片
+save_path = Path(data_root) / "pca_terrain_cmd0.png"
+plt.savefig(save_path)
+print(f"[*] Plot saved to {save_path}")
+plt.show()

scripts/run_save_latent.bash

@@ -5,6 +5,13 @@
 
 source /root/Programs/miniforge3/bin/activate robot
 
+# MODEL_PATH="/root/Coding/RoboGauge/mytest/go2_rem_cts_137k_0.6745.pt"
+# MODEL_PATH="/root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt"
+# MODEL_PATH="/root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt"
+# MODEL_PATH="/root/Coding/RoboGauge/mytest/go2_rem_cts_103k_0.6357.pt"
+# MODEL_PATH="/root/Coding/RoboGauge/mytest/go2_cts_130k_60%.pt"
+MODEL_PATH="/root/Coding/RoboGauge/mytest/go2_moe_cts_79k_0.6637.pt"
+
 # python robogauge/scripts/run.py \
 #     --task-name go2_moe \
 #     --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
@@ -20,7 +27,7 @@ source /root/Programs/miniforge3/bin/activate robot
 
 python robogauge/scripts/run.py \
     --task-name go2_moe.flat \
-    --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
+    --model-path ${MODEL_PATH} \
     --experiment-name latent \
     --seed 0 \
     --friction 1.5 \
@@ -29,8 +36,8 @@ python robogauge/scripts/run.py \
 
 python robogauge/scripts/run.py \
     --task-name go2_moe.obstacle \
-    --level 9 \
+    --level 5 \
-    --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
+    --model-path ${MODEL_PATH} \
     --experiment-name latent \
     --seed 0 \
     --friction 1.5 \
@@ -39,8 +46,8 @@ python robogauge/scripts/run.py \
 
 python robogauge/scripts/run.py \
     --task-name go2_moe.slope_bd \
-    --level 6 \
+    --level 5 \
-    --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
+    --model-path ${MODEL_PATH} \
     --experiment-name latent \
     --seed 0 \
     --friction 1.5 \
@@ -49,8 +56,8 @@ python robogauge/scripts/run.py \
 
 python robogauge/scripts/run.py \
     --task-name go2_moe.slope_fd \
-    --level 6 \
+    --level 5 \
-    --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
+    --model-path ${MODEL_PATH} \
     --experiment-name latent \
     --seed 0 \
     --friction 1.5 \
@@ -59,8 +66,8 @@ python robogauge/scripts/run.py \
 
 python robogauge/scripts/run.py \
     --task-name go2_moe.stairs_bd \
-    --level 7 \
+    --level 5 \
-    --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
+    --model-path ${MODEL_PATH} \
     --experiment-name latent \
     --seed 0 \
     --friction 1.5 \
@@ -69,8 +76,8 @@ python robogauge/scripts/run.py \
 
 python robogauge/scripts/run.py \
     --task-name go2_moe.stairs_fd \
-    --level 9 \
+    --level 5 \
-    --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
+    --model-path ${MODEL_PATH} \
     --experiment-name latent \
     --seed 0 \
     --friction 1.5 \
@@ -79,8 +86,8 @@ python robogauge/scripts/run.py \
 
 python robogauge/scripts/run.py \
     --task-name go2_moe.wave \
-    --level 6 \
+    --level 5 \
-    --model-path /root/Coding/RoboGauge/mytest/go2_moe_cts_111k_61.09%.pt \
+    --model-path ${MODEL_PATH} \
     --experiment-name latent \
     --seed 0 \
     --friction 1.5 \