Compare commits
11 Commits
1491833f26
...
c1a4146f89
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
c1a4146f89 | ||
|
|
51a67fc81b | ||
|
|
35a1d278e8 | ||
|
|
c1b347c89f | ||
|
|
30643dfed8 | ||
|
|
9c172a0d39 | ||
|
|
efde17bd87 | ||
|
|
58f54f56a5 | ||
|
|
fb607670d1 | ||
|
|
3701bbe2d5 | ||
|
|
01b761b4f3 |
@@ -240,11 +240,10 @@ Thanks to [@windigal](https://github.com/windigal) for terrains generation and e
|
|||||||
## Citation
|
## Citation
|
||||||
If you find our work helpful, please cite:
|
If you find our work helpful, please cite:
|
||||||
```bibtex
|
```bibtex
|
||||||
@article{wu2026robogauge,
|
@inproceedings{wu2026robogauge,
|
||||||
title={Toward Reliable Sim-to-Real Predictability for MoE-based Robust Quadrupedal Locomotion},
|
title={Toward Reliable Sim-to-Real Predictability for MoE-based Robust Quadrupedal Locomotion},
|
||||||
author={Tianyang Wu and Hanwei Guo and Yuhang Wang and Junshu Yang and Xinyang Sui and Jiayi Xie and Xingyu Chen and Zeyang Liu and Xuguang Lan},
|
author={Tianyang Wu and Hanwei Guo and Yuhang Wang and Junshu Yang and Xinyang Sui and Jiayi Xie and Xingyu Chen and Zeyang Liu and Xuguang Lan},
|
||||||
year={2026},
|
booktitle={Proceedings of Robotics: Science and Systems},
|
||||||
journal={arXiv preprint arXiv:2602.00678},
|
year={2026}
|
||||||
url={https://arxiv.org/abs/2602.00678},
|
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|||||||
@@ -36,6 +36,7 @@
|
|||||||
| 机器人型号 | 机器人类型 | 速度追踪指令 |
|
| 机器人型号 | 机器人类型 | 速度追踪指令 |
|
||||||
| - | - | - |
|
| - | - | - |
|
||||||
| Unitree go2 | 四足 | ✅ |
|
| Unitree go2 | 四足 | ✅ |
|
||||||
|
| [Unitree go1](./assets/docs/go1_policy_io_zh.md) | 四足 | ✅ |
|
||||||
|
|
||||||
## 安装
|
## 安装
|
||||||
推荐安装`python=3.8`, 参考[PyTorch官网](https://pytorch.org/get-started/locally/)安装2.0以上任意版本, 安装本仓库
|
推荐安装`python=3.8`, 参考[PyTorch官网](https://pytorch.org/get-started/locally/)安装2.0以上任意版本, 安装本仓库
|
||||||
@@ -214,11 +215,10 @@ https://github.com/user-attachments/assets/7e19d831-1fc9-49db-99e0-c83cddddfb0b
|
|||||||
## 引用
|
## 引用
|
||||||
如果觉得我们的工作有帮助,请引用:
|
如果觉得我们的工作有帮助,请引用:
|
||||||
```bibtex
|
```bibtex
|
||||||
@article{wu2026robogauge,
|
@inproceedings{wu2026robogauge,
|
||||||
title={Toward Reliable Sim-to-Real Predictability for MoE-based Robust Quadrupedal Locomotion},
|
title={Toward Reliable Sim-to-Real Predictability for MoE-based Robust Quadrupedal Locomotion},
|
||||||
author={Tianyang Wu and Hanwei Guo and Yuhang Wang and Junshu Yang and Xinyang Sui and Jiayi Xie and Xingyu Chen and Zeyang Liu and Xuguang Lan},
|
author={Tianyang Wu and Hanwei Guo and Yuhang Wang and Junshu Yang and Xinyang Sui and Jiayi Xie and Xingyu Chen and Zeyang Liu and Xuguang Lan},
|
||||||
year={2026},
|
booktitle={Proceedings of Robotics: Science and Systems},
|
||||||
journal={arXiv preprint arXiv:2602.00678},
|
year={2026}
|
||||||
url={https://arxiv.org/abs/2602.00678},
|
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|||||||
@@ -1,4 +1,8 @@
|
|||||||
# UPDATE
|
# UPDATE
|
||||||
|
## 20260518
|
||||||
|
### v1.1.7
|
||||||
|
1. 新增go2 lab环境评估
|
||||||
|
2. 新增server, client通讯检测
|
||||||
## 20260324
|
## 20260324
|
||||||
### v1.1.6
|
### v1.1.6
|
||||||
1. 修复multi_pipeline中统计terrain_weighted_summary的地形等级加权分错误,但不影响stress_pipeline最终计算的benchmark_score,只影响metrics里面的mean和mean@50的计算
|
1. 修复multi_pipeline中统计terrain_weighted_summary的地形等级加权分错误,但不影响stress_pipeline最终计算的benchmark_score,只影响metrics里面的mean和mean@50的计算
|
||||||
|
|||||||
329
assets/docs/go1_policy_io_zh.md
Normal file
329
assets/docs/go1_policy_io_zh.md
Normal file
@@ -0,0 +1,329 @@
|
|||||||
|
# Go1 MoE 策略输入输出与部署说明
|
||||||
|
|
||||||
|
本文说明 `go1` 分支中 Go1 MoE locomotion 策略的输入、输出、状态管理和
|
||||||
|
RoboGauge/MuJoCo 接入方式。适用于由 `go2_rl_gym` 导出的 Go1 MoE CTS
|
||||||
|
TorchScript 模型,例如:
|
||||||
|
|
||||||
|
- `resources/models/go1/policy.pt`
|
||||||
|
- `resources/models/go1/policy_10k.pt`
|
||||||
|
- `resources/models/go1/policy_15k.pt`
|
||||||
|
- `resources/models/go1/policy_15k.onnx`
|
||||||
|
|
||||||
|
## 1. 接口概览
|
||||||
|
|
||||||
|
| 项目 | 规格 |
|
||||||
|
| - | - |
|
||||||
|
| 控制频率 | 50 Hz,控制周期 `0.02 s` |
|
||||||
|
| TorchScript 输入 | `torch.float32`,形状 `[1, 45]` |
|
||||||
|
| 内部历史 | 5 帧,形状 `[1, 5, 45]` |
|
||||||
|
| 策略动作 | `torch.float32`,形状 `[1, 12]` |
|
||||||
|
| MoE 权重 | `torch.float32`,形状 `[1, 8]` |
|
||||||
|
| Student latent | `torch.float32`,形状 `[1, 32]` |
|
||||||
|
| 控制方式 | 关节位置 PD 控制 |
|
||||||
|
| 关节位置增量比例 | `action_scale = 0.25` |
|
||||||
|
| PD 参数 | `Kp = 28.0`,`Kd = 0.7` |
|
||||||
|
|
||||||
|
导出的 TorchScript 是有状态模型。调用方每个控制周期只输入当前 45 维
|
||||||
|
观测,模型会在内部更新 5 帧历史。不要在 TorchScript 输入端再次拼接成
|
||||||
|
225 维。
|
||||||
|
|
||||||
|
## 2. 坐标系和关节顺序
|
||||||
|
|
||||||
|
机身坐标系约定:
|
||||||
|
|
||||||
|
- `+x`:前方
|
||||||
|
- `+y`:左方
|
||||||
|
- `+z`:上方
|
||||||
|
- 正 yaw:绕 `+z` 轴逆时针旋转
|
||||||
|
- 四元数顺序:`[w, x, y, z]`
|
||||||
|
|
||||||
|
策略的 12 个关节严格按照以下顺序排列:
|
||||||
|
|
||||||
|
```text
|
||||||
|
0 FR_hip 1 FR_thigh 2 FR_calf
|
||||||
|
3 FL_hip 4 FL_thigh 5 FL_calf
|
||||||
|
6 RR_hip 7 RR_thigh 8 RR_calf
|
||||||
|
9 RL_hip 10 RL_thigh 11 RL_calf
|
||||||
|
```
|
||||||
|
|
||||||
|
即腿序为 `FR, FL, RR, RL`,每条腿内部为 `hip, thigh, calf`。
|
||||||
|
|
||||||
|
默认站立关节位置为:
|
||||||
|
|
||||||
|
```python
|
||||||
|
DEFAULT_DOF_POS = np.array([
|
||||||
|
-0.1, 0.8, -1.5, # FR
|
||||||
|
0.1, 0.8, -1.5, # FL
|
||||||
|
-0.1, 1.0, -1.5, # RR
|
||||||
|
0.1, 1.0, -1.5, # RL
|
||||||
|
], dtype=np.float32)
|
||||||
|
```
|
||||||
|
|
||||||
|
Isaac Gym 中 Go1 资产的原始关节顺序不同,但训练环境已经转换成上述策略
|
||||||
|
顺序。MuJoCo 和真机部署端必须直接使用上述顺序,不能再次交换左右腿。
|
||||||
|
|
||||||
|
## 3. 45 维单帧观测
|
||||||
|
|
||||||
|
每一帧观测按下表顺序拼接:
|
||||||
|
|
||||||
|
| 索引 | 维度 | 原始信号 | 输入值 |
|
||||||
|
| - | -: | - | - |
|
||||||
|
| `[0:3]` | 3 | 机身坐标系角速度 `[wx, wy, wz]` | `base_ang_vel * 0.25` |
|
||||||
|
| `[3:6]` | 3 | 世界重力方向在机身坐标系中的投影 | `projected_gravity` |
|
||||||
|
| `[6:9]` | 3 | 速度指令 `[vx, vy, yaw_rate]` | `command * [2.0, 2.0, 0.25]` |
|
||||||
|
| `[9:21]` | 12 | 关节位置 | `(q - q_default) * 1.0` |
|
||||||
|
| `[21:33]` | 12 | 关节速度 | `dq * 0.05` |
|
||||||
|
| `[33:45]` | 12 | 上一个控制周期的原始策略动作 | `last_action` |
|
||||||
|
|
||||||
|
所有关节相关数组都必须先转换成 `FR, FL, RR, RL` 策略顺序。
|
||||||
|
|
||||||
|
### 3.1 角速度
|
||||||
|
|
||||||
|
角速度必须是机身坐标系下的陀螺仪读数,单位为 `rad/s`。输入模型前乘以
|
||||||
|
`0.25`。不要传入世界坐标系角速度。
|
||||||
|
|
||||||
|
### 3.2 投影重力
|
||||||
|
|
||||||
|
策略不直接输入 roll、pitch 或四元数,而是输入单位重力向量在机身坐标系
|
||||||
|
中的投影。对于 `[w, x, y, z]` 四元数,可使用 RoboGauge 的实现:
|
||||||
|
|
||||||
|
```python
|
||||||
|
from robogauge.utils.math_utils import get_projected_gravity
|
||||||
|
|
||||||
|
projected_gravity = get_projected_gravity(base_quat_wxyz)
|
||||||
|
```
|
||||||
|
|
||||||
|
机器人水平站立且朝向不影响姿态时,该向量约为 `[0, 0, -1]`。
|
||||||
|
|
||||||
|
### 3.3 速度指令
|
||||||
|
|
||||||
|
输入的是期望机身速度,不是目标点坐标:
|
||||||
|
|
||||||
|
```text
|
||||||
|
[forward_velocity, lateral_velocity, yaw_rate]
|
||||||
|
```
|
||||||
|
|
||||||
|
RoboGauge 的目标点控制器会先把目标点误差转换为速度指令,再构建观测。
|
||||||
|
|
||||||
|
15k 楼梯专项模型的训练范围为:
|
||||||
|
|
||||||
|
```text
|
||||||
|
vx = [-1.0, 1.0] m/s
|
||||||
|
vy = [-0.5, 0.5] m/s
|
||||||
|
yaw_rate = [-1.0, 1.0] rad/s
|
||||||
|
```
|
||||||
|
|
||||||
|
当前 `Go1TerrainConfig` 允许更宽的 `vy` 和 `yaw_rate`,用于通用 RoboGauge
|
||||||
|
测试。评估 15k 楼梯模型时,超出上述范围的结果应视为训练分布外表现。
|
||||||
|
|
||||||
|
### 3.4 上一动作
|
||||||
|
|
||||||
|
`last_action` 是上一个控制周期模型输出的 12 维原始 action,不是 PD 目标
|
||||||
|
位置,也不是实际关节位置。episode 开始时应初始化为全零。
|
||||||
|
|
||||||
|
## 4. 历史观测
|
||||||
|
|
||||||
|
Student MoE encoder 使用 5 个连续控制帧:
|
||||||
|
|
||||||
|
```text
|
||||||
|
[obs(t-4), obs(t-3), obs(t-2), obs(t-1), obs(t)]
|
||||||
|
```
|
||||||
|
|
||||||
|
控制周期为 20 ms,因此历史缓冲包含 5 帧、跨度 80 ms。flatten 后 encoder
|
||||||
|
实际处理 225 维数据,但该缓冲已经封装在 TorchScript 模型中。
|
||||||
|
|
||||||
|
调用 `model.reset()` 后,历史缓冲为全零。随后每次调用模型都会丢弃最旧
|
||||||
|
一帧并追加当前观测。这与训练和 RoboGauge 中的行为一致。
|
||||||
|
|
||||||
|
## 5. TorchScript 输出
|
||||||
|
|
||||||
|
MoE TorchScript 返回:
|
||||||
|
|
||||||
|
```python
|
||||||
|
action, (weights, latent) = model(obs_tensor)
|
||||||
|
```
|
||||||
|
|
||||||
|
各输出含义:
|
||||||
|
|
||||||
|
| 输出 | 形状 | 含义 |
|
||||||
|
| - | - | - |
|
||||||
|
| `action` | `[1, 12]` | 确定性关节位置增量动作 |
|
||||||
|
| `weights` | `[1, 8]` | 8 个 Student MoE expert 的门控权重 |
|
||||||
|
| `latent` | `[1, 32]` | Student encoder 估计的环境隐变量 |
|
||||||
|
|
||||||
|
实际控制只需要 `action`。`weights` 和 `latent` 用于分析与可视化,可以忽略。
|
||||||
|
|
||||||
|
将原始 action 转换为关节位置目标:
|
||||||
|
|
||||||
|
```python
|
||||||
|
target_dof_pos = DEFAULT_DOF_POS + 0.25 * action
|
||||||
|
```
|
||||||
|
|
||||||
|
随后使用位置 PD 控制:
|
||||||
|
|
||||||
|
```text
|
||||||
|
tau = Kp * (target_dof_pos - q) - Kd * dq
|
||||||
|
Kp = 28.0
|
||||||
|
Kd = 0.7
|
||||||
|
```
|
||||||
|
|
||||||
|
实际部署还应按电机能力限制输出力矩。
|
||||||
|
|
||||||
|
## 6. ONNX 输入输出
|
||||||
|
|
||||||
|
`policy_15k.onnx` 是无状态模型,不在模型内部保存历史。它与 TorchScript
|
||||||
|
的输入接口不同:
|
||||||
|
|
||||||
|
| 项目 | ONNX 规格 |
|
||||||
|
| - | - |
|
||||||
|
| 输入名称 | `obs` |
|
||||||
|
| 输入形状 | `[1, 225]` |
|
||||||
|
| 输出 0 | `actions`,形状 `[1, 12]` |
|
||||||
|
| 输出 1 | `weights`,形状 `[1, 8]` |
|
||||||
|
| 输出 2 | `latent`,形状 `[1, 32]` |
|
||||||
|
|
||||||
|
ONNX 输入不是直接对 `[1, 5, 45]` 执行 `reshape`。225 维输入按观测项分组,
|
||||||
|
每组内部再按时间从旧到新排列:
|
||||||
|
|
||||||
|
```text
|
||||||
|
[
|
||||||
|
angular_velocity(t-4:t), # 5 * 3 = 15
|
||||||
|
projected_gravity(t-4:t), # 5 * 3 = 15
|
||||||
|
command(t-4:t), # 5 * 3 = 15
|
||||||
|
dof_position_error(t-4:t), # 5 * 12 = 60
|
||||||
|
dof_velocity(t-4:t), # 5 * 12 = 60
|
||||||
|
last_action(t-4:t), # 5 * 12 = 60
|
||||||
|
]
|
||||||
|
```
|
||||||
|
|
||||||
|
调用方需要维护 5 帧历史,并在 episode reset 时将历史清零。RoboGauge 当前
|
||||||
|
直接使用 TorchScript;ONNX 主要用于其他推理后端。
|
||||||
|
|
||||||
|
## 7. 最小调用示例
|
||||||
|
|
||||||
|
```python
|
||||||
|
import numpy as np
|
||||||
|
import torch
|
||||||
|
|
||||||
|
from robogauge.utils.math_utils import get_projected_gravity
|
||||||
|
|
||||||
|
|
||||||
|
JOINT_ORDER = [
|
||||||
|
"FR_hip", "FR_thigh", "FR_calf",
|
||||||
|
"FL_hip", "FL_thigh", "FL_calf",
|
||||||
|
"RR_hip", "RR_thigh", "RR_calf",
|
||||||
|
"RL_hip", "RL_thigh", "RL_calf",
|
||||||
|
]
|
||||||
|
|
||||||
|
DEFAULT_DOF_POS = np.array([
|
||||||
|
-0.1, 0.8, -1.5,
|
||||||
|
0.1, 0.8, -1.5,
|
||||||
|
-0.1, 1.0, -1.5,
|
||||||
|
0.1, 1.0, -1.5,
|
||||||
|
], dtype=np.float32)
|
||||||
|
|
||||||
|
model = torch.jit.load("resources/models/go1/policy_10k.pt", map_location="cpu")
|
||||||
|
model.eval()
|
||||||
|
|
||||||
|
# 每个 episode 开始时执行。
|
||||||
|
model.reset()
|
||||||
|
last_action = np.zeros(12, dtype=np.float32)
|
||||||
|
|
||||||
|
|
||||||
|
def build_observation(base_ang_vel, base_quat_wxyz, command, q, dq, last_action):
|
||||||
|
"""All joint arrays must use the FR, FL, RR, RL policy order."""
|
||||||
|
obs = np.concatenate([
|
||||||
|
np.asarray(base_ang_vel, dtype=np.float32) * 0.25,
|
||||||
|
get_projected_gravity(np.asarray(base_quat_wxyz, dtype=np.float32)),
|
||||||
|
np.asarray(command, dtype=np.float32) * np.array([2.0, 2.0, 0.25], dtype=np.float32),
|
||||||
|
(np.asarray(q, dtype=np.float32) - DEFAULT_DOF_POS),
|
||||||
|
np.asarray(dq, dtype=np.float32) * 0.05,
|
||||||
|
np.asarray(last_action, dtype=np.float32),
|
||||||
|
]).astype(np.float32)
|
||||||
|
assert obs.shape == (45,)
|
||||||
|
return np.clip(obs, -100.0, 100.0)
|
||||||
|
|
||||||
|
|
||||||
|
# 以下变量由 IMU、关节编码器和上层速度控制器提供。
|
||||||
|
obs = build_observation(
|
||||||
|
base_ang_vel=base_ang_vel_body,
|
||||||
|
base_quat_wxyz=base_quat_wxyz,
|
||||||
|
command=velocity_command,
|
||||||
|
q=joint_position,
|
||||||
|
dq=joint_velocity,
|
||||||
|
last_action=last_action,
|
||||||
|
)
|
||||||
|
|
||||||
|
with torch.inference_mode():
|
||||||
|
obs_tensor = torch.from_numpy(obs).unsqueeze(0)
|
||||||
|
action_tensor, (weights, latent) = model(obs_tensor)
|
||||||
|
|
||||||
|
action = action_tensor.squeeze(0).cpu().numpy()
|
||||||
|
target_dof_pos = DEFAULT_DOF_POS + 0.25 * action
|
||||||
|
last_action = action.copy()
|
||||||
|
```
|
||||||
|
|
||||||
|
## 8. Reset 要求
|
||||||
|
|
||||||
|
以下情况必须同时重置模型历史和上一动作:
|
||||||
|
|
||||||
|
- 仿真 episode reset
|
||||||
|
- 机器人摔倒后重新站立
|
||||||
|
- 策略重新加载
|
||||||
|
- 控制器长时间暂停后重新启动
|
||||||
|
- 真机急停解除后重新接管
|
||||||
|
|
||||||
|
```python
|
||||||
|
model.reset()
|
||||||
|
last_action.fill(0.0)
|
||||||
|
```
|
||||||
|
|
||||||
|
只清零 `last_action` 而不调用 `model.reset()`,会残留上一段运行的历史状态。
|
||||||
|
|
||||||
|
## 9. 不需要的部署信号
|
||||||
|
|
||||||
|
部署使用 Student 策略,不需要训练阶段 Teacher/Critic 的 263 维 privileged
|
||||||
|
observation。以下信号不进入部署策略:
|
||||||
|
|
||||||
|
- 机身线速度
|
||||||
|
- 足端接触力
|
||||||
|
- 电机力矩
|
||||||
|
- 关节加速度
|
||||||
|
- 187 维地形高度扫描
|
||||||
|
- 深度图、相机图像或 LiDAR
|
||||||
|
|
||||||
|
这些信息仅在训练阶段用于 Teacher/Critic。Student 通过 5 帧本体观测历史估计
|
||||||
|
32 维 latent。
|
||||||
|
|
||||||
|
## 10. RoboGauge 对应实现
|
||||||
|
|
||||||
|
RoboGauge 已实现上述接口:
|
||||||
|
|
||||||
|
- 观测构建:`robogauge/tasks/robots/go2/go2.py`
|
||||||
|
- Go1 参数和关节顺序:`robogauge/tasks/robots/go1/go1_config.py`
|
||||||
|
- MoE 输出处理:`robogauge/tasks/robots/go2/go2_moe.py`
|
||||||
|
- 投影重力:`robogauge/utils/math_utils.py`
|
||||||
|
|
||||||
|
在 RoboGauge 中加载 Go1 MoE 模型时,通常无需手动构建观测:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
MUJOCO_GL=glfw \
|
||||||
|
PYTHONPATH=/path/to/RoboGauge \
|
||||||
|
python robogauge/scripts/run.py \
|
||||||
|
--task-name go1_moe.stairs_fd \
|
||||||
|
--experiment-name go1_policy_test \
|
||||||
|
--model-path /path/to/policy.pt \
|
||||||
|
--level 6 \
|
||||||
|
--goals target_pos_velocity
|
||||||
|
```
|
||||||
|
|
||||||
|
## 11. 常见错误检查
|
||||||
|
|
||||||
|
1. 输入形状应为 `[1, 45]`,不是 `[45]` 或 `[1, 225]`。
|
||||||
|
2. 输入 dtype 必须为 `torch.float32`。
|
||||||
|
3. 角速度必须在机身坐标系下。
|
||||||
|
4. 四元数必须是 `[w, x, y, z]`。
|
||||||
|
5. 关节顺序必须是 `FR, FL, RR, RL`。
|
||||||
|
6. `last_action` 必须使用未乘 `0.25` 的原始模型输出。
|
||||||
|
7. 每次 reset 必须同时调用 `model.reset()` 并清零 `last_action`。
|
||||||
|
8. 不要向部署模型输入 privileged observation 或高度扫描。
|
||||||
591
assets/docs/mujoco_contact_version_analysis_zh.md
Normal file
591
assets/docs/mujoco_contact_version_analysis_zh.md
Normal file
@@ -0,0 +1,591 @@
|
|||||||
|
# MuJoCo 3.2.3 与 3.10 接触动力学差异排查
|
||||||
|
|
||||||
|
最终还是不能提高代码版本,版本维持在`<3.3.0`以下
|
||||||
|
|
||||||
|
本文记录 RoboGauge 中 Go2 MoE 策略在 MuJoCo 3.2.3 与 3.10.0 下的接触动力学差异、wave 地形失败、flat 评分差异,以及 `solref`、native CCD 和摩擦指标之间的关系。
|
||||||
|
|
||||||
|
## 1. 测试环境
|
||||||
|
|
||||||
|
本次排查使用两个 Python 环境:
|
||||||
|
|
||||||
|
| 环境 | Python 路径 | MuJoCo | dm_control |
|
||||||
|
|---|---|---:|---:|
|
||||||
|
| RoboGauge | `/home/user/Coding/robotics/RoboGauge/.venv/bin/python` | 3.2.3 | 1.0.23 |
|
||||||
|
| 新版本 | `/home/user/isaaclab/bin/python` | 3.10.0 | 1.0.43 |
|
||||||
|
|
||||||
|
主要测试命令:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python robogauge/scripts/run.py \
|
||||||
|
--task go2_moe.wave \
|
||||||
|
--experiment-name debug \
|
||||||
|
--headless
|
||||||
|
```
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python robogauge/scripts/run.py \
|
||||||
|
--task go2_moe.flat \
|
||||||
|
--experiment-name debug \
|
||||||
|
--headless
|
||||||
|
```
|
||||||
|
|
||||||
|
Go2 脚部碰撞配置位于 [`resources/robots/go2/go2.xml`](../resources/robots/go2/go2.xml):
|
||||||
|
|
||||||
|
```xml
|
||||||
|
<option cone="elliptic" impratio="100" />
|
||||||
|
|
||||||
|
<default class="foot">
|
||||||
|
<geom size="0.022" pos="-0.002 0 -0.213"
|
||||||
|
priority="1" condim="6"
|
||||||
|
friction="0.4 0.02 0.01"
|
||||||
|
solref="0.006 1"/>
|
||||||
|
</default>
|
||||||
|
```
|
||||||
|
|
||||||
|
仿真步长为:
|
||||||
|
|
||||||
|
```text
|
||||||
|
simulation_dt = 0.002 s
|
||||||
|
```
|
||||||
|
|
||||||
|
## 2. 结论摘要
|
||||||
|
|
||||||
|
1. wave 地形上的巨大轨迹差异主要来自高度场接触点和接触法线的细微变化,随后被闭环策略快速放大。
|
||||||
|
2. MuJoCo 3.10 的 native CCD 不是普遍更差,而是策略训练时使用的接触动力学分布与新接触实现不一致。
|
||||||
|
3. 将脚部 `solref` 从默认的 `0.02 1` 改为 `0.006 1`,可以显著减少 3.10 native CCD 在高度场上的脚部下陷,并恢复 wave run 42 的成功。
|
||||||
|
4. `solref="0.006 1"` 不是一个小修改:法向刚度约提高 11.1 倍,阻尼约提高 3.33 倍。
|
||||||
|
5. 当脚部使用 `condim="6"` 和椭圆摩擦锥时,动态生成的接触默认让摩擦维度继承同一个 `solref`。因此 `0.006` 不仅改变法向穿透,还会显著改变切向摩擦冲量。
|
||||||
|
6. flat 地面使用球–平面的解析碰撞,不依赖通用 native-CCD 碰撞管线。flat 分数差异主要不是 native CCD,而是新的接触约束求解结果在摩擦锥边界附近产生了更多低摩擦裕度样本。
|
||||||
|
7. RoboGauge 使用逐帧几何平均计算质量分数;一个为零的摩擦指标就可能把该帧质量分数压到约 `0.1`,因此少量接触力异常会被明显放大。
|
||||||
|
8. `solreffriction` 不是 `<geom>` 属性,只能用于显式 `<contact><pair>`。在当前 RoboGauge 架构中,机器人和地形分别解析后再 attach,因此脚–地面 contact pair 必须在 attach 后、模型编译前由 Python 创建。
|
||||||
|
|
||||||
|
## 3. native CCD 的版本变化
|
||||||
|
|
||||||
|
MuJoCo 3.2.3 引入了实验性的 `nativeccd` 开关:
|
||||||
|
|
||||||
|
- 默认仍使用旧的 libccd 路径;
|
||||||
|
- native CCD 当时处于早期测试阶段;
|
||||||
|
- 在本项目的 3.2.3 高度场任务中,强制开启 native CCD 会在运行阶段触发 MuJoCo 原生层 segmentation fault,Python 无法捕获。
|
||||||
|
|
||||||
|
MuJoCo 3.3.0 将 native CCD 设为默认实现。它使用 MuJoCo 原生的 GJK/EPA 管线替代旧 libccd/MPR 路径。
|
||||||
|
|
||||||
|
后续版本还改变了其他默认行为,例如:
|
||||||
|
|
||||||
|
- 3.3.6:constraint island 默认开启,并修改了 `qacc_warmstart` 的更新时间;
|
||||||
|
- 3.8.0:multiccd 默认开启;
|
||||||
|
- 3.10.0:继续改进约束求解器收敛和 constraint island 内部实现。
|
||||||
|
|
||||||
|
关闭 `nativeccd`、`island` 和 `multiccd` 只能关闭对应功能,不能把 MuJoCo 3.10 的全部约束求解代码恢复成 3.2.3。
|
||||||
|
|
||||||
|
官方参考:
|
||||||
|
|
||||||
|
- [MuJoCo changelog](https://mujoco.readthedocs.io/en/stable/changelog.html)
|
||||||
|
- [MuJoCo computation](https://mujoco.readthedocs.io/en/latest/computation/index.html)
|
||||||
|
|
||||||
|
## 4. wave 地形:微小接触差异如何被策略放大
|
||||||
|
|
||||||
|
在 MuJoCo 3.10 中仅切换 native CCD,第一次 RR 脚与高度场接触发生在约 `0.056 s`:
|
||||||
|
|
||||||
|
- 接触位置差异约 `0.26 mm`;
|
||||||
|
- 接触法线方向差异约 `0.05°`。
|
||||||
|
|
||||||
|
机器人构型差异随后快速增加:
|
||||||
|
|
||||||
|
| 时间 | 构型差异超过 |
|
||||||
|
|---:|---:|
|
||||||
|
| 0.060 s | `1e-6` |
|
||||||
|
| 0.174 s | `1e-3` |
|
||||||
|
| 0.694 s | `0.1` |
|
||||||
|
|
||||||
|
放大链路如下:
|
||||||
|
|
||||||
|
```text
|
||||||
|
接触点/法线轻微变化
|
||||||
|
↓
|
||||||
|
地面反作用力与摩擦冲量变化
|
||||||
|
↓
|
||||||
|
关节速度和机身姿态变化
|
||||||
|
↓
|
||||||
|
策略观测变化
|
||||||
|
↓
|
||||||
|
MoE 选择或输出不同动作
|
||||||
|
↓
|
||||||
|
下一次落脚位置变化
|
||||||
|
↓
|
||||||
|
下一步产生更大的接触差异
|
||||||
|
```
|
||||||
|
|
||||||
|
这是一个典型的闭环混沌放大过程。初始接触差异小,并不意味着一秒后的轨迹仍然接近。
|
||||||
|
|
||||||
|
### 4.1 二值成功指标也会放大表面差异
|
||||||
|
|
||||||
|
wave 的成功条件是机器人底座在超时前进入目标周围半径 `0.1 m` 的区域。相关逻辑位于 [`robogauge/tasks/gauge/goals/velocity_goals.py`](../robogauge/tasks/gauge/goals/velocity_goals.py)。
|
||||||
|
|
||||||
|
因此,3.10 的失败轨迹不一定代表机器人已经失稳。原始失败结果仍有相近的质量分数和较好的姿态稳定性,只是轨迹发生偏移,未在 20 秒内进入很小的目标区域。
|
||||||
|
|
||||||
|
## 5. wave 地形的脚部下陷测量
|
||||||
|
|
||||||
|
测量实际 foot–heightfield `contact.dist` 后得到:
|
||||||
|
|
||||||
|
| 配置 | 平均穿透 | P95 | P99 | 最大穿透 | 成功 | 质量分数 |
|
||||||
|
|---|---:|---:|---:|---:|---:|---:|
|
||||||
|
| 3.10 native,默认 `solref=.020` | 9.818 mm | 23.0 mm | 24.4 mm | 31.661 mm | 0 | 0.4992 |
|
||||||
|
| 3.10 native,`solref=.010` | 1.261 mm | 5.62 mm | 8.61 mm | 15.64 mm | 1 | 0.5120 |
|
||||||
|
| 3.10 native,`solref=.006` | 0.540 mm | 2.603 mm | 4.609 mm | 8.592 mm | 1 | 0.5061 |
|
||||||
|
| 3.10 legacy,默认接触 | 4.83 mm | 16.0 mm | 20.6 mm | 29.8 mm | 1 | 0.4041 |
|
||||||
|
|
||||||
|
`solref=.006` 在 3.10 native CCD 下将平均穿透从约 `9.8 mm` 降到 `0.54 mm`,并恢复 run 42 成功。
|
||||||
|
|
||||||
|
以下尝试没有解决问题,部分设置反而恶化结果:
|
||||||
|
|
||||||
|
- 增大 geom margin 到 `0.003`;
|
||||||
|
- 只提高 `solimp`;
|
||||||
|
- 将 `impratio` 从 100 降到 10 或 1;
|
||||||
|
- 将脚部 `condim` 从 6 改为 3;
|
||||||
|
- 同时修改 `solref` 和更硬的 `solimp`。
|
||||||
|
|
||||||
|
因此,针对当前策略和模型,最有效的单项修改是缩短脚部接触 time constant。
|
||||||
|
|
||||||
|
## 6. `solref` 的物理含义
|
||||||
|
|
||||||
|
当 `solref` 两个值均为正数时,其格式为:
|
||||||
|
|
||||||
|
```text
|
||||||
|
(timeconst, dampratio)
|
||||||
|
```
|
||||||
|
|
||||||
|
MuJoCo 使用近似关系:
|
||||||
|
|
||||||
|
```text
|
||||||
|
k ∝ 1 / timeconst²
|
||||||
|
b ∝ 1 / timeconst
|
||||||
|
```
|
||||||
|
|
||||||
|
从默认 `0.02` 改成 `0.006`:
|
||||||
|
|
||||||
|
```text
|
||||||
|
法向刚度比例 ≈ (0.02 / 0.006)² ≈ 11.1
|
||||||
|
阻尼比例 ≈ 0.02 / 0.006 ≈ 3.33
|
||||||
|
```
|
||||||
|
|
||||||
|
因此 `0.006` 代表明显更硬、更快的接触响应。
|
||||||
|
|
||||||
|
MuJoCo 建议:
|
||||||
|
|
||||||
|
```text
|
||||||
|
timeconst >= 2 × timestep
|
||||||
|
```
|
||||||
|
|
||||||
|
本项目中:
|
||||||
|
|
||||||
|
```text
|
||||||
|
timestep = 0.002
|
||||||
|
2 × timestep = 0.004
|
||||||
|
```
|
||||||
|
|
||||||
|
所以:
|
||||||
|
|
||||||
|
- `0.006` 等于 3 个仿真步,虽然高于推荐下限,但已经比较硬;
|
||||||
|
- `0.004` 正好位于推荐下限,数值安全余量较小;
|
||||||
|
- 如果还需要进一步减少瞬态穿透,更合理的方法是减小 timestep,再重新调整 `solref`,而不是继续在 `dt=0.002` 下无限减小 time constant。
|
||||||
|
|
||||||
|
官方参考:
|
||||||
|
|
||||||
|
- [MuJoCo modeling: solver parameters](https://mujoco.readthedocs.io/en/stable/modeling.html)
|
||||||
|
|
||||||
|
## 7. 为什么 flat 分数也会变化
|
||||||
|
|
||||||
|
flat 对比结果:
|
||||||
|
|
||||||
|
- 3.2.3:[`logs/go2_moe_flat_debug_mj3.2.3/20260722-20-10-25_run_42/results.yaml`](../logs/go2_moe_flat_debug_mj3.2.3/20260722-20-10-25_run_42/results.yaml)
|
||||||
|
- 3.10:[`logs/go2_moe_flat_debug_mj3.10/20260722-20-10-33_run_42/results.yaml`](../logs/go2_moe_flat_debug_mj3.10/20260722-20-10-33_run_42/results.yaml)
|
||||||
|
|
||||||
|
总体结果:
|
||||||
|
|
||||||
|
| 配置 | 总质量分数 | max-velocity 分数 | max-velocity `mean@25` |
|
||||||
|
|---|---:|---:|---:|
|
||||||
|
| 3.2.3,`solref=.006` | 0.8033 | 0.8519 | 0.7491 |
|
||||||
|
| 3.10,`solref=.006` | 0.7884 | 0.8279 | 0.6569 |
|
||||||
|
|
||||||
|
总体平均分只下降约 `0.0149`,即约 1.5 个百分点。看起来特别大的部分主要是 max-velocity 的最差 25% 样本。
|
||||||
|
|
||||||
|
### 7.1 机器人的主要运动指标实际上非常接近
|
||||||
|
|
||||||
|
| 指标 | 3.2.3 | 3.10 |
|
||||||
|
|---|---:|---:|
|
||||||
|
| lin_vel_err | 0.7947 | 0.7941 |
|
||||||
|
| orientation_stability | 0.9769 | 0.9772 |
|
||||||
|
| dof_power | 0.9026 | 0.9028 |
|
||||||
|
| torque_smoothness | 0.8265 | 0.8264 |
|
||||||
|
|
||||||
|
这说明主要运动质量没有发生同等幅度的恶化。
|
||||||
|
|
||||||
|
差异集中在摩擦指标:
|
||||||
|
|
||||||
|
| 指标 | 3.2.3 | 3.10 |
|
||||||
|
|---|---:|---:|
|
||||||
|
| max-velocity friction mean | 0.7613 | 0.7564 |
|
||||||
|
| max-velocity friction `mean@25` | 0.6069 | 0.5366 |
|
||||||
|
|
||||||
|
## 8. flat 上不是 native CCD 导致的
|
||||||
|
|
||||||
|
flat 地面是脚部球体与无限平面的接触:
|
||||||
|
|
||||||
|
```text
|
||||||
|
sphere foot ↔ plane floor
|
||||||
|
```
|
||||||
|
|
||||||
|
MuJoCo 对 sphere–plane 使用解析 primitive collision,而不是通用 GJK/EPA native-CCD 管线。
|
||||||
|
|
||||||
|
隔离实验结果:
|
||||||
|
|
||||||
|
| MuJoCo 3.10 配置 | 总分 | max-velocity | max-velocity `mean@25` |
|
||||||
|
|---|---:|---:|---:|
|
||||||
|
| 默认,`solref=.006` | 0.7884 | 0.8279 | 0.6569 |
|
||||||
|
| 关闭 native CCD,`.006` | 0.7875 | 0.8261 | 0.6505 |
|
||||||
|
| native/island/multiccd 全关闭,`.006` | 0.7864 | 0.8221 | 0.6353 |
|
||||||
|
| 默认新特性,`solref=.020` | 0.7987 | 0.8496 | 0.7427 |
|
||||||
|
| 3.2.3,`solref=.006` | 0.8033 | 0.8519 | 0.7491 |
|
||||||
|
|
||||||
|
结论:
|
||||||
|
|
||||||
|
- 关闭 native CCD 没有恢复分数;
|
||||||
|
- 关闭 island 和 multiccd 也没有恢复分数;
|
||||||
|
- 将 `solref` 恢复为 `0.02` 后,3.10 的 flat 分数明显接近 3.2.3。
|
||||||
|
|
||||||
|
因此 flat 差异不是 native CCD 的碰撞点差异,而是硬接触参数在两个版本的约束求解实现中产生了不同的摩擦力尖峰和摩擦锥饱和样本。
|
||||||
|
|
||||||
|
## 9. 为什么法向 `solref` 会影响摩擦分数
|
||||||
|
|
||||||
|
脚部使用:
|
||||||
|
|
||||||
|
```xml
|
||||||
|
condim="6"
|
||||||
|
```
|
||||||
|
|
||||||
|
同时全局使用:
|
||||||
|
|
||||||
|
```xml
|
||||||
|
cone="elliptic"
|
||||||
|
```
|
||||||
|
|
||||||
|
在椭圆摩擦锥中,摩擦维度的位置残差恒为零,因此:
|
||||||
|
|
||||||
|
- 摩擦维度的刚度项为零;
|
||||||
|
- `timeconst` 控制切向约束速度的指数衰减;
|
||||||
|
- `dampratio` 在正数格式下对摩擦维度不生效;
|
||||||
|
- 如果没有显式指定 `solreffriction`,摩擦维度继承接触的 `solref`。
|
||||||
|
|
||||||
|
所以:
|
||||||
|
|
||||||
|
```text
|
||||||
|
solref=.006
|
||||||
|
↓
|
||||||
|
法向接触更硬
|
||||||
|
+
|
||||||
|
切向摩擦速度衰减更快
|
||||||
|
↓
|
||||||
|
切向冲量更尖锐
|
||||||
|
↓
|
||||||
|
更多样本接近或达到摩擦锥边界
|
||||||
|
```
|
||||||
|
|
||||||
|
## 10. RoboGauge 评分如何放大少量摩擦异常
|
||||||
|
|
||||||
|
摩擦裕度在 [`robogauge/tasks/gauge/metrics/stable_metric.py`](../robogauge/tasks/gauge/metrics/stable_metric.py) 中近似计算为:
|
||||||
|
|
||||||
|
```text
|
||||||
|
friction_margin = max(0, 1 - ||Ft|| / (μ Fn))
|
||||||
|
```
|
||||||
|
|
||||||
|
其中:
|
||||||
|
|
||||||
|
- `Fn` 是法向接触力;
|
||||||
|
- `Ft` 是两个切向分量的范数;
|
||||||
|
- `μ` 是滑动摩擦系数。
|
||||||
|
|
||||||
|
当:
|
||||||
|
|
||||||
|
```text
|
||||||
|
||Ft|| >= μ Fn
|
||||||
|
```
|
||||||
|
|
||||||
|
该脚的摩擦裕度会被截断为零。
|
||||||
|
|
||||||
|
质量分数在 [`robogauge/tasks/gauge/goals/base_goal.py`](../robogauge/tasks/gauge/goals/base_goal.py) 中逐帧使用加权几何平均:
|
||||||
|
|
||||||
|
```text
|
||||||
|
quality = geometric_mean(metrics)
|
||||||
|
```
|
||||||
|
|
||||||
|
零指标会先被限制为 `1e-9`。当前质量指标权重之和为 10,因此一个权重为 1 的零摩擦指标会贡献:
|
||||||
|
|
||||||
|
```text
|
||||||
|
(1e-9)^(1/10) ≈ 0.126
|
||||||
|
```
|
||||||
|
|
||||||
|
其他指标再与其相乘后,该帧质量分数通常约为 `0.1`。
|
||||||
|
|
||||||
|
原始逐帧统计显示:
|
||||||
|
|
||||||
|
| max-velocity 统计 | 3.2.3 | 3.10 |
|
||||||
|
|---|---:|---:|
|
||||||
|
| quality minimum | 0.070 | 0.100 |
|
||||||
|
| quality P1 | 0.634 | 0.106 |
|
||||||
|
| friction minimum | 0.035 | 0.000 |
|
||||||
|
| worst-quality 帧上的 friction mean | 0.692 | 0.630 |
|
||||||
|
|
||||||
|
3.10 出现了多个摩擦裕度恰好为零的样本,因此 quality P1 从 `0.634` 降至 `0.106`。
|
||||||
|
|
||||||
|
### 10.1 `mean@25` 的含义
|
||||||
|
|
||||||
|
`mean@25` 不是仿真的前 25% 时间。
|
||||||
|
|
||||||
|
实现会:
|
||||||
|
|
||||||
|
1. 将全部指标样本从小到大排序;
|
||||||
|
2. 取最差的 25%;
|
||||||
|
3. 对这些最差样本求平均。
|
||||||
|
|
||||||
|
因此几个接近零的异常帧会显著拉低 `mean@25`,即使总体平均运动质量变化不大。
|
||||||
|
|
||||||
|
## 11. 3.2.3 与 3.10 的“结算”差异在哪里
|
||||||
|
|
||||||
|
两边编译后的主要外部选项一致:
|
||||||
|
|
||||||
|
| 参数 | 3.2.3 | 3.10 |
|
||||||
|
|---|---:|---:|
|
||||||
|
| solver | Newton | Newton |
|
||||||
|
| integrator | Euler | Euler |
|
||||||
|
| iterations | 100 | 100 |
|
||||||
|
| tolerance | `1e-8` | `1e-8` |
|
||||||
|
| cone | elliptic | elliptic |
|
||||||
|
| impratio | 100 | 100 |
|
||||||
|
|
||||||
|
flat 上的差异不主要发生在接触点生成阶段,而发生在约束力求解阶段:
|
||||||
|
|
||||||
|
```text
|
||||||
|
相同的 sphere–plane 解析接触
|
||||||
|
↓
|
||||||
|
构造法向与切向约束 Jacobian
|
||||||
|
↓
|
||||||
|
根据 solref/solimp 计算 K、B 和 impedance
|
||||||
|
↓
|
||||||
|
Newton 求解、warm-start、摩擦锥约束
|
||||||
|
↓
|
||||||
|
得到 Fn、Ft 和广义约束力
|
||||||
|
↓
|
||||||
|
RoboGauge 计算 Ft / (μ Fn)
|
||||||
|
```
|
||||||
|
|
||||||
|
MuJoCo 3.2.3 到 3.10 之间对约束 warm-start、island、接触数据管线和求解器收敛实现进行了多次修改。即使两个版本的机器人位姿和速度十分接近,单个时刻的 `Fn`/`Ft` 分配仍可能略有不同。
|
||||||
|
|
||||||
|
当 `solref=.02` 时,接触较软,两个版本的细微差异通常不会频繁跨过摩擦锥边界。
|
||||||
|
|
||||||
|
当 `solref=.006` 时,接触响应更快、更硬,摩擦利用率更容易接近:
|
||||||
|
|
||||||
|
```text
|
||||||
|
||Ft|| / (μ Fn) = 1
|
||||||
|
```
|
||||||
|
|
||||||
|
此时非常小的数值差异就可能让 RoboGauge 摩擦裕度从一个小正数直接变成零。
|
||||||
|
|
||||||
|
需要注意:目前的隔离实验可以确认差异位于接触约束求解/力分配以及评分放大链路,但如果要把剩余差异精确归因到某一个 MuJoCo commit,仍需要在 3.2.3 到 3.10 之间做版本二分测试。
|
||||||
|
|
||||||
|
## 12. `solreffriction` 的正确用法
|
||||||
|
|
||||||
|
以下写法是无效的:
|
||||||
|
|
||||||
|
```xml
|
||||||
|
<geom solref="0.006 1" solreffriction="0.02 1"/>
|
||||||
|
```
|
||||||
|
|
||||||
|
MuJoCo 会报错:
|
||||||
|
|
||||||
|
```text
|
||||||
|
AttributeError: 'solreffriction' is not a valid attribute for <geom>
|
||||||
|
```
|
||||||
|
|
||||||
|
`solreffriction` 只支持显式 contact pair:
|
||||||
|
|
||||||
|
```xml
|
||||||
|
<contact>
|
||||||
|
<pair geom1="FL" geom2="floor"
|
||||||
|
condim="6"
|
||||||
|
friction="0.4 0.4 0.02 0.01 0.01"
|
||||||
|
solref="0.006 1"
|
||||||
|
solreffriction="0.02 1"/>
|
||||||
|
</contact>
|
||||||
|
```
|
||||||
|
|
||||||
|
其中:
|
||||||
|
|
||||||
|
- `solref="0.006 1"` 控制法向接触;
|
||||||
|
- `solreffriction="0.02 1"` 控制椭圆摩擦锥的切向维度;
|
||||||
|
- `solreffriction="0 0"` 表示继续继承 `solref`。
|
||||||
|
|
||||||
|
官方参考:
|
||||||
|
|
||||||
|
- [MuJoCo XML contact/pair reference](https://mujoco.readthedocs.io/en/latest/XMLreference.html)
|
||||||
|
|
||||||
|
## 13. 为什么不能直接把 contact pair 写进 `flat.xml`
|
||||||
|
|
||||||
|
当前加载流程位于 [`robogauge/tasks/simulator/mujoco_simulator.py`](../../robogauge/tasks/simulator/mujoco_simulator.py):
|
||||||
|
|
||||||
|
```python
|
||||||
|
robot_mjcf = mjcf.from_path(robot_xml)
|
||||||
|
terrain_mjcf = mjcf.from_path(terrain_xmls[0])
|
||||||
|
|
||||||
|
attachment_frame = terrain_mjcf.attach(robot_mjcf)
|
||||||
|
self.mj_physics = mjcf.Physics.from_mjcf_model(terrain_mjcf)
|
||||||
|
```
|
||||||
|
|
||||||
|
机器人和地形首先被分别解析:
|
||||||
|
|
||||||
|
- `go2.xml` 中不存在 `floor`;
|
||||||
|
- `flat.xml` 中不存在 `FL`、`FR`、`RL`、`RR`;
|
||||||
|
- attach 后机器人 geom 还会获得类似 `go2/FL` 的命名空间。
|
||||||
|
|
||||||
|
因此脚–地面显式 pair 必须在 attach 后、`Physics.from_mjcf_model()` 前创建。
|
||||||
|
|
||||||
|
RoboGauge 目前已在统一的 MuJoCo 加载入口实现这一逻辑。配置位于
|
||||||
|
`MujocoConfig.physics.foot_contact_solreffriction`,默认值为
|
||||||
|
`[0.02, 1.0]`。pipeline 会把
|
||||||
|
`robot_cfg.assets.foot_geom_names` 传给 simulator;simulator 在 attach
|
||||||
|
机器人之前记录所有地形 geom(包括附加的 wall),再为每只脚与每个地形
|
||||||
|
geom 创建显式 pair。
|
||||||
|
|
||||||
|
实现的核心形式如下:
|
||||||
|
|
||||||
|
```python
|
||||||
|
floor = terrain_mjcf.find('geom', 'floor')
|
||||||
|
feet = [
|
||||||
|
robot_mjcf.find('geom', name)
|
||||||
|
for name in ('FL', 'FR', 'RL', 'RR')
|
||||||
|
]
|
||||||
|
|
||||||
|
attachment_frame = terrain_mjcf.attach(robot_mjcf)
|
||||||
|
attachment_frame.add('freejoint', name='root')
|
||||||
|
|
||||||
|
for foot in feet:
|
||||||
|
terrain_mjcf.contact.add(
|
||||||
|
'pair',
|
||||||
|
geom1=foot,
|
||||||
|
geom2=floor,
|
||||||
|
condim=6,
|
||||||
|
friction=[0.4, 0.4, 0.02, 0.01, 0.01],
|
||||||
|
solref=[0.006, 1.0],
|
||||||
|
solreffriction=[0.02, 1.0],
|
||||||
|
)
|
||||||
|
|
||||||
|
self.mj_physics = mjcf.Physics.from_mjcf_model(terrain_mjcf)
|
||||||
|
```
|
||||||
|
|
||||||
|
显式 pair 会接管 geom 自动合成的接触参数,因此实现还会复制足端继承到的
|
||||||
|
`condim`、friction、`solref`、`solimp`、margin 和 gap,只单独覆盖
|
||||||
|
`solreffriction`。domain randomization 修改 friction 时也会同步修改 pair 的
|
||||||
|
两个滑动摩擦系数。
|
||||||
|
|
||||||
|
编译后两个版本都能正确保存:
|
||||||
|
|
||||||
|
```text
|
||||||
|
pair_solref = [0.006, 1.0]
|
||||||
|
pair_solreffriction = [0.020, 1.0]
|
||||||
|
```
|
||||||
|
|
||||||
|
验证覆盖了资源目录中的 43 个主地形 XML:MuJoCo 3.2.3 和 3.10.0
|
||||||
|
均成功编译。根据地形 geom 数量,每个模型生成 4 到 152 个 foot–terrain
|
||||||
|
pair。flat 实际运行时生成 4 个 pair;wave(floor、heightfield 和四面 wall)
|
||||||
|
实际运行时生成 24 个 pair。
|
||||||
|
|
||||||
|
## 14. 推荐方案
|
||||||
|
|
||||||
|
### 方案 A:按地形使用不同 `solref`
|
||||||
|
|
||||||
|
这是实现最简单、风险最低的方案:
|
||||||
|
|
||||||
|
```text
|
||||||
|
flat / 普通平面:solref = 0.02 1
|
||||||
|
wave / hfield: solref = 0.006 1
|
||||||
|
```
|
||||||
|
|
||||||
|
优点:
|
||||||
|
|
||||||
|
- flat 保持平滑摩擦响应;
|
||||||
|
- wave 减少 native CCD 高度场穿透;
|
||||||
|
- 不需要显式 contact pair。
|
||||||
|
|
||||||
|
缺点:
|
||||||
|
|
||||||
|
- 需要 terrain-specific 模型或加载时覆盖;
|
||||||
|
- 同一个机器人 XML 无法单独表达这种差异。
|
||||||
|
|
||||||
|
### 方案 B:分离法向和摩擦 `solref`
|
||||||
|
|
||||||
|
使用显式 contact pair:
|
||||||
|
|
||||||
|
```text
|
||||||
|
normal solref = 0.006 1
|
||||||
|
friction solref = 0.02 1
|
||||||
|
```
|
||||||
|
|
||||||
|
优点:
|
||||||
|
|
||||||
|
- 法向接触足够硬,可减少高度场下陷;
|
||||||
|
- 切向摩擦保持原先较平滑的动力学;
|
||||||
|
- 物理含义最清晰。
|
||||||
|
|
||||||
|
缺点:
|
||||||
|
|
||||||
|
- 需要在机器人与地形 attach 后创建 pair;
|
||||||
|
- 每类地形碰撞 geom 都需要覆盖;
|
||||||
|
- 显式 pair 会覆盖由两个 geom 自动混合得到的接触参数,需要完整指定 `condim`、friction、solref 和 solimp 相关行为并回归测试。
|
||||||
|
|
||||||
|
### 方案 C:使用折中 time constant
|
||||||
|
|
||||||
|
例如:
|
||||||
|
|
||||||
|
```text
|
||||||
|
solref = 0.01 1
|
||||||
|
```
|
||||||
|
|
||||||
|
已有 wave 测试中,`0.01` 可以成功,并将平均穿透降到约 `1.26 mm`。它比 `0.006` 更软,可能减少 flat 上的摩擦力尖峰。
|
||||||
|
|
||||||
|
但该方案必须对 flat、wave、slope、stairs 和 obstacle 做完整回归,不能仅凭单个 run 选择。
|
||||||
|
|
||||||
|
## 15. 推荐的回归测试矩阵
|
||||||
|
|
||||||
|
修改接触参数后至少测试:
|
||||||
|
|
||||||
|
| 版本 | flat | wave | slope | stairs | obstacle |
|
||||||
|
|---|---|---|---|---|---|
|
||||||
|
| MuJoCo 3.2.3 | 必测 | 必测 | 建议 | 建议 | 建议 |
|
||||||
|
| MuJoCo 3.10 | 必测 | 必测 | 建议 | 建议 | 建议 |
|
||||||
|
|
||||||
|
每个任务同时关注:
|
||||||
|
|
||||||
|
- overall quality score;
|
||||||
|
- `mean@25` 和 `mean@50`;
|
||||||
|
- 线速度与角速度误差;
|
||||||
|
- orientation stability;
|
||||||
|
- friction margin,尤其是零值数量;
|
||||||
|
- ZMP margin;
|
||||||
|
- 最大和 P95 接触穿透;
|
||||||
|
- 是否在目标区域判定上成功;
|
||||||
|
- 是否出现 segmentation fault、penetration reset 或数值发散。
|
||||||
|
|
||||||
|
不要只比较最终 success。对闭环策略来说,二值成功可能因为很小的终点位置差异发生翻转。
|
||||||
|
|
||||||
|
## 16. 最终建议
|
||||||
|
|
||||||
|
针对当前 Go2 MoE 策略,建议优先采用以下顺序:
|
||||||
|
|
||||||
|
1. 保留 MuJoCo 3.10 的 native CCD 默认行为;
|
||||||
|
2. 不在 MuJoCo 3.2.3 高度场任务中强制开启实验性 native CCD;
|
||||||
|
3. wave/hfield 使用更短的法向 time constant,例如 `0.006` 或经过回归后的 `0.01`;
|
||||||
|
4. flat 不要无条件继承同样硬的切向摩擦 time constant;
|
||||||
|
5. 若需要统一模型,使用 attach 后显式 contact pair,将 `solref` 与 `solreffriction` 分离;
|
||||||
|
6. 若优先追求代码简单,采用 terrain-specific `solref`;
|
||||||
|
7. 评估版本差异时同时查看逐项指标和原始接触力,不要仅依赖几何平均总分及二值 success。
|
||||||
|
|
||||||
|
这组实验说明:`solref` 不是单纯的“防穿透参数”。在 `condim=6`、椭圆摩擦锥和闭环策略评估中,它会同时改变法向接触、切向摩擦、策略轨迹和评分分布。
|
||||||
BIN
resources/models/go1/policy.pt
Normal file
BIN
resources/models/go1/policy.pt
Normal file
Binary file not shown.
BIN
resources/models/go1/policy_10k.pt
Normal file
BIN
resources/models/go1/policy_10k.pt
Normal file
Binary file not shown.
BIN
resources/models/go1/policy_15k.onnx
Normal file
BIN
resources/models/go1/policy_15k.onnx
Normal file
Binary file not shown.
BIN
resources/models/go1/policy_15k.pt
Normal file
BIN
resources/models/go1/policy_15k.pt
Normal file
Binary file not shown.
BIN
resources/robots/go1/assets/calf.stl
Normal file
BIN
resources/robots/go1/assets/calf.stl
Normal file
Binary file not shown.
BIN
resources/robots/go1/assets/hip.stl
Normal file
BIN
resources/robots/go1/assets/hip.stl
Normal file
Binary file not shown.
BIN
resources/robots/go1/assets/thigh.stl
Normal file
BIN
resources/robots/go1/assets/thigh.stl
Normal file
Binary file not shown.
BIN
resources/robots/go1/assets/thigh_mirror.stl
Normal file
BIN
resources/robots/go1/assets/thigh_mirror.stl
Normal file
Binary file not shown.
BIN
resources/robots/go1/assets/trunk.stl
Normal file
BIN
resources/robots/go1/assets/trunk.stl
Normal file
Binary file not shown.
197
resources/robots/go1/go1.xml
Normal file
197
resources/robots/go1/go1.xml
Normal file
@@ -0,0 +1,197 @@
|
|||||||
|
<mujoco model="go1">
|
||||||
|
<compiler angle="radian" meshdir="assets" />
|
||||||
|
<size njmax="500" nconmax="100" />
|
||||||
|
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
|
||||||
|
|
||||||
|
<light castshadow="false" diffuse="1 1 1"/>
|
||||||
|
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
|
||||||
|
<camera fovy="60"/>
|
||||||
|
<joint damping="0.01" armature="0.01" frictionloss="0.2" />
|
||||||
|
|
||||||
|
</default>
|
||||||
|
<asset>
|
||||||
|
<mesh name="trunk" file="trunk.stl" />
|
||||||
|
<mesh name="hip" file="hip.stl" />
|
||||||
|
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
|
||||||
|
<mesh name="calf" file="calf.stl" />
|
||||||
|
<mesh name="thigh" file="thigh.stl" />
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<body name="base_link" pos="0 0 0.35">
|
||||||
|
<inertial pos="0.0116053 0.00442221 0.000106692" quat="0.0111438 0.707126 -0.00935374 0.706938" mass="4.801" diaginertia="0.0447997 0.0366257 0.0162187" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" />
|
||||||
|
<geom size="0.13 0.04675 0.057" type="box" rgba="0.913725 0.913725 0.847059 1" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" rgba="0.8 0 0 0" />
|
||||||
|
<site name="imu" pos="0 0 0"/>
|
||||||
|
|
||||||
|
<body name="FR_hip" pos="0.1881 -0.04675 0">
|
||||||
|
<inertial pos="-0.00406411 -0.0193463 4.50733e-06" quat="0.467526 0.531662 -0.466259 0.530431" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="FR_thigh" pos="0 -0.08 0">
|
||||||
|
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FR_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom name="FR" size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
<body name="FR_foot" pos="0 0 -0.213" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="FL_hip" pos="0.1881 0.04675 0">
|
||||||
|
<inertial pos="-0.00406411 0.0193463 4.50733e-06" quat="0.531662 0.467526 -0.530431 0.466259" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="FL_thigh" pos="0 0.08 0">
|
||||||
|
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FL_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom name="FL" size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
<body name="FL_foot" pos="0 0 -0.213" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RR_hip" pos="-0.1881 -0.04675 0">
|
||||||
|
<inertial pos="0.00406411 -0.0193463 4.50733e-06" quat="0.530431 0.466259 -0.531662 0.467526" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
|
||||||
|
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="RR_thigh" pos="0 -0.08 0">
|
||||||
|
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RR_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom name="RR" size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
<body name="RR_foot" pos="0 0 -0.213" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RL_hip" pos="-0.1881 0.04675 0">
|
||||||
|
<inertial pos="0.00406411 0.0193463 4.50733e-06" quat="0.466259 0.530431 -0.467526 0.531662" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
|
||||||
|
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="RL_thigh" pos="0 0.08 0">
|
||||||
|
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RL_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom name="RL" size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
<body name="RL_foot" pos="0 0 -0.213" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
<actuator>
|
||||||
|
|
||||||
|
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
|
||||||
|
<motor name="FR_thigh" gear="1" joint="FR_thigh_joint"/>
|
||||||
|
<motor name="FR_calf" gear="1" joint="FR_calf_joint"/>
|
||||||
|
<motor name="FL_hip" gear="1" joint="FL_hip_joint"/>
|
||||||
|
<motor name="FL_thigh" gear="1" joint="FL_thigh_joint"/>
|
||||||
|
<motor name="FL_calf" gear="1" joint="FL_calf_joint"/>
|
||||||
|
<motor name="RR_hip" gear="1" joint="RR_hip_joint"/>
|
||||||
|
<motor name="RR_thigh" gear="1" joint="RR_thigh_joint"/>
|
||||||
|
<motor name="RR_calf" gear="1" joint="RR_calf_joint" />
|
||||||
|
<motor name="RL_hip" gear="1" joint="RL_hip_joint"/>
|
||||||
|
<motor name="RL_thigh" gear="1" joint="RL_thigh_joint"/>
|
||||||
|
<motor name="RL_calf" gear="1" joint="RL_calf_joint"/>
|
||||||
|
</actuator>
|
||||||
|
|
||||||
|
<sensor>
|
||||||
|
|
||||||
|
<jointpos name="FR_hip_pos" joint="FR_hip_joint"/>
|
||||||
|
<jointpos name="FR_thigh_pos" joint="FR_thigh_joint"/>
|
||||||
|
<jointpos name="FR_calf_pos" joint="FR_calf_joint"/>
|
||||||
|
<jointpos name="FL_hip_pos" joint="FL_hip_joint"/>
|
||||||
|
<jointpos name="FL_thigh_pos" joint="FL_thigh_joint"/>
|
||||||
|
<jointpos name="FL_calf_pos" joint="FL_calf_joint"/>
|
||||||
|
<jointpos name="RR_hip_pos" joint="RR_hip_joint"/>
|
||||||
|
<jointpos name="RR_thigh_pos" joint="RR_thigh_joint"/>
|
||||||
|
<jointpos name="RR_calf_pos" joint="RR_calf_joint" />
|
||||||
|
<jointpos name="RL_hip_pos" joint="RL_hip_joint"/>
|
||||||
|
<jointpos name="RL_thigh_pos" joint="RL_thigh_joint"/>
|
||||||
|
<jointpos name="RL_calf_pos" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<jointvel name="FR_hip_vel" joint="FR_hip_joint"/>
|
||||||
|
<jointvel name="FR_thigh_vel" joint="FR_thigh_joint"/>
|
||||||
|
<jointvel name="FR_calf_vel" joint="FR_calf_joint"/>
|
||||||
|
<jointvel name="FL_hip_vel" joint="FL_hip_joint"/>
|
||||||
|
<jointvel name="FL_thigh_vel" joint="FL_thigh_joint"/>
|
||||||
|
<jointvel name="FL_calf_vel" joint="FL_calf_joint"/>
|
||||||
|
<jointvel name="RR_hip_vel" joint="RR_hip_joint"/>
|
||||||
|
<jointvel name="RR_thigh_vel" joint="RR_thigh_joint"/>
|
||||||
|
<jointvel name="RR_calf_vel" joint="RR_calf_joint" />
|
||||||
|
<jointvel name="RL_hip_vel" joint="RL_hip_joint"/>
|
||||||
|
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
|
||||||
|
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<jointactuatorfrc name="FR_hip_torque" joint="FR_hip_joint" />
|
||||||
|
<jointactuatorfrc name="FR_thigh_torque" joint="FR_thigh_joint" />
|
||||||
|
<jointactuatorfrc name="FR_calf_torque" joint="FR_calf_joint" />
|
||||||
|
<jointactuatorfrc name="FL_hip_torque" joint="FL_hip_joint" />
|
||||||
|
<jointactuatorfrc name="FL_thigh_torque" joint="FL_thigh_joint" />
|
||||||
|
<jointactuatorfrc name="FL_calf_torque" joint="FL_calf_joint" />
|
||||||
|
<jointactuatorfrc name="RR_hip_torque" joint="RR_hip_joint" />
|
||||||
|
<jointactuatorfrc name="RR_thigh_torque" joint="RR_thigh_joint" />
|
||||||
|
<jointactuatorfrc name="RR_calf_torque" joint="RR_calf_joint" />
|
||||||
|
<jointactuatorfrc name="RL_hip_torque" joint="RL_hip_joint" />
|
||||||
|
<jointactuatorfrc name="RL_thigh_torque" joint="RL_thigh_joint" />
|
||||||
|
<jointactuatorfrc name="RL_calf_torque" joint="RL_calf_joint" />
|
||||||
|
|
||||||
|
<accelerometer name="Body_Acc" site="imu"/>
|
||||||
|
|
||||||
|
<gyro name="Body_Gyro" site="imu"/>
|
||||||
|
|
||||||
|
<framepos name="Body_Pos" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
<framelinvel name="Body_LinVel" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
<framequat name="Body_Quat" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
|
||||||
|
</sensor>
|
||||||
|
</mujoco>
|
||||||
@@ -6,12 +6,36 @@ from typing import Dict, Any, Optional
|
|||||||
from robogauge.scripts.server import ResponseStatus
|
from robogauge.scripts.server import ResponseStatus
|
||||||
|
|
||||||
class RoboGaugeClient:
|
class RoboGaugeClient:
|
||||||
def __init__(self, base_url: str = "http://127.0.0.1:9973"):
|
def __init__(self, base_url: str = "http://127.0.0.1:9973", request_timeout: float = 5.0):
|
||||||
self.base_url = base_url
|
self.base_url = base_url
|
||||||
|
self.request_timeout = request_timeout
|
||||||
self.processing_ids = []
|
self.processing_ids = []
|
||||||
self.task_id2info = {}
|
self.task_id2info = {}
|
||||||
self.response_data = {}
|
self.response_data = {}
|
||||||
|
|
||||||
|
def is_server_available(self) -> bool:
|
||||||
|
"""Return True when the RoboGauge API and evaluator main process are healthy."""
|
||||||
|
try:
|
||||||
|
response = requests.get(
|
||||||
|
f"{self.base_url}/health",
|
||||||
|
timeout=self.request_timeout,
|
||||||
|
)
|
||||||
|
if response.status_code != 200:
|
||||||
|
return False
|
||||||
|
return response.json().get("status") == "ok"
|
||||||
|
except (requests.exceptions.RequestException, ValueError):
|
||||||
|
return False
|
||||||
|
|
||||||
|
def wait_until_available(self, retry_interval: int = 2) -> None:
|
||||||
|
"""Block until the RoboGauge API can be reached."""
|
||||||
|
while not self.is_server_available():
|
||||||
|
print(
|
||||||
|
f"[RoboGaugeClient]⏳ Waiting for RoboGauge server at {self.base_url} "
|
||||||
|
f"(retrying in {retry_interval}s)..."
|
||||||
|
)
|
||||||
|
time.sleep(retry_interval)
|
||||||
|
print(f"[RoboGaugeClient]✅ Connected to RoboGauge server at {self.base_url}.")
|
||||||
|
|
||||||
def submit_task(self,
|
def submit_task(self,
|
||||||
model_path: str,
|
model_path: str,
|
||||||
step: int,
|
step: int,
|
||||||
@@ -40,9 +64,16 @@ class RoboGaugeClient:
|
|||||||
|
|
||||||
print(f"[RoboGaugeClient]📤 Preparing to submit task: {task_name}")
|
print(f"[RoboGaugeClient]📤 Preparing to submit task: {task_name}")
|
||||||
|
|
||||||
|
if wait_for_server:
|
||||||
|
self.wait_until_available(retry_interval=retry_interval)
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
try:
|
try:
|
||||||
response = requests.post(f"{self.base_url}/submit_eval", json=payload)
|
response = requests.post(
|
||||||
|
f"{self.base_url}/submit_eval",
|
||||||
|
json=payload,
|
||||||
|
timeout=self.request_timeout,
|
||||||
|
)
|
||||||
|
|
||||||
if response.status_code == 200:
|
if response.status_code == 200:
|
||||||
data = response.json()
|
data = response.json()
|
||||||
@@ -55,9 +86,9 @@ class RoboGaugeClient:
|
|||||||
print(f"[RoboGaugeClient]❌ Server returned error: {response.text}")
|
print(f"[RoboGaugeClient]❌ Server returned error: {response.text}")
|
||||||
return None
|
return None
|
||||||
|
|
||||||
except requests.exceptions.ConnectionError:
|
except requests.exceptions.RequestException:
|
||||||
if not wait_for_server:
|
if not wait_for_server:
|
||||||
print("[RoboGaugeClient]❌ Unable to connect to server (Connection Refused).")
|
print("[RoboGaugeClient]❌ Unable to connect to server.")
|
||||||
return None
|
return None
|
||||||
|
|
||||||
print(f"[RoboGaugeClient]⏳ Server not responding, retrying in {retry_interval} seconds... (Make sure server.py is running)")
|
print(f"[RoboGaugeClient]⏳ Server not responding, retrying in {retry_interval} seconds... (Make sure server.py is running)")
|
||||||
@@ -66,11 +97,24 @@ class RoboGaugeClient:
|
|||||||
print(f"[RoboGaugeClient]❌ Unknown error: {e}")
|
print(f"[RoboGaugeClient]❌ Unknown error: {e}")
|
||||||
return None
|
return None
|
||||||
|
|
||||||
def monitor_tasks(self):
|
def monitor_tasks(self, wait_for_server: bool = True, retry_interval: int = 2):
|
||||||
print("[RoboGaugeClient]⏱️ Monitoring submitted tasks...")
|
print("[RoboGaugeClient]⏱️ Monitoring submitted tasks...")
|
||||||
""" Monitor all submitted tasks until completion. """
|
""" Monitor all submitted tasks until completion. """
|
||||||
|
if wait_for_server:
|
||||||
|
self.wait_until_available(retry_interval=retry_interval)
|
||||||
|
|
||||||
for task_id in reversed(self.processing_ids):
|
for task_id in reversed(self.processing_ids):
|
||||||
respone = requests.get(f"{self.base_url}/get_result/{task_id}")
|
try:
|
||||||
|
respone = requests.get(f"{self.base_url}/get_result/{task_id}", timeout=self.request_timeout)
|
||||||
|
except requests.exceptions.RequestException:
|
||||||
|
if not wait_for_server:
|
||||||
|
raise
|
||||||
|
print(
|
||||||
|
f"[RoboGaugeClient]⏳ Lost connection to RoboGauge server at {self.base_url}; "
|
||||||
|
f"waiting before checking task results..."
|
||||||
|
)
|
||||||
|
self.wait_until_available(retry_interval=retry_interval)
|
||||||
|
continue
|
||||||
if respone.status_code != 200:
|
if respone.status_code != 200:
|
||||||
continue
|
continue
|
||||||
resp_data = respone.json()
|
resp_data = respone.json()
|
||||||
|
|||||||
@@ -8,14 +8,17 @@
|
|||||||
@Desc : Run Robogauge Pipeline
|
@Desc : Run Robogauge Pipeline
|
||||||
'''
|
'''
|
||||||
import os
|
import os
|
||||||
|
import sys
|
||||||
|
|
||||||
# Headless solution for mujoco
|
# Select the rendering backend before importing MuJoCo. EGL provides an
|
||||||
# For GPU
|
# offscreen context on headless Linux, while GLFW is used by the GUI viewer.
|
||||||
# os.environ['MUJOCO_GL'] = 'egl'
|
# Keep an explicit MUJOCO_GL value so callers can override either default.
|
||||||
# For CPU (Slow)
|
default_mujoco_gl = (
|
||||||
# os.environ['MUJOCO_GL'] = 'osmesa'
|
'egl'
|
||||||
# With a graphical user interface (GUI)
|
if sys.platform.startswith('linux') and '--headless' in sys.argv
|
||||||
os.environ['MUJOCO_GL'] = 'glfw'
|
else 'glfw'
|
||||||
|
)
|
||||||
|
os.environ.setdefault('MUJOCO_GL', default_mujoco_gl)
|
||||||
|
|
||||||
os.environ["OMP_NUM_THREADS"] = "1"
|
os.environ["OMP_NUM_THREADS"] = "1"
|
||||||
os.environ["MKL_NUM_THREADS"] = "1"
|
os.environ["MKL_NUM_THREADS"] = "1"
|
||||||
|
|||||||
@@ -19,13 +19,15 @@ os.environ['MUJOCO_GL'] = 'glfw'
|
|||||||
|
|
||||||
os.environ["OMP_NUM_THREADS"] = "1"
|
os.environ["OMP_NUM_THREADS"] = "1"
|
||||||
os.environ["MKL_NUM_THREADS"] = "1"
|
os.environ["MKL_NUM_THREADS"] = "1"
|
||||||
|
os.environ["TORCH_CPP_LOG_LEVEL"] = "ERROR"
|
||||||
|
|
||||||
import multiprocessing
|
import multiprocessing
|
||||||
|
import threading
|
||||||
import uvicorn
|
import uvicorn
|
||||||
import queue
|
import queue
|
||||||
import time
|
import time
|
||||||
import uuid
|
import uuid
|
||||||
from fastapi import FastAPI
|
from fastapi import FastAPI, HTTPException
|
||||||
from pydantic import BaseModel
|
from pydantic import BaseModel
|
||||||
from typing import Dict, Optional
|
from typing import Dict, Optional
|
||||||
import argparse
|
import argparse
|
||||||
@@ -47,6 +49,8 @@ default_args_list = [
|
|||||||
'--headless',
|
'--headless',
|
||||||
]
|
]
|
||||||
|
|
||||||
|
HEARTBEAT_TIMEOUT_SECONDS = 10.0
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class EvalTaskData:
|
class EvalTaskData:
|
||||||
model_path: str
|
model_path: str
|
||||||
@@ -67,15 +71,42 @@ class ResponseStatus:
|
|||||||
ERROR = "error"
|
ERROR = "error"
|
||||||
NOT_FOUND = "not_found"
|
NOT_FOUND = "not_found"
|
||||||
|
|
||||||
def run_api_server(input_queue: multiprocessing.Queue, result_dict: dict, port=9973):
|
def update_main_heartbeat(health_dict: dict, stop_event: threading.Event, interval: float = 1.0):
|
||||||
|
while not stop_event.is_set():
|
||||||
|
health_dict["main_pid"] = os.getpid()
|
||||||
|
health_dict["last_heartbeat"] = time.time()
|
||||||
|
stop_event.wait(interval)
|
||||||
|
|
||||||
|
def is_main_process_healthy(health_dict: dict) -> bool:
|
||||||
|
last_heartbeat = float(health_dict.get("last_heartbeat", 0.0))
|
||||||
|
return time.time() - last_heartbeat <= HEARTBEAT_TIMEOUT_SECONDS
|
||||||
|
|
||||||
|
def run_api_server(input_queue: multiprocessing.Queue, result_dict: dict, health_dict: dict, port=9973):
|
||||||
"""
|
"""
|
||||||
Running in a separate subprocess.
|
Running in a separate subprocess.
|
||||||
I/O Process: submit requests -> put into queue -> return ID.
|
I/O Process: submit requests -> put into queue -> return ID.
|
||||||
"""
|
"""
|
||||||
app = FastAPI()
|
app = FastAPI()
|
||||||
|
|
||||||
|
@app.get("/health")
|
||||||
|
def health():
|
||||||
|
last_heartbeat = float(health_dict.get("last_heartbeat", 0.0))
|
||||||
|
heartbeat_age = time.time() - last_heartbeat
|
||||||
|
if heartbeat_age > HEARTBEAT_TIMEOUT_SECONDS:
|
||||||
|
raise HTTPException(
|
||||||
|
status_code=503,
|
||||||
|
detail=f"RoboGauge main process heartbeat is stale ({heartbeat_age:.1f}s).",
|
||||||
|
)
|
||||||
|
return {
|
||||||
|
"status": "ok",
|
||||||
|
"main_pid": health_dict.get("main_pid"),
|
||||||
|
"heartbeat_age": heartbeat_age,
|
||||||
|
}
|
||||||
|
|
||||||
@app.post("/submit_eval")
|
@app.post("/submit_eval")
|
||||||
def submit_eval(req: EvalRequest):
|
def submit_eval(req: EvalRequest):
|
||||||
|
if not is_main_process_healthy(health_dict):
|
||||||
|
raise HTTPException(status_code=503, detail="RoboGauge main process is not healthy.")
|
||||||
task_id = str(uuid.uuid4())
|
task_id = str(uuid.uuid4())
|
||||||
task_data = EvalTaskData(
|
task_data = EvalTaskData(
|
||||||
model_path=req.model_path,
|
model_path=req.model_path,
|
||||||
@@ -110,10 +141,18 @@ def main():
|
|||||||
manager = ctx.Manager()
|
manager = ctx.Manager()
|
||||||
task_queue = manager.Queue()
|
task_queue = manager.Queue()
|
||||||
results_store = manager.dict()
|
results_store = manager.dict()
|
||||||
|
health_store = manager.dict()
|
||||||
|
heartbeat_stop = threading.Event()
|
||||||
|
heartbeat_thread = threading.Thread(
|
||||||
|
target=update_main_heartbeat,
|
||||||
|
args=(health_store, heartbeat_stop),
|
||||||
|
daemon=True,
|
||||||
|
)
|
||||||
|
heartbeat_thread.start()
|
||||||
|
|
||||||
api_p = ctx.Process(
|
api_p = ctx.Process(
|
||||||
target=run_api_server,
|
target=run_api_server,
|
||||||
args=(task_queue, results_store, args_cli.port),
|
args=(task_queue, results_store, health_store, args_cli.port),
|
||||||
daemon=True
|
daemon=True
|
||||||
)
|
)
|
||||||
api_p.start()
|
api_p.start()
|
||||||
@@ -165,6 +204,8 @@ def main():
|
|||||||
print("\n🛑 Shutting down...")
|
print("\n🛑 Shutting down...")
|
||||||
api_p.terminate()
|
api_p.terminate()
|
||||||
api_p.join()
|
api_p.join()
|
||||||
|
heartbeat_stop.set()
|
||||||
|
heartbeat_thread.join(timeout=2.0)
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|||||||
@@ -1,6 +1,16 @@
|
|||||||
from robogauge.utils.task_register import task_register
|
from robogauge.utils.task_register import task_register
|
||||||
from robogauge.tasks.simulator.mujoco_config import MujocoConfig
|
from robogauge.tasks.simulator.mujoco_config import MujocoConfig
|
||||||
from robogauge.tasks.robots import RobotConfig, Go2Config, Go2MoEConfig, Go2TerrainConfig, Go2MoETerrainConfig
|
from robogauge.tasks.robots import (
|
||||||
|
RobotConfig,
|
||||||
|
Go2Config,
|
||||||
|
Go2LabConfig,
|
||||||
|
Go2MoEConfig,
|
||||||
|
Go2TerrainConfig,
|
||||||
|
Go2LabTerrainConfig,
|
||||||
|
Go2MoETerrainConfig,
|
||||||
|
Go1MoEConfig,
|
||||||
|
Go1MoETerrainConfig,
|
||||||
|
)
|
||||||
from robogauge.tasks.pipeline import BasePipeline
|
from robogauge.tasks.pipeline import BasePipeline
|
||||||
from robogauge.tasks.gauge import BaseGaugeConfig
|
from robogauge.tasks.gauge import BaseGaugeConfig
|
||||||
|
|
||||||
@@ -26,3 +36,21 @@ task_register.register('go2_moe.wave', BasePipeline, MujocoConfig, Go2WaveGaugeC
|
|||||||
task_register.register('go2_moe.stairs_fd', BasePipeline, MujocoConfig, Go2StairsForwardGaugeConfig, Go2MoETerrainConfig)
|
task_register.register('go2_moe.stairs_fd', BasePipeline, MujocoConfig, Go2StairsForwardGaugeConfig, Go2MoETerrainConfig)
|
||||||
task_register.register('go2_moe.stairs_bd', BasePipeline, MujocoConfig, Go2StairsBackwardGaugeConfig, Go2MoETerrainConfig)
|
task_register.register('go2_moe.stairs_bd', BasePipeline, MujocoConfig, Go2StairsBackwardGaugeConfig, Go2MoETerrainConfig)
|
||||||
task_register.register('go2_moe.obstacle', BasePipeline, MujocoConfig, Go2ObstacleGaugeConfig, Go2MoETerrainConfig)
|
task_register.register('go2_moe.obstacle', BasePipeline, MujocoConfig, Go2ObstacleGaugeConfig, Go2MoETerrainConfig)
|
||||||
|
|
||||||
|
# Go1 MoE (the policy is evaluated in the Go1 MuJoCo dynamics model)
|
||||||
|
task_register.register('go1_moe.flat', BasePipeline, MujocoConfig, Go2FlatGaugeConfig, Go1MoEConfig)
|
||||||
|
task_register.register('go1_moe.slope_fd', BasePipeline, MujocoConfig, Go2SlopeForwardGaugeConfig, Go1MoETerrainConfig)
|
||||||
|
task_register.register('go1_moe.slope_bd', BasePipeline, MujocoConfig, Go2SlopeBackwardGaugeConfig, Go1MoETerrainConfig)
|
||||||
|
task_register.register('go1_moe.wave', BasePipeline, MujocoConfig, Go2WaveGaugeConfig, Go1MoETerrainConfig)
|
||||||
|
task_register.register('go1_moe.stairs_fd', BasePipeline, MujocoConfig, Go2StairsForwardGaugeConfig, Go1MoETerrainConfig)
|
||||||
|
task_register.register('go1_moe.stairs_bd', BasePipeline, MujocoConfig, Go2StairsBackwardGaugeConfig, Go1MoETerrainConfig)
|
||||||
|
task_register.register('go1_moe.obstacle', BasePipeline, MujocoConfig, Go2ObstacleGaugeConfig, Go1MoETerrainConfig)
|
||||||
|
|
||||||
|
# Go2 Lab
|
||||||
|
task_register.register('go2_lab.flat', BasePipeline, MujocoConfig, Go2FlatGaugeConfig, Go2LabConfig)
|
||||||
|
task_register.register('go2_lab.slope_fd', BasePipeline, MujocoConfig, Go2SlopeForwardGaugeConfig, Go2LabTerrainConfig)
|
||||||
|
task_register.register('go2_lab.slope_bd', BasePipeline, MujocoConfig, Go2SlopeBackwardGaugeConfig, Go2LabTerrainConfig)
|
||||||
|
task_register.register('go2_lab.wave', BasePipeline, MujocoConfig, Go2WaveGaugeConfig, Go2LabTerrainConfig)
|
||||||
|
task_register.register('go2_lab.stairs_fd', BasePipeline, MujocoConfig, Go2StairsForwardGaugeConfig, Go2LabTerrainConfig)
|
||||||
|
task_register.register('go2_lab.stairs_bd', BasePipeline, MujocoConfig, Go2StairsBackwardGaugeConfig, Go2LabTerrainConfig)
|
||||||
|
task_register.register('go2_lab.obstacle', BasePipeline, MujocoConfig, Go2ObstacleGaugeConfig, Go2LabTerrainConfig)
|
||||||
|
|||||||
@@ -58,6 +58,9 @@ class BaseGauge:
|
|||||||
elif name == 'joystick':
|
elif name == 'joystick':
|
||||||
self.goals.append(JoystickGoal(robot_cfg.commands, **kwargs))
|
self.goals.append(JoystickGoal(robot_cfg.commands, **kwargs))
|
||||||
log_str += f" - Joystick Goal: {kwargs}\n"
|
log_str += f" - Joystick Goal: {kwargs}\n"
|
||||||
|
elif name == 'keyboard':
|
||||||
|
self.goals.append(KeyboardGoal(robot_cfg.commands, **kwargs))
|
||||||
|
log_str += f" - Keyboard Goal: {kwargs}\n"
|
||||||
else:
|
else:
|
||||||
raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.")
|
raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.")
|
||||||
self.info['goal'].append(name)
|
self.info['goal'].append(name)
|
||||||
|
|||||||
@@ -55,6 +55,10 @@ class BaseGaugeConfig(Config):
|
|||||||
goal_type = 'velocity' # 'velocity'
|
goal_type = 'velocity' # 'velocity'
|
||||||
dead_zone = 0.1 # joystick dead zone
|
dead_zone = 0.1 # joystick dead zone
|
||||||
|
|
||||||
|
class keyboard: # goal controlled by a keyboard listener independent of MuJoCo
|
||||||
|
enabled = False
|
||||||
|
goal_type = 'velocity'
|
||||||
|
|
||||||
class metrics:
|
class metrics:
|
||||||
metric_dt = 0.1 # [s] frequency to compute metrics
|
metric_dt = 0.1 # [s] frequency to compute metrics
|
||||||
class dof_limits:
|
class dof_limits:
|
||||||
|
|||||||
@@ -1,3 +1,4 @@
|
|||||||
from robogauge.tasks.gauge.goals.base_goal import BaseGoal
|
from robogauge.tasks.gauge.goals.base_goal import BaseGoal
|
||||||
from robogauge.tasks.gauge.goals.joystick_goal import JoystickGoal
|
from robogauge.tasks.gauge.goals.joystick_goal import JoystickGoal
|
||||||
|
from robogauge.tasks.gauge.goals.keyboard_goal import KeyboardGoal
|
||||||
from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal, DiagonalVelocityGoal, TargetPosVelocityGoal
|
from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal, DiagonalVelocityGoal, TargetPosVelocityGoal
|
||||||
|
|||||||
149
robogauge/tasks/gauge/goals/keyboard_goal.py
Normal file
149
robogauge/tasks/gauge/goals/keyboard_goal.py
Normal file
@@ -0,0 +1,149 @@
|
|||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
"""Keyboard teleoperation goal using an input listener separate from MuJoCo."""
|
||||||
|
|
||||||
|
import atexit
|
||||||
|
import queue
|
||||||
|
import threading
|
||||||
|
|
||||||
|
from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal
|
||||||
|
from robogauge.tasks.gauge.goals.base_goal import BaseGoal
|
||||||
|
from robogauge.tasks.simulator.sim_data import SimData
|
||||||
|
from robogauge.utils.helpers import class_to_dict
|
||||||
|
from robogauge.utils.logger import logger
|
||||||
|
|
||||||
|
|
||||||
|
class _KeyboardState:
|
||||||
|
"""Track held keys without replacing MuJoCo viewer shortcuts."""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.events = queue.Queue()
|
||||||
|
self.running = False
|
||||||
|
self.held = set()
|
||||||
|
self.lock = threading.Lock()
|
||||||
|
self.thread = None
|
||||||
|
self.listener = None
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _name(key):
|
||||||
|
try:
|
||||||
|
if hasattr(key, 'char') and key.char:
|
||||||
|
return key.char.lower()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
return str(key).lower()
|
||||||
|
|
||||||
|
def _worker(self):
|
||||||
|
while self.running:
|
||||||
|
try:
|
||||||
|
event_type, key = self.events.get(timeout=0.05)
|
||||||
|
except queue.Empty:
|
||||||
|
continue
|
||||||
|
name = self._name(key)
|
||||||
|
with self.lock:
|
||||||
|
if event_type == 'press':
|
||||||
|
self.held.add(name)
|
||||||
|
else:
|
||||||
|
self.held.discard(name)
|
||||||
|
|
||||||
|
def start(self):
|
||||||
|
try:
|
||||||
|
from pynput import keyboard
|
||||||
|
except ImportError as exc:
|
||||||
|
raise ImportError(
|
||||||
|
"Keyboard control requires 'pynput'. Install it with "
|
||||||
|
"'python -m pip install pynput'."
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
self.running = True
|
||||||
|
self.listener = keyboard.Listener(
|
||||||
|
on_press=lambda key: self.events.put(('press', key)),
|
||||||
|
on_release=lambda key: self.events.put(('release', key)),
|
||||||
|
)
|
||||||
|
self.listener.start()
|
||||||
|
self.thread = threading.Thread(target=self._worker, daemon=True)
|
||||||
|
self.thread.start()
|
||||||
|
|
||||||
|
def snapshot(self):
|
||||||
|
with self.lock:
|
||||||
|
return set(self.held)
|
||||||
|
|
||||||
|
def clear(self):
|
||||||
|
with self.lock:
|
||||||
|
self.held.clear()
|
||||||
|
|
||||||
|
def stop(self):
|
||||||
|
self.running = False
|
||||||
|
if self.listener is not None:
|
||||||
|
self.listener.stop()
|
||||||
|
self.listener = None
|
||||||
|
|
||||||
|
|
||||||
|
class KeyboardGoal(BaseGoal):
|
||||||
|
"""Produce velocity commands while the configured teleoperation keys are held."""
|
||||||
|
|
||||||
|
name = 'keyboard'
|
||||||
|
|
||||||
|
def __init__(self, max_velocity, goal_type='velocity', **kwargs):
|
||||||
|
super().__init__()
|
||||||
|
if goal_type != 'velocity':
|
||||||
|
raise NotImplementedError("Only 'velocity' goal type is implemented for KeyboardGoal.")
|
||||||
|
|
||||||
|
kwargs.pop('enabled', None)
|
||||||
|
if kwargs:
|
||||||
|
logger.warning(f"Unused kwargs in KeyboardGoal: {kwargs}")
|
||||||
|
|
||||||
|
self.max_velocity = class_to_dict(max_velocity)
|
||||||
|
self.total = 1
|
||||||
|
self.current_command = (0.0, 0.0, 0.0)
|
||||||
|
self.keyboard = _KeyboardState()
|
||||||
|
self.keyboard.start()
|
||||||
|
atexit.register(self.keyboard.stop)
|
||||||
|
logger.info(
|
||||||
|
"Keyboard control: Up/Down=forward/back, Left/Right=yaw, "
|
||||||
|
"','=left strafe, '.'=right strafe, K=stop. "
|
||||||
|
"MuJoCo viewer shortcuts remain independent."
|
||||||
|
)
|
||||||
|
|
||||||
|
def is_reset(self, sim_data: SimData) -> bool:
|
||||||
|
return False
|
||||||
|
|
||||||
|
def pre_get_goal(self, sim_data: SimData) -> bool:
|
||||||
|
return False
|
||||||
|
|
||||||
|
def reset_goal(self):
|
||||||
|
self.keyboard.clear()
|
||||||
|
self.current_command = (0.0, 0.0, 0.0)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _axis_command(held, positive_key, negative_key, limits):
|
||||||
|
positive = positive_key in held
|
||||||
|
negative = negative_key in held
|
||||||
|
if positive == negative:
|
||||||
|
return 0.0
|
||||||
|
return float(limits[1] if positive else limits[0])
|
||||||
|
|
||||||
|
def command_from_keys(self, held):
|
||||||
|
if 'k' in held:
|
||||||
|
return 0.0, 0.0, 0.0
|
||||||
|
return (
|
||||||
|
self._axis_command(held, 'key.up', 'key.down', self.max_velocity['lin_vel_x']),
|
||||||
|
self._axis_command(held, ',', '.', self.max_velocity['lin_vel_y']),
|
||||||
|
self._axis_command(held, 'key.left', 'key.right', self.max_velocity['ang_vel_yaw']),
|
||||||
|
)
|
||||||
|
|
||||||
|
def get_goal(self, sim_data: SimData) -> GoalData:
|
||||||
|
command = self.command_from_keys(self.keyboard.snapshot())
|
||||||
|
if command != self.current_command:
|
||||||
|
self.current_command = command
|
||||||
|
logger.info(
|
||||||
|
"Keyboard command: vx=%.2f, vy=%.2f, yaw=%.2f",
|
||||||
|
*command,
|
||||||
|
)
|
||||||
|
return GoalData(
|
||||||
|
goal_type='velocity',
|
||||||
|
velocity_goal=VelocityGoal(
|
||||||
|
lin_vel_x=command[0],
|
||||||
|
lin_vel_y=command[1],
|
||||||
|
ang_vel_yaw=command[2],
|
||||||
|
),
|
||||||
|
)
|
||||||
@@ -17,7 +17,7 @@ from copy import deepcopy
|
|||||||
from robogauge.utils.logger import logger
|
from robogauge.utils.logger import logger
|
||||||
from robogauge.tasks.simulator import MujocoSimulator, MujocoConfig, SimData
|
from robogauge.tasks.simulator import MujocoSimulator, MujocoConfig, SimData
|
||||||
from robogauge.tasks.robots import (
|
from robogauge.tasks.robots import (
|
||||||
BaseRobot, RobotConfig, Go2Config, Go2, Go2MoEConfig, Go2MoE
|
BaseRobot, RobotConfig, Go2Config, Go2, Go2MoEConfig, Go2MoE, Go1MoE
|
||||||
)
|
)
|
||||||
from robogauge.tasks.gauge import BaseGauge, BaseGaugeConfig
|
from robogauge.tasks.gauge import BaseGauge, BaseGaugeConfig
|
||||||
from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal, PositionGoal
|
from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal, PositionGoal
|
||||||
|
|||||||
@@ -1,6 +1,8 @@
|
|||||||
from .base_robot_config import RobotConfig
|
from .base_robot_config import RobotConfig
|
||||||
from .base_robot import BaseRobot
|
from .base_robot import BaseRobot
|
||||||
from .go2.go2_config import Go2Config, Go2TerrainConfig
|
from .go2.go2_config import Go2Config, Go2TerrainConfig
|
||||||
|
from .go2.go2_lab_config import Go2LabConfig, Go2LabTerrainConfig
|
||||||
from .go2.go2 import Go2
|
from .go2.go2 import Go2
|
||||||
from .go2.go2_moe_config import Go2MoEConfig, Go2MoETerrainConfig
|
from .go2.go2_moe_config import Go2MoEConfig, Go2MoETerrainConfig
|
||||||
from .go2.go2_moe import Go2MoE
|
from .go2.go2_moe import Go2MoE
|
||||||
|
from .go1 import Go1Config, Go1TerrainConfig, Go1, Go1MoEConfig, Go1MoETerrainConfig, Go1MoE
|
||||||
|
|||||||
4
robogauge/tasks/robots/go1/__init__.py
Normal file
4
robogauge/tasks/robots/go1/__init__.py
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
from .go1_config import Go1Config, Go1TerrainConfig
|
||||||
|
from .go1 import Go1
|
||||||
|
from .go1_moe_config import Go1MoEConfig, Go1MoETerrainConfig
|
||||||
|
from .go1_moe import Go1MoE
|
||||||
13
robogauge/tasks/robots/go1/go1.py
Normal file
13
robogauge/tasks/robots/go1/go1.py
Normal file
@@ -0,0 +1,13 @@
|
|||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
"""Go1 policy adapter.
|
||||||
|
|
||||||
|
The trained policy and the Go1 MuJoCo asset both use FR, FL, RR, RL joint
|
||||||
|
groups. Isaac Gym exposed the training asset as FL, FR, RL, RR, but Go1Robot
|
||||||
|
permuted its observations and actions during training.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from robogauge.tasks.robots.go2.go2 import Go2
|
||||||
|
|
||||||
|
|
||||||
|
class Go1(Go2):
|
||||||
|
pass
|
||||||
38
robogauge/tasks/robots/go1/go1_config.py
Normal file
38
robogauge/tasks/robots/go1/go1_config.py
Normal file
@@ -0,0 +1,38 @@
|
|||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
"""Go1 configuration for the RoboGauge MuJoCo evaluator."""
|
||||||
|
|
||||||
|
from robogauge.tasks.robots.go2.go2_config import Go2Config
|
||||||
|
|
||||||
|
|
||||||
|
class Go1Config(Go2Config):
|
||||||
|
robot_name = 'go1'
|
||||||
|
robot_class = 'Go1'
|
||||||
|
|
||||||
|
class assets(Go2Config.assets):
|
||||||
|
robot_xml = "{ROBOGAUGE_ROOT_DIR}/resources/robots/go1/go1.xml"
|
||||||
|
foot_geom_names = ['FR', 'FL', 'RR', 'RL']
|
||||||
|
|
||||||
|
class control(Go2Config.control):
|
||||||
|
model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go1/policy.pt"
|
||||||
|
control_dt = 0.02
|
||||||
|
p_gains = [28.0] * 12
|
||||||
|
d_gains = [0.7] * 12
|
||||||
|
|
||||||
|
# Both this MuJoCo model and the trained policy use FR, FL, RR, RL.
|
||||||
|
# Go1Robot permuted Isaac Gym's FL, FR, RL, RR asset state into this
|
||||||
|
# order during training, so applying that permutation again here
|
||||||
|
# would swap every left/right leg pair.
|
||||||
|
default_dof_pos = [
|
||||||
|
-0.1, 0.8, -1.5,
|
||||||
|
0.1, 0.8, -1.5,
|
||||||
|
-0.1, 1.0, -1.5,
|
||||||
|
0.1, 1.0, -1.5,
|
||||||
|
]
|
||||||
|
mj2model_dof_indices = list(range(12))
|
||||||
|
|
||||||
|
|
||||||
|
class Go1TerrainConfig(Go1Config):
|
||||||
|
class commands(Go1Config.commands):
|
||||||
|
lin_vel_x = [-1.0, 1.0]
|
||||||
|
lin_vel_y = [-1.0, 1.0]
|
||||||
|
ang_vel_yaw = [-1.5, 1.5]
|
||||||
8
robogauge/tasks/robots/go1/go1_moe.py
Normal file
8
robogauge/tasks/robots/go1/go1_moe.py
Normal file
@@ -0,0 +1,8 @@
|
|||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
"""MoE Go1 policy adapter."""
|
||||||
|
|
||||||
|
from robogauge.tasks.robots.go2.go2_moe import Go2MoE
|
||||||
|
|
||||||
|
|
||||||
|
class Go1MoE(Go2MoE):
|
||||||
|
pass
|
||||||
12
robogauge/tasks/robots/go1/go1_moe_config.py
Normal file
12
robogauge/tasks/robots/go1/go1_moe_config.py
Normal file
@@ -0,0 +1,12 @@
|
|||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
"""MoE policy configuration for the trained Go1 checkpoint."""
|
||||||
|
|
||||||
|
from robogauge.tasks.robots.go1.go1_config import Go1Config, Go1TerrainConfig
|
||||||
|
|
||||||
|
|
||||||
|
class Go1MoEConfig(Go1Config):
|
||||||
|
robot_class = 'Go1MoE'
|
||||||
|
|
||||||
|
|
||||||
|
class Go1MoETerrainConfig(Go1TerrainConfig, Go1MoEConfig):
|
||||||
|
robot_class = 'Go1MoE'
|
||||||
24
robogauge/tasks/robots/go2/go2_lab_config.py
Normal file
24
robogauge/tasks/robots/go2/go2_lab_config.py
Normal file
@@ -0,0 +1,24 @@
|
|||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
'''
|
||||||
|
@File : go2_lab_config.py
|
||||||
|
@Time : 2026/04/11 17:59:09
|
||||||
|
@Author : wty-yy
|
||||||
|
@Version : 1.0
|
||||||
|
@Blog : https://wty-yy.github.io/
|
||||||
|
@Desc : Go2 Lab Robot Configuration
|
||||||
|
'''
|
||||||
|
from robogauge.tasks.robots.go2.go2_config import Go2Config, Go2TerrainConfig
|
||||||
|
|
||||||
|
|
||||||
|
class Go2LabConfig(Go2Config):
|
||||||
|
"""Go2 Lab robot configuration aligned with RobotLab observation scaling."""
|
||||||
|
|
||||||
|
class control(Go2Config.control):
|
||||||
|
p_gains = [25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0] # [N*m/rad]
|
||||||
|
class scales(Go2Config.control.scales):
|
||||||
|
# RobotLab policy command observation uses unit scale.
|
||||||
|
cmd = [1.0, 1.0, 1.0]
|
||||||
|
|
||||||
|
|
||||||
|
class Go2LabTerrainConfig(Go2LabConfig, Go2TerrainConfig):
|
||||||
|
"""Go2 Lab robot configuration for terrain tasks."""
|
||||||
@@ -30,7 +30,28 @@ from robogauge.tasks.simulator.sim_data import (
|
|||||||
from robogauge.tasks.gauge.goal_data import VelocityGoal
|
from robogauge.tasks.gauge.goal_data import VelocityGoal
|
||||||
|
|
||||||
class MujocoSimulator:
|
class MujocoSimulator:
|
||||||
|
_MAX_MUJOCO_VERSION = (3, 3, 0)
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def _require_supported_mujoco_version(cls):
|
||||||
|
version_string = getattr(mujoco, '__version__', '')
|
||||||
|
version_match = re.match(r'^(\d+)\.(\d+)\.(\d+)', version_string)
|
||||||
|
if version_match is None:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"Unable to parse the installed MuJoCo version: "
|
||||||
|
f"{version_string!r}. RoboGauge requires mujoco<3.3.0."
|
||||||
|
)
|
||||||
|
|
||||||
|
installed_version = tuple(map(int, version_match.groups()))
|
||||||
|
if installed_version >= cls._MAX_MUJOCO_VERSION:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"Unsupported MuJoCo version {version_string}. "
|
||||||
|
"RoboGauge evaluation requires mujoco<3.3.0 because "
|
||||||
|
"MuJoCo 3.3.0 and newer use different contact dynamics."
|
||||||
|
)
|
||||||
|
|
||||||
def __init__(self, sim_cfg: MujocoConfig):
|
def __init__(self, sim_cfg: MujocoConfig):
|
||||||
|
self._require_supported_mujoco_version()
|
||||||
self.cfg = sim_cfg
|
self.cfg = sim_cfg
|
||||||
self.terrain_xmls = None
|
self.terrain_xmls = None
|
||||||
self.robot_xml = None
|
self.robot_xml = None
|
||||||
@@ -781,6 +802,13 @@ class MujocoSimulator:
|
|||||||
|
|
||||||
contact_force = np.zeros(6, dtype=np.float64)
|
contact_force = np.zeros(6, dtype=np.float64)
|
||||||
mujoco.mj_contactForce(self.mj_model, self.mj_data, i, contact_force)
|
mujoco.mj_contactForce(self.mj_model, self.mj_data, i, contact_force)
|
||||||
|
max_contact_force = 1e6
|
||||||
|
contact_force = np.nan_to_num(
|
||||||
|
contact_force,
|
||||||
|
nan=0.0, posinf=max_contact_force, neginf=-max_contact_force
|
||||||
|
)
|
||||||
|
contact_force = np.clip(contact_force, -max_contact_force, max_contact_force)
|
||||||
|
|
||||||
friction = np.array(contact.friction, dtype=np.float32).reshape(-1)
|
friction = np.array(contact.friction, dtype=np.float32).reshape(-1)
|
||||||
|
|
||||||
positions.append(np.array(contact.pos, dtype=np.float32))
|
positions.append(np.array(contact.pos, dtype=np.float32))
|
||||||
|
|||||||
504
scripts/eval_jit_models_to_tensorboard.py
Executable file
504
scripts/eval_jit_models_to_tensorboard.py
Executable file
@@ -0,0 +1,504 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Batch-submit exported JIT checkpoints to RoboGauge and log scores to TensorBoard.
|
||||||
|
|
||||||
|
Overview:
|
||||||
|
The script discovers policy_jit_<step>.pt files under rsl_rl experiment logs,
|
||||||
|
submits checkpoints through robogauge/scripts/client.py, caches full
|
||||||
|
RoboGauge results as YAML, and writes scores as RoboGauge/{key} TensorBoard
|
||||||
|
scalars using the checkpoint id as global_step.
|
||||||
|
|
||||||
|
Quick Start:
|
||||||
|
python scripts/eval_jit_models_to_tensorboard.py
|
||||||
|
|
||||||
|
Full Command:
|
||||||
|
python scripts/eval_jit_models_to_tensorboard.py \
|
||||||
|
--source-root /home/user/go2_rl_robotlab/logs/rsl_rl \
|
||||||
|
--experiment-glob "go2_moe_cts_v3*" \
|
||||||
|
--log-root logs/go2_lab \
|
||||||
|
--task-name go2_lab \
|
||||||
|
--server-url http://127.0.0.1:9973 \
|
||||||
|
--reuse-source-results \
|
||||||
|
--clear-tensorboard
|
||||||
|
|
||||||
|
Options:
|
||||||
|
--source-root: Directory containing rsl_rl experiment logs.
|
||||||
|
--experiment-glob: One or more experiment directory patterns under source root.
|
||||||
|
--experiments: Explicit experiment names that override the glob.
|
||||||
|
--log-root: TensorBoard and result-cache output root.
|
||||||
|
--task-name: RoboGauge task name sent to the evaluation server.
|
||||||
|
--server-url: RoboGauge server URL used by RoboGaugeClient.
|
||||||
|
--request-timeout: HTTP request timeout for client calls.
|
||||||
|
--retry-interval: Delay between server availability retries.
|
||||||
|
--poll-interval: Delay between task status checks.
|
||||||
|
--start-step: Minimum checkpoint step to include.
|
||||||
|
--end-step: Maximum checkpoint step to include.
|
||||||
|
--max-checkpoints: Limit the number of discovered checkpoints.
|
||||||
|
--overwrite: Re-submit checkpoints even when output result YAML exists.
|
||||||
|
--reuse-source-results: Reuse source-run result YAML before submitting.
|
||||||
|
--clear-tensorboard: Delete existing output event files before writing.
|
||||||
|
--keep-going: Continue after failed checkpoints.
|
||||||
|
--dry-run: Print planned work without writing logs or contacting the server.
|
||||||
|
|
||||||
|
Notes:
|
||||||
|
Start robogauge/scripts/server.py before running checkpoints that cannot be
|
||||||
|
satisfied from cached YAML. Output event files are written under
|
||||||
|
logs/go2_lab/<experiment>, and full results are cached under
|
||||||
|
logs/go2_lab/<experiment>/robogauge_results.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import re
|
||||||
|
import time
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import yaml
|
||||||
|
from torch.utils.tensorboard import SummaryWriter
|
||||||
|
|
||||||
|
|
||||||
|
DEFAULT_SOURCE_ROOT = "/home/user/go2_rl_robotlab/logs/rsl_rl"
|
||||||
|
DEFAULT_LOG_ROOT = "logs/go2_lab"
|
||||||
|
CHECKPOINT_RE = re.compile(r"policy_jit_(\d+)\.pt$")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class Checkpoint:
|
||||||
|
"""Represent one exported JIT checkpoint and its source run context."""
|
||||||
|
|
||||||
|
experiment_name: str
|
||||||
|
run_dir: Path
|
||||||
|
model_path: Path
|
||||||
|
step: int
|
||||||
|
|
||||||
|
@property
|
||||||
|
def source_result_path(self) -> Path:
|
||||||
|
"""Return the source training run's expected RoboGauge result path.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Path to the source results_<step>.yaml file.
|
||||||
|
"""
|
||||||
|
return self.run_dir / "robogauge_results" / f"results_{self.step}.yaml"
|
||||||
|
|
||||||
|
|
||||||
|
def parse_args() -> argparse.Namespace:
|
||||||
|
"""Build and parse the command line interface for batch evaluation.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Parsed CLI arguments.
|
||||||
|
"""
|
||||||
|
parser = argparse.ArgumentParser(
|
||||||
|
description=(
|
||||||
|
"Evaluate exported policy_jit_*.pt files through robogauge/scripts/client.py "
|
||||||
|
"and write scores with tags matching rsl_rl's RoboGauge/{key} TensorBoard logs."
|
||||||
|
)
|
||||||
|
)
|
||||||
|
parser.add_argument("--source-root", type=Path, default=Path(DEFAULT_SOURCE_ROOT))
|
||||||
|
parser.add_argument(
|
||||||
|
"--experiment-glob",
|
||||||
|
nargs="+",
|
||||||
|
default=["go2_moe_cts_v3*"],
|
||||||
|
help="One or more experiment directory glob patterns under --source-root.",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--experiments",
|
||||||
|
nargs="+",
|
||||||
|
help="Explicit experiment directory names under --source-root. Overrides --experiment-glob.",
|
||||||
|
)
|
||||||
|
parser.add_argument("--log-root", type=Path, default=Path(DEFAULT_LOG_ROOT))
|
||||||
|
parser.add_argument("--task-name", default="go2_lab")
|
||||||
|
parser.add_argument("--server-url", default="http://127.0.0.1:9973")
|
||||||
|
parser.add_argument("--request-timeout", type=float, default=5.0)
|
||||||
|
parser.add_argument("--retry-interval", type=float, default=2.0)
|
||||||
|
parser.add_argument("--poll-interval", type=float, default=60.0)
|
||||||
|
parser.add_argument("--start-step", type=int)
|
||||||
|
parser.add_argument("--end-step", type=int)
|
||||||
|
parser.add_argument("--max-checkpoints", type=int)
|
||||||
|
parser.add_argument(
|
||||||
|
"--overwrite",
|
||||||
|
action="store_true",
|
||||||
|
help="Re-submit checkpoints even when logs/go2_lab/<experiment>/robogauge_results/results_<step>.yaml exists.",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--reuse-source-results",
|
||||||
|
action="store_true",
|
||||||
|
help="Reuse existing results from the source training run's robogauge_results before submitting to the server.",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--clear-tensorboard",
|
||||||
|
action="store_true",
|
||||||
|
help="Delete existing TensorBoard event files in each output experiment directory before writing scalars.",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--keep-going",
|
||||||
|
action="store_true",
|
||||||
|
help="Continue with later checkpoints after a failed evaluation.",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--dry-run",
|
||||||
|
action="store_true",
|
||||||
|
help="Print discovered checkpoints and planned actions without contacting the server or writing logs.",
|
||||||
|
)
|
||||||
|
return parser.parse_args()
|
||||||
|
|
||||||
|
|
||||||
|
def checkpoint_step(path: Path) -> int | None:
|
||||||
|
"""Extract the checkpoint step from a policy_jit_<step>.pt filename.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
path: Checkpoint path whose filename should match the export pattern.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Parsed checkpoint step, or None when the filename does not match.
|
||||||
|
"""
|
||||||
|
match = CHECKPOINT_RE.match(path.name)
|
||||||
|
if match is None:
|
||||||
|
return None
|
||||||
|
return int(match.group(1))
|
||||||
|
|
||||||
|
|
||||||
|
def discover_experiment_dirs(source_root: Path, experiment_globs: list[str], experiments: list[str] | None) -> list[Path]:
|
||||||
|
"""Discover experiment directories under the source root.
|
||||||
|
|
||||||
|
Explicit experiment names take precedence over the glob. Missing explicit
|
||||||
|
directories raise an error so typos do not silently skip evaluations.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
source_root: Directory that contains rsl_rl experiment directories.
|
||||||
|
experiment_globs: Glob patterns used when explicit experiments are absent.
|
||||||
|
experiments: Optional experiment directory names to evaluate.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Existing experiment directories to scan.
|
||||||
|
"""
|
||||||
|
if experiments:
|
||||||
|
dirs = [source_root / name for name in experiments]
|
||||||
|
else:
|
||||||
|
matched_dirs = {
|
||||||
|
path
|
||||||
|
for experiment_glob in experiment_globs
|
||||||
|
for path in source_root.glob(experiment_glob)
|
||||||
|
if path.is_dir()
|
||||||
|
}
|
||||||
|
dirs = sorted(matched_dirs)
|
||||||
|
missing = [path for path in dirs if not path.is_dir()]
|
||||||
|
if missing:
|
||||||
|
names = "\n".join(f" - {path}" for path in missing)
|
||||||
|
raise FileNotFoundError(f"Experiment directories not found:\n{names}")
|
||||||
|
return dirs
|
||||||
|
|
||||||
|
|
||||||
|
def discover_checkpoints(args: argparse.Namespace) -> list[Checkpoint]:
|
||||||
|
"""Find exported policy JIT checkpoints that match CLI filters.
|
||||||
|
|
||||||
|
The discovered checkpoints are sorted by experiment name, step, and path so
|
||||||
|
repeated runs process checkpoints in a stable order.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
args: Parsed CLI arguments containing discovery roots and filters.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Ordered checkpoints selected for evaluation.
|
||||||
|
"""
|
||||||
|
source_root = args.source_root.expanduser().resolve()
|
||||||
|
experiment_dirs = discover_experiment_dirs(source_root, args.experiment_glob, args.experiments)
|
||||||
|
checkpoints: list[Checkpoint] = []
|
||||||
|
|
||||||
|
for experiment_dir in experiment_dirs:
|
||||||
|
for model_path in sorted(experiment_dir.glob("*/jit_models/policy_jit_*.pt")):
|
||||||
|
step = checkpoint_step(model_path)
|
||||||
|
if step is None:
|
||||||
|
continue
|
||||||
|
if args.start_step is not None and step < args.start_step:
|
||||||
|
continue
|
||||||
|
if args.end_step is not None and step > args.end_step:
|
||||||
|
continue
|
||||||
|
checkpoints.append(
|
||||||
|
Checkpoint(
|
||||||
|
experiment_name=experiment_dir.name,
|
||||||
|
run_dir=model_path.parents[1],
|
||||||
|
model_path=model_path.resolve(),
|
||||||
|
step=step,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
checkpoints.sort(key=lambda item: (item.experiment_name, item.step, str(item.model_path)))
|
||||||
|
if args.max_checkpoints is not None:
|
||||||
|
checkpoints = checkpoints[: args.max_checkpoints]
|
||||||
|
return checkpoints
|
||||||
|
|
||||||
|
|
||||||
|
def load_yaml(path: Path) -> dict[str, Any]:
|
||||||
|
"""Load a YAML mapping from disk.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
path: YAML file to read.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Parsed YAML mapping.
|
||||||
|
"""
|
||||||
|
with path.open("r", encoding="utf-8") as file:
|
||||||
|
data = yaml.safe_load(file)
|
||||||
|
if not isinstance(data, dict):
|
||||||
|
raise ValueError(f"Expected a mapping in {path}, got {type(data).__name__}.")
|
||||||
|
return data
|
||||||
|
|
||||||
|
|
||||||
|
def save_yaml(path: Path, data: dict[str, Any]) -> None:
|
||||||
|
"""Write a YAML mapping to disk.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
path: Output YAML path.
|
||||||
|
data: Mapping to serialize.
|
||||||
|
"""
|
||||||
|
path.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
with path.open("w", encoding="utf-8") as file:
|
||||||
|
yaml.safe_dump(data, file, allow_unicode=True, sort_keys=False)
|
||||||
|
|
||||||
|
|
||||||
|
def result_scores(results: dict[str, Any], source: Path | str) -> dict[str, Any]:
|
||||||
|
"""Return the scores mapping from a RoboGauge result payload.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
results: RoboGauge result payload.
|
||||||
|
source: Human-readable origin used in validation errors.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Mapping of score names to scalar values.
|
||||||
|
"""
|
||||||
|
scores = results.get("scores")
|
||||||
|
if not isinstance(scores, dict):
|
||||||
|
raise ValueError(f"RoboGauge results from {source} do not contain a 'scores' mapping.")
|
||||||
|
return scores
|
||||||
|
|
||||||
|
|
||||||
|
def log_scores(writer: SummaryWriter, results: dict[str, Any], step: int, source: Path | str) -> None:
|
||||||
|
"""Write RoboGauge score scalars to TensorBoard.
|
||||||
|
|
||||||
|
Scores are logged as RoboGauge/{key}, matching the training runner's
|
||||||
|
TensorBoard tag format.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
writer: TensorBoard writer for the output experiment directory.
|
||||||
|
results: RoboGauge result payload containing a scores mapping.
|
||||||
|
step: TensorBoard global_step taken from the checkpoint id.
|
||||||
|
source: Human-readable result origin used in validation errors.
|
||||||
|
"""
|
||||||
|
for key, value in result_scores(results, source).items():
|
||||||
|
writer.add_scalar(f"RoboGauge/{key}", value, step)
|
||||||
|
writer.flush()
|
||||||
|
|
||||||
|
|
||||||
|
def clear_tensorboard_events(log_dir: Path) -> None:
|
||||||
|
"""Delete TensorBoard event files from an experiment output directory.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
log_dir: Experiment output directory that may contain event files.
|
||||||
|
"""
|
||||||
|
for event_file in log_dir.glob("events.out.tfevents*"):
|
||||||
|
event_file.unlink()
|
||||||
|
|
||||||
|
|
||||||
|
def output_paths(log_root: Path, checkpoint: Checkpoint) -> tuple[Path, Path]:
|
||||||
|
"""Build output paths for an experiment and checkpoint result.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
log_root: Root directory for TensorBoard logs and result caches.
|
||||||
|
checkpoint: Checkpoint whose experiment and step define the paths.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Experiment log directory and checkpoint result YAML path.
|
||||||
|
"""
|
||||||
|
experiment_log_dir = log_root / checkpoint.experiment_name
|
||||||
|
result_path = experiment_log_dir / "robogauge_results" / f"results_{checkpoint.step}.yaml"
|
||||||
|
return experiment_log_dir, result_path
|
||||||
|
|
||||||
|
|
||||||
|
def write_cached_result(
|
||||||
|
writer: SummaryWriter,
|
||||||
|
results: dict[str, Any],
|
||||||
|
step: int,
|
||||||
|
result_path: Path,
|
||||||
|
source: Path | str,
|
||||||
|
) -> None:
|
||||||
|
"""Persist a result payload and write its score scalars.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
writer: TensorBoard writer for the output experiment directory.
|
||||||
|
results: RoboGauge result payload to cache and log.
|
||||||
|
step: TensorBoard global_step taken from the checkpoint id.
|
||||||
|
result_path: Output YAML path for the cached payload.
|
||||||
|
source: Human-readable result origin used in validation errors.
|
||||||
|
"""
|
||||||
|
save_yaml(result_path, results)
|
||||||
|
log_scores(writer, results, step, source)
|
||||||
|
|
||||||
|
|
||||||
|
def submit_and_wait(
|
||||||
|
client: Any,
|
||||||
|
checkpoint: Checkpoint,
|
||||||
|
task_name: str,
|
||||||
|
poll_interval: float,
|
||||||
|
retry_interval: float,
|
||||||
|
) -> dict[str, Any]:
|
||||||
|
"""Submit one checkpoint through RoboGaugeClient and wait for results.
|
||||||
|
|
||||||
|
The function polls the existing client until the submitted task returns a
|
||||||
|
finished result, preserving one-checkpoint-at-a-time queueing for the batch.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
client: RoboGaugeClient instance used for submission and monitoring.
|
||||||
|
checkpoint: Checkpoint to evaluate.
|
||||||
|
task_name: RoboGauge task name sent with the evaluation request.
|
||||||
|
poll_interval: Seconds to sleep between task status checks.
|
||||||
|
retry_interval: Seconds to sleep between server availability retries.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
RoboGauge result payload returned by the server.
|
||||||
|
"""
|
||||||
|
task_id = client.submit_task(
|
||||||
|
model_path=str(checkpoint.model_path),
|
||||||
|
step=checkpoint.step,
|
||||||
|
task_name=task_name,
|
||||||
|
experiment_name=checkpoint.experiment_name,
|
||||||
|
wait_for_server=True,
|
||||||
|
retry_interval=retry_interval,
|
||||||
|
)
|
||||||
|
if task_id is None:
|
||||||
|
raise RuntimeError(f"Failed to submit {checkpoint.model_path}.")
|
||||||
|
|
||||||
|
while True:
|
||||||
|
client.monitor_tasks(wait_for_server=True, retry_interval=retry_interval)
|
||||||
|
response = client.response_data.pop(task_id, None)
|
||||||
|
if response is not None:
|
||||||
|
results = response.get("results")
|
||||||
|
if not isinstance(results, dict):
|
||||||
|
raise ValueError(f"Task {task_id} finished without a valid results mapping: {response}")
|
||||||
|
response_step = int(response.get("step", checkpoint.step))
|
||||||
|
if response_step != checkpoint.step:
|
||||||
|
raise ValueError(
|
||||||
|
f"Task {task_id} returned step {response_step}, expected {checkpoint.step}."
|
||||||
|
)
|
||||||
|
return results
|
||||||
|
|
||||||
|
if task_id not in client.processing_ids:
|
||||||
|
raise RuntimeError(f"Task {task_id} ended without a finished result.")
|
||||||
|
|
||||||
|
time.sleep(poll_interval)
|
||||||
|
|
||||||
|
|
||||||
|
def print_plan(checkpoints: list[Checkpoint], log_root: Path, overwrite: bool, reuse_source_results: bool) -> None:
|
||||||
|
"""Print a compact summary of the batch evaluation plan.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
checkpoints: Checkpoints selected for this run.
|
||||||
|
log_root: Output root for TensorBoard logs and result caches.
|
||||||
|
overwrite: Whether cached output results will be ignored.
|
||||||
|
reuse_source_results: Whether source-run result YAML can satisfy work.
|
||||||
|
"""
|
||||||
|
counts: dict[str, int] = {}
|
||||||
|
for checkpoint in checkpoints:
|
||||||
|
counts[checkpoint.experiment_name] = counts.get(checkpoint.experiment_name, 0) + 1
|
||||||
|
print("Discovered checkpoints:")
|
||||||
|
for experiment_name in sorted(counts):
|
||||||
|
print(f" - {experiment_name}: {counts[experiment_name]}")
|
||||||
|
print(f"Output TensorBoard root: {log_root}")
|
||||||
|
print(f"Overwrite output results: {overwrite}")
|
||||||
|
print(f"Reuse source results: {reuse_source_results}")
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
"""Run checkpoint discovery, evaluation, caching, and TensorBoard logging."""
|
||||||
|
args = parse_args()
|
||||||
|
log_root = args.log_root.expanduser().resolve()
|
||||||
|
checkpoints = discover_checkpoints(args)
|
||||||
|
print_plan(checkpoints, log_root, args.overwrite, args.reuse_source_results)
|
||||||
|
|
||||||
|
if not checkpoints:
|
||||||
|
print("No checkpoints matched the requested filters.")
|
||||||
|
return
|
||||||
|
|
||||||
|
if args.dry_run:
|
||||||
|
for checkpoint in checkpoints[:20]:
|
||||||
|
experiment_log_dir, result_path = output_paths(log_root, checkpoint)
|
||||||
|
action = "evaluate"
|
||||||
|
if result_path.exists() and not args.overwrite:
|
||||||
|
action = "use-output-cache"
|
||||||
|
elif args.reuse_source_results and checkpoint.source_result_path.exists():
|
||||||
|
action = "reuse-source-result"
|
||||||
|
print(f"{action}: step={checkpoint.step} model={checkpoint.model_path} tb={experiment_log_dir}")
|
||||||
|
if len(checkpoints) > 20:
|
||||||
|
print(f"... {len(checkpoints) - 20} more checkpoints omitted from dry-run preview.")
|
||||||
|
return
|
||||||
|
|
||||||
|
log_root.mkdir(parents=True, exist_ok=True)
|
||||||
|
writers: dict[str, SummaryWriter] = {}
|
||||||
|
cleared_experiments: set[str] = set()
|
||||||
|
client: Any | None = None
|
||||||
|
completed = 0
|
||||||
|
failed = 0
|
||||||
|
|
||||||
|
try:
|
||||||
|
for index, checkpoint in enumerate(checkpoints, start=1):
|
||||||
|
experiment_log_dir, result_path = output_paths(log_root, checkpoint)
|
||||||
|
experiment_log_dir.mkdir(parents=True, exist_ok=True)
|
||||||
|
if args.clear_tensorboard and checkpoint.experiment_name not in cleared_experiments:
|
||||||
|
clear_tensorboard_events(experiment_log_dir)
|
||||||
|
cleared_experiments.add(checkpoint.experiment_name)
|
||||||
|
|
||||||
|
writer = writers.get(checkpoint.experiment_name)
|
||||||
|
if writer is None:
|
||||||
|
writer = SummaryWriter(str(experiment_log_dir))
|
||||||
|
writers[checkpoint.experiment_name] = writer
|
||||||
|
|
||||||
|
prefix = f"[{index}/{len(checkpoints)}] {checkpoint.experiment_name} step {checkpoint.step}"
|
||||||
|
try:
|
||||||
|
if result_path.exists() and not args.overwrite:
|
||||||
|
results = load_yaml(result_path)
|
||||||
|
log_scores(writer, results, checkpoint.step, result_path)
|
||||||
|
print(f"{prefix}: logged cached output result.")
|
||||||
|
completed += 1
|
||||||
|
continue
|
||||||
|
|
||||||
|
if args.reuse_source_results and checkpoint.source_result_path.exists() and not args.overwrite:
|
||||||
|
results = load_yaml(checkpoint.source_result_path)
|
||||||
|
write_cached_result(writer, results, checkpoint.step, result_path, checkpoint.source_result_path)
|
||||||
|
print(f"{prefix}: reused source result and logged TensorBoard scalars.")
|
||||||
|
completed += 1
|
||||||
|
continue
|
||||||
|
|
||||||
|
if client is None:
|
||||||
|
from robogauge.scripts.client import RoboGaugeClient
|
||||||
|
|
||||||
|
client = RoboGaugeClient(args.server_url, request_timeout=args.request_timeout)
|
||||||
|
client.wait_until_available(retry_interval=args.retry_interval)
|
||||||
|
|
||||||
|
print(f"{prefix}: submitting {checkpoint.model_path}.")
|
||||||
|
results = submit_and_wait(
|
||||||
|
client=client,
|
||||||
|
checkpoint=checkpoint,
|
||||||
|
task_name=args.task_name,
|
||||||
|
poll_interval=args.poll_interval,
|
||||||
|
retry_interval=args.retry_interval,
|
||||||
|
)
|
||||||
|
write_cached_result(writer, results, checkpoint.step, result_path, "server")
|
||||||
|
print(f"{prefix}: evaluation finished and logged.")
|
||||||
|
completed += 1
|
||||||
|
|
||||||
|
except Exception as exc:
|
||||||
|
failed += 1
|
||||||
|
print(f"{prefix}: failed: {exc}")
|
||||||
|
if not args.keep_going:
|
||||||
|
raise
|
||||||
|
|
||||||
|
finally:
|
||||||
|
for writer in writers.values():
|
||||||
|
writer.close()
|
||||||
|
|
||||||
|
print(f"Done. Completed: {completed}, failed: {failed}, output: {log_root}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -2,6 +2,19 @@
|
|||||||
|
|
||||||
source /data/user/wutianyang/Programs/miniforge3/bin/activate go2-gym
|
source /data/user/wutianyang/Programs/miniforge3/bin/activate go2-gym
|
||||||
|
|
||||||
|
python robogauge/scripts/run.py \
|
||||||
|
--task-name go2_lab \
|
||||||
|
--model-path /home/user/go2_rl_robotlab/logs/rsl_rl/go2_moe_cts_v4.2/2026-06-08_14-58-39/exported/policy.pt \
|
||||||
|
--experiment-name go2_moe_cts_v4.2_0.6984 \
|
||||||
|
--stress-benchmark \
|
||||||
|
--stress-terrain-names flat slope_fd slope_bd stairs_fd stairs_bd wave obstacle \
|
||||||
|
--num-processes 70 \
|
||||||
|
--seeds 0 1 2 \
|
||||||
|
--search-seeds 0 1 2 3 4 \
|
||||||
|
--frictions 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 \
|
||||||
|
--compress-logs \
|
||||||
|
--headless
|
||||||
|
|
||||||
python robogauge/scripts/run.py \
|
python robogauge/scripts/run.py \
|
||||||
--task-name go2_moe \
|
--task-name go2_moe \
|
||||||
--model-path /data/user/wutianyang/Coding/go2_rl_gym/mytest_merge_data/rem_cts/go2_moe_cts_expert_goal_137000_0.6745/policies/policy.pt \
|
--model-path /data/user/wutianyang/Coding/go2_rl_gym/mytest_merge_data/rem_cts/go2_moe_cts_expert_goal_137000_0.6745/policies/policy.pt \
|
||||||
|
|||||||
9
setup.py
9
setup.py
@@ -10,10 +10,10 @@ setup(
|
|||||||
packages=find_packages(),
|
packages=find_packages(),
|
||||||
install_requires=[
|
install_requires=[
|
||||||
"torch", # Refer: https://pytorch.org/get-started/locally/
|
"torch", # Refer: https://pytorch.org/get-started/locally/
|
||||||
"numpy==1.20.0",
|
"numpy<2",
|
||||||
"pillow==9.0.0",
|
"pillow<10",
|
||||||
"mujoco>=3.0.0",
|
"mujoco==3.2.3",
|
||||||
"dm_control>=1.0.14",
|
"dm_control==1.0.23",
|
||||||
"scipy",
|
"scipy",
|
||||||
"matplotlib==3.6.3",
|
"matplotlib==3.6.3",
|
||||||
"tqdm",
|
"tqdm",
|
||||||
@@ -23,6 +23,7 @@ setup(
|
|||||||
"fastapi",
|
"fastapi",
|
||||||
"uvicorn",
|
"uvicorn",
|
||||||
"pygame",
|
"pygame",
|
||||||
|
"pynput",
|
||||||
],
|
],
|
||||||
python_requires=">=3.8",
|
python_requires=">=3.8",
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user