/**
 * @function smoothPath
 */
void PathPlanner::smoothPath( int _robotId, 
                              const Eigen::VectorXi &_links, 
                              std::list<Eigen::VectorXd> &_path ) {
  
  // =========== YOUR CODE HERE ==================
  // HINT: Use whatever technique you like better, first try to shorten a path and then you can try to make it smoother
  //////RRT rrt( world, _robotId, _links, _start, stepSize );
  //;
  if(path.empty() == true || path.size() <= 1)
  {return;}
  std::list<Eigen::VectorXd>::iterator pathhead, pathend1, pathend2; 
  ////////////////////////////////////////
  //
  //  pathend2 = pathend1 + 1;
  //  pathend1 = pathhead;
  //  check whether pathend2 is reachable for pathhead
  //  if no, (if pathend1 != pathhead, clear(pathhead, pathend1)
  //            if pathend1 == pathhead, pathhead = pathhead2);
  //  if yes, pathend1 = pathend2, pathend2++;
  //
  ////////////////////////////////////////
  pathhead = pathend1 = pathend2 = path.begin();
  pathend2 ++;
  while(pathend2 != path.end())
  {
	  if(checkPathSegment(_robotId, _links, (*pathhead), (*pathend2)) == true)
	  {
		  pathend1 = pathend2;
		  pathend2 ++;
	  }
	  else
	  {
		  std::list<Eigen::VectorXd> tempPath(pathhead,pathend2);
		  if( tempPath.size() <= 1)
		  {
			  pathhead = pathend1 = pathend2;
			  pathend2 ++;
		  }
		  else
		  {
			  pathhead ++;
			  path.erase(pathhead, pathend1);

			  pathhead = pathend1 = pathend2 = path.begin();
			  pathend2 ++;
		  }
	  }
  }
  /////////////////////////////////////////////////////////
  ///
  ///
  ///
  ///
  ///
  ///
  ///
  ////////////////////////////////////////////////////////
  return;
  // ========================================	
}
示例#2
0
文件: JG_RRT.cpp 项目: ana-GT/Lucy
void JG_RRT::smoothPath( std::vector<Eigen::VectorXd> &path ) const {
    std::vector<Eigen::VectorXd>::iterator temp, config1;
    std::vector<Eigen::VectorXd>::iterator config2 = path.begin();

    while(true) {
        config1 = config2;
        config2++;
        if(config2 == path.end()) return;
        temp = config2;
        config2++;
        if(config2 == path.end()) return;

        while(checkPathSegment( *config1, *config2) ) {
            path.erase(temp);
            temp = config2;
            config2++;
            if(config2 == path.end()) return;
        }
    }
}
/**
 * @function smoothPath
 */
void PathPlanner::smoothPath( int _robotId, 
                              const Eigen::VectorXi &_links, 
                              std::list<Eigen::VectorXd> &_path ) {
  
  // =========== YOUR CODE HERE ==================
  // HINT: Use whatever technique you like better, first try to shorten a path and then you can try to make it smoother
 if(path.empty() == true || path.size() <= 1)
  {return;}
  std::list<Eigen::VectorXd>::iterator pathhead, pathend1, pathend2; 
  std::list<Eigen::VectorXd> originalCopy = _path;
  //int oLength = originalCopy.size();
  ////////////////////////////////////////
  //
  //  pathend2 = pathend1 + 1;
  //  pathend1 = pathhead;
  //  check whether pathend2 is reachable for pathhead
  //  if no, (if pathend1 != pathhead, clear(pathhead, pathend1)
  //            if pathend1 == pathhead, pathhead = pathhead2);
  //  if yes, pathend1 = pathend2, pathend2++;
  //
  ////////////////////////////////////////
  pathhead = pathend1 = pathend2 = path.begin();
  pathend2 ++;
  while(pathend2 != path.end())
  {
	  if(checkPathSegment(_robotId, _links, (*pathhead), (*pathend2)) == true)
	  {
		  pathend1 = pathend2;
		  pathend2 ++;
		  std::cout<<"Moving a step further"<<std::endl;
	  }
	  else
	  {
		  std::list<Eigen::VectorXd> tempPath(pathhead,pathend2);
		  if( tempPath.size() <= 1)
		  {
			  pathhead = pathend1 = pathend2;
			  pathend2 ++;
			  std::cout<<"all  Moving a step further"<<std::endl;
		  }
		  else
		  {
			   std::list<Eigen::VectorXd>::iterator t = pathhead;

			  t ++;
			  std::list<Eigen::VectorXd> nSeg;
			  /*
			  int n = (int)((_config2 - _config1).norm() / stepSize );
  
  for( int i = 0; i < n; i++ ) {
    Eigen::VectorXd conf = (double)(n - i)/(double)n * _config1 + (double)(i)/(double)n * _config2;
    world->getRobot(_robotId)->setDofs( conf, _links );
    if( world->checkCollision() ) {
      return false;
    }
			  */
			  int n = (int)((*(pathhead)-*(pathend1)).norm() / stepSize);
			  for(int i = 0;i < n; i++)
			  {
				  Eigen::VectorXd conf = (double)(n - i)/(double)n * (*pathhead) + (double)(i)/(double)n * (*pathend1);
				  nSeg.push_back(conf);
			  }
			  path.insert(t,nSeg.begin(),nSeg.end());
			  ///////////////////////////////////////////////////
			  path.erase(t, pathend1);

			  pathhead = pathend1 = pathend2 = path.begin();
			  pathend2 ++; 
			  std::cout<<"Path shortened!"<<std::endl;
		  } 
	  }
  }
  //int nLength = _path.size();
	std::cout << "Finished Optimizing!  Final path length: " << _path.size() << std::endl;  
  return;
  // ========================================	
}