ompl::geometric::BiTRRT::GrowResult ompl::geometric::BiTRRT::extendTree(Motion* nearest, TreeData& tree, Motion* toMotion, Motion*& result) { bool reach = true; // Compute the state to extend toward double d = si_->distance(nearest->state, toMotion->state); // Truncate the random state to be no more than maxDistance_ from nearest neighbor if (d > maxDistance_) { si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state); d = maxDistance_; reach = false; } // Validating the motion // If we are in the goal tree, we validate the motion in reverse // si_->checkMotion assumes that the first argument is valid, so we must check this explicitly // If the motion is valid, check the probabilistic transition test and the // expansion control to ensure high quality nodes are added. bool validMotion = (tree == tStart_ ? si_->checkMotion(nearest->state, toMotion->state) : si_->isValid(toMotion->state) && si_->checkMotion(toMotion->state, nearest->state)) && transitionTest(opt_->motionCost(nearest->state, toMotion->state)) && minExpansionControl(d); if (validMotion) { result = addMotion(toMotion->state, tree, nearest); return reach ? SUCCESS : ADVANCED; } return FAILED; }
bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion *> &solution) { Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension()); projectionEvaluator_->computeCoordinates(motion->state, coord); Grid<MotionInfo>::Cell *cell = otherTree.grid.getCell(coord); if (cell && !cell->data.empty()) { Motion *connectOther = cell->data[rng_.uniformInt(0, cell->data.size() - 1)]; if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root)) { auto *connect = new Motion(si_); si_->copyState(connect->state, connectOther->state); connect->parent = motion; connect->root = motion->root; motion->children.push_back(connect); addMotion(tree, connect); if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther)) { if (start) connectionPoint_ = std::make_pair(motion->state, connectOther->state); else connectionPoint_ = std::make_pair(connectOther->state, motion->state); /* extract the motions and put them in solution vector */ std::vector<Motion *> mpath1; while (motion != nullptr) { mpath1.push_back(motion); motion = motion->parent; } std::vector<Motion *> mpath2; while (connectOther != nullptr) { mpath2.push_back(connectOther); connectOther = connectOther->parent; } if (!start) mpath1.swap(mpath2); for (int i = mpath1.size() - 1; i >= 0; --i) solution.push_back(mpath1[i]); solution.insert(solution.end(), mpath2.begin(), mpath2.end()); return true; } } } return false; }
ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); // Initializing tree with start state(s) while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(siC_); si_->copyState(motion->state, st); siC_->nullControl(motion->control); addMotion(motion); } if (tree_.grid.size() == 0) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } // Ensure that we have a state sampler AND a control sampler if (!sampler_) sampler_ = si_->allocValidStateSampler(); if (!controlSampler_) controlSampler_ = siC_->allocDirectedControlSampler(); OMPL_INFORM("%s: Starting with %u states", getName().c_str(), tree_.size); Motion *solution = NULL; Motion *approxsol = NULL; double approxdif = std::numeric_limits<double>::infinity(); Motion *rmotion = new Motion(siC_); bool solved = false; while (!ptc) { // Select a state to expand the tree from Motion *existing = selectMotion(); assert (existing); // sample a random state (with goal biasing) near the state selected for expansion if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) goal_s->sampleGoal(rmotion->state); else { if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_)) continue; } // Extend a motion toward the state we just sampled unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control, existing->state, rmotion->state); // If the system was propagated for a meaningful amount of time, save into the tree if (duration >= siC_->getMinControlDuration()) { // create a motion to the resulting state Motion *motion = new Motion(siC_); si_->copyState(motion->state, rmotion->state); siC_->copyControl(motion->control, rmotion->control); motion->steps = duration; motion->parent = existing; // save the state addMotion(motion); // Check if this state is the goal state, or improves the best solution so far double dist = 0.0; solved = goal->isSatisfied(motion->state, &dist); if (solved) { approxdif = dist; solution = motion; break; } if (dist < approxdif) { approxdif = dist; approxsol = motion; } } } bool approximate = false; if (solution == NULL) { solution = approxsol; approximate = true; } // Constructing the solution path if (solution != NULL) { lastGoalMotion_ = solution; std::vector<Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } PathControl *path = new PathControl(si_); for (int i = mpath.size() - 1 ; i >= 0 ; --i) if (mpath[i]->parent) path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); else path->append(mpath[i]->state); solved = true; pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif); } // Cleaning up memory if (rmotion->state) si_->freeState(rmotion->state); if (rmotion->control) siC_->freeControl(rmotion->control); delete rmotion; OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size()); return base::PlannerStatus(solved, approximate); }
LLMotion::LLMotionInitStatus LLPhysicsMotionController::onInitialize(LLCharacter *character) { mCharacter = character; mMotions.clear(); // Breast Cleavage { controller_map_t controller; controller["Mass"] = "Breast_Physics_Mass"; controller["Gravity"] = "Breast_Physics_Gravity"; controller["Drag"] = "Breast_Physics_Drag"; controller["Damping"] = "Breast_Physics_InOut_Damping"; controller["MaxEffect"] = "Breast_Physics_InOut_Max_Effect"; controller["Spring"] = "Breast_Physics_InOut_Spring"; controller["Gain"] = "Breast_Physics_InOut_Gain"; LLPhysicsMotion *motion = new LLPhysicsMotion("Breast_Physics_InOut_Controller", "mChest", character, LLVector3(-1,0,0), controller); if (!motion->initialize()) { llassert_always(FALSE); return STATUS_FAILURE; } addMotion(motion); } // Breast Bounce { controller_map_t controller; controller["Mass"] = "Breast_Physics_Mass"; controller["Gravity"] = "Breast_Physics_Gravity"; controller["Drag"] = "Breast_Physics_Drag"; controller["Damping"] = "Breast_Physics_UpDown_Damping"; controller["MaxEffect"] = "Breast_Physics_UpDown_Max_Effect"; controller["Spring"] = "Breast_Physics_UpDown_Spring"; controller["Gain"] = "Breast_Physics_UpDown_Gain"; LLPhysicsMotion *motion = new LLPhysicsMotion("Breast_Physics_UpDown_Controller", "mChest", character, LLVector3(0,0,1), controller); if (!motion->initialize()) { llassert_always(FALSE); return STATUS_FAILURE; } addMotion(motion); } // Breast Sway { controller_map_t controller; controller["Mass"] = "Breast_Physics_Mass"; controller["Gravity"] = "Breast_Physics_Gravity"; controller["Drag"] = "Breast_Physics_Drag"; controller["Damping"] = "Breast_Physics_LeftRight_Damping"; controller["MaxEffect"] = "Breast_Physics_LeftRight_Max_Effect"; controller["Spring"] = "Breast_Physics_LeftRight_Spring"; controller["Gain"] = "Breast_Physics_LeftRight_Gain"; LLPhysicsMotion *motion = new LLPhysicsMotion("Breast_Physics_LeftRight_Controller", "mChest", character, LLVector3(0,-1,0), controller); if (!motion->initialize()) { llassert_always(FALSE); return STATUS_FAILURE; } addMotion(motion); } // Butt Bounce { controller_map_t controller; controller["Mass"] = "Butt_Physics_Mass"; controller["Gravity"] = "Butt_Physics_Gravity"; controller["Drag"] = "Butt_Physics_Drag"; controller["Damping"] = "Butt_Physics_UpDown_Damping"; controller["MaxEffect"] = "Butt_Physics_UpDown_Max_Effect"; controller["Spring"] = "Butt_Physics_UpDown_Spring"; controller["Gain"] = "Butt_Physics_UpDown_Gain"; LLPhysicsMotion *motion = new LLPhysicsMotion("Butt_Physics_UpDown_Controller", "mPelvis", character, LLVector3(0,0,-1), controller); if (!motion->initialize()) { llassert_always(FALSE); return STATUS_FAILURE; } addMotion(motion); } // Butt LeftRight { controller_map_t controller; controller["Mass"] = "Butt_Physics_Mass"; controller["Gravity"] = "Butt_Physics_Gravity"; controller["Drag"] = "Butt_Physics_Drag"; controller["Damping"] = "Butt_Physics_LeftRight_Damping"; controller["MaxEffect"] = "Butt_Physics_LeftRight_Max_Effect"; controller["Spring"] = "Butt_Physics_LeftRight_Spring"; controller["Gain"] = "Butt_Physics_LeftRight_Gain"; LLPhysicsMotion *motion = new LLPhysicsMotion("Butt_Physics_LeftRight_Controller", "mPelvis", character, LLVector3(0,-1,0), controller); if (!motion->initialize()) { llassert_always(FALSE); return STATUS_FAILURE; } addMotion(motion); } // Belly Bounce { controller_map_t controller; controller["Mass"] = "Belly_Physics_Mass"; controller["Gravity"] = "Belly_Physics_Gravity"; controller["Drag"] = "Belly_Physics_Drag"; controller["Damping"] = "Belly_Physics_UpDown_Damping"; controller["MaxEffect"] = "Belly_Physics_UpDown_Max_Effect"; controller["Spring"] = "Belly_Physics_UpDown_Spring"; controller["Gain"] = "Belly_Physics_UpDown_Gain"; LLPhysicsMotion *motion = new LLPhysicsMotion("Belly_Physics_UpDown_Controller", "mPelvis", character, LLVector3(0,0,-1), controller); if (!motion->initialize()) { llassert_always(FALSE); return STATUS_FAILURE; } addMotion(motion); } return STATUS_SUCCESS; }
ompl::base::PlannerStatus ompl::geometric::SBL::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get()); if (!goal) { OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; } while (const base::State *st = pis_.nextStart()) { auto *motion = new Motion(si_); si_->copyState(motion->state, st); motion->valid = true; motion->root = motion->state; addMotion(tStart_, motion); } if (tStart_.size == 0) { OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (!goal->couldSample()) { OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } if (!sampler_) sampler_ = si_->allocValidStateSampler(); OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(tStart_.size + tGoal_.size)); std::vector<Motion *> solution; base::State *xstate = si_->allocState(); bool startTree = true; bool solved = false; while (ptc == false) { TreeData &tree = startTree ? tStart_ : tGoal_; startTree = !startTree; TreeData &otherTree = startTree ? tStart_ : tGoal_; // if we have not sampled too many goals already if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2) { const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal(); if (st) { auto *motion = new Motion(si_); si_->copyState(motion->state, st); motion->root = motion->state; motion->valid = true; addMotion(tGoal_, motion); } if (tGoal_.size == 0) { OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str()); break; } } Motion *existing = selectMotion(tree); assert(existing); if (!sampler_->sampleNear(xstate, existing->state, maxDistance_)) continue; /* create a motion */ auto *motion = new Motion(si_); si_->copyState(motion->state, xstate); motion->parent = existing; motion->root = existing->root; existing->children.push_back(motion); addMotion(tree, motion); if (checkSolution(!startTree, tree, otherTree, motion, solution)) { auto path(std::make_shared<PathGeometric>(si_)); for (auto &i : solution) path->append(i->state); pdef_->addSolutionPath(path, false, 0.0, getName()); solved = true; break; } } si_->freeState(xstate); OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(), tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size()); return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; }
bool ompl::geometric::BallTreeRRTstar::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); if (!goal) { msg_.error("Goal undefined"); return false; } while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(si_, rO_); si_->copyState(motion->state, st); addMotion(motion); } if (nn_->size() == 0) { msg_.error("There are no valid initial states!"); return false; } if (!sampler_) sampler_ = si_->allocStateSampler(); msg_.inform("Starting with %u states", nn_->size()); Motion *solution = NULL; Motion *approximation = NULL; double approximatedist = std::numeric_limits<double>::infinity(); bool sufficientlyShort = false; Motion *rmotion = new Motion(si_, rO_); Motion *toTrim = NULL; base::State *rstate = rmotion->state; base::State *xstate = si_->allocState(); base::State *tstate = si_->allocState(); std::vector<Motion*> solCheck; std::vector<Motion*> nbh; std::vector<double> dists; std::vector<int> valid; long unsigned int rewireTest = 0; std::pair<base::State*,double> lastValid(tstate, 0.0); while (ptc() == false) { bool rejected = false; /* sample until a state not within any of the existing volumes is found */ do { /* sample random state (with goal biasing) */ if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) goal_s->sampleGoal(rstate); else sampler_->sampleUniform(rstate); /* check to see if it is inside an existing volume */ if (inVolume(rstate)) { rejected = true; /* see if the state is valid */ if(!si_->isValid(rstate)) { /* if it's not, reduce the size of the nearest volume to the distance between its center and the rejected state */ toTrim = nn_->nearest(rmotion); double newRad = si_->distance(toTrim->state, rstate); if (newRad < toTrim->volRadius) toTrim->volRadius = newRad; } } else rejected = false; } while (rejected); /* find closest state in the tree */ Motion *nmotion = nn_->nearest(rmotion); base::State *dstate = rstate; /* find state to add */ double d = si_->distance(nmotion->state, rstate); if (d > maxDistance_) { si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate); dstate = xstate; } if (si_->checkMotion(nmotion->state, dstate, lastValid)) { /* create a motion */ double distN = si_->distance(dstate, nmotion->state); Motion *motion = new Motion(si_, rO_); si_->copyState(motion->state, dstate); motion->parent = nmotion; motion->cost = nmotion->cost + distN; /* find nearby neighbors */ double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))), ballRadiusMax_); nn_->nearestR(motion, r, nbh); rewireTest += nbh.size(); // cache for distance computations dists.resize(nbh.size()); // cache for motion validity valid.resize(nbh.size()); std::fill(valid.begin(), valid.end(), 0); if (delayCC_) { // calculate all costs and distances for (unsigned int i = 0 ; i < nbh.size() ; ++i) if (nbh[i] != nmotion) { double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate); nbh[i]->cost = c; } // sort the nodes std::sort(nbh.begin(), nbh.end(), compareMotion); for (unsigned int i = 0 ; i < nbh.size() ; ++i) if (nbh[i] != nmotion) { dists[i] = si_->distance(nbh[i]->state, dstate); nbh[i]->cost -= dists[i]; } // collision check until a valid motion is found for (unsigned int i = 0 ; i < nbh.size() ; ++i) if (nbh[i] != nmotion) { dists[i] = si_->distance(nbh[i]->state, dstate); double c = nbh[i]->cost + dists[i]; if (c < motion->cost) { if (si_->checkMotion(nbh[i]->state, dstate, lastValid)) { motion->cost = c; motion->parent = nbh[i]; valid[i] = 1; break; } else { valid[i] = -1; /* if a collision is found, trim radius to distance from motion to last valid state */ double nR = si_->distance(nbh[i]->state, lastValid.first); if (nR < nbh[i]->volRadius) nbh[i]->volRadius = nR; } } } else { valid[i] = 1; dists[i] = distN; break; } } else{ /* find which one we connect the new state to*/ for (unsigned int i = 0 ; i < nbh.size() ; ++i) if (nbh[i] != nmotion) { dists[i] = si_->distance(nbh[i]->state, dstate); double c = nbh[i]->cost + dists[i]; if (c < motion->cost) { if (si_->checkMotion(nbh[i]->state, dstate, lastValid)) { motion->cost = c; motion->parent = nbh[i]; valid[i] = 1; } else { valid[i] = -1; /* if a collision is found, trim radius to distance from motion to last valid state */ double newR = si_->distance(nbh[i]->state, lastValid.first); if (newR < nbh[i]->volRadius) nbh[i]->volRadius = newR; } } } else { valid[i] = 1; dists[i] = distN; } } /* add motion to tree */ addMotion(motion); motion->parent->children.push_back(motion); solCheck.resize(1); solCheck[0] = motion; /* rewire tree if needed */ for (unsigned int i = 0 ; i < nbh.size() ; ++i) if (nbh[i] != motion->parent) { double c = motion->cost + dists[i]; if (c < nbh[i]->cost) { bool v = false; if (valid[i] == 0) { if(!si_->checkMotion(nbh[i]->state, dstate, lastValid)) { /* if a collision is found, trim radius to distance from motion to last valid state */ double R = si_->distance(nbh[i]->state, lastValid.first); if (R < nbh[i]->volRadius) nbh[i]->volRadius = R; } else { v = true; } } if (valid[i] == 1) v = true; if (v) { // Remove this node from its parent list removeFromParent (nbh[i]); double delta = c - nbh[i]->cost; nbh[i]->parent = motion; nbh[i]->cost = c; nbh[i]->parent->children.push_back(nbh[i]); solCheck.push_back(nbh[i]); // Update the costs of the node's children updateChildCosts(nbh[i], delta); } } } // check if we found a solution for (unsigned int i = 0 ; i < solCheck.size() ; ++i) { double dist = 0.0; bool solved = goal->isSatisfied(solCheck[i]->state, &dist); sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false; if (solved) { if (sufficientlyShort) { solution = solCheck[i]; break; } else if (!solution || (solCheck[i]->cost < solution->cost)) { solution = solCheck[i]; } } else if (!solution && dist < approximatedist) { approximation = solCheck[i]; approximatedist = dist; } } // terminate if a sufficient solution is found if (solution && sufficientlyShort) break; } else { /* if a collision is found, trim radius to distance from motion to last valid state */ toTrim = nn_->nearest(nmotion); double newRadius = si_->distance(toTrim->state, lastValid.first); if (newRadius < toTrim->volRadius) toTrim->volRadius = newRadius; } } double solutionCost; bool approximate = (solution == NULL); bool addedSolution = false; if (approximate) { solution = approximation; solutionCost = approximatedist; } else solutionCost = solution->cost; if (solution != NULL) { // construct the solution path std::vector<Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } // set the solution path PathGeometric *path = new PathGeometric(si_); for (int i = mpath.size() - 1 ; i >= 0 ; --i) path->append(mpath[i]->state); goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost); addedSolution = true; } si_->freeState(xstate); if (rmotion->state) si_->freeState(rmotion->state); delete rmotion; msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest); return addedSolution; }
ompl::base::PlannerStatus ompl::geometric::ProjEST::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal); while (const base::State *st = pis_.nextStart()) { auto *motion = new Motion(si_); si_->copyState(motion->state, st); addMotion(motion); } if (tree_.grid.size() == 0) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (!sampler_) sampler_ = si_->allocValidStateSampler(); OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size); Motion *solution = nullptr; Motion *approxsol = nullptr; double approxdif = std::numeric_limits<double>::infinity(); base::State *xstate = si_->allocState(); while (ptc == false) { /* Decide on a state to expand from */ Motion *existing = selectMotion(); assert(existing); /* sample random state (with goal biasing) */ if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) goal_s->sampleGoal(xstate); else if (!sampler_->sampleNear(xstate, existing->state, maxDistance_)) continue; if (si_->checkMotion(existing->state, xstate)) { /* create a motion */ auto *motion = new Motion(si_); si_->copyState(motion->state, xstate); motion->parent = existing; addMotion(motion); double dist = 0.0; bool solved = goal->isSatisfied(motion->state, &dist); if (solved) { approxdif = dist; solution = motion; break; } if (dist < approxdif) { approxdif = dist; approxsol = motion; } } } bool solved = false; bool approximate = false; if (solution == nullptr) { solution = approxsol; approximate = true; } if (solution != nullptr) { lastGoalMotion_ = solution; /* construct the solution path */ std::vector<Motion *> mpath; while (solution != nullptr) { mpath.push_back(solution); solution = solution->parent; } /* set the solution path */ auto path(std::make_shared<PathGeometric>(si_)); for (int i = mpath.size() - 1; i >= 0; --i) path->append(mpath[i]->state); pdef_->addSolutionPath(path, approximate, approxdif, getName()); solved = true; } si_->freeState(xstate); OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size()); return base::PlannerStatus(solved, approximate); }
bool ompl::control::KPIECE1::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::Goal *goal = pdef_->getGoal().get(); while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(siC_); si_->copyState(motion->state, st); siC_->nullControl(motion->control); addMotion(motion, 1.0); } if (tree_.grid.size() == 0) { msg_.error("There are no valid initial states!"); return false; } if (!controlSampler_) controlSampler_ = siC_->allocControlSampler(); msg_.inform("Starting with %u states", tree_.size); Motion *solution = NULL; Motion *approxsol = NULL; double approxdif = std::numeric_limits<double>::infinity(); Control *rctrl = siC_->allocControl(); std::vector<base::State*> states(siC_->getMaxControlDuration() + 1); std::vector<Grid::Coord> coords(states.size()); std::vector<Grid::Cell*> cells(coords.size()); for (unsigned int i = 0 ; i < states.size() ; ++i) states[i] = si_->allocState(); // samples that were found to be the best, so far CloseSamples closeSamples(nCloseSamples_); while (ptc() == false) { tree_.iteration++; /* Decide on a state to expand from */ Motion *existing = NULL; Grid::Cell *ecell = NULL; if (closeSamples.canSample() && rng_.uniform01() < goalBias_) { if (!closeSamples.selectMotion(existing, ecell)) selectMotion(existing, ecell); } else selectMotion(existing, ecell); assert(existing); /* sample a random control */ controlSampler_->sampleNext(rctrl, existing->control, existing->state); /* propagate */ unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration()); cd = siC_->propagateWhileValid(existing->state, rctrl, cd, states, false); /* if we have enough steps */ if (cd >= siC_->getMinControlDuration()) { std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size()); bool interestingMotion = false; // split the motion into smaller ones, so we do not cross cell boundaries for (unsigned int i = 0 ; i < cd ; ++i) { projectionEvaluator_->computeCoordinates(states[i], coords[i]); cells[i] = tree_.grid.getCell(coords[i]); if (!cells[i]) interestingMotion = true; else { if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds) interestingMotion = true; } } if (interestingMotion || rng_.uniform01() < 0.05) { unsigned int index = 0; while (index < cd) { unsigned int nextIndex = findNextMotion(coords, index, cd); Motion *motion = new Motion(siC_); si_->copyState(motion->state, states[nextIndex]); siC_->copyControl(motion->control, rctrl); motion->steps = nextIndex - index + 1; motion->parent = existing; double dist = 0.0; bool solv = goal->isSatisfied(motion->state, &dist); Grid::Cell *toCell = addMotion(motion, dist); if (solv) { approxdif = dist; solution = motion; break; } if (dist < approxdif) { approxdif = dist; approxsol = motion; } closeSamples.consider(toCell, motion, dist); // new parent will be the newly created motion existing = motion; index = nextIndex + 1; } if (solution) break; } // update cell score ecell->data->score *= goodScoreFactor_; } else ecell->data->score *= badScoreFactor_; tree_.grid.update(ecell); } bool solved = false; bool approximate = false; if (solution == NULL) { solution = approxsol; approximate = true; } if (solution != NULL) { /* construct the solution path */ std::vector<Motion*> mpath; while (solution != NULL) { mpath.push_back(solution); solution = solution->parent; } /* set the solution path */ PathControl *path = new PathControl(si_); for (int i = mpath.size() - 1 ; i >= 0 ; --i) if (mpath[i]->parent) path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); else path->append(mpath[i]->state); goal->addSolutionPath(base::PathPtr(path), approximate, approxdif); solved = true; } siC_->freeControl(rctrl); for (unsigned int i = 0 ; i < states.size() ; ++i) si_->freeState(states[i]); msg_.inform("Created %u states in %u cells (%u internal + %u external)", tree_.size, tree_.grid.size(), tree_.grid.countInternal(), tree_.grid.countExternal()); return solved; }
ompl::base::PlannerStatus ompl::geometric::pSBL::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); base::GoalState *goal = dynamic_cast<base::GoalState*>(pdef_->getGoal().get()); if (!goal) { OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; } while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(si_); si_->copyState(motion->state, st); motion->valid = true; motion->root = motion->state; addMotion(tStart_, motion); } if (tGoal_.size == 0) { if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState())) { Motion *motion = new Motion(si_); si_->copyState(motion->state, goal->getState()); motion->valid = true; motion->root = motion->state; addMotion(tGoal_, motion); } else OMPL_ERROR("%s: Goal state is invalid!", getName().c_str()); } if (tStart_.size == 0) { OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (tGoal_.size == 0) { OMPL_ERROR("%s: Motion planning goal tree could not be initialized!", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } samplerArray_.resize(threadCount_); OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(tStart_.size + tGoal_.size)); SolutionInfo sol; sol.found = false; loopCounter_ = 0; std::vector<std::thread*> th(threadCount_); for (unsigned int i = 0 ; i < threadCount_ ; ++i) th[i] = new std::thread(std::bind(&pSBL::threadSolve, this, i, ptc, &sol)); for (unsigned int i = 0 ; i < threadCount_ ; ++i) { th[i]->join(); delete th[i]; } OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(), tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size()); return sol.found ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; }
void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol) { RNG rng; std::vector<Motion*> solution; base::State *xstate = si_->allocState(); bool startTree = rng.uniformBool(); while (!sol->found && ptc == false) { bool retry = true; while (retry && !sol->found && ptc == false) { removeList_.lock.lock(); if (!removeList_.motions.empty()) { if (loopLock_.try_lock()) { retry = false; std::map<Motion*, bool> seen; for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i) if (seen.find(removeList_.motions[i].motion) == seen.end()) removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen); removeList_.motions.clear(); loopLock_.unlock(); } } else retry = false; removeList_.lock.unlock(); } if (sol->found || ptc) break; loopLockCounter_.lock(); if (loopCounter_ == 0) loopLock_.lock(); loopCounter_++; loopLockCounter_.unlock(); TreeData &tree = startTree ? tStart_ : tGoal_; startTree = !startTree; TreeData &otherTree = startTree ? tStart_ : tGoal_; Motion *existing = selectMotion(rng, tree); if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_)) continue; /* create a motion */ Motion *motion = new Motion(si_); si_->copyState(motion->state, xstate); motion->parent = existing; motion->root = existing->root; existing->lock.lock(); existing->children.push_back(motion); existing->lock.unlock(); addMotion(tree, motion); if (checkSolution(rng, !startTree, tree, otherTree, motion, solution)) { sol->lock.lock(); if (!sol->found) { sol->found = true; PathGeometric *path = new PathGeometric(si_); for (unsigned int i = 0 ; i < solution.size() ; ++i) path->append(solution[i]->state); pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName()); } sol->lock.unlock(); } loopLockCounter_.lock(); loopCounter_--; if (loopCounter_ == 0) loopLock_.unlock(); loopLockCounter_.unlock(); } si_->freeState(xstate); }
ompl::base::PlannerStatus ompl::geometric::BiTRRT::solve(const base::PlannerTerminationCondition &ptc) { // Basic error checking checkValidity(); // Goal information base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *gsr = dynamic_cast<base::GoalSampleableRegion*>(goal); if (!gsr) { OMPL_ERROR("%s: Goal object does not derive from GoalSampleableRegion", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } // Loop through the (valid) input states and add them to the start tree while (const base::State *state = pis_.nextStart()) { Motion *motion = new Motion(si_); si_->copyState(motion->state, state); motion->cost = opt_->stateCost(motion->state); motion->root = motion->state; // this state is the root of a tree if (tStart_->size() == 0) // do not overwrite best/worst from a prior call to solve worstCost_ = bestCost_ = motion->cost; // Add start motion to the tree tStart_->add(motion); } if (tStart_->size() == 0) { OMPL_ERROR("%s: Start tree has no valid states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } // Do the same for the goal tree, if it is empty, but only once if (tGoal_->size() == 0) { const base::State *state = pis_.nextGoal(ptc); if (state) { Motion* motion = addMotion(state, tGoal_); motion->root = motion->state; // this state is the root of a tree } } if (tGoal_->size() == 0) { OMPL_ERROR("%s: Goal tree has no valid states!", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } OMPL_INFORM("%s: Planning started with %d states already in datastructure", getName().c_str(), (int)(tStart_->size() + tGoal_->size())); base::StateSamplerPtr sampler = si_->allocStateSampler(); Motion *rmotion = new Motion(si_); base::State *rstate = rmotion->state; Motion *xmotion = new Motion(si_); base::State *xstate = xmotion->state; TreeData tree = tStart_; TreeData otherTree = tGoal_; bool solved = false; // Planning loop while (ptc == false) { // Check if there are more goal states if (pis_.getSampledGoalsCount() < tGoal_->size() / 2) { if (const base::State *state = pis_.nextGoal()) { Motion* motion = addMotion(state, tGoal_); motion->root = motion->state; // this state is the root of a tree } } // Sample a state uniformly at random sampler->sampleUniform(rstate); Motion* result; // the motion that gets added in extendTree if (extendTree(rmotion, tree, result) != FAILED) // we added something new to the tree { // Try to connect the other tree to the node we just added if (connectTrees(result, otherTree, xmotion)) { // The trees have been connected. Construct the solution path Motion *solution = connectionPoint_.first; std::vector<Motion*> mpath1; while (solution != NULL) { mpath1.push_back(solution); solution = solution->parent; } solution = connectionPoint_.second; std::vector<Motion*> mpath2; while (solution != NULL) { mpath2.push_back(solution); solution = solution->parent; } PathGeometric *path = new PathGeometric(si_); path->getStates().reserve(mpath1.size() + mpath2.size()); for (int i = mpath1.size() - 1 ; i >= 0 ; --i) path->append(mpath1[i]->state); for (unsigned int i = 0 ; i < mpath2.size() ; ++i) path->append(mpath2[i]->state); pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName()); solved = true; break; } } std::swap(tree, otherTree); } si_->freeState(rstate); si_->freeState(xstate); delete rmotion; delete xmotion; OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size()); return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; }
void Animal::frame(float dt) { if( info.status.exp >= 100 ) { info.level++; info.status.exp = 0; } state.timerHappy += dt; state.timerHungry += dt; state.timerSleepy += dt; if( state.timerHappy > 4.f ) { state.timerHappy = 0.f; if( info.status.joy > 15 ) info.status.joy -= rand()%10 + 5; else { // int chap = rand()%3; // if( chap == 0 ) // doAction(ACTION_PLAYING_SWING, true); // else if( chap == 1 ) // doAction(ACTION_TRAINING_RUNNING, true); // else // doAction(ACTION_TRAINING_ROPE, true); } } if( state.timerSleepy > 300.f ) { state.timerSleepy = 0.f; if( info.status.sleep > 15 ) info.status.sleep -= rand()%10 + 5; else { addMotion(SLEEP, 100, true); } } if( info.status.poop >= 100 ) { doAction(ACTION_BASIC_POOP, true); } if ( info.status.fullness < 10 && state.timerHungry > 4.f) { state.timerHungry = 0.f; if( info.status.health > 20 ) info.status.health -= rand()%10 + 5; else { addMotion(SICK, 1, true); } } state.isHappy = info.status.joy >= 90; state.isUnhappy = info.status.joy <= 20; state.isHungry = info.status.fullness <= 40; state.isSick = info.status.health <= 20; state.isSleepy = info.status.sleep <= 40; state.isPoop = info.status.poop >= 70; CCPoint point; point.x = pBody->getContentSize().width*0.5f; point.y = pBody->getContentSize().height + 30; pBody->getChildByTag(STATE_HAPPY)->setPosition(point); pBody->getChildByTag(STATE_UNHAPPY)->setPosition(point); pBody->getChildByTag(STATE_HUNGRY)->setPosition(point); pBody->getChildByTag(STATE_SICK)->setPosition(point); pBody->getChildByTag(STATE_SLEEPY)->setPosition(point); pBody->getChildByTag(STATE_POOP)->setPosition(point); timer += dt; if( stateIndex == 0 ) { if( state.isHappy ) { pBody->getChildByTag(STATE_HAPPY)->setVisible(true); pBody->getChildByTag(STATE_UNHAPPY)->setVisible(false); pBody->getChildByTag(STATE_HUNGRY)->setVisible(false); pBody->getChildByTag(STATE_SICK)->setVisible(false); pBody->getChildByTag(STATE_SLEEPY)->setVisible(false); pBody->getChildByTag(STATE_POOP)->setVisible(false); } else { stateIndex = (++stateIndex)%6; pBody->getChildByTag(STATE_HAPPY)->setVisible(false); } } else if( stateIndex == 1 ) { if( state.isHungry ) { pBody->getChildByTag(STATE_HAPPY)->setVisible(false); pBody->getChildByTag(STATE_UNHAPPY)->setVisible(false); pBody->getChildByTag(STATE_HUNGRY)->setVisible(true); pBody->getChildByTag(STATE_SICK)->setVisible(false); pBody->getChildByTag(STATE_SLEEPY)->setVisible(false); pBody->getChildByTag(STATE_POOP)->setVisible(false); } else { stateIndex = (++stateIndex)%6; pBody->getChildByTag(STATE_HUNGRY)->setVisible(false); } } else if( stateIndex == 2 ) { if( state.isSick ) { pBody->getChildByTag(STATE_HAPPY)->setVisible(false); pBody->getChildByTag(STATE_UNHAPPY)->setVisible(false); pBody->getChildByTag(STATE_HUNGRY)->setVisible(false); pBody->getChildByTag(STATE_SICK)->setVisible(true); pBody->getChildByTag(STATE_SLEEPY)->setVisible(false); pBody->getChildByTag(STATE_POOP)->setVisible(false); } else { stateIndex = (++stateIndex)%6; pBody->getChildByTag(STATE_SICK)->setVisible(false); } } else if( stateIndex == 3 ) { if( state.isSleepy ) { pBody->getChildByTag(STATE_HAPPY)->setVisible(false); pBody->getChildByTag(STATE_UNHAPPY)->setVisible(false); pBody->getChildByTag(STATE_HUNGRY)->setVisible(false); pBody->getChildByTag(STATE_SICK)->setVisible(false); pBody->getChildByTag(STATE_SLEEPY)->setVisible(true); pBody->getChildByTag(STATE_POOP)->setVisible(false); } else { stateIndex = (++stateIndex)%6; pBody->getChildByTag(STATE_SLEEPY)->setVisible(false); } } else if( stateIndex == 4 ) { if( state.isUnhappy ) { pBody->getChildByTag(STATE_HAPPY)->setVisible(false); pBody->getChildByTag(STATE_UNHAPPY)->setVisible(true); pBody->getChildByTag(STATE_HUNGRY)->setVisible(false); pBody->getChildByTag(STATE_SICK)->setVisible(false); pBody->getChildByTag(STATE_SLEEPY)->setVisible(false); pBody->getChildByTag(STATE_POOP)->setVisible(false); } else { stateIndex = (++stateIndex)%6; pBody->getChildByTag(STATE_UNHAPPY)->setVisible(false); } } else { if( state.isPoop ) { pBody->getChildByTag(STATE_HAPPY)->setVisible(false); pBody->getChildByTag(STATE_UNHAPPY)->setVisible(false); pBody->getChildByTag(STATE_HUNGRY)->setVisible(false); pBody->getChildByTag(STATE_SICK)->setVisible(false); pBody->getChildByTag(STATE_SLEEPY)->setVisible(false); pBody->getChildByTag(STATE_POOP)->setVisible(true); } else { stateIndex = (++stateIndex)%6; } } if( timer >= 3.f ) { timer = 0.f; stateIndex = (++stateIndex)%6; } // pBody->getChildByTag(STATE_HAPPY)->setVisible(state.isHappy); // pBody->getChildByTag(STATE_HUNGRY)->setVisible(state.isHungry); // pBody->getChildByTag(STATE_SICK)->setVisible(state.isSick); // pBody->getChildByTag(STATE_SLEEPY)->setVisible(state.isSleepy); // pBody->getChildByTag(STATE_POOP)->setVisible(state.isPoop); }
void Animal::doAction(int action, bool isOrder) { // if( isOrder && motionQueue.front()->isOrder == false ) // pBody->stopAllActions(); cancelAllMotions(0); switch (action) { case ACTION_BASIC_COME: break; case ACTION_BASIC_CURE: cancelAllMotions(ACTION_BASIC_CURE); setStatus(STATUS_FULLNESS, 80); setStatus(STATUS_HEALTH, 70); setStatus(STATUS_JOY, 95); addStatus(STATUS_EXP, 10); break; case ACTION_BASIC_EAT: addMotion(EAT, 3, isOrder); addStatus(STATUS_FULLNESS, 20); addStatus(STATUS_POOP, 30); addStatus(STATUS_EXP, 10); break; case ACTION_BASIC_REST: addMotion(SIT, 3, isOrder); addStatus(STATUS_JOY, 10); addStatus(STATUS_HEALTH, 10); addStatus(STATUS_EXP, 10); break; case ACTION_BASIC_RUN: addMotion(RUN_LEFT, 3, isOrder); addStatus(STATUS_SLEEP, 10); addStatus(STATUS_HEALTH, 10); addStatus(STATUS_EXP, 10); break; case ACTION_BASIC_SLEEP: addMotion(SLEEP, 100, isOrder); addStatus(STATUS_HEALTH, 10); addStatus(STATUS_FULLNESS, -20); addStatus(STATUS_EXP, 10); break; case ACTION_BASIC_STOP: cancelAllMotions(true); addStatus(STATUS_EXP, 20); break; case ACTION_BASIC_WAKE: cancelAllMotions(true); addStatus(STATUS_EXP, 20); addStatus(STATUS_SLEEP, 60); break; case ACTION_BASIC_POOP: addMotion(POOP, 3, isOrder); addStatus(STATUS_EXP, 20); setStatus(STATUS_POOP, 0); break; case ACTION_TRAINING_CLEANPOOP: break; case ACTION_TRAINING_ROPE: addMotion(FUN_ROPE, 10, isOrder); addStatus(STATUS_JOY, 50); addStatus(STATUS_HEALTH, 30); addStatus(STATUS_FULLNESS, -20); addStatus(STATUS_EXP, 20); break; case ACTION_TRAINING_RUNNING: addMotion(FUN_RUNNING, 10, isOrder); addStatus(STATUS_JOY, 50); addStatus(STATUS_HEALTH, 30); addStatus(STATUS_FULLNESS, -20); addStatus(STATUS_EXP, 20); break; case ACTION_PLAYING_PLAY: switch (rand()%3) { case 0: addMotion(FUN_ROPE, 10, isOrder); break; case 1: addMotion(FUN_SWING, 10, isOrder); break; case 2: addMotion(FUN_RUNNING, 10, isOrder); break; } addStatus(STATUS_JOY, 50); addStatus(STATUS_HEALTH, 30); addStatus(STATUS_FULLNESS, -20); addStatus(STATUS_EXP, 20); break; case ACTION_PLAYING_SWING: addMotion(FUN_SWING, 10, isOrder); addStatus(STATUS_JOY, 50); addStatus(STATUS_HEALTH, 30); addStatus(STATUS_FULLNESS, -20); addStatus(STATUS_EXP, 20); break; case ACTION_EXTRA_DIE: break; default: CCLog("Animal::doAction : wrong action input"); break; } }
void Animal::addMotion(MOTIONPACK pack, bool cleanQueue) { addMotion(pack.name, pack.num_of_repeat, pack.isOrder, cleanQueue); }
void Animal::finishChk(cocos2d::CCNode *node) { pBody->stopAllActions(); //#define REPLAY_LAST_MOTION //#define ITERATE_MOTION #define MOVE_WITH_AI #ifdef REPLAY_LAST_MOTION switch( motionState.name ) { case WALK: step = FORWARD_LEFT; runActionWithMotion( WALK, step ); break; case WALK_BACK: step = BACK_LEFT; runActionWithMotion( WALK_BACK, step ); break; case RUN: step = FORWARD_LEFT; runActionWithMotion( RUN, step ); break; case RUN_BACK: step = BACK_LEFT; runActionWithMotion( RUN_BACK, step ); break; case STAND: runActionWithMotion( STAND ); break; case SIT: runActionWithMotion( SIT ); break; case SLEEP: runActionWithMotion( SLEEP ); break; case EAT: runActionWithMotion( EAT ); break; case POOP: runActionWithMotion( POOP ); break; case SICK: runActionWithMotion( SICK ); break; } #endif #ifdef ITERATE_MOTION switch( motionState.name ) { case WALK: step = BACK_LEFT; runActionWithMotion( WALK_BACK, step ); break; case WALK_BACK: step = FORWARD_LEFT; runActionWithMotion( RUN, step ); break; case RUN: step = BACK_LEFT; runActionWithMotion( RUN_BACK, step ); break; case RUN_BACK: runActionWithMotion( STAND ); break; case STAND: runActionWithMotion( SIT ); break; case SIT: runActionWithMotion( SLEEP ); break; case SLEEP: runActionWithMotion( EAT ); break; case EAT: runActionWithMotion( POOP ); break; case POOP: runActionWithMotion( SICK ); break; case SICK: step = FORWARD_LEFT; runActionWithMotion( WALK, step ); break; } #endif #ifdef MOVE_WITH_AI if( motionQueue.empty() ) { runActionWithMotion(STAND); addMotion(WALK_LEFT, rand()%3+1, false); addMotion(WALK_RIGHT, rand()%3+1, false); addMotion(WALK_BACK_RIGHT, rand()%3+1,false); addMotion(WALK_BACK_LEFT, rand()%3+1, false); return; } MOTIONPACK *nextMotion = motionQueue.front(); if( nextMotion == NULL ) return; while(nextMotion->num_of_repeat == 0) { if( nextMotion->name == POOP ) pBody->getChildByTag(STATE_POOP)->setVisible(false); motionQueue.pop(); if( motionQueue.empty() ) { runActionWithMotion(STAND); return; } nextMotion = motionQueue.front(); } motionState.isOrder = nextMotion->isOrder; if( nextMotion->name != SICK ) nextMotion->num_of_repeat--; runActionWithMotion(nextMotion->name); #endif }
ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminationCondition &ptc) { // Make sure the planner is configured correctly. // ompl::base::Planner::checkValidity checks that there is at least one // start state and an ompl::base::Goal object specified and throws an // exception if this is not the case. checkValidity(); // depending on how the planning problem is set up, this may be necessary bsp_->bounds_ = projectionEvaluator_->getBounds(); base::Goal *goal = pdef_->getGoal().get(); goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal); // Ensure that we have a state sampler AND a control sampler if (!sampler_) sampler_ = si_->allocStateSampler(); if (!controlSampler_) controlSampler_ = siC_->allocDirectedControlSampler(); // Initialize to correct values depending on whether or not previous calls to solve // generated an approximate or exact solution. If solve is being called for the first // time then initializes hasSolution to false and isApproximate to true. double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity(); bool hasSolution = lastGoalMotion_ != nullptr; bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal); unsigned int ndim = projectionEvaluator_->getDimension(); // If an exact solution has already been found, do not run for another iteration. if (hasSolution && !isApproximate) return ompl::base::PlannerStatus::EXACT_SOLUTION; // Initialize tree with start state(s) while (const base::State *st = pis_.nextStart()) { auto *startMotion = new Motion(si_->cloneState(st)); bsp_->addMotion(startMotion); startMotion->heapElement_ = priorityQueue_.insert(startMotion); } if (priorityQueue_.empty()) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), priorityQueue_.size()); base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState(); base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim); while (!ptc) { // Get the top priority path. Motion *motionSelected = priorityQueue_.top()->data; motionSelected->updatePriority(); priorityQueue_.update(motionSelected->heapElement_); Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2); if (newMotion == nullptr) continue; addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2); // Check if the newMotion reached the goal. hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal); if (hasSolution) { closestDistanceToGoal = distanceToGoal; lastGoalMotion_ = newMotion; isApproximate = false; break; } else if (distanceToGoal < closestDistanceToGoal) { closestDistanceToGoal = distanceToGoal; lastGoalMotion_ = newMotion; } // subdivide cell that contained selected motion, put motions of that // cell in subcells and split motions so that they contained within // one subcell Cell *cellSelected = motionSelected->cell_; std::vector<Motion *> motions; cellSelected->subdivide(ndim); motions.swap(cellSelected->motions_); for (auto &motion : motions) addMotion(motion, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2); } if (lastGoalMotion_ != nullptr) hasSolution = true; // If a solution path has been computed, save it in the problem definition object. if (hasSolution) { Motion *m; std::vector<unsigned int> durations( 1, findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m)); std::vector<Motion *> mpath(1, m); while (m->parent_) { durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m)); mpath.push_back(m); } auto path(std::make_shared<PathControl>(si_)); double dt = siC_->getPropagationStepSize(); path->append(mpath.back()->endState_); for (int i = (int)mpath.size() - 2; i > 0; i--) path->append(mpath[i - 1]->startState_, mpath[i]->control_, durations[i] * dt); path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt); pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName()); } si_->freeState(tmpState1); si_->freeState(tmpState2); OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size()); return base::PlannerStatus(hasSolution, isApproximate); }