# robot_development

**Repository Path**: zhouByFrank/robot_development

## Basic Information

- **Project Name**: robot_development
- **Description**: unitree g1 29dof机器人传感器数据获取，shell命令行版
- **Primary Language**: Python
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2026-03-20
- **Last Updated**: 2026-03-25

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Unitree G1 传感器数据读取工具

适用于 `unitree_mujoco` 仿真环境的 G1 机器人传感器数据读取工具。

## 功能特性

- **basic** - 打印所有传感器数据（一次性完整显示）
- **continuous** - 持续读取并显示关键数据（简洁模式）
- **joint** - 监控指定关节的实时状态
- **imu** - 仅显示 IMU 数据
- **log** - 记录传感器数据到 CSV 文件
- **fusion** - 传感器数据融合与平衡监控（含关节功率计算、足端接触检测）
- **viz** - IMU 数据可视化（文本条形图）

## 传感器数据说明

G1 `LowState_` 消息包含以下传感器数据：

| 传感器 | 数据内容 | 说明 |
|--------|----------|------|
| `imu_state` | 四元数 `[w,x,y,z]` | 身体姿态（pelvis） |
| | 陀螺仪 `[x,y,z]` | 角速度 (rad/s) |
| | 加速度计 `[x,y,z]` | 线性加速度 (m/s²) |
| | 欧拉角 `[roll,pitch,yaw]` | (rad) |
| `motor_state` | 位置、速度、力矩 | 29个关节的状态 |

### G1 关节列表（29 DOF）

```
# 左腿 (6)
left_hip_pitch, left_hip_roll, left_hip_yaw, left_knee, left_ankle_pitch, left_ankle_roll

# 右腿 (6)
right_hip_pitch, right_hip_roll, right_hip_yaw, right_knee, right_ankle_pitch, right_ankle_roll

# 腰部 (3)
waist_yaw, waist_roll, waist_pitch

# 左臂 (7)
left_shoulder_pitch, left_shoulder_roll, left_shoulder_yaw, left_elbow, 
left_wrist_roll, left_wrist_pitch, left_wrist_yaw

# 右臂 (7)
right_shoulder_pitch, right_shoulder_roll, right_shoulder_yaw, right_elbow,
right_wrist_roll, right_wrist_pitch, right_wrist_yaw
```

## 安装依赖

```bash
# 确保已安装 unitree_sdk2py
pip install unitree_sdk2py

# 或者从源码安装
git clone https://github.com/unitreerobotics/unitree_sdk2_python.git
cd unitree_sdk2_python
pip install -e .
```

## 使用方法

### 0. 启动仿真器

在运行传感器读取工具前，确保 `unitree_mujoco` 仿真器已启动：

```bash
cd /path/to/unitree_mujoco/simulate_python
python unitree_mujoco.py
```

### 1. 基本传感器读取

打印所有传感器数据的完整信息：

```bash
python g1_sensor_reader.py basic
```

### 2. 持续读取模式

简洁的持续监控，显示 IMU 欧拉角和关节数量：

```bash
python g1_sensor_reader.py continuous
```

### 3. 关节监控

监控指定关节（默认监控膝关节）：

```bash
# 监控默认关节（左右膝关节）
python g1_sensor_reader.py joint

# 监控指定关节
python g1_sensor_reader.py joint --joints left_hip_pitch right_hip_pitch

# 监控多个关节
python g1_sensor_reader.py joint --joints left_knee right_knee left_ankle_pitch right_ankle_pitch
```

### 4. 仅显示 IMU 数据

```bash
python g1_sensor_reader.py imu
```

### 5. 数据记录

将传感器数据记录到 CSV 文件：

```bash
# 记录 30 秒数据（默认）
python g1_sensor_reader.py log

# 记录 60 秒数据
python g1_sensor_reader.py log --duration 60

# 指定输出文件名
python g1_sensor_reader.py log --duration 120 --output ./data/g1_trial_1.csv
```

**CSV 文件格式：**
- `timestamp` - 时间戳
- `imu_qw,qx,qy,qz` - 四元数
- `imu_gyro_x,y,z` - 陀螺仪
- `imu_acc_x,y,z` - 加速度计
- `imu_roll,pitch,yaw` - 欧拉角
- `joint{i}_pos,vel,tau` - 各关节位置、速度、力矩

### 6. 传感器融合与平衡监控

实时监控机器人平衡状态、关节功率和足端接触：

```bash
python g1_sensor_reader.py fusion
```

显示内容：
- ✅/⚠️ 平衡状态（基于 roll/pitch 角度阈值）
- 总功率和各关节最大功率
- 足端接触状态（左/右脚）
- IMU 欧拉角

### 7. IMU 可视化

文本条形图形式的 IMU 数据可视化：

```bash
python g1_sensor_reader.py viz
```

## 配置选项

### DDS 配置

对于不同的运行环境，需要调整 DDS 配置：

```bash
# 仿真环境（默认）
python g1_sensor_reader.py basic --domain-id 1 --interface lo

# 真机环境
python g1_sensor_reader.py basic --domain-id 0 --interface eth0
```

| 参数 | 仿真推荐值 | 真机推荐值 | 说明 |
|------|-----------|-----------|------|
| `--domain-id` | 1 | 0 | DDS 域 ID，用于区分不同网络 |
| `--interface` | lo | eth0 或 wlan0 | 网络接口名称 |

## 测试

项目包含自动化测试脚本 `test_g1_sensor_reader.py`，用于验证所有功能是否正常工作。

### 运行测试

```bash
# 使用默认配置测试（仿真环境）
python test_g1_sensor_reader.py

# 真机测试
python test_g1_sensor_reader.py --domain-id 0 --interface eth0

# 设置持续命令超时时间
python test_g1_sensor_reader.py --timeout 5
```

### 测试内容

测试脚本会自动执行以下测试：

| 测试项 | 说明 | 预期结果 |
|--------|------|----------|
| `basic` | 基本传感器读取 | 显示 IMU 和关节数据 |
| `imu` | IMU 数据读取 | 显示四元数、陀螺仪、加速度计、欧拉角 |
| `continuous` | 持续读取模式 | 持续显示数据，2秒后自动停止 |
| `joint` | 关节监控（默认） | 监控左右膝关节 |
| `joint --joints` | 关节监控（指定） | 监控指定关节 |
| `log` | 数据记录 | 记录 3 秒数据到 CSV 文件 |
| `fusion` | 传感器融合 | 显示平衡状态、功率、足端接触 |
| `viz` | IMU 可视化 | 文本条形图显示 IMU 数据 |

### 测试结果示例

```
============================================================
  测试摘要
============================================================
  ✅ basic - 基本传感器读取                通过         (2.35s)
  ✅ imu - IMU 数据读取                    通过         (1.98s)
  ✅ continuous - 持续读取模式              通过(超时)   (3s)
  ✅ joint - 关节监控（默认）               通过(超时)   (3s)
  ✅ joint - 关节监控（指定）               通过(超时)   (3s)
  ✅ log - 数据记录                        通过         (5.42s)
  ✅ fusion - 传感器融合与平衡监控          通过(超时)   (3s)
  ✅ viz - IMU 可视化                      通过(超时)   (3s)
------------------------------------------------------------
  总计: 8  |  通过: 8  |  失败: 0
  🎉 所有测试通过！
============================================================
```

**注意：**
- 测试前请确保 `unitree_mujoco` 仿真器已启动
- 持续运行的命令会在 `--timeout` 指定的时间后自动停止（默认 3 秒）
- 测试完成后会自动清理生成的临时 CSV 文件

## 常见问题

### 1. 超时无法接收到数据

```
[错误] 超时: 未能接收到传感器数据
```

**解决方法：**
1. 确认 `unitree_mujoco` 仿真器已启动
2. 检查 `--domain-id` 和 `--interface` 参数是否与仿真器配置一致
3. 检查仿真器的 `config.py` 中的 `DOMAIN_ID` 和 `INTERFACE` 设置

### 2. 未找到 unitree_sdk2py 模块

```
[错误] 未找到 unitree_sdk2py 模块
```

**解决方法：**
```bash
pip install unitree_sdk2py
```

### 3. 关节名称错误

```
[警告] 忽略无效关节名: ['xxx']
```

**解决方法：**
参考上方"G1 关节列表"使用正确的关节名称。

## 文件说明

```
g1_sensor_reader.py        # 主程序（传感器数据读取）
test_g1_sensor_reader.py   # 测试脚本（自动化测试所有功能）
README.md                  # 本说明文档
```

## 高级用法示例

### 在 Python 代码中使用

```python
from g1_sensor_reader import G1SensorReader

# 创建读取器
reader = G1SensorReader(domain_id=1, interface="lo")

# 等待数据
if reader.wait_for_data(timeout=10.0):
    # 获取 IMU 数据
    imu = reader.get_imu_data()
    print(f"Roll: {imu['rpy'][0]}")
    
    # 获取关节状态
    joints = reader.get_joint_states()
    print(f"Joint positions: {joints['positions']}")
    
    # 获取特定关节
    knee = reader.get_joint_state_by_name("left_knee")
    print(f"Left knee position: {knee['position']}")
```

### 数据后处理

```python
import pandas as pd

# 读取记录的 CSV 数据
df = pd.read_csv('g1_sensor_data_xxx.csv')

# 绘制 IMU 数据
df[['imu_roll', 'imu_pitch', 'imu_yaw']].plot()

# 分析关节力矩
max_torque = df[[col for col in df.columns if 'tau' in col]].max()
```

## 许可证

参考 unitree_sdk2py 和 unitree_mujoco 的许可证。

## 参考链接

- [unitree_mujoco](https://github.com/unitreerobotics/unitree_mujoco)
- [unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python)
- [Unitree G1 文档](https://support.unitree.com/home/en/G1_developer)