MoveIt轨迹规划问题_将三次多项式轨迹规划方法应用于moveit

本文是求助贴，请大神帮忙，谢谢！（在古月居交流社区发帖，一周多都没人回复，请各位帮忙）
问题描述：
请看代码，pos存储三维点，posOffset存储三维点的偏差。
想通过设置一个初始点，然后根据偏差值规划一系列的点，期望机械臂末端（法兰盘中心）根据这些点运动。
目前遇到的问题：假设规划路径为A—B—C—……，A—B时，机器人会绕一整圈到达B点，并没有实现直线到达。
其实三维点描述的是S型轨迹（代码中的三维点已经被省略了），想让机械臂末端走S型曲线。
另外，四元素表示的姿态到底是怎么设置的？怎么知道它要取什么值？四元素感觉不直观。
请哪位大神帮忙解答，万分感谢！

//实现关节空间轨迹规划
#include<moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <vector>

int main(int argc, char **argv)

{
    //初始化，其中ur_test02为节点名
    ros::init(argc, argv, "ur_test01");
    //多线程
    ros::AsyncSpinner spinner(1);
    //开启新的线程
    spinner.start();

    //初始化需要使用move group控制的机械臂中的arm group
    moveit::planning_interface::MoveGroupInterface arm("manipulator");   

    //允许误差
    arm.setGoalJointTolerance(0.001);
    //允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);

    // 控制机械臂先回到水平位置，这个位置下载的文件中已经设置好了
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
    //关节空间
    std::vector<vector<double>> pos = {
{      1516.38824   24.37429249 4.945091105 }，
{      1517.288232 24.70266148 4.910326798 }，
{      1517.828202 25.1301746   4.869095553 }，
{      1518.6382     25.65316643 4.823924466 }，
{      1519.538144 26.23986586 4.779723388 }，
{      1520.43816   26.84294131 4.7401259     }，
{      1521.518101 27.43266236 4.705291704 }，
{      1522.418002 27.97653605 4.675852832 }，
{      1523.317976 28.47903696 4.648267775 }，
{      1524.038019 28.94768219 4.620115082 }，
{      1524.847979 29.38823305 4.590313158 }
};

    int n = pos.size();
    int m = pos[0].size();
    std::vector<vector<double>> posOffset(n-1,vector<double>(m,0));
    for(int i = 0; i < n-1; i++){
        posOffset[i][1] = pos[i+1][1] - pos[i][1];
        posOffset[i][2] = pos[i+1][2] - pos[i][2];
        posOffset[i][3] = pos[i+1][3] - pos[i][3];
    }
    geometry_msgs::Pose target_pose1;
    target_pose1.orientation.w = 1.0;
    target_pose1.position.x = 0.35;
    target_pose1.position.y = -0.05;
    target_pose1.position.z = 0.6;
    arm.setPoseTarget(target_pose1);

    for(int i = 0; i < n-1; i++){
        target_pose1.position.x += posOffset[i][1];
        target_pose1.position.y += posOffset[i][2];
        target_pose1.position.z += posOffset[i][3];
        arm.setPoseTarget(target_pose1);
    } 
    arm.move();

    //输出确定是否运行成功
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
     
     //控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
    ros::shutdown();
    
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83