Invalid Trajectory: start point deviates from current robot state more than 0.01

笛卡尔直线规划报错：Invalid Trajectory: start point deviates from current robot state more than 0.011

问题分析

1、机械臂抖动大，导致起始位姿改变

2、move_group获取的机械臂状态不对

解决措施

1、针对第一种原因，可以采用以下解决措施
设置机械臂当前的状态作为运动初始状态

group.setStartStateToCurrentState();

放大误差允许范围

group.setGoalPositionTolerance(0.001);//单位是米
group.setGoalOrientationTolerance(0.02);//单位是弧度

2、针对第二种原因，可以采用以下解决措施
检查下发布节点

rqt_graph

如果发现除了gazebo发布机械臂状态外，还有另一个节点/joint_state_publisher发布机械臂状态，则具体原因是出现在move_group接收了多个节点发布的机械臂状态
很明显，解决措施就是找到启动/joint_state_publisher节点的luanch文件，将该启动节点命令注释掉

最后附上一些无关但有用的知识：

机器人总是处于驱动所认为的相同位置。或者几乎是，机器人以125hz的频率向驱动发送它的位置，所以驱动对机器人位置的感知可能会有多达8毫秒的延迟，这取决于你向驱动询问信息的时间。当驱动程序从机器人接收到该信息时，它将该信息发布到/joint_states

计算轨迹时遵循的步骤：
①计算目标的IK
②构建轨迹等待/joint_states
③等待/joint_states的消息
④根据/joint_state的新值更新起始位置
⑤将轨迹发送给驱动程序