尝试对MoveIt2的轨迹进行插值控制舵机机械手

1.现状分析

机械手在运动时，要尽可能平稳、少冲击，显得丝滑。
Moveit给到的轨迹，除了描述了各个关键时刻的各个关节的位置，还描述了此时的各个关节的速度、加速度。假如我们的执行机构可以完全按照这些位置、速度、加速度来进行操作，理论上就可以实现平滑的移动。
但是由于我们目前用的是舵机，除了位置可控外，速度、加速度都不能直接控制。
所以，只能通过插值来插补两个位置之间的各个小位置，让运动大概实现【加速-匀速-减速 】的过程。
舵机的转动速度会因为负载的不同而不同，不能够简单认为是一个固定值。因此，想要间接通过设置定位的方式来设定速度，可行性也不太。
假如我们让舵机从位置a走到位置b，等待时间为T1。舵机实际从a走到b的时间为T2。
那么只有T1大于等于T2才能确保舵机运动到位。

2.委曲求全

由于测算舵机实际的运算速度太麻烦了。
我们先进行固定时间步进的方式算了。
从ros上可以收到各个点位的信息，以及时间戳。因此可以根据时间戳，把位置均分一下。然后再发送给下位机。
实现的代码如下，仅供参考

void MainWindow::processGoal(std::shared_ptr<const FollowJointTrajectory::Goal> goal)
{
    // 该轨迹分成若干个点位（每个点位都包括了此点位下，若干关节的角度信息）
    int pointSize = goal->trajectory.points.size();
    if(pointSize <= 0)
    {
        return;
    }

    // 关于time_from_start，
    // 第一个点位的time_from_start为0,后面的每个时间都是与第一个位置时间做的偏移。
    // 直观地理解，该轨迹要求在各个时间点，机械手要在要求的位置
    // 我们先人为地规定，timeStep(ms)更新一次位置给下位机

    int timeStep = 50;         // 更新频率

    // 先不进行加速、匀速、减速的操作。现在直接均分。
    // 目前是按照时间来插值

    // 存放移动数据的队列
    static QQueue<QList<float>> angleQueue;
    angleQueue.clear();

    QList<float> angleList;
    for(int i = 0; i < pointSize; i++)
    {
        // 当前点位
        auto curPoint = goal->trajectory.points.at(i);

        if(i == 0) // 第一个是起始位置，直接放进去进行
        {
            angleList.clear();
            foreach(auto pos, curPoint.positions)
            {
                angleList << qRadiansToDegrees(pos);
            }

            angleQueue << angleList;

            continue;
        }

        auto lastPoint = goal->trajectory.points.at(i - 1);

        auto last_time = lastPoint.time_from_start;
        auto current_time = curPoint.time_from_start;

        rclcpp::Time time1(last_time.sec, last_time.nanosec);
        rclcpp::Time time2(current_time.sec, current_time.nanosec);
        auto duration_time = time2 - time1;

        // 从上一个点位跑到此处计划耗时
        qint64 duration_ms = duration_time.nanoseconds() / 1e6;

        qDebug() << "[sec:" << current_time.sec << ", nanosec:" << current_time.nanosec << "]" << duration_time.seconds() << duration_time.nanoseconds() ;
        qDebug() << "duration ms:" << duration_ms;

        if(duration_ms < timeStep) // 小于时间步进，无需插值
        {
            angleList.clear();
            foreach(auto pos, lastPoint.positions)
            {
                angleList << qRadiansToDegrees(pos);
            }

            angleQueue << angleList;
        }
        else // 需要插值
        {
            // 需要分几步进行插值
            float divCount = duration_ms / timeStep;
            if(duration_ms % timeStep)
            {
                divCount++;
            }

            // 插值步进
            QList<float> stepList;
            for(int idx = 0; idx < lastPoint.positions.size(); idx++)
            {
                stepList << (curPoint.positions[idx] - lastPoint.positions[idx]) / divCount;
            }

            // 均分插值
            for(int divIdx = 0; divIdx < divCount; divIdx++)
            {
                angleList.clear();
                for(int idx = 0; idx < lastPoint.positions.size(); idx++)
                {
                    angleList << qRadiansToDegrees(lastPoint.positions[idx] + stepList[idx] * (divIdx + 1));
                }

                angleQueue << angleList;
            }

        }

    }

    qDebug() << angleQueue;

    // 算出了整个移动队列，通过定时器定时发送该数据给单片机
    //（也可以把整个移动数据发送至单片机，然后单片机再在自己内部慢慢跑）
    QTimer *timer = new QTimer();
    timer->setInterval(timeStep);
    connect(timer, &QTimer::timeout, [=](){
        if(angleQueue.length() == 0)
        {
            timer->stop();
            timer->deleteLater();

            return;
        }
        QList<float> angleList = angleQueue.dequeue();

        // 转动方向修正(根据实际情况)
        angleList[0] = -angleList[0];
        angleList[1] = -angleList[1];
        angleList[3] = -angleList[3];

        sendPose(angleList);
    });
    timer->start();

}

3.实际效果

不好。还是晃得厉害，有时候甚至还不如原来的不插值的。下面视频的是用原来的没有插值的。
【Ros2中使用MoveIt进行虚拟避障】

---

参考资料
http://www.51hei.com/mcu/2548.html
https://www.elecfans.com/d/608940.html