问题描述：

请看代码，pos存储三维点，posOffset存储三维点的偏差。

想通过设置一个初始点，然后根据偏差值规划一系列的点，期望机械臂末端（法兰盘中心）根据这些点运动。

 

目前遇到的问题：假设规划路径为A---B---C---……，A---B时，机器人会绕一整圈到达B点，并没有实现直线到达。

 

请哪位大神帮忙分析，万分谢谢！

//实现关节空间轨迹规划

#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);

    //控制机械臂到达竖直位置，这个位置下载的文件中也设置好了

   // arm.setNamedTarget("up");

   // 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;

}