# 6自由度机械臂正运动学使用说明

## 📋 文件说明

- **arm.h** - 机械臂接口头文件
- **arm.cpp** - 机械臂实现文件（基于toolbox/robotics库）
- **arm_main.c** - 使用示例

## 🚀 快速开始

### 1. 初始化机械臂

```c
#include "component/arm.h"

// 定义机器人尺寸参数 (TODO: 替换为实际测量值)
ArmParams_t arm_params = {
    .h0 = 100.0f,  // 基座高度 (mm)
    .L1 = 200.0f,  // 大臂长度 (mm)
    .L2 = 150.0f,  // 小臂长度 (mm)
    .L3 = 80.0f    // 夹爪长度 (mm)
};

// 初始化
Arm_Init(&arm_params);
```

### 2. 正运动学计算

```c
// 输入：6个关节角度
JointAngles_t joint_angles;
joint_angles.q[0] = 0.0f;      // J1: 基座Yaw (rad)
joint_angles.q[1] = M_PI/4;    // J2: 大臂Pitch (rad)
joint_angles.q[2] = -M_PI/4;   // J3: 大臂末端Pitch (rad)
joint_angles.q[3] = 0.0f;      // J4: 小臂Roll (rad)
joint_angles.q[4] = M_PI/6;    // J5: 小臂末端Pitch (rad)
joint_angles.q[5] = 0.0f;      // J6: 夹爪Roll (rad)

// 输出：末端位姿
Pose_t end_pose;

// 执行正运动学
if (Arm_ForwardKinematics(&joint_angles, &end_pose) == 0) {
    // 成功！
    printf("末端位置: x=%.2f, y=%.2f, z=%.2f (mm)\n", 
           end_pose.x, end_pose.y, end_pose.z);
    printf("末端姿态: roll=%.2f, pitch=%.2f, yaw=%.2f (rad)\n", 
           end_pose.roll, end_pose.pitch, end_pose.yaw);
}
```

### 3. 获取中间关节位姿（可选）

```c
Pose_t joint3_pose;

// 获取第3个关节的位姿
if (Arm_GetJointPose(&joint_angles, 3, &joint3_pose) == 0) {
    printf("J3位置: x=%.2f, y=%.2f, z=%.2f\n", 
           joint3_pose.x, joint3_pose.y, joint3_pose.z);
}
```

## 📐 关节配置说明

| 关节 | 名称 | 类型 | 旋转轴 | 说明 |
|-----|------|------|--------|------|
| J1 | 基座 | Yaw | Z轴 | 基座旋转 |
| J2 | 大臂起点 | Pitch | Y轴 | 大臂俯仰 |
| J3 | 大臂末端 | Pitch | Y轴 | 控制小臂 |
| J4 | 小臂起点 | Roll | X轴 | 小臂旋转 |
| J5 | 小臂末端 | Pitch | Y轴 | 控制夹爪段 |
| J6 | 夹爪 | Roll | X轴 | 夹爪旋转 |

## ⚙️ DH参数配置

当前使用的是**标准DH参数**，定义在 `arm.cpp` 的 `CreateDHLinks()` 函数中：

```cpp
// J1: 基座 Yaw
s_links[0] = robotics::Link(0, params->h0, 0, 0, robotics::R);

// J2: 大臂 Pitch  
s_links[1] = robotics::Link(0, 0, 0, M_PI_2, robotics::R);

// J3: 大臂末端 Pitch
s_links[2] = robotics::Link(0, 0, params->L1, 0, robotics::R);

// J4: 小臂起点 Roll
s_links[3] = robotics::Link(0, 0, 0, M_PI_2, robotics::R);

// J5: 小臂末端 Pitch
s_links[4] = robotics::Link(0, 0, params->L2, M_PI_2, robotics::R);

// J6: 夹爪 Roll
s_links[5] = robotics::Link(0, params->L3, 0, M_PI_2, robotics::R);
```

⚠️ **需要根据实际机器人测量结果调整！**

## 📊 数据结构

### ArmParams_t - 机器人尺寸参数
```c
typedef struct {
    float h0;  // 基座高度 (基座到J2的高度, mm)
    float L1;  // 大臂长度 (J2到J3的距离, mm)
    float L2;  // 小臂长度 (J4到J5的距离, mm)
    float L3;  // 夹爪长度 (J6到末端的距离, mm)
} ArmParams_t;
```

### JointAngles_t - 关节角度
```c
typedef struct {
    float q[6];  // 6个关节角度 (rad)
} JointAngles_t;
```

### Pose_t - 位姿
```c
typedef struct {
    float x, y, z;           // 位置 (mm)
    float roll, pitch, yaw;  // 姿态 (rad) - RPY欧拉角
} Pose_t;
```

## 🔧 待办事项

- [ ] **测量实际机器人尺寸**：h0, L1, L2, L3
- [ ] **拍摄零位照片**：确认零位时各关节的姿态
- [ ] **验证DH参数**：根据实际机器人调整 `CreateDHLinks()` 函数
- [ ] **集成电机反馈**：将实际电机角度输入到正运动学
- [ ] **添加逆运动学**：实现末端位姿到关节角度的计算

## 📚 依赖库

- **toolbox/robotics.h** - 机器人学工具箱
- **toolbox/matrix.h** - 矩阵运算库
- **ARM CMSIS DSP** - 数学计算库

## 🎯 使用示例（完整）

参考 `User/task/arm_main.c` 中的实现：

```c
void Task_arm_main(void *argument) {
    // 1. 初始化
    ArmParams_t params = {100.0f, 200.0f, 150.0f, 80.0f};
    Arm_Init(&params);
    
    // 2. 设置关节角度
    JointAngles_t q = {0};
    
    while (1) {
        // 3. 读取电机角度（示例）
        q.q[0] = get_motor_angle(0);
        q.q[1] = get_motor_angle(1);
        // ...
        
        // 4. 计算正运动学
        Pose_t pose;
        Arm_ForwardKinematics(&q, &pose);
        
        // 5. 使用末端位姿
        // ...
        
        osDelay(10);
    }
}
```

## 🐛 调试提示

如果计算结果不符合预期：
1. 检查DH参数是否正确
2. 确认角度单位是弧度（不是角度）
3. 验证关节零位的定义
4. 检查关节旋转方向（正负）

---

**下一步**：提供实际机器人的尺寸数据，我将帮您完善DH参数！