moveit 编程技巧笔记——圆弧轨迹规划+修改轨迹

1 笛卡尔空间圆弧轨迹规划
之前学习过笛卡尔空间下轨迹规划API：(plan, fraction) = arm.compute_cartesian_path
返回值：
plan：规划出来的运动轨迹
fraction：描述规划成功的轨迹在给定路径点列表的覆盖率【0~1】。如果fraction小于1，说明给定的路径点列表没有办法完整规划

如果将圆弧轨迹微分为一段段的小直线段，就能通过调用上述API构建出圆弧轨迹。

#include<math.h>
#include<ros/ros.h>
#include<moveit/move_group_interface/move_group_interface.h>
#include<moveit/robot_trajectory/robot_trajectory.h>

int main(int argc, char **argv)
{
    ros::init(argc,argv,"moveit_circle_demo");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface arm("xmate_arm");
    std::string end_effector_link = arm.getEndEffectorLink();
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

    //设置允许的最大速度和加速度
    ar