# 低头匍匐前进步态设计

## 概述

本模块为小米CyberDog机器狗设计了一套低头匍匐前进的自定义步态，实现保持低身位、头部下垂的四足步态行走。

## 特点

- **低身位姿态**: 机器人身体降低约5-8cm，保持稳定的低重心
- **头部下垂**: 通过调整pitch角实现约14度的低头姿态  
- **稳定匍匐**: 采用对角步态，确保至少三足支撑的稳定性
- **可控速度**: 缓慢而稳定的前进速度，适合低姿态移动
- **安全恢复**: 提供完整的姿态过渡和恢复机制

## 文件结构

```
task_2/
├── crawl_gait.py              # 主要步态控制模块
├── Gait_Def_crawl.toml        # 步态定义文件
├── Gait_Params_crawl.toml     # 步态参数文件
├── file_send_lcmt.py          # LCM文件传输消息
├── robot_control_cmd_lcmt.py  # 机器人控制命令消息
├── task_2.py                  # 集成了匍匐步态的任务2
└── README_crawl_gait.md       # 本说明文件
```

## 步态设计原理

### 1. 步态定义（Gait_Def_crawl.toml）

采用16段式对角步态循环：
- **四足着地期**: 提供稳定支撑，持续8个时间单位
- **单足摆动期**: 每次只有一条腿离地，持续6个时间单位
- **步态顺序**: FL → RR → FR → RL（对角步态模式）

### 2. 步态参数（Gait_Params_crawl.toml）

关键参数设置：
- `body_pos_des`: [0.0, 0.25, 0.0, 0.0, 0.0, -0.05] - 低头且身体下降
- `body_vel_des`: [0.08, 0.0, 0.0] - 缓慢前进速度（8cm/s）
- `step_height`: [0.0, 0.06, 0.0, 0.0] - 摆动腿抬高6cm
- `landing_gain`: 2.0 - 增加落地增益适应低姿态
- `mu`: 0.25 - 摩擦系数

### 3. 姿态控制

- **Roll**: 0.0° - 保持水平
- **Pitch**: 14.3° (0.25 rad) - 实现低头姿态
- **Yaw**: 0.0° - 保持朝向
- **Height**: -5cm - 身体降低到匍匐位置

## 使用方法

### 1. 基本使用

```python
from task_2.crawl_gait import run_low_crawl_demo

# 执行完整的匍匐前进演示（包含姿态过渡）
success = run_low_crawl_demo(ctrl, msg, distance=2.0, observe=True)
```

### 2. 分步控制

```python
from task_2.crawl_gait import (
    transition_to_crawl_position,
    low_crawl_gait, 
    recover_from_crawl_position
)

# 步骤1: 过渡到匍匐姿态
transition_to_crawl_position(ctrl, msg, observe=True)

# 步骤2: 执行匍匐步态（8步约1米）
low_crawl_gait(ctrl, msg, step_count=8, observe=True)

# 步骤3: 恢复正常姿态
recover_from_crawl_position(ctrl, msg, observe=True)
```

### 3. 与任务2集成

```python
from task_2.task_2 import run_task_2_with_crawl_demo

# 执行S型弯道 + 匍匐演示的完整任务
arrow_direction, crawl_success = run_task_2_with_crawl_demo(ctrl, msg, crawl_distance=1.5)
```

### 4. 测试函数

```python
from task_2.task_2 import test_crawl_gait_only, test_crawl_transition_only

# 仅测试匍匐步态
test_crawl_gait_only(ctrl, msg, distance=2.0)

# 仅测试姿态过渡
test_crawl_transition_only(ctrl, msg)
```

## 参数调整指南

### 速度调整
修改`Gait_Params_crawl.toml`中的`body_vel_des`：
- 降低值 (如0.05) → 更慢更稳定
- 提高值 (如0.12) → 更快但可能不够稳定

### 身体高度调整
修改`body_pos_des`的第6个值：
- -0.03 → 身体降低3cm（较轻度匍匐）
- -0.08 → 身体降低8cm（更深度匍匐）

### 低头角度调整
修改`body_pos_des`的第2个值：
- 0.15 → 轻度低头（约8.6度）
- 0.35 → 深度低头（约20度）

### 步高调整
修改`step_height`值：
- [0.0, 0.04, 0.0, 0.0] → 降低摆动高度（4cm）
- [0.0, 0.08, 0.0, 0.0] → 增加摆动高度（8cm）

## 安全注意事项

1. **地面要求**: 确保地面平整，避免障碍物
2. **空间需求**: 保证足够的头部空间，避免碰撞
3. **监控状态**: 使用`observe=True`监控执行状态
4. **急停机制**: 支持KeyboardInterrupt安全停止
5. **故障恢复**: 自动尝试恢复到安全姿态

## 性能指标

- **前进速度**: 约8cm/s
- **每步距离**: 约10-15cm
- **姿态稳定性**: 始终保持三足以上支撑
- **能耗效率**: 低速步态，较为节能
- **适用场景**: 低空间、需要低姿态通过的环境

## 故障排除

### 常见问题

1. **步态不执行**
   - 检查TOML文件路径是否正确
   - 确认LCM通信正常
   - 验证机器人处于正确的初始状态

2. **姿态不稳定**
   - 降低前进速度
   - 增加步态持续时间
   - 检查地面平整度

3. **无法恢复正常姿态**
   - 手动调用`recover_from_crawl_position()`
   - 检查机器人关节状态
   - 重启机器人系统

### 调试模式

启用详细日志输出：
```python
# 在导入后设置
import logging
logging.basicConfig(level=logging.DEBUG)
```

## 技术参考

- 基于小米CyberDog自定义步态API
- 参考文档：[小米机器人实验室自定义步态设计](https://miroboticslab.github.io/blogs/#/cn/cyberdog_loco_cn?id=_233-%e8%87%aa%e5%ae%9a%e4%b9%89%e6%ad%a5%e6%80%81)
- 步态设计参考task_4的框架设计

## 版本历史

- v1.0 - 初始版本，实现基本低头匍匐步态
- 设计日期：2024年
- 作者：基于task_4步态框架设计