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);
        }
Beispiel #2
0
    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);
        }