本质上只在做一件事：根据机械臂当前的姿势，实时算出现在每个关节需要出多大力，才能刚刚好托住自己的钢铁身躯，让它处于一种像在太空中一样的“失重悬停”状态。

分为3大步骤：

1：单片机先读取7个电机的此时的角度

2：把读到的角度传进：一个包含机械臂真实质心（重心）、重量和长度的“物理模拟器”里。算出如果在这个姿态下要抵抗地心引力，每个电机需要输出多少牛·米（Nm）的力矩。

3：撤销所有电机用于保持该位置的弹簧力（刚度清零），只给它们下发刚刚算出来的那一点点抵抗重力的力矩。这样机械臂就不会僵硬，你用手一碰就能推动，一松手它又能自己悬停。

市面上常见方法：牛顿-欧拉迭代法：采用匹诺曹库，需要强悍的cpu与算力支持

1：对步骤二：前项查分法

总结：

通过sw算出机械臂每个部位的质心，用电子秤称量出每个部分重量，算出每个部分重力

从每个关节的旋转角度采用机械臂顺解，得到矩阵。

给每个关节加入0.001弧度的扰动，扰动会让质心产生高度差，产生了重力势能变化量。

用变化量除0.001角度变化，得到每个关节应该输出的力矩大小。

机械臂电机id从下到上：4563217

以下是坐标系分布情况：

该函数作用：输入旋转角度，输出理论力矩大小

static void gc_calc_gravity_torques(float joints[6], float torques[6])
{
    float delta = 0.001f;
    float inv_delta = 1000.0f; // 用乘法代替除法
    const float c1[3] = {0.000013f,  0.004462f,  0.035527f};//装配体1跟id5电机：基于坐标系1的质心位置
    const float c2[3] = {0.118681f,  0.000014f, -0.021056f};//大臂跟id6电机：基于坐标系2的质心位置
    const float c3[3] = {-0.004001f, 0.067747f, -0.024595f};
    const float c4[3] = {-0.000014f,-0.002074f,  0.132758f};
    const float c5[3] = {0.033346f,  0.000011f,  0.003049f};
    const float c6[3] = {0.000062f,  0.000002f,  0.056100f};

以上数值是基于urdf文件，ai计算得出的结果

c2代表joint2基于坐标系2的质心位置，在sw计算出来的c2是：131.617 0.018 -27.369

与上方计算有偏差，但是问题不大，因为现实中也有摩擦与阻力，还有螺丝的重量还没计算进去。

后面会加上手动调参数

    float W1 = g_mass_tuner[0] * 0.510f * 9.81f;//算出重力
    float W2 = g_mass_tuner[1] * 0.493f * 9.81f;
    float W3 = g_mass_tuner[2] * 0.495f * 9.81f;
    float W4 = g_mass_tuner[3] * 0.411f * 9.81f;
    float W5 = g_mass_tuner[4] * 0.412f * 9.81f;
    float W6 = g_mass_tuner[5] * (0.4836f) * 9.81f;//爪子重量

0.51：id5底座电机0.362+上方连接件 0.148

0.493：大臂0.131+id6电机0.362

0.495：id3电机与6电机连接件0.17+id3电机0.325

0.411：3与2连接件0.086+id2电机0.325

0.412：2与1连接件0.087+id1电机0.325

0.4836：爪子+爪子电机+id1与id7电机连接件重量

g_mass_tuner：现实中机械臂内部塞满了沉重的纯铜线缆、螺丝、轴承甚至润滑脂。用该参数补偿。

    // 1. 基准状态缓存计算 (仅算一次)
    gc_Matrix4x4 T1 = gc_get_T1(joints[0]);//坐标系1基于坐标系0的 距离与姿态信息
    gc_Matrix4x4 T2 = gc_get_T2(joints[1]);
    gc_Matrix4x4 T3 = gc_get_T3(joints[2]);
    gc_Matrix4x4 T4 = gc_get_T4(joints[3]);
    gc_Matrix4x4 T5 = gc_get_T5(joints[4]);
    gc_Matrix4x4 T6 = gc_get_T6(joints[5]);//坐标系6基于坐标系5的距离与姿态信息

计算出：每个坐标系基于前一个坐标系的距离与姿态信息

    //机械臂顺解
    gc_Matrix4x4 T12 = fast_mat_mult(&T1, &T2);//坐标系2基于坐标系0的位置与姿态关系
    gc_Matrix4x4 T123 = fast_mat_mult(&T12, &T3);
    gc_Matrix4x4 T1234 = fast_mat_mult(&T123, &T4);
    gc_Matrix4x4 T12345 = fast_mat_mult(&T1234, &T5);
    gc_Matrix4x4 T_end = fast_mat_mult(&T12345, &T6);//坐标系6基于坐标系0的位置与姿态关系

得到：每一个坐标系基于坐标系0的距离与姿态关系

    // 提取基准 Z 高度
    float z1 = get_link_Z(&T1, c1);//6个零件重心离坐标系0的绝对高度
    float z2 = get_link_Z(&T12, c2);
    float z3 = get_link_Z(&T123, c3);
    float z4 = get_link_Z(&T1234, c4);
    float z5 = get_link_Z(&T12345, c5);
    float z6 = get_link_Z(&T_end, c6);

T矩阵只有：每一个坐标系基于坐标系0的距离与姿态关系

c质心只有：每个部件基于自己坐标系的位置关系

static inline float get_link_Z(const gc_Matrix4x4* T, const float c[3]) {
    return T->m[2][0]*c[0] + T->m[2][1]*c[1] + T->m[2][2]*c[2] + T->m[2][3];
}

把T矩阵的距离与质心相乘，得到：质心基于坐标系0的关系

系统为了知道 J6 电机现在需要出多大的力，它在极短的时间里“想象” J6 往下掉了一点点，然后测量这个“掉落”释放了多少重力势能，最后用这个势能变化率反推出电机需要提供的等量抵抗力（扭矩）。

    gc_Matrix4x4 T6_p = gc_get_T6(joints[5] + delta);//假设j6电机稍微转动
    gc_Matrix4x4 Tend_p6 = fast_mat_mult(&T12345, &T6_p);//两个矩阵相乘，得到新的末端绝对矩阵
    torques[5] = W6 * (get_link_Z(&Tend_p6, c6) - z6) * inv_delta;//重力*高度差：重力势能

1：把第 6 轴电机往下稍微掰动一点点角度（delta 通常是 0.001 弧度），坐标系 6 相对于坐标系 5 的姿态发生了极其微小的一丝倾斜。

2：系统得出：掰动那一丝角度后，最末端夹爪基于底座的全新绝对姿态

3：重力势能变化量（做功大小）=W6是末端爪子重力*（由于掰动一点点角度产生的质心高度差）

4：重力势能变化量除角度变化量（相当于×1000）就求出力矩大小

    // --- 扰动 J5 ---
    gc_Matrix4x4 T5_p = gc_get_T5(joints[4] + delta);//j5加入扰动，坐标系5基于坐标系4的距离与姿态关系
    gc_Matrix4x4 T12345_p5 = fast_mat_mult(&T1234, &T5_p);//扰动之后：坐标系5基于坐标系0的距离与姿态关系
    gc_Matrix4x4 Tend_p5 = fast_mat_mult(&T12345_p5, &T6);//扰动之后：坐标系6基于坐标系0的距离与姿态关系
    torques[4] = (W5*(get_link_Z(&T12345_p5, c5) - z5) + W6*(get_link_Z(&Tend_p5, c6) - z6)) * inv_delta;

刚才我们分析了 J6 轴，它只背负着它自己。

转动 J5 轴，不仅会带动 Link 5，还会像挥舞鞭子一样，把挂在它末端的 Link 6 也一起甩动。

Tend_p5：既然 J5 动了，那长在它身上的 J6 在绝对空间里肯定也跟着动了。注意，此时 T6 并没有加 delta，因为 J6 的电机自己没转（相对于 J5 没动），它是被动跟着 J5 在空间中划过了一道弧线。这一步算出了由于 J5 的转动，导致夹爪末端在绝对空间里的全新位置。

力矩分析：

• Link 5 产生的势能变化：W5 * (新高度 - 老高度z5)

  算出了因为 J5 转动，导致第 5 轴零件本身的重心下降，释放了多少做功。

• Link 6 产生的势能变化：W6 * (新高度 - 老高度z6)

  算出了因为 J5 转动，导致挂在它身上的夹爪重心下降，又释放了多少做功。

• 合二为一并求偏导：将两者相加，得到总做功量。最后乘以 inv_delta（即除以 0.001 弧度），得出 J5 电机所需的理论扭矩。

剩下的以此类推：

    // --- 扰动 J4 ---
    gc_Matrix4x4 T4_p = gc_get_T4(joints[3] + delta);
    gc_Matrix4x4 T1234_p4 = fast_mat_mult(&T123, &T4_p);
    gc_Matrix4x4 T12345_p4 = fast_mat_mult(&T1234_p4, &T5);
    gc_Matrix4x4 Tend_p4 = fast_mat_mult(&T12345_p4, &T6);
    torques[3] = (W4*(get_link_Z(&T1234_p4,c4)-z4) + W5*(get_link_Z(&T12345_p4,c5)-z5) + W6*(get_link_Z(&Tend_p4,c6)-z6)) * inv_delta;

    // --- 扰动 J3 ---
    gc_Matrix4x4 T3_p = gc_get_T3(joints[2] + delta);
    gc_Matrix4x4 T123_p3 = fast_mat_mult(&T12, &T3_p);
    gc_Matrix4x4 T1234_p3 = fast_mat_mult(&T123_p3, &T4);
    gc_Matrix4x4 T12345_p3 = fast_mat_mult(&T1234_p3, &T5);
    gc_Matrix4x4 Tend_p3 = fast_mat_mult(&T12345_p3, &T6);
    torques[2] = (W3*(get_link_Z(&T123_p3,c3)-z3) + W4*(get_link_Z(&T1234_p3,c4)-z4) +
                  W5*(get_link_Z(&T12345_p3,c5)-z5) + W6*(get_link_Z(&Tend_p3,c6)-z6)) * inv_delta;

    // --- 扰动 J2 ---
    gc_Matrix4x4 T2_p = gc_get_T2(joints[1] + delta);
    gc_Matrix4x4 T12_p2 = fast_mat_mult(&T1, &T2_p);
    gc_Matrix4x4 T123_p2 = fast_mat_mult(&T12_p2, &T3);
    gc_Matrix4x4 T1234_p2 = fast_mat_mult(&T123_p2, &T4);
    gc_Matrix4x4 T12345_p2 = fast_mat_mult(&T1234_p2, &T5);
    gc_Matrix4x4 Tend_p2 = fast_mat_mult(&T12345_p2, &T6);
    torques[1] = (W2*(get_link_Z(&T12_p2,c2)-z2) + W3*(get_link_Z(&T123_p2,c3)-z3) + W4*(get_link_Z(&T1234_p2,c4)-z4) +
                  W5*(get_link_Z(&T12345_p2,c5)-z5) + W6*(get_link_Z(&Tend_p2,c6)-z6)) * inv_delta;

    // --- 扰动 J1 ---
    gc_Matrix4x4 T1_p = gc_get_T1(joints[0] + delta);
    gc_Matrix4x4 T12_p1 = fast_mat_mult(&T1_p, &T2);
    gc_Matrix4x4 T123_p1 = fast_mat_mult(&T12_p1, &T3);
    gc_Matrix4x4 T1234_p1 = fast_mat_mult(&T123_p1, &T4);
    gc_Matrix4x4 T12345_p1 = fast_mat_mult(&T1234_p1, &T5);
    gc_Matrix4x4 Tend_p1 = fast_mat_mult(&T12345_p1, &T6);
    torques[0] = (W1*(get_link_Z(&T1_p,c1)-z1) + W2*(get_link_Z(&T12_p1,c2)-z2) + W3*(get_link_Z(&T123_p1,c3)-z3) +
                  W4*(get_link_Z(&T1234_p1,c4)-z4) + W5*(get_link_Z(&T12345_p1,c5)-z5) + W6*(get_link_Z(&Tend_p1,c6)-z6)) * inv_delta;

2：500hz循环函数

输入：每个电机的角度，解算出每个电机应该是什么力矩

把该函数放入你的500hz循环中

void gravity_comp_loop_hook_500hz(void)
{
    if (!g_enabled) return;

        这是实时控制中的标准操作。由于动力学解算会消耗算力，函数开头直接检查全局标志位 g_enabled。如果重力补偿未被激活，则直接返回，避免占用 MCU 资源。

    /* ---- 1. 物理真实位置映射为数学运动学角度 ---- */
    float math_joints[6];
    math_joints[0] = (motor[3].para.pos * J1_DIR) + J1_HW_OFFSET + g_arm_cali.j1_bias;
    math_joints[1] = (motor[4].para.pos * J2_DIR) + J2_HW_OFFSET + g_arm_cali.j2_bias;
    math_joints[2] = (motor[5].para.pos * J3_DIR) + J3_HW_OFFSET + g_arm_cali.j3_bias;
    math_joints[3] = (motor[2].para.pos * J4_DIR) + J4_HW_OFFSET + g_arm_cali.j4_bias;
    math_joints[4] = (motor[1].para.pos * J5_DIR) + J5_HW_OFFSET + g_arm_cali.j5_bias;
    math_joints[5] = (motor[0].para.pos * J6_DIR) + J6_HW_OFFSET + g_arm_cali.j6_bias;

在进行运动学/动力学计算前，必须将电机编码器的“物理绝对位置”转换为数学关节角度。

方向校正 (J_DIR)：由于减速机安装方向不同，电机的正转可能对应关节的反转，需乘以方向系数

零点校准 ：叠加了硬件设计零点和标定后的误差补偿值，以获取绝对精准的数学角度。

    float math_torques[6] = {0};
    gc_calc_gravity_torques(math_joints, math_torques);

通过扰动微小角度来求解雅可比矩阵，进而算出各关节的理论重力维持力矩。

    float motor_torques[7];
    motor_torques[3] = math_torques[0] * J1_DIR; // 底座
    motor_torques[4] = math_torques[1] * J2_DIR; // 大臂
    motor_torques[5] = math_torques[2] * J3_DIR; // 小臂
    motor_torques[2] = math_torques[3] * J4_DIR; // 腕1
    motor_torques[1] = math_torques[4] * J5_DIR; // 腕2
    motor_torques[0] = math_torques[5] * J6_DIR; // 腕3
    motor_torques[6] = 0.0f;                     // 夹爪


    motor_torques[2] = motor_torques[2] * 0.70f;  // ID 3 削弱到 70% (可微调)
    motor_torques[1] = motor_torques[1] * 0.68f;  // ID 2 削弱到 70% (可微调)

代码对 ID 3 和 ID 2的力矩分别做了 0.70f 和 0.68f 的人为削减。

在实际测试过程中，i3与id2电机很容易往上抬，所以之前计算出来的理论力矩过大，要对其进行减少。减少之后，效果非常好。

补偿摩擦力：

    for (int i = 0; i < 7; i++)
    {
        
        float real_vel = motor[i].para.vel;
        float fric_tor = 0.0f;
        float deadband = 0.01f;//弧度，在这个速度区间之外
        float ramp_width = 0.05f;

        if (real_vel > deadband) {//绝对值大于0.01弧度每秒的速度，进行摩擦力补偿
            float ratio = (real_vel - deadband) / ramp_width;
            fric_tor = fric_pos[i] * (ratio > 1.0f ? 1.0f : ratio);
        } else if (real_vel < -deadband) {
            float ratio = (-real_vel - deadband) / ramp_width;
            fric_tor = fric_neg[i] * (ratio > 1.0f ? 1.0f : ratio);
        }

        if (motor_torques[i] > 8.0f) motor_torques[i] = 8.0f;
        if (motor_torques[i] < -8.0f) motor_torques[i] = -8.0f;

        motor[i].cmd.pos_set = motor[i].para.pos;
        motor[i].cmd.vel_set = 0.0f;
        motor[i].cmd.kp_set  = 0.0f;
        motor[i].cmd.kd_set  = KD_SMOOTH;
        motor[i].cmd.tor_set = motor_torques[i] + fric_tor;
    }

在只有重力补偿的情况下，拖拽机械臂时你会感觉到减速器内部有明显的“发涩”感。这段代码就是为了消除这种物理阻力，让机械臂像涂了润滑油一样丝滑。

• 爬升区间：当速度在 0.01到 0.06 (即 0.01 + 0.05) 之间时，ratio 会从 0.0， 平滑上升到  1.0。

• 物理效果：随着你推动机械臂的手速越来越快，系统帮你抵抗的摩擦力也是逐渐、柔和地增加的。

• 钳位保护：(ratio > 1.0f ? 1.0f : ratio) 是一句极简的钳位代码。一旦转速超过 0.06，ratio 就被锁死在 1.0，此时电机输出满额的静态摩擦力补偿值（即数组中预设的 fric_pos[i] 或 fric_neg[i]）。

#include "gravity_comp.h"
#include <rtthread.h>
#include <rthw.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "dm4310_ctrl.h"
#include "motor_ctrl.h"
#include "kinematics.h"

//拖拽示教
extern void drag_teach_loop_hook_500hz(void);
/* ================================================================= *
 * 全局配置参数
 * ================================================================= */
static const float fric_pos[7] = {0.02f, 0.02f, 0.02f, 0.04f, 0.08f, 0.08f, 0.01f};
static const float fric_neg[7] = {-0.02f,-0.02f,-0.02f,-0.04f,-0.08f,-0.08f,-0.01f};

#define KD_SMOOTH    0.02f

float g_mass_tuner[6] = {1.0f, 1.0f, 1.7f, 1.0f, 1.0f, 1.0f};// 4 5 6 3 2 1

static uint8_t g_enabled = 0;

/* ================================================================= *
 * 极速版动力学数学库：矩阵乘法展开 + 状态级联缓存
 * ================================================================= */
typedef struct { float m[4][4]; } gc_Matrix4x4;

/* 优化1：全展开矩阵乘法，消除所有 for 循环分支开销 */
static inline gc_Matrix4x4 fast_mat_mult(const gc_Matrix4x4* A, const gc_Matrix4x4* B) {
    gc_Matrix4x4 C;
    C.m[0][0] = A->m[0][0]*B->m[0][0] + A->m[0][1]*B->m[1][0] + A->m[0][2]*B->m[2][0];
    C.m[0][1] = A->m[0][0]*B->m[0][1] + A->m[0][1]*B->m[1][1] + A->m[0][2]*B->m[2][1];
    C.m[0][2] = A->m[0][0]*B->m[0][2] + A->m[0][1]*B->m[1][2] + A->m[0][2]*B->m[2][2];
    C.m[0][3] = A->m[0][0]*B->m[0][3] + A->m[0][1]*B->m[1][3] + A->m[0][2]*B->m[2][3] + A->m[0][3];

    C.m[1][0] = A->m[1][0]*B->m[0][0] + A->m[1][1]*B->m[1][0] + A->m[1][2]*B->m[2][0];
    C.m[1][1] = A->m[1][0]*B->m[0][1] + A->m[1][1]*B->m[1][1] + A->m[1][2]*B->m[2][1];
    C.m[1][2] = A->m[1][0]*B->m[0][2] + A->m[1][1]*B->m[1][2] + A->m[1][2]*B->m[2][2];
    C.m[1][3] = A->m[1][0]*B->m[0][3] + A->m[1][1]*B->m[1][3] + A->m[1][2]*B->m[2][3] + A->m[1][3];

    C.m[2][0] = A->m[2][0]*B->m[0][0] + A->m[2][1]*B->m[1][0] + A->m[2][2]*B->m[2][0];
    C.m[2][1] = A->m[2][0]*B->m[0][1] + A->m[2][1]*B->m[1][1] + A->m[2][2]*B->m[2][1];
    C.m[2][2] = A->m[2][0]*B->m[0][2] + A->m[2][1]*B->m[1][2] + A->m[2][2]*B->m[2][2];
    C.m[2][3] = A->m[2][0]*B->m[0][3] + A->m[2][1]*B->m[1][3] + A->m[2][2]*B->m[2][3] + A->m[2][3];

    C.m[3][0] = 0.0f; C.m[3][1] = 0.0f; C.m[3][2] = 0.0f; C.m[3][3] = 1.0f;
    return C;
}

static inline float get_link_Z(const gc_Matrix4x4* T, const float c[3]) {
    return T->m[2][0]*c[0] + T->m[2][1]*c[1] + T->m[2][2]*c[2] + T->m[2][3];
}

static gc_Matrix4x4 gc_get_T1(float t1) {
    gc_Matrix4x4 T = {0}; float c = cosf(t1), s = sinf(t1);
    T.m[0][0] = c; T.m[0][1] = -s; T.m[0][2] = 0; T.m[0][3] = 0;
    T.m[1][0] = s; T.m[1][1] = c;  T.m[1][2] = 0; T.m[1][3] = 0;
    T.m[2][0] = 0; T.m[2][1] = 0;  T.m[2][2] = 1; T.m[2][3] = L1_Z_OFFSET / 1000.0f;
    T.m[3][3] = 1; return T;
}
static gc_Matrix4x4 gc_get_T2(float t2) {
    gc_Matrix4x4 T = {0}; float c = cosf(t2), s = sinf(t2);
    T.m[0][0] = c; T.m[0][1] = -s; T.m[0][2] = 0;  T.m[0][3] = 0;
    T.m[1][0] = 0; T.m[1][1] = 0;  T.m[1][2] = -1; T.m[1][3] = L2_Y_OFFSET / 1000.0f;
    T.m[2][0] = s; T.m[2][1] = c;  T.m[2][2] = 0;  T.m[2][3] = L2_Z_OFFSET / 1000.0f;
    T.m[3][3] = 1; return T;
}
static gc_Matrix4x4 gc_get_T3(float t3) {
    gc_Matrix4x4 T = {0}; float c = cosf(-t3), s = sinf(-t3);
    T.m[0][0] = -c; T.m[0][1] = s; T.m[0][2] = 0;  T.m[0][3] = L3_X_OFFSET / 1000.0f;
    T.m[1][0] = s;  T.m[1][1] = c; T.m[1][2] = 0;  T.m[1][3] = 0;
    T.m[2][0] = 0;  T.m[2][1] = 0; T.m[2][2] = -1; T.m[2][3] = L3_Z_OFFSET / 1000.0f;
    T.m[3][3] = 1; return T;
}
static gc_Matrix4x4 gc_get_T4(float t4) {
    gc_Matrix4x4 T = {0}; float c = cosf(t4), s = sinf(t4);
    T.m[0][0] = 0;  T.m[0][1] = 0; T.m[0][2] = 1; T.m[0][3] = L4_X_OFFSET / 1000.0f;
    T.m[1][0] = s;  T.m[1][1] = c; T.m[1][2] = 0; T.m[1][3] = L4_Y_OFFSET / 1000.0f;
    T.m[2][0] = -c; T.m[2][1] = s; T.m[2][2] = 0; T.m[2][3] = L4_Z_OFFSET / 1000.0f;
    T.m[3][3] = 1; return T;
}
static gc_Matrix4x4 gc_get_T5(float t5) {
    gc_Matrix4x4 T = {0}; float c = cosf(t5), s = sinf(t5);
    T.m[0][0] = -s; T.m[0][1] = -c; T.m[0][2] = 0;  T.m[0][3] = 0;
    T.m[1][0] = 0;  T.m[1][1] = 0;  T.m[1][2] = -1; T.m[1][3] = 0;
    T.m[2][0] = c;  T.m[2][1] = -s; T.m[2][2] = 0;  T.m[2][3] = L5_Z_OFFSET / 1000.0f;
    T.m[3][3] = 1; return T;
}
static gc_Matrix4x4 gc_get_T6(float t6) {
    gc_Matrix4x4 T = {0}; float c = cosf(t6), s = sinf(t6);
    T.m[0][0] = 0; T.m[0][1] = 0;  T.m[0][2] = 1; T.m[0][3] = L6_X_OFFSET / 1000.0f;
    T.m[1][0] = c; T.m[1][1] = -s; T.m[1][2] = 0; T.m[1][3] = 0;
    T.m[2][0] = s; T.m[2][1] = c;  T.m[2][2] = 0; T.m[2][3] = 0;
    T.m[3][3] = 1; return T;
}

/**
 * @brief 优化版重力力矩计算 (前向差分 + 增量状态缓存)
 *        性能提升：计算量骤降 80%
 */
static void gc_calc_gravity_torques(float joints[6], float torques[6])
{
    float delta = 0.001f;
    float inv_delta = 1000.0f; // 用乘法代替除法

        // =====================================================================
        //       完全同步最新 URDF (装配体完整.SLDASM) 的真实质心坐标
        // =====================================================================
    const float c1[3] = {0.000013f,  0.004462f,  0.035527f};//装配体1跟id5电机：基于坐标系1的质心位置
    const float c2[3] = {0.118681f,  0.000014f, -0.021056f};//大臂跟id6电机：基于坐标系2的质心位置
    const float c3[3] = {-0.004001f, 0.067747f, -0.024595f};
    const float c4[3] = {-0.000014f,-0.002074f,  0.132758f};
    const float c5[3] = {0.033346f,  0.000011f,  0.003049f};
    const float c6[3] = {0.000062f,  0.000002f,  0.056100f};

    // 预计算重力系数 (W = m * g * tuner)
    float W1 = g_mass_tuner[0] * 0.510f * 9.81f;//算出重力
    float W2 = g_mass_tuner[1] * 0.493f * 9.81f;
    float W3 = g_mass_tuner[2] * 0.495f * 9.81f;
    float W4 = g_mass_tuner[3] * 0.411f * 9.81f;
    float W5 = g_mass_tuner[4] * 0.412f * 9.81f;
    float W6 = g_mass_tuner[5] * (0.4836f) * 9.81f;//爪子重量
    
    // 1. 基准状态缓存计算 (仅算一次)
    gc_Matrix4x4 T1 = gc_get_T1(joints[0]);//坐标系1基于坐标系0的 距离与姿态信息
    gc_Matrix4x4 T2 = gc_get_T2(joints[1]);
    gc_Matrix4x4 T3 = gc_get_T3(joints[2]);
    gc_Matrix4x4 T4 = gc_get_T4(joints[3]);
    gc_Matrix4x4 T5 = gc_get_T5(joints[4]);
    gc_Matrix4x4 T6 = gc_get_T6(joints[5]);//坐标系6基于坐标系5的距离与姿态信息
    //机械臂顺解
    gc_Matrix4x4 T12 = fast_mat_mult(&T1, &T2);//坐标系2基于坐标系0的位置与姿态关系
    gc_Matrix4x4 T123 = fast_mat_mult(&T12, &T3);
    gc_Matrix4x4 T1234 = fast_mat_mult(&T123, &T4);
    gc_Matrix4x4 T12345 = fast_mat_mult(&T1234, &T5);
    gc_Matrix4x4 T_end = fast_mat_mult(&T12345, &T6);//坐标系6基于坐标系0的位置与姿态关系

    // 提取基准 Z 高度
    float z1 = get_link_Z(&T1, c1);//6个零件重心离坐标系0的绝对高度
    float z2 = get_link_Z(&T12, c2);
    float z3 = get_link_Z(&T123, c3);
    float z4 = get_link_Z(&T1234, c4);
    float z5 = get_link_Z(&T12345, c5);
    float z6 = get_link_Z(&T_end, c6);

    // 2. 增量扰动计算 (只需局部级联重算)
    // --- 扰动 J6 ---
    gc_Matrix4x4 T6_p = gc_get_T6(joints[5] + delta);//假设j6电机稍微转动
    gc_Matrix4x4 Tend_p6 = fast_mat_mult(&T12345, &T6_p);//两个矩阵相乘，得到新的末端绝对矩阵
    torques[5] = W6 * (get_link_Z(&Tend_p6, c6) - z6) * inv_delta;//重力*高度差（质心缩小了1000倍）*1000：重力势能

    // --- 扰动 J5 ---
    gc_Matrix4x4 T5_p = gc_get_T5(joints[4] + delta);//j5加入扰动，坐标系5基于坐标系4的距离与姿态关系
    gc_Matrix4x4 T12345_p5 = fast_mat_mult(&T1234, &T5_p);//扰动之后：坐标系5基于坐标系0的距离与姿态关系
    gc_Matrix4x4 Tend_p5 = fast_mat_mult(&T12345_p5, &T6);//扰动之后：坐标系6基于坐标系0的距离与姿态关系
    torques[4] = (W5*(get_link_Z(&T12345_p5, c5) - z5) + W6*(get_link_Z(&Tend_p5, c6) - z6)) * inv_delta;

    // --- 扰动 J4 ---
    gc_Matrix4x4 T4_p = gc_get_T4(joints[3] + delta);
    gc_Matrix4x4 T1234_p4 = fast_mat_mult(&T123, &T4_p);
    gc_Matrix4x4 T12345_p4 = fast_mat_mult(&T1234_p4, &T5);
    gc_Matrix4x4 Tend_p4 = fast_mat_mult(&T12345_p4, &T6);
    torques[3] = (W4*(get_link_Z(&T1234_p4,c4)-z4) + W5*(get_link_Z(&T12345_p4,c5)-z5) + W6*(get_link_Z(&Tend_p4,c6)-z6)) * inv_delta;

    // --- 扰动 J3 ---
    gc_Matrix4x4 T3_p = gc_get_T3(joints[2] + delta);
    gc_Matrix4x4 T123_p3 = fast_mat_mult(&T12, &T3_p);
    gc_Matrix4x4 T1234_p3 = fast_mat_mult(&T123_p3, &T4);
    gc_Matrix4x4 T12345_p3 = fast_mat_mult(&T1234_p3, &T5);
    gc_Matrix4x4 Tend_p3 = fast_mat_mult(&T12345_p3, &T6);
    torques[2] = (W3*(get_link_Z(&T123_p3,c3)-z3) + W4*(get_link_Z(&T1234_p3,c4)-z4) +
                  W5*(get_link_Z(&T12345_p3,c5)-z5) + W6*(get_link_Z(&Tend_p3,c6)-z6)) * inv_delta;

    // --- 扰动 J2 ---
    gc_Matrix4x4 T2_p = gc_get_T2(joints[1] + delta);
    gc_Matrix4x4 T12_p2 = fast_mat_mult(&T1, &T2_p);
    gc_Matrix4x4 T123_p2 = fast_mat_mult(&T12_p2, &T3);
    gc_Matrix4x4 T1234_p2 = fast_mat_mult(&T123_p2, &T4);
    gc_Matrix4x4 T12345_p2 = fast_mat_mult(&T1234_p2, &T5);
    gc_Matrix4x4 Tend_p2 = fast_mat_mult(&T12345_p2, &T6);
    torques[1] = (W2*(get_link_Z(&T12_p2,c2)-z2) + W3*(get_link_Z(&T123_p2,c3)-z3) + W4*(get_link_Z(&T1234_p2,c4)-z4) +
                  W5*(get_link_Z(&T12345_p2,c5)-z5) + W6*(get_link_Z(&Tend_p2,c6)-z6)) * inv_delta;

    // --- 扰动 J1 ---
    gc_Matrix4x4 T1_p = gc_get_T1(joints[0] + delta);
    gc_Matrix4x4 T12_p1 = fast_mat_mult(&T1_p, &T2);
    gc_Matrix4x4 T123_p1 = fast_mat_mult(&T12_p1, &T3);
    gc_Matrix4x4 T1234_p1 = fast_mat_mult(&T123_p1, &T4);
    gc_Matrix4x4 T12345_p1 = fast_mat_mult(&T1234_p1, &T5);
    gc_Matrix4x4 Tend_p1 = fast_mat_mult(&T12345_p1, &T6);
    torques[0] = (W1*(get_link_Z(&T1_p,c1)-z1) + W2*(get_link_Z(&T12_p1,c2)-z2) + W3*(get_link_Z(&T123_p1,c3)-z3) +
                  W4*(get_link_Z(&T1234_p1,c4)-z4) + W5*(get_link_Z(&T12345_p1,c5)-z5) + W6*(get_link_Z(&Tend_p1,c6)-z6)) * inv_delta;
}

/* ================================================================= *
 * 核心钩子 & API
 * ================================================================= */
void gravity_comp_enable(void)  { g_enabled = 1; }
void gravity_comp_disable(void) { g_enabled = 0; }
uint8_t gravity_comp_is_enabled(void) { return g_enabled; }

void gravity_comp_loop_hook_500hz(void)
{
    if (!g_enabled) return;

    /* ---- 1. 物理真实位置映射为数学运动学角度 ---- */
    float math_joints[6];
    math_joints[0] = (motor[3].para.pos * J1_DIR) + J1_HW_OFFSET + g_arm_cali.j1_bias;
    math_joints[1] = (motor[4].para.pos * J2_DIR) + J2_HW_OFFSET + g_arm_cali.j2_bias;
    math_joints[2] = (motor[5].para.pos * J3_DIR) + J3_HW_OFFSET + g_arm_cali.j3_bias;
    math_joints[3] = (motor[2].para.pos * J4_DIR) + J4_HW_OFFSET + g_arm_cali.j4_bias;
    math_joints[4] = (motor[1].para.pos * J5_DIR) + J5_HW_OFFSET + g_arm_cali.j5_bias;
    math_joints[5] = (motor[0].para.pos * J6_DIR) + J6_HW_OFFSET + g_arm_cali.j6_bias;

    /* ---- 2. 核心极速数学引擎 ---- */
    float math_torques[6] = {0};
    gc_calc_gravity_torques(math_joints, math_torques);

    float motor_torques[7];
    motor_torques[3] = math_torques[0] * J1_DIR; // 底座
    motor_torques[4] = math_torques[1] * J2_DIR; // 大臂
    motor_torques[5] = math_torques[2] * J3_DIR; // 小臂
    motor_torques[2] = math_torques[3] * J4_DIR; // 腕1
    motor_torques[1] = math_torques[4] * J5_DIR; // 腕2
    motor_torques[0] = math_torques[5] * J6_DIR; // 腕3
    motor_torques[6] = 0.0f;                     // 夹爪


    motor_torques[2] = motor_torques[2] * 0.70f;  // ID 3 削弱到 70% (可微调)
    motor_torques[1] = motor_torques[1] * 0.68f;  // ID 2 削弱到 70% (可微调)
    /* ---- 3. 全局下发零刚度纯力矩指令 (扩容至 7 轴) ---- */
    for (int i = 0; i < 7; i++)
    {
        
        float real_vel = motor[i].para.vel;
        float fric_tor = 0.0f;
        float deadband = 0.01f;
        float ramp_width = 0.05f;

        if (real_vel > deadband) {
            float ratio = (real_vel - deadband) / ramp_width;
            fric_tor = fric_pos[i] * (ratio > 1.0f ? 1.0f : ratio);
        } else if (real_vel < -deadband) {
            float ratio = (-real_vel - deadband) / ramp_width;
            fric_tor = fric_neg[i] * (ratio > 1.0f ? 1.0f : ratio);
        }

        if (motor_torques[i] > 8.0f) motor_torques[i] = 8.0f;
        if (motor_torques[i] < -8.0f) motor_torques[i] = -8.0f;

        motor[i].cmd.pos_set = motor[i].para.pos;
        motor[i].cmd.vel_set = 0.0f;
        motor[i].cmd.kp_set  = 0.0f;
        motor[i].cmd.kd_set  = KD_SMOOTH;
        motor[i].cmd.tor_set = motor_torques[i] + fric_tor;
    }
}

/* ================================================================= *
 * FinSH 交互命令
 * ================================================================= */
static void gravity_start(int argc, char **argv)
{
    rt_kprintf("\n+------------------------------------------------+\n");
    rt_kprintf("|  全局零刚度纯力矩补偿 V11.0 (极速引擎重构版)   |\n");
    rt_kprintf("|  [+] 算力消耗骤降 80%%，MCU 运行极度从容      |\n");
    rt_kprintf("|  [+] 7 轴协同解锁，J5 大臂智能分段补偿         |\n");
    rt_kprintf("+------------------------------------------------+\n");

    gravity_comp_enable();
}
MSH_CMD_EXPORT(gravity_start, Enable 7DOF Zero-Stiffness Gravity Comp);

static void gravity_stop(int argc, char **argv)
{
    if (!gravity_comp_is_enabled()) return;
    gravity_comp_disable();

    extern quintic_traj_t arm_trajs[7];

    float math_joints[6];
    math_joints[0] = (motor[3].para.pos * J1_DIR) + J1_HW_OFFSET + g_arm_cali.j1_bias;
    math_joints[1] = (motor[4].para.pos * J2_DIR) + J2_HW_OFFSET + g_arm_cali.j2_bias;
    math_joints[2] = (motor[5].para.pos * J3_DIR) + J3_HW_OFFSET + g_arm_cali.j3_bias;
    math_joints[3] = (motor[2].para.pos * J4_DIR) + J4_HW_OFFSET + g_arm_cali.j4_bias;
    math_joints[4] = (motor[1].para.pos * J5_DIR) + J5_HW_OFFSET + g_arm_cali.j5_bias;
    math_joints[5] = (motor[0].para.pos * J6_DIR) + J6_HW_OFFSET + g_arm_cali.j6_bias;

    float math_torques[6] = {0};
    gc_calc_gravity_torques(math_joints, math_torques);

    float lock_torques[7];
    lock_torques[3] = math_torques[0] * J1_DIR;
    lock_torques[4] = math_torques[1] * J2_DIR;
    lock_torques[5] = math_torques[2] * J3_DIR;
    lock_torques[2] = math_torques[3] * J4_DIR;
    lock_torques[1] = math_torques[4] * J5_DIR;
    lock_torques[0] = math_torques[5] * J6_DIR;
    lock_torques[6] = 0.0f;

    for (int i = 0; i < 7; i++) {
        arm_trajs[i].start_pos  = motor[i].para.pos;
        arm_trajs[i].target_pos = motor[i].para.pos;
        arm_trajs[i].is_running = 0;

        motor[i].cmd.pos_set = motor[i].para.pos;
        motor[i].cmd.vel_set = 0.0f;

        if (i >= 3 && i <= 5) {
            motor[i].cmd.kp_set  = 300.0f;
            motor[i].cmd.kd_set  = 2.2f;
        } else if (i < 3) {
            motor[i].cmd.kp_set  = 65.0f;
            motor[i].cmd.kd_set  = 0.7f;
        }

        if (i == 6) {
            motor[i].cmd.kp_set  = 50.0f;
            motor[i].cmd.kd_set  = 0.5f;
        }

        motor[i].cmd.tor_set = lock_torques[i];
    }
    printf("\n[GRAVITY] 补偿已关闭，全轴(含夹爪)恢复正确的刚度锁定。\n");
}
MSH_CMD_EXPORT(gravity_stop, Disable gravity compensation);

static void tune_mass(int argc, char **argv)
{
    if (argc != 3) {
        printf("用法: tune_mass <电机ID> <百分比>\n");
        printf("例如: tune_mass 5 120  (代表将ID为5的大臂重量调大到 120%%)\n");
        return;
    }

    int motor_id = atoi(argv[1]);
    int percent = atoi(argv[2]);
    int index = -1;

    switch(motor_id) {
        case 4: index = 0; break;
        case 5: index = 1; break;
        case 6: index = 2; break;
        case 3: index = 3; break;
        case 2: index = 4; break;
        case 1: index = 5; break;
        default: index = -1; break;
    }

    if (index != -1) {
        g_mass_tuner[index] = (float)percent / 100.0f;
        int ratio_int = percent / 100;
        int ratio_frac = percent % 100;
        printf("[成功] 电机 ID %d (动力学连杆索引 %d) 物理补偿比例已设为: %d.%02d 倍\n",
               motor_id, index, ratio_int, ratio_frac);
    } else {
        printf("[错误] 找不到该电机 ID！请输入本体电机 ID (1, 2, 3, 4, 5, 6)\n");
    }
}
MSH_CMD_EXPORT(tune_mass, Tune gravity mass tuner dynamically);