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