20161222note

文件功能记录

1、RRTconnect  

规划算法逻辑实现

2、Model_based_planning_context.cpp

moveit层调用ompl层操作：

（1）interpolateSolution   插入圆滑值

（2）simplifySolution      简化轨迹？

（3）solve

3、SimpleSetup.h

ompl层接口：

对规划数据的操作：规划器、起始状态、目标状态、获取轨迹

（1）setStartState

（2）setGoalState

（3）setPlanner

（4）getSolutionPath

4、ProblemDefinition.cpp

===========================================================================================

group的值就存在这里，这好像也是RRTconnect的规划状态

state->as<StateType>()->values

ompl::base::State *state

printf("copyToOMPLState position:");

  for(int i=0; i<10; i++)

  {

printf("%f ", state->as<StateType>()->values[i]);

  }

  printf("\n");

拷贝关节值到ompl

void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State *state, const robot_state::RobotState &rstate) const

0.728120  0.257358  0.537034  1.352581  -0.012596  1.070373  0.062254  0.571360  1.329300  0.945363