arm/User/component/ARM_USAGE.md

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

📋 文件说明

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

🚀 快速开始

1. 初始化机械臂

#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. 正运动学计算

// 输入：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. 获取中间关节位姿（可选）

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() 函数中：

// 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 - 机器人尺寸参数

typedef struct {
    float h0;  // 基座高度 (基座到J2的高度, mm)
    float L1;  // 大臂长度 (J2到J3的距离, mm)
    float L2;  // 小臂长度 (J4到J5的距离, mm)
    float L3;  // 夹爪长度 (J6到末端的距离, mm)
} ArmParams_t;

JointAngles_t - 关节角度

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

Pose_t - 位姿

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 中的实现：

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参数！