/**
* @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;
// ========================================
}
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;
// ========================================
}
