解决CHOMP插件出现start state is empty，无法执行动作的问题

解决CHOMP插件出现start state is empty，无法执行动作的问题

之前把moveit运动学插件从OMPL换成了CHOMP，想着试一试轨迹能不能更优化一点，因为OMPL在OrientationConstraints和PositionConstraints存在的情况下轨迹实在是奇葩得捉鸡（CHOMP有没有更好一点，有待验证）。结果发现原来的代码在CHOMP下跑不了了。

代码如下：

  std::vector<double> home;
  home.push_back(16.45/180*pi);
  home.push_back(3.52/180*pi);
  home.push_back(110.05/180*pi);
  home.push_back(107.48/180*pi);
  home.push_back(70.01/180*pi);
  home.push_back(-90.28/180*pi);
  move_group.setJointValueTarget(home);
  move_group.move();

机器人无任何反应。打开rviz终端，发现错误信息如下：

[ERROR] [1635749227.631148088]: Start state is empty

但是这个代码片在OMPL下执行得好好的啊！！！

随后，我分析原因，OMPL中的moveit是通过sudo apt安装的，而这样的二进制moveit中没有集成CHOMP，所以为了使用CHOMP，这里的moveit是从github上clone下来通过源码安装的。应该差异就在这里。

可是，找遍谷歌、StackOverflow，没找到一个像样的解答，唯一一条带有CHOMP、Start state is empty的问题下也没有我想要的回答……

最终，我想起以前见过的网上代码中好像有这样两条看起来用不上的语句：

robot_state::RobotState start_state(*move_group.getCurrentState());
move_group.setStartState(start_state);

加上这两条语句后，问题奇迹般地解决了。

相关部分的整体代码如下：

  // 取消注释这两行，可以使问题得到解决
  // robot_state::RobotState start_state(*move_group.getCurrentState());
  // move_group.setStartState(start_state); 
  std::vector<double> home;
  home.push_back(16.45/180*pi);
  home.push_back(3.52/180*pi);
  home.push_back(110.05/180*pi);
  home.push_back(107.48/180*pi);
  home.push_back(70.01/180*pi);
  home.push_back(-90.28/180*pi);
  move_group.setJointValueTarget(home);
  move_group.move();

大功告成。