机械臂正逆动力学c++实现，计算逻辑。

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

// 定义 DH 参数结构体
struct DHParam {
    double a;  // 连杆长度
    double alpha;  // 连杆扭转角
    double d;  // 连杆偏移
    double theta;  // 关节角
};

// 机械臂的 DH 参数
const DHParam dhParams[6] = {
    {1.0, 0.0, 0.0, 0.0},
    {1.0, -M_PI_2, 0.0, 0.0},
    {1.0, 0.0, 0.0, 0.0},
    {1.0, -M_PI_2, 0.0, 0.0},
    {1.0, 0.0, 0.0, 0.0},
    {1.0, -M_PI_2, 0.0, 0.0}
};

// 机械臂关节结构体
struct Joint {
    double angle;
    double velocity;
    double acceleration;
};

// 计算变换矩阵
Eigen::Matrix4d transformationMatrix(const DHParam& dh) {
    // 基于 DH 参数计算 4x4 的变换矩阵
    Eigen::Matrix4d T;
    T << std::cos(dh.theta), -std::sin(dh.theta) * std::cos(dh.alpha), std::sin(dh.theta) * std::sin(dh.alpha), dh.a * std::cos(dh.theta),
         std::sin(dh.theta), std::cos(dh.theta) * std::cos(dh.alpha), -std::cos(dh.theta) * std::sin(dh.alpha), dh.a * std::sin(dh.theta),
         0, std::sin(dh.alpha), std::cos(dh.alpha), dh.d,
         0, 0, 0, 1;
    return T;
}

// 更精确的惯性矩阵计算，考虑连杆的详细质量分布
Eigen::Matrix<double, 6, 6> computeInertiaMatrix(const std::vector<Joint>& joints) {
    Eigen::Matrix<double, 6, 6> I;
    I.setZero();

    // 基于详细的连杆模型计算惯性矩阵
    for (int i = 0; i < 6; ++i) {
        // 计算连杆的位姿变换矩阵
        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
        for (int j = 0; j <= i; ++j) {
            T = T * transformationMatrix(dhParams[j]);
        }

        // 假设使用有限元分析获取的更精确的惯性张量
        Eigen::Matrix3d preciseInertiaTensor; 
        // 在此处填充获取精确惯性张量的逻辑

        // 将惯性张量转换到全局坐标系
        Eigen::Matrix3d globalInertia = T.block<3, 3>(0, 0) * preciseInertiaTensor * T.block<3, 3>(0, 0).transpose();

        // 计算惯性矩阵的对角元素
        I(i, i) = linkMasses[i] * (T(0, 3) * T(0, 3) + T(1, 3) * T(1, 3) + T(2, 3) * T(2, 3)) + globalInertia(0, 0) + globalInertia(1, 1) + globalInertia(2, 2);
    }

    return I;
}

// 考虑非线性摩擦的科氏力和离心力矩阵计算
Eigen::Matrix<double, 6, 6> computeCoriolisCentrifugalMatrix(const std::vector<Joint>& joints) {
    Eigen::Matrix<double, 6, 6> C;
    C.setZero();

    // 基于 DH 参数和关节状态计算科氏力和离心力矩阵
    for (int i = 0; i < 6; ++i) {
        for (int j = 0; j < 6; ++j) {
            double cij = 0.0;
            if (i < j) {
                // 计算中间变量
                Eigen::Matrix4d T_i = Eigen::Matrix4d::Identity();
                Eigen::Matrix4d T_j = Eigen::Matrix4d::Identity();

                for (int k = 0; k <= i; ++k) {
                    T_i = T_i * transformationMatrix(dhParams[k]);
                }

                for (int k = 0; k <= j; ++k) {
                    T_j = T_j * transformationMatrix(dhParams[k]);
                }

                // 计算科氏力和离心力系数
                double h_ij = 0.5 * (linkInertiaTensors[i](1, 1) + linkInertiaTensors[i](2, 2) - linkInertiaTensors[i](0, 0));

                // 考虑非线性摩擦
                double frictionTerm = calculateFrictionTerm(i, joints[i].velocity); 

                // 计算关节速度的乘积
                double vij = joints[j].velocity * joints[i].velocity;

                cij = h_ij * vij + frictionTerm;
            }
            C(i, j) = cij;
            C(j, i) = -cij;
        }
    }

    return C;
}

// 计算非线性摩擦项
double calculateFrictionTerm(int jointIndex, double velocity) {
    // 假设使用简单的库仑 + 粘性摩擦模型
    double coulombFriction = 1.0;  // 库仑摩擦力常量
    double viscousFriction = 0.5;  // 粘性摩擦系数

    if (velocity > 0) {
        return coulombFriction + viscousFriction * velocity;
    } else if (velocity < 0) {
        return -coulombFriction + viscousFriction * velocity;
    } else {
        return std::abs(coulombFriction);
    }
}

// 重力向量
Eigen::Vector3d gravity = Eigen::Vector3d(0.0, 0.0, -9.81);

// 正动力学计算
void forwardDynamics(const std::vector<Joint>& joints, Eigen::Vector3d& endEffectorForce, Eigen::Vector3d& endEffectorTorque) {
    Eigen::Matrix<double, 6, 6> I = computeInertiaMatrix(joints);
    Eigen::Matrix<double, 6, 6> C = computeCoriolisCentrifugalMatrix(joints);

    Eigen::Vector<double, 6> jointAccelerations;
    Eigen::Vector<double, 6> jointForces;

    // 计算关节力
    jointForces = I * Eigen::Map<const Eigen::Vector<double, 6>>(joints[0].acceleration, 6) + C * Eigen::Map<const Eigen::Vector<double, 6>>(joints[0].velocity, 6) + gravity;

    // 提取末端执行器的力和扭矩
    endEffectorForce = Eigen::Vector3d(jointForces(0), jointForces(1), jointForces(2));
    endEffectorTorque = Eigen::Vector3d(jointForces(3), jointForces(4), jointForces(5));
}

// 计算雅可比矩阵
Eigen::Matrix<double, 6, 6> computeJacobian(const std::vector<Joint>& joints) {
    Eigen::Matrix<double, 6, 6> J;
    J.setZero();

    // 基于 DH 参数和关节角度计算雅可比矩阵
    for (int i = 0; i < 6; ++i) {
        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
        for (int j = 0; j <= i; ++j) {
            T = T * transformationMatrix(dhParams[j]);
        }

        // 计算位置向量 p
        Eigen::Vector3d p(T(0, 3), T(1, 3), T(2, 3));

        // 计算 z 轴单位向量 z
        Eigen::Vector3d z(T(0, 2), T(1, 2), T(2, 2));

        // 计算雅可比矩阵的列
        J.block<3, 1>(0, i) = z.cross(p);
        J.block<3, 1>(3, i) = z;
    }

    return J;
}

// 逆动力学计算
void inverseDynamics(const std::vector<Joint>& joints, const Eigen::Vector3d& endEffectorForce, const Eigen::Vector3d& endEffectorTorque, std::vector<double>& jointTorques) {
    Eigen::Matrix<double, 6, 6> J = computeJacobian(joints);

    Eigen::Vector<double, 6> tau;
    tau = J.transpose() * Eigen::Vector<double, 6>{endEffectorForce(0), endEffectorForce(1), endEffectorForce(2), endEffectorTorque(0), endEffectorTorque(1), endEffectorTorque(2)};
    for (int i = 0; i < 6; ++i) {
        jointTorques.push_back(tau(i));
    }
}

int main() {
    std::vector<Joint> joints(6);
    // 初始化关节状态

    Eigen::Vector3d endEffectorForce, endEffectorTorque;
    // 设定末端执行器的力和扭矩

    std::vector<double> jointTorques;

    forwardDynamics(joints, endEffectorForce, endEffectorTorque);
    inverseDynamics(joints, endEffectorForce, endEffectorTorque, jointTorques);

    return 0;
}