最近在复现一个多路点，利用Moveit（python）下的笛卡尔运动规划进行UR5机械臂的控制实验。

在多路点运动规划时，可以进行多点规划，但是在执行时，会出现报错的情况，总体来看是可以规划，但是不能执行。

报错情况下：

在 roslaunch ur_gazebo ur5.launch limited:=true 运行的终端下

在roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true limited:=true终端下

在执行笛卡尔运动的节点下

总结来看，主要报了以下错误：

error: Trajectory message contains waypoints that are not strictly increasing in time
#####
[ WARN] [1610200121.539126017, 53.226000000]: Controller  failed with error code INVALID_GOAL
[ WARN] [1610200121.539203351, 53.226000000]: Controller handle  reports status FAILED
###############
[ INFO] [1610200121.259221852, 52.946000000]: ABORTED: Solution found but controller failed during execution

发现这个问题的一个原因是容忍度太小，但是修改后并不管用

https://get-help.robotigniteacademy.com/t/project-aborted-solution-found-but-controller-failed-during-execution/4644

错误原因：

在Move Group Interface接口中，他们早已添加了初始路点用于可视化。
我们只需要直接向笛卡尔规划的路径中添加一系列的末端路点。初始状态并不需要被添加，它只是用于可视化部分。

Not adding start point into waypoints solved this problem for me. In the Move Group Interface Tutorial they’ve added start point for visualisation:

You can plan a cartesian path directly by specifying a list of waypoints for the end-effector to go through. Note that we are starting from the new start state above. The initial pose (start state) does not need to be added to the waypoint list but adding it can help with visualizations。

解决方案：

在笛卡尔路径规划中，我们只需要直接添加一系列的路点。

根据参考网址：http://docs.ros.org/en/indigo/api/pr2_moveit_tutorials/html/planning/scripts/doc/move_group_python_interface_tutorial.html 中的Cartesian Paths

waypoints = []

# start with the current pose
waypoints.append(group.get_current_pose().pose)

# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))

# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))

# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))

(plan3, fraction) = group.compute_cartesian_path(
                             waypoints,   # waypoints to follow
                             0.01,        # eef_step
                             0.0)         # jump_threshold

print "============ Waiting while RVIZ displays plan3..."
group.execute(plan3, wait=True)
rospy.sleep(5)

但是依靠这样的结构，我发现它首先获取了当前状态然后添加到路点中，它是与之前错误原因相违背的，我因为使用了这种结构一直出现上面的报错。于是我去掉了初始位置的获取并添加到路点部分。，注释掉下面这条语句。

# waypoints.append(group.get_current_pose().pose)

这可以解决 Python MoveIt interface中"Trajectory message contains waypoints that are not strictly increasing in time"报错，可以实现笛卡尔空间下多路点运动规划。

This solves the “Trajectory message contains waypoints that are not strictly increasing in time” issue for me, using the Python MoveIt interface (which is definitely undocumented!).

对比视频：
添加初始路点时候，只规划，不能执行

而当去掉初始位置点时，便可以规划并执行。

参考网址：

https://answers.ros.org/question/253004/moveit-problem-error-trajectory-message-contains-waypoints-that-are-not-strictly-increasing-in-time/

总结：

好久没有这么激动开心的解决bug了，虽然花费了将近两天的时间。前前后后几乎把googel相关的网页都打开选择性的测试了仪表，但都没有解决。最后还是通过ros官网的论坛才找到答案。只能说ROS牛，开源牛。