virtual PlannerStatus PlanPath(TrajectoryBasePtr ptraj)
{
BOOST_ASSERT(!!_parameters && !!ptraj );
if( ptraj->GetNumWaypoints() < 2 ) {
return PS_Failed;
}
RobotBase::RobotStateSaverPtr statesaver;
if( !!_probot ) {
statesaver.reset(new RobotBase::RobotStateSaver(_probot));
}
uint32_t basetime = utils::GetMilliTime();
PlannerParametersConstPtr parameters = GetParameters();
// subsample trajectory and add to list
list< std::pair< vector<dReal>, dReal> > listpath;
vector<dReal> vtrajdata(parameters->GetDOF());
ptraj->GetWaypoint(0,vtrajdata,parameters->_configurationspecification);
dReal totaldist = 0;
listpath.push_back(make_pair(vtrajdata,0));
for(size_t i = 1; i < ptraj->GetNumWaypoints(); ++i) {
ptraj->GetWaypoint(i,vtrajdata,parameters->_configurationspecification);
dReal dist = parameters->_distmetricfn(listpath.back().first, vtrajdata);
if( dist > 0 ) {
listpath.push_back(make_pair(vtrajdata,dist));
totaldist += dist;
}
}
if( listpath.size() > 1 ) {
if( _bUseSingleDOFSmoothing ) {
dReal newdist1 = _OptimizePath(listpath, totaldist, parameters->_nMaxIterations*8/10);
RAVELOG_DEBUG(str(boost::format("path optimizing first stage - dist %f->%f, computation time=%fs\n")%totaldist%newdist1%(0.001f*(float)(utils::GetMilliTime()-basetime))));
uint32_t basetime2 = utils::GetMilliTime();
dReal newdist2 = _OptimizePathSingleDOF(listpath, newdist1, parameters->_nMaxIterations*2/10);
RAVELOG_DEBUG(str(boost::format("path optimizing second stage - dist %f->%f computation time=%fs\n")%newdist1%newdist2%(0.001f*(float)(utils::GetMilliTime()-basetime2))));
}
else {
dReal newdist1 = _OptimizePath(listpath, totaldist, parameters->_nMaxIterations);
RAVELOG_DEBUG(str(boost::format("path optimizing stage - dist %f->%f, computation time=%fs\n")%totaldist%newdist1%(0.001f*(float)(utils::GetMilliTime()-basetime))));
}
}
else {
// trajectory contains similar points, so at least add another point and send to the next post-processing stage
listpath.push_back(make_pair(vtrajdata,0));
}
ptraj->Init(parameters->_configurationspecification);
FOREACH(it, listpath) {
ptraj->Insert(ptraj->GetNumWaypoints(),it->first);
}
virtual PlannerStatus PlanPath(TrajectoryBasePtr ptraj)
{
BOOST_ASSERT(!!_parameters && !!ptraj );
if( ptraj->GetNumWaypoints() < 2 ) {
return PS_Failed;
}
RobotBase::RobotStateSaverPtr statesaver;
if( !!_probot ) {
statesaver.reset(new RobotBase::RobotStateSaver(_probot));
}
uint32_t basetime = utils::GetMilliTime();
PlannerParametersConstPtr parameters = GetParameters();
// subsample trajectory and add to list
list< std::pair< vector<dReal>, dReal> > listpath;
_SubsampleTrajectory(ptraj,listpath);
_OptimizePath(listpath);
ptraj->Init(parameters->_configurationspecification);
FOREACH(it, listpath) {
ptraj->Insert(ptraj->GetNumWaypoints(),it->first);
}
