MotrixSim 仓库分析与 go2.py 详解

Felix Christian Lv3
  2025-12-29 16:01 2025-12-29 16:01 创建   2025-12-29 18:46:44 2025-12-29 18:46:44 更新    3.9k 字  17 分钟  

🔍 仓库分析

特征分析

特征分析
核心引擎代码❌ 未公开。motrixsim 核心通过 pip install motrixsim 从 PyPI 安装
Python 绑定❌ 未公开。底层使用 Rust 实现，只暴露 Python API
示例代码✅ 完整。包含 39+ 个 Python 示例文件
API 文档✅ 完整。可以从 https://motrixsim.readthedocs.io 访问
资源文件✅ 完整。包含机器人模型、场景、策略网络等

仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
motrixsim-docs/
├── docs/                    # 文档源码
│   └── source/              # 包含 API 参考、教程等
├── examples/                # ⭐ 示例代码 (39+ 个 Python 文件)
│   ├── go1.py               # 四足机器人 Go1 示例
│   ├── go2.py               # 四足机器人 Go2 示例
│   ├── g1_keyboard_control.py  # 人形机器人 G1 键盘控制
│   ├── robotic_arm.py       # 机械臂示例
│   └── assets/              # 模型和资源文件
├── legged_gym/              # 腿式机器人强化学习环境
├── pyproject.toml           # 依赖配置
├── README.md                # 项目说明
└── LICENSE                  # Apache 2.0 许可证

重要

核心物理引擎代码未公开
motrixsim 的核心实现（物理求解器、碰撞检测、渲染引擎等）是闭源的。 这个仓库只提供：使用说明、API 示例、机器人模型文件。

📖 go2.py 详细解读

GitHub: examples/go2.py

功能概述

这个脚本演示了一个四足机器人 Go2 在物理仿真环境中自主行走的完整流程：

flowchart LR
    A[加载场景模型] --> B[创建物理数据]
    B --> C[加载神经网络策略]
    C --> D[仿真循环]
    D --> E{机器人摔倒?}
    E -->|是| F[重置场景]
    E -->|否| G[收集观测数据]
    F --> D
    G --> H[神经网络推理]
    H --> I[应用动作到关节]
    I --> J[物理引擎步进]
    J --> K[渲染显示]
    K --> D

逐段代码解析

1️⃣ 导入模块

1
2
3
4
5
6
7
8
9
10
import time
import random
from collections import deque

import numpy as np
import onnxruntime as ort
from scipy.spatial.transform import Rotation

from motrixsim import SceneData, SceneModel, load_model, step
from motrixsim.render import CaptureTask, RenderApp
模块用途
time用于帧率控制 (time.sleep)
random生成随机运动目标
deque双端队列，用于管理截图任务
numpy数值计算（向量、矩阵操作）
onnxruntime运行神经网络模型（ONNX 格式）
scipy.spatial.transform.Rotation四元数与旋转矩阵的转换
motrixsim.SceneData存储仿真的动态状态（位置、速度、力）
motrixsim.SceneModel场景的静态描述（几何、物理参数）
motrixsim.load_model从 MJCF/XML 文件加载场景
motrixsim.step执行一次物理仿真步进
motrixsim.render.RenderApp可视化渲染窗口
motrixsim.render.CaptureTask异步截图任务

2️⃣ 全局参数定义

1
2
3
4
default_joint_pos = np.array([0.1, 0.9, -1.8, -0.1, 0.9, -1.8, 0.1, 0.9, -1.8, -0.1, 0.9, -1.8])
action_scale = 0.5
lin_vel_scale = 1.0
ang_vel_scale = 1.5
参数说明
default_joint_pos机器人 12 个关节的默认角度（弧度）。Go2 有 4 条腿，每条腿 3 个关节（髋/大腿/小腿）
action_scale神经网络输出的动作缩放因子（0.5 表示输出值减半）
lin_vel_scale线速度目标缩放因子（Go2 是 1.0，比 Go1 的 0.7 更高）
ang_vel_scale角速度目标缩放因子

Go2 vs Go1 差异

  • Go2 躯干名称："base"，Go1 是 "trunk"
  • Go2 第一个执行器："FL_hip"，Go1 是 "FR_hip"
  • Go2 线速度缩放 1.0，Go1 是 0.7

关节顺序：[FL_hip, FL_thigh, FL_calf, FR_hip, FR_thigh, FR_calf, RL_hip, RL_thigh, RL_calf, RR_hip, RR_thigh, RR_calf] - FL = Front Left（前左腿） - FR = Front Right（前右腿） - RL = Rear Left（后左腿） - RR = Rear Right（后右腿）

3️⃣ 观测数据收集函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
def compute_observations(last_actions, target_action, model: SceneModel, data: SceneData):
    # 获取身体索引
    body_index = model.get_body_index("base")  # Go2 使用 "base"，Go1 使用 "trunk"
    body = model.get_body(body_index)

    # 线速度 (3D) - 机器人在本地坐标系的速度
    linear_vel = model.get_sensor_value("local_linvel", data)
    
    # 角速度 (3D) - 陀螺仪读数
    gyro = model.get_sensor_value("gyro", data)
    
    # 重力方向 (3D) - 表示机器人姿态
    pose = body.get_pose(data)
    inv_rotation = Rotation.from_quat(pose[3:7]).inv()
    gravity = inv_rotation.apply(np.array([0.0, 0.0, -1.0]))

    # 关节位置和速度
    dof_pos = body.get_joint_dof_pos(data)
    dof_vel = body.get_joint_dof_vel(data)
    
    # 使用 np.hstack 高效拼接所有观测量
    obs = np.hstack([
        linear_vel,                      # 线速度 (3)
        gyro,                            # 角速度 (3)
        gravity,                         # 重力方向 (3)
        dof_pos - default_joint_pos,     # 关节位置差 (12)
        dof_vel,                         # 关节速度 (12)
        last_actions,                    # 上一帧动作 (12)
        target_action                    # 目标速度 (3)
    ])
    return obs

这是神经网络的"眼睛" - 收集机器人当前状态作为输入。

观测量维度说明
线速度3机器人身体在本地坐标系的速度 (x, y, z)
角速度3机器人绕三个轴的旋转速度（陀螺仪读数）
重力方向3重力在机器人本体坐标系中的方向，反映倾斜程度
关节位置差12当前关节角度与默认姿态的偏差
关节速度12各关节的角速度
上一帧动作12上一步施加的控制命令
目标速度3期望的线速度 (x, y) 和角速度 (yaw)
总计48神经网络输入维度

为什么要包含上一帧动作

这让神经网络了解控制的连续性，避免输出剧烈抖动的动作。这是强化学习中常用的技巧。

Go2 代码优化

使用 np.hstack() 一次性拼接数组，比 Go1 的 obs.extend() 方式更高效。

4️⃣ 目标更新函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
def update_target(goback, body_position: np.ndarray):
    target_action = [0, 0]
    if goback:
        # 返回原点：计算指向原点的方向
        v = -body_position[:2]           # 取 x, y 坐标，取反指向原点
        norm = v / np.linalg.norm(v)     # 归一化为单位向量
        target_action = [norm[0] * lin_vel_scale, norm[1] * , 0]
    else:
        # 随机方向：生成 -2 到 2 范围内的随机速度
        x = random.random() * 4.0 - 2.0
        y = random.random() * 4.0 - 2.0
        rot = random.random() * 4.0 - 2.0
        v = np.array([x, y])
        norm = v / np.linalg.norm(v)
        target_action = [norm[0] * lin_vel_scale, norm[1] * lin_vel_scale, rot * ang_vel_scale]
    return target_action

这个函数生成机器人的运动目标：

  • 随机漫游模式 (goback=False)：生成随机方向和转向速度
  • 返回原点模式 (goback=True)：计算指向原点的方向，直线返回

返回的 target_action 包含 3 个值：[vx, vy, yaw_rate]（前后速度、左右速度、转向速度）

5️⃣ 动作应用函数

1
2
3
4
5
6
7
def apply_actions(actions, model: SceneModel, data: SceneData):
    start_actuator_index = model.get_actuator_index("FL_hip")  # Go2 从 FL_hip 开始
    for index, act in enumerate(actions):
        actuator_index = start_actuator_index + index
        ctrl = act * action_scale + default_joint_pos[index]  # 缩放 + 偏移
        actuator = model.get_actuator(actuator_index)
        actuator.set_ctrl(data, ctrl)

这是神经网络的"手" - 将决策转化为关节控制。

控制流程：

  1. 神经网络输出 12 个动作值（范围约 -1 到 1）
  2. 每个动作乘以 action_scale（0.5）进行缩放
  3. 加上默认关节角度 default_joint_pos，得到目标关节角度
  4. 通过 actuator.set_ctrl() 设置关节电机的目标位置

计算示例：

1
2
3
神经网络输出: act = 0.5
缩放后: 0.5 * 0.5 = 0.25
加上默认值: 0.25 + 0.1 = 0.35 (弧度)

6️⃣ 摔倒检测函数

1
2
3
4
5
6
7
def is_fall(model: SceneModel, data: SceneData):
    pose = model.get_link("base").get_pose(data)  # Go2 使用 "base"
    rotation = Rotation.from_quat(pose[3:7])
    rotated_z_axis = rotation.apply(np.array([0.0, 0.0, 1.0]))
    thr = 0.3
    dot = np.dot(rotated_z_axis, np.array([0.0, 0.0, 1.0]))
    return dot < thr

判断机器人是否摔倒的逻辑：

  1. 获取机器人躯干(base)的姿态四元数 [x, y, z, w]
  2. 将四元数转换为旋转对象
  3. 计算机器人的"上方向"（本地 Z 轴）在世界坐标系中的朝向
  4. 与世界坐标系的 Z 轴（垂直向上）做点积
  5. 如果点积 < 0.3，说明机器人已经严重倾斜/翻倒
点积值含义
1.0完全直立
0.7倾斜约 45°
0.3倾斜约 72°（触发重置）
0.0完全侧翻 90°
-1.0完全翻转（背朝下）

7️⃣ 主函数详解

初始化渲染和加载模型
1
2
3
4
5
6
7
8
def main():
    # 创建渲染窗口
    with RenderApp() as render:
        render.opt.set_left_panel_vis(True)  # 显示左侧控制面板
        
        # 加载场景模型
        path = "examples/assets/go2/scene_flat.xml"
        model = load_model(path)

scene_flat.xml 包含： - Go2 机器人模型（引用 go2_mjx.xml） - 平坦地面 - 光照设置 - 物理参数

相机配置
1
2
3
4
5
6
7
8
9
10
11
12
cameras = model.cameras
# RGB 相机
cameras[0].set_render_target("image", 320, 240)

# 深度相机
cameras[1].set_render_target("image", 640, 480)
cameras[1].depth_only = True       # 只输出深度图
cameras[1].set_near_far(0.1, 1)    # 深度范围 0.1-1 米

# 可预览的相机列表（None 表示自由视角）
preview_cameras = [None, *cameras[2:]]
preview_camera_idx = 0
创建物理数据和加载神经网络
1
2
3
4
5
6
7
8
9
10
# 启动渲染
render.launch(model)
# 创建物理仿真状态数据
data = SceneData(model)

# 加载预训练神经网络
session = ort.InferenceSession("examples/assets/go2/go2_policy.onnx", 
                                providers=["CPUExecutionProvider"])
input_name = session.get_inputs()[0].name   # 输入张量名称
output_name = session.get_outputs()[0].name # 输出张量名称

神经网络规格： - 输入：48 维浮点数向量 - 输出：12 维浮点数向量（12 个关节的控制命令）

控制变量初始化
1
2
3
4
5
6
7
8
9
last_actions = [0] * 12           # 上一帧动作（初始为零）
n_infer_interval = 5              # 每 5 个物理步执行一次推理（Go1 是 10）
n_set_tartget_interval = 750      # 每 750 步更换目标
go_back = False                   # 是否返回原点
nsteps = 0                        # 步数计数
target_action = [0.5, 0, 0]       # 初始目标：向前走

capture_tasks = deque()           # 截图任务队列
capture_index = 0                 # 截图编号

Go2 vs Go1 差异

Go2 的 n_infer_interval = 5，比 Go1 的 10 更频繁，意味着控制频率更高。

主仿真循环
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
while True:
    for _ in range(4):
        # 物理引擎步进
        step(model, data)

        # 摔倒检测与重置
        if is_fall(model, data):
            data = SceneData(model)  # 重新创建数据以重置场景

        # 步数计数
        nsteps += 1
        if nsteps % n_infer_interval == 0:
            # 每 750 步更换运动目标
            if nsteps % n_set_tartget_interval == 0:
                body_pose = model.get_body(model.get_body_index("base")).get_pose(data)
                target_action = update_target(go_back, body_pose[:3])
                go_back = not go_back  # 切换模式
            
            # 收集观测数据
            obs = compute_observations(last_actions, target_action, model, data)
            # 准备输入（reshape 为 [1, 48]）
            input_data = np.array(obs).reshape(1, -1).astype(np.float32)
            # 神经网络推理
            outputs = session.run([output_name], {input_name: input_data})
            actions = outputs[0][0]
            # 应用动作
            apply_actions(actions, model, data)
            last_actions = actions

时序分析： - 每帧执行 4 次物理步进（提高仿真精度） - 每 5 个物理步进执行一次神经网络推理（控制频率 = 仿真频率 / 5） - 每 750 步（约几秒钟）切换一次运动目标

截图功能
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# 按空格键截图
if render.input.is_key_just_pressed("space"):
    rcam = render.get_camera(0)
    capture_tasks.append((capture_index, rcam.capture()))
    capture_index += 1

# 同步渲染并限制帧率
render.sync(data)
time.sleep(1/60.)  # 限制为 60 FPS

# 处理截图任务
while len(capture_tasks) > 0:
    idx, task = capture_tasks[0]
    if task.state != "pending":
        capture_tasks.popleft()
        img = task.take_image()
        os.makedirs("shot", exist_ok=True)
        img.save_to_disk(f"shot/capture_{idx}.png")
    else:
        break
相机切换
1
2
3
4
5
6
7
8
9
# 右方向键：下一个相机
if render.input.is_key_just_pressed("right"):
    preview_camera_idx = (preview_camera_idx + 1) % len(preview_cameras)
    render.set_main_camera(preview_cameras[preview_camera_idx])

# 左方向键：上一个相机
if render.input.is_key_just_pressed("left"):
    preview_camera_idx = (preview_camera_idx + len(preview_cameras) - 1) % len(preview_cameras)
    render.set_main_camera(preview_cameras[preview_camera_idx])

📖 go2_keyboard_control.py 详细解读

GitHub: examples/go2_keyboard_control.py

功能概述

通过键盘实时控制 Go2 机器人的移动方向和速度。与 go2.py 的主要区别是使用面向对象设计和高级 API。

flowchart TB
    subgraph 初始化
        A[加载模型] --> B[创建 OnnxController]
        B --> C[启动渲染循环]
    end
    
    subgraph "渲染循环 (60 FPS)"
        D[读取键盘输入] --> E[更新 command 向量]
        E --> F[get_control 物理步进]
        F --> G[神经网络推理]
        G --> H[应用动作]
        H --> I[渲染同步]
    end
    
    C --> D
    I --> D

全局参数

1
2
3
4
default_joint_pos = np.array([0.1, 0.9, -1.8, -0.1, 0.9, -1.8, 0.1, 0.9, -1.8, -0.1, 0.9, -1.8])
action_scale = 0.5
lin_vel_scale = 2.0   # 比 go2.py 的 1.0 更快！
ang_vel_scale = 3.0   # 比 go2.py 的 1.5 更快！

键盘控制版本更快

键盘控制版本的速度更快，因为手动控制需要更灵敏的响应。

OnnxController 类详解

这是与 go2.py 最大的区别 —— 使用面向对象设计封装所有控制逻辑：

构造函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
class OnnxController:
    def __init__(
        self,
        model: SceneModel,
        policy_path: str,
        default_angles: np.ndarray,
        ctrl_dt: float,          # 控制周期（秒）
        action_scale: float = 0.5,
    ):
        self._model = model
        self._data = SceneData(self._model)  # 创建物理数据
        
        # 加载神经网络
        self._policy = ort.InferenceSession(policy_path, providers=["CPUExecutionProvider"])
        self._input_name = self._policy.get_inputs()[0].name
        self._output_name = self._policy.get_outputs()[0].name
        
        # 控制参数
        self.command = np.zeros(3, dtype=np.float32)  # [vx, vy, yaw_rate] 外部可修改
        self._action_scale = action_scale
        self._default_angles = default_angles.copy()
        self._last_action = np.zeros_like(default_angles, dtype=np.float32)
        
        # 计算子步数：每个控制周期内执行多少次物理步进
        self._counter = 0
        self._n_substeps = int(round(ctrl_dt / self._model.options.timestep))
属性说明
command速度指令向量 [前后, 左右, 转向]，键盘输入直接修改此属性
_n_substeps每次神经网络推理之间的物理步进次数
_last_action上一帧的动作输出，用于观测

控制频率计算示例：

1
2
3
4
ctrl_dt = 0.02 秒 (20ms, 即 50Hz)
timestep = 0.002 秒 (默认物理步长)
n_substeps = 0.02 / 0.002 = 10
即：每 10 次物理步进执行一次神经网络推理

观测收集方法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
def get_obs(self, model: SceneModel, data: SceneData, command):
    body = model.get_body(model.get_body_index("base"))
    
    linear_vel = model.get_sensor_value("local_linvel", data)
    gyro = model.get_sensor_value("gyro", data)
    
    pose = body.get_pose(data)
    inv_rotation = Rotation.from_quat(pose[3:7]).inv()
    gravity = inv_rotation.apply(np.array([0.0, 0.0, -1.0]))
    
    dof_pos = body.get_joint_dof_pos(data)
    dof_vel = body.get_joint_dof_vel(data)
    
    obs = np.hstack([
        linear_vel, gyro, gravity,
        dof_pos - self._default_angles,
        dof_vel,
        self._last_action,   # 注意：使用实例变量
        command
    ])
    return obs.astype(np.float32)

核心控制方法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
def get_control(self):
    self._counter += 1
    step(self._model, self._data)  # 物理步进
    
    # 摔倒检测
    if self.is_fall(self._model, self._data):
        self._data = SceneData(self._model)  # 重置
        self.command = np.zeros(3, dtype=np.float32)  # 清空指令
    
    # 按控制频率执行推理
    if self._counter % self._n_substeps == 0:
        obs = self.get_obs(self._model, self._data, self.command)
        outputs = self._policy.run([self._output_name], {self._input_name: obs.reshape(1, -1)})
        actions = outputs[0][0]
        self._last_action = actions.copy()
        self.apply_actions(actions, self._model, self._data)

这个方法每调用一次执行一个物理步进，并在适当的时机执行神经网络推理。

主函数详解

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
def main():
    with RenderApp() as render:
        render.opt.set_left_panel_vis(True)
        path = "examples/assets/go2/scene_flat.xml"
        model = load_model(path)
        render.launch(model)

        # 创建控制器
        policy = OnnxController(
            model,
            policy_path="examples/assets/go2/go2_policy.onnx",
            ctrl_dt=0.02,               # 控制周期 20ms (50Hz)
            default_angles=default_joint_pos,
            action_scale=action_scale,
        )

键盘输入处理

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
input = render.input

def render_step():
    # 前后移动
    if input.is_key_pressed("up") or input.is_key_pressed("w"):
        policy.command[0] = 1.0 * lin_vel_scale   # 前进
    elif input.is_key_pressed("down") or input.is_key_pressed("s"):
        policy.command[0] = -1.0 * lin_vel_scale  # 后退
    else:
        policy.command[0] = 0.0

    # 左右平移
    if input.is_key_pressed("left"):
        policy.command[1] = 0.5 * lin_vel_scale   # 左移
    elif input.is_key_pressed("right"):
        policy.command[1] = -0.5 * lin_vel_scale  # 右移
    else:
        policy.command[1] = 0.0

    # 转向
    if input.is_key_pressed("a"):
        policy.command[2] = 2.0 * ang_vel_scale   # 左转
    elif input.is_key_pressed("d"):
        policy.command[2] = -2.0 * ang_vel_scale  # 右转
    else:
        policy.command[2] = 0.0

    render.sync(policy.data)

🎮 完整键盘控制映射表

按键功能command[0]command[1]command[2]
W / ↑前进2.000
S / ↓后退-2.000
←左平移01.00
→右平移0-1.00
A左转006.0
D右转00-6.0

render_loop 高级 API

1
2
3
4
5
6
7
8
9
10
11
12
13
14
print("Keyboard Controls:")
print("- Press W / Up Arrow to move forward")
print("- Press S / Down Arrow to move backward")
print("- Press Left Arrow to move left")
print("- Press Right Arrow to move right")  
print("- Press A to rotate left")
print("- Press D to rotate right")

run.render_loop(
    model.options.timestep,   # 物理时间步长
    60,                       # 目标帧率 (FPS)
    policy.get_control,       # 物理更新回调（每物理步调用）
    render_step               # 渲染更新回调（每帧调用）
)

run.render_loop 的工作原理：

  1. 根据目标帧率计算每帧需要执行多少次物理步进
  2. 每个物理步进调用 policy.get_control()
  3. 每帧结束调用 render_step() 处理输入和渲染
  4. 自动处理时间同步，保持稳定帧率

这比手动写 while True 循环更简洁、更可靠。

🔄 go2.py vs go2_keyboard_control.py 完整对比

特性go2.pygo2_keyboard_control.py
控制模式自动随机漫游键盘手动控制
代码风格函数式（独立函数）面向对象 (OnnxController 类)
lin_vel_scale1.02.0 (更快)
ang_vel_scale1.53.0 (更快)
n_infer_interval5 步由 ctrl_dt 计算
主循环手动 while Truerun.render_loop() 高级 API
帧率控制time.sleep(1/60)render_loop 自动管理
相机截图✅ 支持❌ 无
相机切换✅ 支持❌ 无
代码行数~207 行~178 行

📁 相关文件

文件说明
go2.py自动漫游示例
go2_keyboard_control.py键盘控制示例
scene_flat.xmlGo2 场景定义
go2_mjx.xmlGo2 机器人模型
go2_policy.onnx预训练神经网络

🧠 神经网络策略说明

1
2
3
4
5
6
7
8
9
10
11
12
go2_policy.onnx
├── 输入: 48 维向量
│   ├── 线速度 (3)
│   ├── 角速度 (3)
│   ├── 重力方向 (3)
│   ├── 关节位置差 (12)
│   ├── 关节速度 (12)
│   ├── 上一帧动作 (12)
│   └── 目标速度 (3)
│
└── 输出: 12 维向量
    └── 12 个关节的控制命令

这个神经网络是通过强化学习 (Reinforcement Learning) 训练得到的，让机器人学会： - 保持平衡 - 按照给定速度行走 - 适应不同地形

  • 标题: MotrixSim 仓库分析与 go2.py 详解
  • 作者: Felix Christian
  • 创建于 : 2025-12-29 16:01:00
  • 更新于 : 2025-12-29 18:46:44
  • 链接: https://felixchristian.top/2025/12/29/24-MotrixSim/
  • 版权声明: 本文章采用 CC BY-NC-SA 4.0 进行许可。
评论
目录
MotrixSim 仓库分析与 go2.py 详解
  1. 🔍 仓库分析
    1. 特征分析
    2. 仓库结构
  2. 📖 go2.py 详细解读
    1. 功能概述
    2. 逐段代码解析
  3. 📖 go2_keyboard_control.py 详细解读
    1. 功能概述
    2. 全局参数
    3. OnnxController 类详解
    4. 主函数详解
    5. 键盘输入处理
    6. 🎮 完整键盘控制映射表
    7. render_loop 高级 API
  4. 🔄 go2.py vs go2_keyboard_control.py 完整对比
  5. 📁 相关文件
  6. 🧠 神经网络策略说明