# 四足机器人从位控转为力控的建议与实施方案

## 1. 当前系统本质上是什么控制

你现在这套程序不是“纯位控”，而是**位置主导 + 力矩辅助**。

从代码上看：

- `User/app/lingzu_task.c` 负责状态机、站立/趴下/行走切换
- `User/driver/Movement.c` 负责步态、IK、VMC 力矩计算
- `User/driver/leg.c` 已经有正逆运动学、Jacobian 对应关系
- `User/driver/lingzu_motor.c` 发送给 RS02 的指令包含：
  - `position`
  - `speed`
  - `kp`
  - `kd`
  - `torque`

这说明你当前已经具备了向力控过渡的基础，不是从零开始。

---

## 2. 你说的“力控”要先明确是哪一种

建议先区分三类目标：

### 2.1 关节力矩控制
直接控制每个关节输出的力矩 `tau`。

特点：
- 最底层
- 柔顺性强
- 最难调
- 风险最大

### 2.2 足端力控制
控制足端对地的作用力，例如支撑力、推进力。

特点：
- 更适合四足
- 需要 Jacobian：

\[
\tau = J^T F
\]

### 2.3 力位混合 / 阻抗控制 / VMC
仍然保留目标位置，但不再靠高刚度死跟位置，而是通过虚拟弹簧阻尼计算力，再映射成关节力矩。

特点：
- 最适合你当前项目
- 最稳妥
- 最容易逐步落地

### 结论
**建议优先做“力位混合控制”，而不是一步切到纯力矩控制。**

---

## 3. 你现在已经有的基础

### 3.1 电机接口已经支持 torque
`User/driver/lingzu_motor.c` 中，发送控制时已经把 `torque` 编码进 CAN 指令。

### 3.2 已有反馈量
你已经能读到：
- 位置 `current_position_f`
- 速度 `current_speed_f`
- 力矩 `current_torque_f`
- 温度 `current_temp_f`

### 3.3 已有运动学模型
`User/driver/leg.c` 已经提供：
- `Inverse_Calculation()`
- `FK_LegPosition()`
- `FK_LegVelocity()`

### 3.4 已有初步 VMC 框架
`User/driver/Movement.c` 中已经有：
- `vmc_legacy_torque()`
- `vmc_full_torque()`

也就是说，你已经有：
- 电机力矩入口
- 状态反馈
- Jacobian 所需模型
- 初步足端力控制框架

---

## 4. 真正切换前必须收集的信息

## 4.1 电机 torque 通道真实性
这是最重要的。

你必须确认 RS02 的 `torque` 到底是什么：

1. 真正关节输出力矩？
2. 电流等效量？
3. 只是内部位置环的前馈项？

### 你必须验证的问题
当你设置：
- `kp = 0`
- `kd = 0`
- `speed = 0`
- `position = 0`
- `torque = 非0`

电机是否仍能稳定输出可控力矩？

如果答案是否定的，那么就不能做真正纯 torque 控制，只能做力位混合。

---

## 4.2 torque 反馈是否可信
你还要确认：

- `current_torque_f` 单位是不是 Nm
- 是估算值还是直接测得
- 噪声有多大
- 延迟有多大
- 正负号是否一致

如果反馈不可靠，闭环力控会很难做。

---

## 4.3 机械参数是否足够准确
力控对模型更敏感，所以要重新确认：

1. `L1`、`L2`
2. 大腿/小腿零点
3. 左右腿符号方向
4. 同步带耦合关系
5. 力矩正方向对应真实腿部运动方向
6. 机械摩擦、死区、间隙

---

## 4.4 控制频率和总延迟
力控对频率要求更高。

必须明确：
- 控制循环周期是多少
- 电机反馈周期是多少
- CAN 总线延迟是多少
- 从“采样反馈”到“发出新 torque”总共延迟多少

如果实际闭环频率偏低，纯力控会很难稳定。

---

## 4.5 接触判断能力
四足做支撑腿力控时，需要知道哪条腿真正接地。

你现在没有足底力传感器，所以至少要有估计方法：
- 关节力矩突变
- 足端速度突然降低
- 足端高度达到接地阈值
- IMU 机身姿态/加速度辅助判断

---

## 5. 我建议你按什么路线做

## 第一阶段：低刚度位控 + 力矩前馈
这是最稳的第一步。

### 核心思路
保留：
- 步态规划
- IK 目标角

但降低位置刚度：
- `kp` 下降
- `kd` 下降
- `torque` 增强

### 目标
把机器人从“硬跟随”变成“柔顺跟随”。

### 先做什么
1. 静站重力补偿
2. 支撑腿附加支撑力矩
3. 落地缓冲 torque
4. 摆动腿保持较轻位置跟踪

这是最适合你现阶段的方案。

---

## 第二阶段：关节阻抗控制
公式通常是：

\[
\tau = K_p(q_d - q) + K_d(\dot q_d - \dot q) + \tau_{ff}
\]

如果 RS02 的 torque 通道足够可靠，可以把更多控制量放到主控里自己算，而不是完全依赖电机内部位置环。

但就你当前项目而言，更现实的做法是：
- 电机仍保留较小 `kp/kd`
- 主控侧逐步增加 `tau_ff`

---

## 第三阶段：足端阻抗 / VMC
这是四足机器人最值得做的一步。

足端空间定义虚拟力：

\[
F_x = -K_x(x - x_d) - B_x \dot x
\]

\[
F_z = -K_z(z - z_d) - B_z \dot z + F_g
\]

再用：

\[
\tau = J^T F
\]

映射成关节力矩。

### 你的代码现状
这一步你已经有雏形了，只需要系统化：
- 摆动腿：位置/轨迹主导
- 支撑腿：足端阻抗主导

这是我最推荐的中期目标。

---

## 第四阶段：纯关节力矩控制
只有在下面条件都满足时才建议：

1. torque 通道已验证可靠
2. torque 反馈可信
3. 控制频率足够高
4. 急停保护很完善
5. 标定很准确

否则风险很大，可能出现：
- 塌腿
- 高速震荡
- 落地冲击失控
- 电机发热严重

---

## 6. 你现在应该收集的数据清单

## 6.1 单电机实验
每个关节都做。

### 实验配置
设置：
- `kp = 0`
- `kd = 0`
- `speed = 0`
- `position = 0`
- 扫描 `torque`

### 记录内容
- 最小起转 torque
- 正负方向是否正确
- 是否平稳
- 空载下速度变化
- `current_torque_f` 与指令是否一致
- 温升情况

---

## 6.2 单腿实验
一条腿悬空测试。

### 要看什么
- 同样 torque 下，不同姿态响应是否一致
- 大腿和小腿耦合程度
- Jacobian 算出来的足端力方向是否正确

---

## 6.3 单腿接地实验
一条腿轻触地面。

### 记录
- 落地瞬间 torque 反馈变化
- 落地前后足端速度变化
- 同一支撑高度下保持力矩需求
- 推腿时柔顺性

---

## 6.4 整机实验顺序
必须按顺序推进：

1. 单电机
2. 单腿悬空
3. 单腿接地
4. 整机架空
5. 整机吊挂半承重
6. 全承重静站
7. 小幅步态
8. 连续步行

不要直接全承重纯力控。

---

## 7. 软件上建议你补的能力

## 7.1 完整日志
建议每个关节记录：
- 目标位置
- 实际位置
- 目标速度
- 实际速度
- 指令 torque
- 反馈 torque
- 温度
- 故障码

建议每条腿记录：
- 足端目标位置
- 足端实际位置
- 足端速度
- 当前步态相位
- 支撑/摆动状态
- 估算足端力

没有日志，后面很难调。

---

## 7.2 安全保护
建议补充：
- torque 限幅
- 速度限幅
- 位置工作空间限幅
- 通信超时保护
- 温度保护
- 姿态异常保护
- 落地冲击保护
- 一键 limp 模式

---

## 7.3 控制模式枚举
建议明确分模式，不要混在一起：

```c
typedef enum {
    CTRL_MODE_POSITION = 0,
    CTRL_MODE_MIXED,
    CTRL_MODE_VMC,
    CTRL_MODE_TORQUE
} control_mode_e;