六轴机械臂点到点控制代码实现逻辑，包括速度加速度限制、控制周期

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

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

// 计算关节角度差值
std::vector<double> calculateJointAngleDifferences(const std::vector<Joint>& startJoints, const std::vector<Joint>& endJoints) {
    std::vector<double> differences;
    for (size_t i = 0; i < startJoints.size(); ++i) {
        differences.push_back(endJoints[i].position - startJoints[i].position);
    }
    return differences;
}

// 生成线性插值的关节轨迹，考虑速度和加速度限制
std::vector<std::vector<double>> generateJointTrajectory(const std::vector<Joint>& startJoints, const std::vector<Joint>& endJoints, double controlPeriod, double timeLimit) {
    std::vector<double> differences = calculateJointAngleDifferences(startJoints, endJoints);
    std::vector<std::vector<double>> trajectory;

    double totalTime = 0.0;
    std::vector<Joint> currentJoints = startJoints;

    while (totalTime < timeLimit) {
        std::vector<double> currentPoint;

        for (size_t i = 0; i < startJoints.size(); ++i) {
            double desiredVelocity = differences[i] / timeLimit;

            // 速度限制
            if (std::abs(desiredVelocity) > startJoints[i].maxVelocity) {
                desiredVelocity = std::copysign(startJoints[i].maxVelocity, desiredVelocity);
            }

            double newAcceleration = (desiredVelocity - currentJoints[i].velocity) / controlPeriod;

            // 加速度限制
            if (std::abs(newAcceleration) > startJoints[i].maxAcceleration) {
                newAcceleration = std::copysign(startJoints[i].maxAcceleration, newAcceleration);
            }

            currentJoints[i].velocity += newAcceleration * controlPeriod;

            // 速度限制检查
            if (std::abs(currentJoints[i].velocity) > startJoints[i].maxVelocity) {
                currentJoints[i].velocity = std::copysign(startJoints[i].maxVelocity, currentJoints[i].velocity);
            }

            currentJoints[i].position += currentJoints[i].velocity * controlPeriod;

            currentPoint.push_back(currentJoints[i].position);
        }

        trajectory.push_back(currentPoint);
        totalTime += controlPeriod;
    }

    return trajectory;
}

int main() {
    std::vector<Joint> startJoints = { 
        {0.0, 0.0, 0.0, 1.0, 0.5}, 
        {0.0, 0.0, 0.0, 1.5, 0.6}, 
        {0.0, 0.0, 0.0, 0.8, 0.4}, 
        {0.0, 0.0, 0.0, 1.2, 0.5}, 
        {0.0, 0.0, 0.0, 0.9, 0.45}, 
        {0.0, 0.0, 0.0, 1.1, 0.55} 
    };
    std::vector<Joint> endJoints = { 
        {1.0, 0.0, 0.0, 1.0, 0.5}, 
        {2.0, 0.0, 0.0, 1.5, 0.6}, 
        {0.5, 0.0, 0.0, 0.8, 0.4}, 
        {1.5, 0.0, 0.0, 1.2, 0.5}, 
        {0.8, 0.0, 0.0, 0.9, 0.45}, 
        {1.2, 0.0, 0.0, 1.1, 0.55} 
    };

    double controlPeriod = 0.01;  // 控制周期
    double timeLimit = 5.0;  // 总运动时间限制

    std::vector<std::vector<double>> trajectory = generateJointTrajectory(startJoints, endJoints, controlPeriod, timeLimit);

    for (const auto& point : trajectory) {
        // 在此处可以添加实际发送关节位置到机械臂的代码
        std::cout << "Current Joint Positions: ";
        for (const auto& jointPosition : point) {
            std::cout << jointPosition << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}