/**
 * @function planSingleRRT
 * @brief Finds a plan using a standard RRT
 */
bool PathPlanner::planSingleTreeRrt( int _robotId, 
                                     const Eigen::VectorXi &_links, 
                                     const Eigen::VectorXd &_start, 
                                     const Eigen::VectorXd &_goal, 
                                     bool _connect, 
                                     bool _greedy,
                                     unsigned int _maxNodes ) {
  
  RRT rrt( world, _robotId, _links, _start, stepSize );
  RRT::StepResult result = RRT::STEP_PROGRESS;

  double smallestGap = DBL_MAX;
  const double greedyProbabilityThreshold = 0.50;
  while ( result != RRT::STEP_REACHED && smallestGap > stepSize )
  {
    if( _greedy )/** greedy section */
    {
      double randValue = (double)rand() / ((double)RAND_MAX);
      if( _connect )/** greedy and connect */
      {
        randValue>greedyProbabilityThreshold?rrt.connect(_goal):rrt.connect();
    	}
      else /** greedy and NO connect */
      {
        randValue>greedyProbabilityThreshold?rrt.tryStep(_goal):rrt.tryStep();
      }
    }
    else/** NO greedy section */
    
    {
      if( _connect )/** NO greedy and Connect */
      {
      	rrt.connect();
	    	
      }
      else/** No greedy and No connect -- PLAIN RRT */
      {
      	rrt.tryStep();
      }
    }
    if( _maxNodes > 0 && rrt.getSize() > _maxNodes ) {
      printf("--(!) Exceeded maximum of %d nodes. No path found (!)--\n", _maxNodes );
      return false;
    }
    
    double gap = rrt.getGap( _goal );
    if( gap < smallestGap ) {
      smallestGap = gap;
      std::cout << "--> [planner] Gap: " << smallestGap << "  Tree size: " << rrt.configVector.size() << std::endl;
    }
  } // End of while
  
    /// Save path  
  printf(" --> Reached goal! : Gap: %.3f \n", rrt.getGap( _goal ) );
  rrt.tracePath( rrt.activeNode, path, false );
  
  return true;
}
Exemple #2
0
void GoTo::execute(Driver & driver){

	Position2d * robotPos = (VideoServer::instance().data())[driver.getModelName()];

	RRT2 rrt(robotName);
	Vector2d tmp;
	rrt.plan(this->dest.pos, tmp);
	Position2d tmpPos;
	tmpPos.pos = tmp;
	tmpPos.rot = this->dest.rot;

	driver.goToPosition(&tmpPos);
}
double LRSDP::EvaluateConstraint(const size_t index,
                                 const arma::mat& coordinates) const
{
  arma::mat rrt = coordinates * trans(coordinates);
  if (aModes[index] == 0)
    return trace(a[index] * rrt) - b[index];
  else
  {
    double value = -b[index];
    for (size_t i = 0; i < a[index].n_cols; ++i)
      value += a[index](2, i) * rrt(a[index](0, i), a[index](1, i));

    return value;
  }
}
void AugLagrangianFunction<LRSDP>::Gradient(const arma::mat& coordinates,
                                            arma::mat& gradient) const
{
  // We can calculate the gradient in a smart way.
  // L'(R, y, s) = 2 * S' * R
  //   with
  // S' = C - sum_{i = 1}^{m} y'_i A_i
  // y'_i = y_i - sigma * (Trace(A_i * (R R^T)) - b_i)
  arma::mat rrt = coordinates * trans(coordinates);
  arma::mat s = function.C();

  for (size_t i = 0; i < function.B().n_elem; ++i)
  {
    double constraint = -function.B()[i];

    if (function.AModes()[i] == 0)
    {
      constraint += trace(function.A()[i] * rrt);
    }
    else
    {
      for (size_t j = 0; j < function.A()[i].n_cols; ++j)
      {
        constraint += function.A()[i](2, j) *
            rrt(function.A()[i](0, j), function.A()[i](1, j));
      }
    }

    double y = lambda[i] - sigma * constraint;

    if (function.AModes()[i] == 0)
    {
      s -= (y * function.A()[i]);
    }
    else
    {
      // We only need to subtract the entries which could be modified.
      for (size_t j = 0; j < function.A()[i].n_cols; ++j)
      {
        s(function.A()[i](0, j), function.A()[i](1, j)) -= y;
      }
    }
  }

  gradient = 2 * s * coordinates;
}
void solveWithRRT(const VREPInterface *interface, const InstanceFileMap *args, const VREPInterface::State &start, const VREPInterface::State &goal) {
	dfpair(stdout, "planner", "%s", "RRT");

	typedef flann::KDTreeSingleIndexParams KDTreeType;
	typedef FLANN_KDTreeWrapper<KDTreeType, flann::L2<double>, VREPInterface::Edge> KDTree;
	typedef UniformSampler<VREPInterface, VREPInterface, KDTree> UniformSampler;
	typedef TreeInterface<VREPInterface, KDTree, UniformSampler> TreeInterface;
	typedef RRT<VREPInterface, VREPInterface, TreeInterface> RRT;

	KDTreeType kdtreeType;
	KDTree kdtree(kdtreeType, interface->getTreeStateSize());
	UniformSampler uniformSampler(*interface, *interface, kdtree);
	TreeInterface treeInterface(kdtree, uniformSampler);

	RRT rrt(*interface, *interface, treeInterface, *args);
	rrt.query(start, goal);
	rrt.dfpairs();
}
Exemple #6
0
void MainWindow::newCellTest()
{
	QString rts("root"), rrts("right of root and quite long at that"), 
		lrts("left of root");
	ZZCell rt((cellID)1, 0, rts),
	       rrt((cellID)2, 0, rrts),
	       lrt((cellID)3, 0, lrts);
	
	// QHash should make a copy for us
	// since these guys will be dead soon
	
	world.insert(1, rt);
	world.insert(2, rrt);
	world.insert(3, lrt);

	// Construct our top-level squares
	
	QGraphicsRectItem *rt_rect = new QGraphicsRectItem(0,0,100,20), 
			  *rrt_rect = new QGraphicsRectItem(0,0,100,20), 
			  *lrt_rect = new QGraphicsRectItem(0,0,100,20);

	QGraphicsTextItem *rt_text = new QGraphicsTextItem(rt.getContent(), rt_rect),
			  *rrt_text = new QGraphicsTextItem(rrt.getContent(), rrt_rect),
			  *lrt_text = new QGraphicsTextItem(lrt.getContent(), lrt_rect);


	rt_rect->setPos(420, 420);
	rrt_rect->setPos(530, 420);
	lrt_rect->setPos(310, 420);

	// Add our toplevel squares to our scene

	scene->addItem(rt_rect);
	scene->addItem(rrt_rect);
	scene->addItem(lrt_rect);
	
	scene->update();
	ui->zzViewWidget->show();
}
double AugLagrangianFunction<LRSDP>::Evaluate(const arma::mat& coordinates)
    const
{
  // We can calculate the entire objective in a smart way.
  // L(R, y, s) = Tr(C * (R R^T)) -
  //     sum_{i = 1}^{m} (y_i (Tr(A_i * (R R^T)) - b_i)) +
  //     (sigma / 2) * sum_{i = 1}^{m} (Tr(A_i * (R R^T)) - b_i)^2

  // Let's start with the objective: Tr(C * (R R^T)).
  // Simple, possibly slow solution.
  arma::mat rrt = coordinates * trans(coordinates);
  double objective = trace(function.C() * rrt);

  // Now each constraint.
  for (size_t i = 0; i < function.B().n_elem; ++i)
  {
    // Take the trace subtracted by the b_i.
    double constraint = -function.B()[i];

    if (function.AModes()[i] == 0)
    {
      constraint += trace(function.A()[i] * rrt);
    }
    else
    {
      for (size_t j = 0; j < function.A()[i].n_cols; ++j)
      {
        constraint += function.A()[i](2, j) *
            rrt(function.A()[i](0, j), function.A()[i](1, j));
      }
    }

    objective -= (lambda[i] * constraint);
    objective += (sigma / 2) * std::pow(constraint, 2.0);
  }

  return objective;
}
Exemple #8
0
ompl::base::PlannerStatus ompl::geometric::LazyLBTRRT::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    // update goal and check validity
    base::Goal *goal = pdef_->getGoal().get();
    auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);

    if (goal == nullptr)
    {
        OMPL_ERROR("%s: Goal undefined", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    while (const base::State *st = pis_.nextStart())
    {
        startMotion_ = createMotion(goal_s, st);
        break;
    }

    if (nn_->size() == 0)
    {
        OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());

    bool solved = false;

    auto *rmotion = new Motion(si_);
    base::State *xstate = si_->allocState();

    goalMotion_ = createGoalMotion(goal_s);

    CostEstimatorLb costEstimatorLb(goal, idToMotionMap_);
    LPAstarLb_ = new LPAstarLb(startMotion_->id_, goalMotion_->id_, graphLb_, costEstimatorLb);  // rooted at source
    CostEstimatorApx costEstimatorApx(this);
    LPAstarApx_ = new LPAstarApx(goalMotion_->id_, startMotion_->id_, graphApx_, costEstimatorApx);  // rooted at target
    double approxdif = std::numeric_limits<double>::infinity();
    // e+e/d.  K-nearest RRT*
    double k_rrg = boost::math::constants::e<double>() +
                   boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension();

    ////////////////////////////////////////////
    // step (1) - RRT
    ////////////////////////////////////////////
    bestCost_ = std::numeric_limits<double>::infinity();
    rrt(ptc, goal_s, xstate, rmotion, approxdif);
    if (!ptc())
    {
        solved = true;

        ////////////////////////////////////////////
        // step (2) - Lazy construction of G_lb
        ////////////////////////////////////////////
        idToMotionMap_.push_back(goalMotion_);
        int k = getK(idToMotionMap_.size(), k_rrg);
        std::vector<Motion *> nnVec;
        nnVec.reserve(k);
        BOOST_FOREACH (Motion *motion, idToMotionMap_)
        {
            nn_->nearestK(motion, k, nnVec);
            BOOST_FOREACH (Motion *neighbor, nnVec)
                if (neighbor->id_ != motion->id_ && !edgeExistsLb(motion, neighbor))
                    addEdgeLb(motion, neighbor, distanceFunction(motion, neighbor));
        }
/**
 * @function planSingleRRT
 * @brief Finds a plan using a standard RRT
 */
bool PathPlanner::planSingleTreeRrt( int _robotId, 
                                     const Eigen::VectorXi &_links, 
                                     const Eigen::VectorXd &_start, 
                                     const Eigen::VectorXd &_goal, 
                                     bool _connect, 
                                     bool _greedy,
                                     unsigned int _maxNodes ) {
  
  RRT rrt( world, _robotId, _links, _start, stepSize );
  RRT::StepResult result = RRT::STEP_PROGRESS;
  
  double smallestGap = DBL_MAX;
  
  while ( result != RRT::STEP_REACHED && smallestGap > stepSize ) {
    
    /** greedy section */
    if( _greedy ) {

      /** greedy and connect */
      if( _connect ) {
        if(rand()%2 == 0)
	      {
	        rrt.connect();
	      } else
	      {
	        rrt.connect(_goal);
	      }
	
	// ================ YOUR CODE HERE ===============
	
	// ===============================================
	      
	/** greedy and NO connect */
      } else {
	
	// ================== YOUR CODE HERE ===================
	
	// =====================================================

	      //std::cout << rrt.getSize() << std::endl;
	      if(rand()%2 == 0)
	      {
	        result = rrt.tryStep();
	      } else
	      {
	        result = rrt.tryStep(_goal);
	      }
      }
      
      /** NO greedy section */
    } else {
      
      /** NO greedy and Connect */
      if( _connect ) {
	      rrt.connect();
	
	/** No greedy and No connect -- PLAIN RRT */
      } else {
	      rrt.tryStep();
      }
      
    }
    
    if( _maxNodes > 0 && rrt.getSize() > _maxNodes ) {
      printf("--(!) Exceeded maximum of %d nodes. No path found (!)--\n", _maxNodes );
      return false;
    }
    
    double gap = rrt.getGap( _goal );
    if( gap < smallestGap ) {
      smallestGap = gap;
      std::cout << "--> [planner] Gap: " << smallestGap << "  Tree size: " << rrt.configVector.size() << std::endl;
    }
  } // End of while
  
    /// Save path  
  printf(" --> Reached goal! : Gap: %.3f ", rrt.getGap( _goal ) );
  rrt.tracePath( rrt.activeNode, path, false );
  printf(" --> Returning \n");
  
  return true;
}
/**
 * @function planSingleRRT
 * @brief Finds a plan using a standard RRT
 */
bool PathPlanner::planSingleTreeRrt( int _robotId, 
                                     const Eigen::VectorXi &_links, 
                                     const Eigen::VectorXd &_start, 
                                     const Eigen::VectorXd &_goal, 
                                     bool _connect, 
                                     bool _greedy,
                                     unsigned int _maxNodes ) {
  
  RRT rrt( world, _robotId, _links, _start, stepSize );
  RRT::StepResult result = RRT::STEP_PROGRESS;
  
  double smallestGap = DBL_MAX;
  
  while ( result != RRT::STEP_REACHED && smallestGap > stepSize ) {
    
    /** greedy section */
    if( _greedy ) {

      /** greedy and connect */
      if( _connect ) {
	
	// ================ YOUR CODE HERE ===============
	    bool reached = false;
		reached = rrt.connect(_goal);
		int currentNN = rrt.getNearestNeighbor(_goal);
		if(reached == false)
		{
			Eigen::VectorXd rdConfig = rrt.getRandomConfig();
			/*Eigen::VectorXd diff_currentNN = rrt.configVector[currentNN] - _goal;
			Eigen::VectorXd diff_rd = rdConfig - _goal;
			while(diff_currentNN.norm() < diff_rd.norm())
			{
				rdConfig = rrt.getRandomConfig();
				diff_currentNN = rrt.configVector[currentNN] - _goal;
				diff_rd = rdConfig - _goal;
			}*/
			rrt.connect(rdConfig);
			////int newNN = rrt.getNearestNeighbor(_goal);
			reached = true; 
				///(newNN != currentNN);
		}
	// ===============================================
	
	/** greedy and NO connect */
      } else {
	
	// ================== YOUR CODE HERE ===================
		RRT::StepResult currentResult = rrt.tryStep(_goal);
		if(currentResult == RRT::STEP_COLLISION)
		{
			Eigen::VectorXd rdConfig = rrt.getRandomConfig();
			///int currentNN = rrt.getNearestNeighbor(_goal);
			currentResult = rrt.tryStep(rdConfig);
		}
	// =====================================================
	
      }
      
      /** NO greedy section */
    } else {
      
      /** NO greedy and Connect */
      if( _connect ) {
	rrt.connect();
	
	/** No greedy and No connect -- PLAIN RRT */
      } else {
	rrt.tryStep();
      }
      
    }
    
    if( _maxNodes > 0 && rrt.getSize() > _maxNodes ) {
      printf("--(!) Exceeded maximum of %d nodes. No path found (!)--\n", _maxNodes );
      return false;
    }
    
    double gap = rrt.getGap( _goal );
    if( gap < smallestGap ) {
      smallestGap = gap;
      std::cout << "--> [planner] Gap: " << smallestGap << "  Tree size: " << rrt.configVector.size() << std::endl;
    }
  } // End of while
  
    /// Save path  
  printf(" --> Reached goal! : Gap: %.3f \n", rrt.getGap( _goal ) );
  rrt.tracePath( rrt.activeNode, path, false );
  
  return true;
}