RRTConnect: Motion planning start tree could not be initialized!的一种解决方案

在gazebo模拟UR5过程中，由于自己是带一定算法延迟的位姿订阅，并输入到moveit的末端位姿，但发现在第一个位姿输入后，通过moveit移动到末端位姿，但再次改变输入位姿的时候出现以下常见的错误：大致就是找不到刚开始的初始化树

自己刚开始的程序如下：

#include 
#include 
#include 

#include 

class ScanNPlan
{
public:
  ScanNPlan(ros::NodeHandle& nh)
  {
    vision_client_ = nh.serviceClient("localize_part");
  }

  void start(const std::string& base_frame)
  {
    ROS_INFO("Attempting to localize part");
    // Localize the part
    apriltags2_node::LocalizePart srv;

    srv.request.base_frame = base_frame;
    ROS_INFO_STREAM("Requesting pose in base frame: " << base_frame);
    //如果服务回调不成功
    if(!vision_client_.call(srv))
    {
      ROS_ERROR("Could not localize part");
      return ;
    }

    ROS_INFO_STREAM("part localized: " << srv.response);
    move_target = srv.response.pose;
    
    //创建一个对象，move_group（）构造函数定义计划组名称
    moveit::planning_interface::MoveGroupInterface move_group("manipulator");
    moveit::planning_interface::MoveItErrorCode success;
    moveit::planning_interface::MoveGroupInterface::Plan plan;
     // ****设置初始位置
    move_group.setStartState(*move_group.getCurrentState());
    
    //move_group.setGoalPositionTolerance(0.01);
    //move_group.setGoalOrientationTolerance(0.05);
    move_group.setPoseReferenceFrame(base_frame);
    //move_group.setStartStateToCurrentState();
    move_group.setPoseTarget(move_target); 
    //move_group.setPlannerId("RRTConnectkConfigDefault");
    move_group.setPlanningTime(10);
    //进行运动规划
    success = move_group.plan(plan);   //运动规划输出
    ROS_INFO("Visualizing plan (stateCatch pose) %s",success == moveit_msgs::MoveItErrorCodes::SUCCESS ? "SUCCESS" : "FAILED");
    if (success  == moveit_msgs::MoveItErrorCodes::SUCCESS)  
    move_group.execute(plan);
    //**//move_group.setStartState(*move_group.getCurrentState());
    //move_group.move();
  }
  
private:
  // Planning components
  ros::ServiceClient vision_client_;
  geometry_msgs::Pose move_target;
  geometry_msgs::Pose move_target1;
  std::string end_effector_link;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "apriltags2_node");
  ros::NodeHandle nh;
  ros::NodeHandle private_node_handle ("~");
  ros::AsyncSpinner async_spinner(1);
  async_spinner.start();

  ros::Rate r(1); // 1 hz,它的功能就是先设定一个频率，然后通过睡眠度过一个循环中剩下的时间，来达到该设定频率。

  while(ros::ok())
  {
    ROS_INFO("ScanNPlan node has been initialized");
    std::string base_frame;
    private_node_handle.param("base_frame", base_frame, "base_link"); // world parameter name, string object reference, default value

    ScanNPlan app(nh);
    ros::Duration(.5).sleep();  // wait for the class to initialize
    app.start(base_frame);
    //ros::waitForShutdown();
  }
  return 0;
}

可以看到我的程序在每次调用app.start(base_frame)函数的时，函数刚开始都会去设置初始位置，然后再去执行规划传入的末端位姿并执行，但是这样出现了上面在UR5移动一次后，便会报错说无法初始化树。

修改方案：是在每一次执行完后，也即move_group.execute(plan);这句语句后，加入move_group.setStartState(*move_group.getCurrentState());这句语句便可以，不再报错，也就是把注释去掉即可，我估计的原因是，每次循环时， move_target就会被释放，所以要在下次执行前把当前的位姿取出，赋予指针，从而可以识别到移动后的位姿作为当前位姿。
经测试，可以实现ur5根据订阅连续的位姿，而不会出现报错的情况。

---

另一种出现：RRTConnect: Motion planning start tree could not be initialized!
在之前实际操作UR5时，刚开始便会出现无法初始化的情况，然后我在gazebo下却不会报错，所以我觉得跟刚开始的位姿，于是我通过示教器把UR5移动到和gazebo一样的位姿，也就是所以六个关节都是0的位姿，发现刚开始便可以移动机械手不会报错。
原因分析：可能刚开始moveit的初始化位姿是六个关节都是0，所以当我们实际的机械手离这个位姿偏差较大时便会出现这种报错。

---

今天在调试实际UR5机械手时，同样出现了初始化树失败，经判断是由于本身的table关节和UR5接触，而触发了碰撞，这时我们只要在moveit配置出来的ur5.srdf添加这两者的碰撞为非碰撞检测条件。我的文件位置是在/home/comeon-harry（用户名）/at_ur5（工作空间）/src/universal_robot/ur5_moveit_config/config下。
具体如下：

可以看到"shoulder_link"与"table" 关节，触发了碰撞检测，导致初始化树失败。
只需要在ur5.srdf的后面添加以下语句：

从而达到忽略此接触，最后发现不会报错，可以进行规划。

声明：以上都是个人实践过程分析的观点，不一定对，请辩证看待。