Beispiel #1
0
  EReturn OMPLsolver::convertPath(const ompl::geometric::PathGeometric &pg,
      Eigen::MatrixXd & traj)
  {
    traj.resize(pg.getStateCount(), state_space_->getDimension());
    Eigen::VectorXd tmp(state_space_->getDimension());

    for (int i = 0; i < (int) pg.getStateCount(); ++i)
    {
      if (!ok(
          compound_ ?
              boost::static_pointer_cast<OMPLSE3RNCompoundStateSpace>(
                  state_space_)->OMPLStateToEigen(pg.getState(i), tmp) :
              boost::static_pointer_cast<OMPLStateSpace>(state_space_)->copyFromOMPLState(
                  pg.getState(i), tmp)))
      {
        ERROR("Can't copy state "<<i);
        return FAILURE;
      }
      else
      {
        traj.row(i) = tmp;
      }
    }
    return SUCCESS;
  }
Beispiel #2
0
void OmnidirectionalMotionModel::generateOpenLoopControlsForPath(const ompl::geometric::PathGeometric path, std::vector<ompl::control::Control*> &openLoopControls)
{
    for(int i=0;i<path.getStateCount()-1;i++)
    {
        std::vector<ompl::control::Control*> olc;

        this->generateOpenLoopControls(path.getState(i),path.getState(i+1),olc) ;

        openLoopControls.insert(openLoopControls.end(),olc.begin(),olc.end());
    }
}
OpenRAVE::PlannerStatus ToORTrajectory(
        OpenRAVE::RobotBasePtr const &robot,
        ompl::geometric::PathGeometric& ompl_traj,
        OpenRAVE::TrajectoryBasePtr or_traj) {
    using ompl::geometric::PathGeometric;

    size_t const num_dof = robot->GetActiveDOF();
    or_traj->Init(robot->GetActiveConfigurationSpecification("linear"));

    ompl::base::StateSpacePtr space = ompl_traj.getSpaceInformation()->getStateSpace();

    for (size_t i = 0; i < ompl_traj.getStateCount(); ++i){
        std::vector<double> values;
        space->copyToReals(values, ompl_traj.getState(i));
        or_traj->Insert(i, values, true);
    }
    return OpenRAVE::PS_HasSolution;
}
Beispiel #4
0
static void
path_cleanup( struct aa_rx_mp *mp, ompl::geometric::PathGeometric &path )
{
    amino::sgSpaceInformation::Ptr &si = mp->space_information;

    if( mp->simplify ) {
        ompl::geometric::PathSimplifier ps(si);
        int n = (int)path.getStateCount();
        path.interpolate(n*10);

        for( int i = 0; i < 10; i ++ ) {
            ps.reduceVertices(path);
            ps.collapseCloseVertices(path);
            ps.shortcutPath(path);
        }

        ps.smoothBSpline(path, 3, path.length()/100.0);
    }
}
void ompl::tools::LightningDB::addPathHelper(ompl::geometric::PathGeometric &solutionPath)
{
    // Create a new planner data instance
    ompl::base::PlannerDataPtr plannerData(new ompl::base::PlannerData(si_));

    // Add the states to one nodes files
    for (std::size_t i = 0; i < solutionPath.getStates().size(); ++i)
    {
        ompl::base::PlannerDataVertex vert( solutionPath.getStates()[i] ); // TODO tag?

        plannerData->addVertex(vert);
    }

    // Deep copy the states in the vertices so that when the planner goes out of scope, all data remains intact
    plannerData->decoupleFromPlanner();

    // Add to nearest neighbor tree
    nn_->add(plannerData);

    numUnsavedPaths_++;
}
Beispiel #6
0
bool ompl::tools::Lightning::reversePathIfNecessary(og::PathGeometric &path1, og::PathGeometric &path2)
{
    // Reverse path2 if it matches better
    const ob::State* s1 = path1.getState(0);
    const ob::State* s2 = path2.getState(0);
    const ob::State* g1 = path1.getState(path1.getStateCount()-1);
    const ob::State* g2 = path2.getState(path2.getStateCount()-1);

    double regularDistance  = si_->distance(s1, s2) + si_->distance(g1, g2);
    double reversedDistance = si_->distance(s1, g2) + si_->distance(s2, g1);

    // Check if path is reversed from normal [start->goal] direction
    if ( regularDistance > reversedDistance )
    {
        // needs to be reversed
        path2.reverse();
        return true;
    }

    return false;
}
Beispiel #7
0
void ompl::tools::Lightning::convertPlannerData(const ob::PlannerDataPtr& plannerData, og::PathGeometric &path)
{
    // Convert the planner data verticies into a vector of states
    for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
        path.append(plannerData->getVertex(i).getState());
}
Beispiel #8
0
  EReturn OMPLsolver::getSimplifiedPath(ompl::geometric::PathGeometric &pg,
      Eigen::MatrixXd & traj, ob::PlannerTerminationCondition &ptc)
  {
    if (smooth_->data)
    {
      HIGHLIGHT("Simplifying solution");
      int original_cnt = pg.getStateCount();
      ros::Time start = ros::Time::now();

      //ompl_simple_setup_->simplifySolution(d);
      // Lets do our own simplifier ~:)
      if (original_cnt >= 3)
      {
        og::PathSimplifierPtr psf_ = ompl_simple_setup_->getPathSimplifier();
        const ob::SpaceInformationPtr &si =
            ompl_simple_setup_->getSpaceInformation();

        bool tryMore = false;
        if (ptc == false) tryMore = psf_->reduceVertices(pg);
        if (ptc == false) psf_->collapseCloseVertices(pg);
        int times = 0;
        while (tryMore && ptc == false)
        {
          tryMore = psf_->reduceVertices(pg);
          times++;
        }
        if (si->getStateSpace()->isMetricSpace())
        {
          if (ptc == false)
            tryMore = psf_->shortcutPath(pg);
          else
            tryMore = false;
          times = 0;
          while (tryMore && ptc == false)
          {
            tryMore = psf_->shortcutPath(pg);
            times++;
          }
        }

        std::vector<ob::State*> &states = pg.getStates();
        //	Calculate number of states required
        unsigned int length = 0;
        const int n1 = states.size() - 1;
        for (int i = 0; i < n1; ++i)
          length += si->getStateSpace()->validSegmentCount(states[i],
              states[i + 1]);
//				//	Forward reducing
//				HIGHLIGHT("States before forward reducing: "<<pg.getStateCount());
//				pg.interpolate(length);
//
//				bool need_backward = true;
//				for (int i = states.size() - 1; i > 0; i--)
//				{
//					if (si->checkMotion(states[0], states[i]))
//					{
//						ob::State *start = si->cloneState(states[0]);
//						pg.keepAfter(states[i]);
//						pg.prepend(start);
//						need_backward = (i == states.size() - 1) ? false : true;
//						break;
//					}
//				}
//
//				//	Backward reducing
//				ob::State *mid;
//				if (need_backward)
//				{
//					HIGHLIGHT("States before backward reducing: "<<pg.getStateCount());
//					pg.interpolate(length);
//					for (int i = 1; i < states.size(); i++)
//					{
//						if (si->checkMotion(states[states.size() - 1], states[i]))
//						{
//							ob::State *goal = si->cloneState(states[states.size() - 1]);
//							pg.keepBefore(states[i]);
//							pg.append(goal);
//							mid = si->cloneState(states[i]);
//							break;
//						}
//					}
//				}

        pg.interpolate(length);
      }

//			if (ompl_simple_setup_->haveSolutionPath())
//			{
//				pg.interpolate();
//			}
      HIGHLIGHT_NAMED("OMPLSolver",
          "Simplification took "<<ros::Duration(ros::Time::now()-start).toSec()<<"sec. States: "<<original_cnt<<"->"<<pg.getStateCount());
    }
    convertPath(pg, traj);

    return SUCCESS;
  }
bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
                                                          ompl::geometric::PathGeometric &primaryPath)
{
    // \todo: we should reuse our collision checking from the previous step to make this faster

    OMPL_INFORM("LightningRetrieveRepair: Repairing path");

    // Error check
    if (primaryPath.getStateCount() < 2)
    {
        OMPL_ERROR("LightningRetrieveRepair: Cannot repair a path with less than 2 states");
        return false;
    }

    // Loop through every pair of states and make sure path is valid.
    // If not, replan between those states
    for (std::size_t toID = 1; toID < primaryPath.getStateCount(); ++toID)
    {
        std::size_t fromID = toID - 1; // this is our last known valid state
        ompl::base::State *fromState = primaryPath.getState(fromID);
        ompl::base::State *toState = primaryPath.getState(toID);

        // Check if our planner is out of time
        if (ptc == true)
        {
            OMPL_DEBUG("LightningRetrieveRepair: Repair path function interrupted because termination condition is true.");
            return false;
        }

        // Check path between states
        if (!si_->checkMotion(fromState, toState))
        {
            // Path between (from, to) states not valid, but perhaps to STATE is
            // Search until next valid STATE is found in existing path
            std::size_t subsearchID = toID;
            ompl::base::State *new_to;
            OMPL_DEBUG("LightningRetrieveRepair: Searching for next valid state, because state %d to %d was not valid out  %d total states",
                       fromID,toID,primaryPath.getStateCount());
            while (subsearchID < primaryPath.getStateCount())
            {
                new_to = primaryPath.getState(subsearchID);
                if (si_->isValid(new_to))
                {
                    OMPL_DEBUG("LightningRetrieveRepair: State %d was found to valid, we can now repair between states", subsearchID);
                    // This future state is valid, we can stop searching
                    toID = subsearchID;
                    toState = new_to;
                    break;
                }
                ++subsearchID; // keep searching for a new state to plan to
            }
            // Check if we ever found a next state that is valid
            if (subsearchID >= primaryPath.getStateCount())
            {
                // We never found a valid state to plan to, instead we reached the goal state and it too wasn't valid. This is bad.
                // I think this is a bug.
                OMPL_ERROR("LightningRetrieveRepair: No state was found valid in the remainder of the path. Invalid goal state. This should not happen.");
                return false;
            }

            // Plan between our two valid states
            PathGeometric newPathSegment(si_);

            // Not valid motion, replan
            OMPL_DEBUG("LightningRetrieveRepair: Planning from %d to %d", fromID, toID);

            if (!replan(fromState, toState, newPathSegment, ptc))
            {
                OMPL_INFORM("LightningRetrieveRepair: Unable to repair path between state %d and %d", fromID, toID);
                return false;
            }

            // TODO make sure not approximate solution

            // Reference to the path
            std::vector<base::State*> &primaryPathStates = primaryPath.getStates();


            // Remove all invalid states between (fromID, toID) - not including those states themselves
            while (fromID != toID - 1)
            {
                OMPL_INFORM("LightningRetrieveRepair: Deleting state %d", fromID + 1);
                primaryPathStates.erase(primaryPathStates.begin() + fromID + 1);
                --toID; // because vector has shrunk
                OMPL_INFORM("LightningRetrieveRepair: toID is now %d", toID);
            }

            // Insert new path segment into current path
            OMPL_DEBUG("LightningRetrieveRepair: Inserting new %d states into old path. Previous length: %d",
                newPathSegment.getStateCount()-2, primaryPathStates.size());

            // Note: skip first and last states because they should be same as our start and goal state, same as `fromID` and `toID`
            for (std::size_t i = 1; i < newPathSegment.getStateCount() - 1; ++i)
            {
                std::size_t insertLocation = toID + i - 1;
                OMPL_DEBUG("LightningRetrieveRepair: Inserting newPathSegment state %d into old path at position %d",
                    i, insertLocation);
                primaryPathStates.insert(primaryPathStates.begin() + insertLocation,
                    si_->cloneState(newPathSegment.getStates()[i]) );
            }
            OMPL_DEBUG("LightningRetrieveRepair: Inserted new states into old path. New length: %d", primaryPathStates.size());

            // Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2 because we ignore start and goal
            toID = toID + newPathSegment.getStateCount() - 2;
            OMPL_DEBUG("LightningRetrieveRepair: Continuing searching at state %d", toID);
        }
    }

    OMPL_INFORM("LightningRetrieveRepair: Done repairing");

    return true;
}
bool ompl::LTLVis::VRVPlanner::DijkstraSearch(const ompl::base::PlannerTerminationCondition &ptc, ompl::geometric::PathGeometric &solution)
{
  //update the value we use to check that a vertex has been initialized, or visited, in the current search
  ++crVisitation_;
  WorkingVertexVector pqueue;
  pqueue.clear();
  bool reachedGoal = false;
  bool goalDisconnected = false;

  ompl::LTLVis::tIndex goalIndexReached = 0;

  WorkingVertexVector pathCandidate;

  ompl::LTLVis::tIndex maxK = vertices_.size();
  for(ompl::LTLVis::tIndex k = 0; k < maxK; k++)
  {
    if(vertices_[k].getIsStart())
    {
      vertices_[k].setNegativeDistance(0);
      vertices_[k].setLastInitAlg(crVisitation_);
      pqueue.push_back(&vertices_[k]);
    }
  }
  make_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare);

  while(pqueue.size() && (ptc == false))
  {
    VRVVertex *cVertex(pqueue.front());
    pop_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare); pqueue.pop_back();

    if(cVertex->getLastVisitation() < crVisitation_)
    {
      cVertex->setLastVisitation(crVisitation_);
      if(cVertex->getIsGoal())
      {
        reachedGoal = true;
        goalIndexReached = cVertex->getIndex();
        break;
      }
      ompl::LTLVis::tIndex kMax = cVertex->getEdgeCount();
      for(ompl::LTLVis::tIndex k = 0; k < kMax; k++)
      {
        double weight;
        ompl::LTLVis::tIndex towards;
        cVertex->getEdge(k, weight, towards);
        VRVVertex *cTarget(&vertices_[towards]);
        if(cTarget->isInteresting())
        {
          //STL heap is a max heap, so we use negative edge weights to 'convert' it to a min-heap
          if(cTarget->getLastInitAlg() < crVisitation_)
          {
            cTarget->setLastInitAlg(crVisitation_);
            cTarget->setNegativeDistance(cVertex->getNegativeDistance() - weight - cTarget->getNodeCost());
            cTarget->setPreceding(cVertex->getIndex());
            cTarget->setPrecedingEdge(k);
            pqueue.push_back(cTarget); push_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare);
          }
          else
          {
            double negativeDist = cVertex->getNegativeDistance() - weight - cTarget->getNodeCost();
            if(negativeDist > cTarget->getNegativeDistance())
            {
              cTarget->setNegativeDistance(negativeDist);
              cTarget->setPreceding(cVertex->getIndex());
              cTarget->setPrecedingEdge(k);
              pqueue.push_back(cTarget); push_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare);
            }
          }
        }
      }
    }
  }

  if(reachedGoal)
  {
    pathCandidate.clear();
    ompl::LTLVis::tIndex cIndex = goalIndexReached;
    while(!vertices_[cIndex].getIsStart())
    {
      pathCandidate.push_back(&vertices_[cIndex]);
      cIndex = vertices_[cIndex].getPreceding();
    }
    pathCandidate.push_back(&vertices_[cIndex]);
    ompl::LTLVis::tIndex kMax = pathCandidate.size();
    for(ompl::LTLVis::tIndex k = 0, kPrev = 0; (k < kMax); k++)
    {
      if(0 < k)
      {
        if(0.000001 < si_->distance(pathCandidate[kMax - 1 - k]->getStateData(), pathCandidate[kMax - 1 - kPrev]->getStateData()))
        {
          solution.append(pathCandidate[kMax - 1 - k]->getStateData());
          kPrev = k;
        }
      }
      else
      {
        solution.append(pathCandidate[kMax - 1 - k]->getStateData());
      }
    }
    return true;
  }
  return false;
}