void PrefsWindow::read_memory_prefs(void)
{
const char *str = rom_control->Text();
if (strlen(str))
PrefsReplaceString("rom", str);
else
PrefsRemoveItem("rom");
}
BView *PrefsWindow::create_volumes_pane(void)
{
BView *pane = new BView(BRect(0, 0, top_frame.right-20, top_frame.bottom-80), GetString(STR_VOLUMES_PANE_TITLE), B_FOLLOW_NONE, B_WILL_DRAW);
pane->SetViewColor(fill_color);
float right = pane->Bounds().right-10;
const char *str;
int32 index = 0;
volume_list = new VolumeListView(BRect(15, 10, pane->Bounds().right-30, 113), "volumes");
while ((str = PrefsFindString("disk", index++)) != NULL)
volume_list->AddItem(new BStringItem(str));
volume_list->SetSelectionMessage(new BMessage(MSG_VOLUME_SELECTED));
volume_list->SetInvocationMessage(new BMessage(MSG_VOLUME_INVOKED));
pane->AddChild(new BScrollView("volumes_border", volume_list, B_FOLLOW_LEFT | B_FOLLOW_TOP, 0, false, true));
pane->AddChild(new BButton(BRect(10, 118, pane->Bounds().right/3, 138), "add_volume", GetString(STR_ADD_VOLUME_BUTTON), new BMessage(MSG_ADD_VOLUME)));
pane->AddChild(new BButton(BRect(pane->Bounds().right/3, 118, pane->Bounds().right*2/3, 138), "create_volume", GetString(STR_CREATE_VOLUME_BUTTON), new BMessage(MSG_CREATE_VOLUME)));
pane->AddChild(new BButton(BRect(pane->Bounds().right*2/3, 118, pane->Bounds().right-11, 138), "remove_volume", GetString(STR_REMOVE_VOLUME_BUTTON), new BMessage(MSG_REMOVE_VOLUME)));
extfs_control = new PathControl(true, BRect(10, 145, right, 160), "extfs", GetString(STR_EXTFS_CTRL), PrefsFindString("extfs"), NULL);
extfs_control->SetDivider(90);
pane->AddChild(extfs_control);
BMenuField *menu_field;
BPopUpMenu *menu = new BPopUpMenu("");
menu_field = new BMenuField(BRect(10, 165, right, 180), "bootdriver", GetString(STR_BOOTDRIVER_CTRL), menu);
menu_field->SetDivider(90);
menu->AddItem(new BMenuItem(GetString(STR_BOOT_ANY_LAB), new BMessage(MSG_BOOT_ANY)));
menu->AddItem(new BMenuItem(GetString(STR_BOOT_CDROM_LAB), new BMessage(MSG_BOOT_CDROM)));
pane->AddChild(menu_field);
int32 i32 = PrefsFindInt32("bootdriver");
BMenuItem *item;
if (i32 == 0) {
if ((item = menu->FindItem(GetString(STR_BOOT_ANY_LAB))) != NULL)
item->SetMarked(true);
} else if (i32 == CDROMRefNum) {
if ((item = menu->FindItem(GetString(STR_BOOT_CDROM_LAB))) != NULL)
item->SetMarked(true);
}
nocdrom_checkbox = new BCheckBox(BRect(10, 185, right, 200), "nocdrom", GetString(STR_NOCDROM_CTRL), new BMessage(MSG_NOCDROM));
pane->AddChild(nocdrom_checkbox);
nocdrom_checkbox->SetValue(PrefsFindBool("nocdrom") ? B_CONTROL_ON : B_CONTROL_OFF);
return pane;
}
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);
}
ompl::base::PlannerStatus ompl::control::SST::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())
{
Motion *motion = new Motion(siC_);
si_->copyState(motion->state_, st);
siC_->nullControl(motion->control_);
nn_->add(motion);
motion->accCost_ = opt_->identityCost();
findClosestWitness(motion);
}
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();
if (!controlSampler_)
controlSampler_ = siC_->allocControlSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure\n", getName().c_str(), nn_->size());
Motion *solution = NULL;
Motion *approxsol = NULL;
double approxdif = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(siC_);
base::State *rstate = rmotion->state_;
Control *rctrl = rmotion->control_;
base::State *xstate = si_->allocState();
unsigned iterations = 0;
while (ptc == false)
{
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* find closest state in the tree */
Motion *nmotion = selectNode(rmotion);
/* sample a random control that attempts to go towards the random state, and also sample a control duration */
controlSampler_->sample(rctrl);
unsigned int cd = rng_.uniformInt(siC_->getMinControlDuration(),siC_->getMaxControlDuration());
unsigned int propCd = siC_->propagateWhileValid(nmotion->state_,rctrl,cd,rstate);
if (propCd == cd)
{
base::Cost incCost = opt_->motionCost(nmotion->state_, rstate);
base::Cost cost = opt_->combineCosts(nmotion->accCost_, incCost);
Witness* closestWitness = findClosestWitness(rmotion);
if (closestWitness->rep_ == rmotion || opt_->isCostBetterThan(cost,closestWitness->rep_->accCost_))
{
Motion* oldRep = closestWitness->rep_;
/* create a motion */
Motion *motion = new Motion(siC_);
motion->accCost_ = cost;
si_->copyState(motion->state_, rmotion->state_);
siC_->copyControl(motion->control_, rctrl);
motion->steps_ = cd;
motion->parent_ = nmotion;
nmotion->numChildren_++;
closestWitness->linkRep(motion);
nn_->add(motion);
double dist = 0.0;
bool solv = goal->isSatisfied(motion->state_, &dist);
if (solv && opt_->isCostBetterThan(motion->accCost_,prevSolutionCost_))
{
approxdif = dist;
solution = motion;
for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
if (prevSolution_[i])
si_->freeState(prevSolution_[i]);
prevSolution_.clear();
for (unsigned int i = 0 ; i < prevSolutionControls_.size() ; ++i)
if (prevSolutionControls_[i])
siC_->freeControl(prevSolutionControls_[i]);
prevSolutionControls_.clear();
prevSolutionSteps_.clear();
Motion* solTrav = solution;
while(solTrav->parent_!=NULL)
{
prevSolution_.push_back(si_->cloneState(solTrav->state_) );
prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_) );
prevSolutionSteps_.push_back(solTrav->steps_ );
solTrav = solTrav->parent_;
}
prevSolution_.push_back(si_->cloneState(solTrav->state_) );
prevSolutionCost_ = solution->accCost_;
OMPL_INFORM("Found solution with cost %.2f",solution->accCost_.value());
sufficientlyShort = opt_->isSatisfied(solution->accCost_);
if (sufficientlyShort)
break;
}
if (solution==NULL && dist < approxdif)
{
approxdif = dist;
approxsol = motion;
for (unsigned int i = 0 ; i < prevSolution_.size() ; ++i)
if (prevSolution_[i])
si_->freeState(prevSolution_[i]);
prevSolution_.clear();
for (unsigned int i = 0 ; i < prevSolutionControls_.size() ; ++i)
if (prevSolutionControls_[i])
siC_->freeControl(prevSolutionControls_[i]);
prevSolutionControls_.clear();
prevSolutionSteps_.clear();
Motion *solTrav = approxsol;
while (solTrav->parent_!=NULL)
{
prevSolution_.push_back(si_->cloneState(solTrav->state_) );
prevSolutionControls_.push_back(siC_->cloneControl(solTrav->control_) );
prevSolutionSteps_.push_back(solTrav->steps_ );
solTrav = solTrav->parent_;
}
prevSolution_.push_back(si_->cloneState(solTrav->state_) );
}
if (oldRep != rmotion)
{
oldRep->inactive_ = true;
nn_->remove(oldRep);
while (oldRep->inactive_ && oldRep->numChildren_==0)
{
if (oldRep->state_)
si_->freeState(oldRep->state_);
if (oldRep->control_)
siC_->freeControl(oldRep->control_);
oldRep->state_=NULL;
oldRep->control_=NULL;
oldRep->parent_->numChildren_--;
Motion* oldRepParent = oldRep->parent_;
delete oldRep;
oldRep = oldRepParent;
}
}
}
}
iterations++;
}
bool solved = false;
bool approximate = false;
if (solution == NULL)
{
solution = approxsol;
approximate = true;
}
if (solution != NULL)
{
/* set the solution path */
PathControl *path = new PathControl(si_);
for (int i = prevSolution_.size() - 1 ; i >= 1 ; --i)
path->append(prevSolution_[i], prevSolutionControls_[i-1], prevSolutionSteps_[i-1] * siC_->getPropagationStepSize());
path->append(prevSolution_[0]);
solved = true;
pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
}
si_->freeState(xstate);
if (rmotion->state_)
si_->freeState(rmotion->state_);
if (rmotion->control_)
siC_->freeControl(rmotion->control_);
delete rmotion;
OMPL_INFORM("%s: Created %u states in %u iterations", getName().c_str(), nn_->size(),iterations);
return base::PlannerStatus(solved, approximate);
}
ompl::base::PlannerStatus ompl::control::RRT::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())
{
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, st);
siC_->nullControl(motion->control);
nn_->add(motion);
}
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();
if (!controlSampler_)
controlSampler_ = siC_->allocDirectedControlSampler();
OMPL_INFORM("%s: Starting with %u states", getName().c_str(), nn_->size());
Motion *solution = NULL;
Motion *approxsol = NULL;
double approxdif = std::numeric_limits<double>::infinity();
Motion *rmotion = new Motion(siC_);
base::State *rstate = rmotion->state;
Control *rctrl = rmotion->control;
base::State *xstate = si_->allocState();
while (ptc == false)
{
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* find closest state in the tree */
Motion *nmotion = nn_->nearest(rmotion);
/* sample a random control that attempts to go towards the random state, and also sample a control duration */
unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state);
if (addIntermediateStates_)
{
// this code is contributed by Jennifer Barry
std::vector<base::State *> pstates;
cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true);
if (cd >= siC_->getMinControlDuration())
{
Motion *lastmotion = nmotion;
bool solved = false;
size_t p = 0;
for ( ; p < pstates.size(); ++p)
{
/* create a motion */
Motion *motion = new Motion();
motion->state = pstates[p];
//we need multiple copies of rctrl
motion->control = siC_->allocControl();
siC_->copyControl(motion->control, rctrl);
motion->steps = 1;
motion->parent = lastmotion;
lastmotion = motion;
nn_->add(motion);
double dist = 0.0;
solved = goal->isSatisfied(motion->state, &dist);
if (solved)
{
approxdif = dist;
solution = motion;
break;
}
if (dist < approxdif)
{
approxdif = dist;
approxsol = motion;
}
}
//free any states after we hit the goal
while (++p < pstates.size())
si_->freeState(pstates[p]);
if (solved)
break;
}
else
for (size_t p = 0 ; p < pstates.size(); ++p)
si_->freeState(pstates[p]);
}
else
{
if (cd >= siC_->getMinControlDuration())
{
/* create a motion */
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, rmotion->state);
siC_->copyControl(motion->control, rctrl);
motion->steps = cd;
motion->parent = nmotion;
nn_->add(motion);
double dist = 0.0;
bool solv = goal->isSatisfied(motion->state, &dist);
if (solv)
{
approxdif = dist;
solution = motion;
break;
}
if (dist < approxdif)
{
approxdif = dist;
approxsol = motion;
}
}
}
}
bool solved = false;
bool approximate = false;
if (solution == NULL)
{
solution = approxsol;
approximate = true;
}
if (solution != NULL)
{
lastGoalMotion_ = solution;
/* 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);
solved = true;
pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
}
if (rmotion->state)
si_->freeState(rmotion->state);
if (rmotion->control)
siC_->freeControl(rmotion->control);
delete rmotion;
si_->freeState(xstate);
OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
return base::PlannerStatus(solved, approximate);
}
bool ompl::control::Syclop::solve(const base::PlannerTerminationCondition& ptc)
{
checkValidity();
if (!graphReady_)
{
numMotions_ = 0;
setupRegionEstimates();
setupEdgeEstimates();
graphReady_ = true;
}
while (const base::State* s = pis_.nextStart())
{
const int region = decomp_->locateRegion(s);
startRegions_.insert(region);
Motion* startMotion = addRoot(s);
graph_[boost::vertex(region,graph_)].motions.push_back(startMotion);
++numMotions_;
updateCoverageEstimate(graph_[boost::vertex(region,graph_)], s);
}
if (startRegions_.empty())
{
msg_.error("There are no valid start states");
return false;
}
//We need at least one valid goal sample so that we can find the goal region
if (goalRegions_.empty())
{
if (const base::State* g = pis_.nextGoal(ptc))
goalRegions_.insert(decomp_->locateRegion(g));
else
{
msg_.error("Unable to sample a valid goal state");
return false;
}
}
msg_.inform("Starting with %u states", numMotions_);
std::vector<Motion*> newMotions;
const Motion* solution = NULL;
base::Goal* goal = pdef_->getGoal().get();
double goalDist = std::numeric_limits<double>::infinity();
bool solved = false;
while (!ptc() && !solved)
{
const int chosenStartRegion = startRegions_.sampleUniform();
int chosenGoalRegion = -1;
// if we have not sampled too many goal regions already
if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2)
{
if (const base::State* g = pis_.nextGoal())
{
chosenGoalRegion = decomp_->locateRegion(g);
goalRegions_.insert(chosenGoalRegion);
}
}
if (chosenGoalRegion == -1)
chosenGoalRegion = goalRegions_.sampleUniform();
computeLead(chosenStartRegion, chosenGoalRegion);
computeAvailableRegions();
for (int i = 0; i < numRegionExpansions_ && !solved && !ptc(); ++i)
{
const int region = selectRegion();
bool improved = false;
for (int j = 0; j < numTreeSelections_ && !solved && !ptc(); ++j)
{
newMotions.clear();
selectAndExtend(graph_[boost::vertex(region,graph_)], newMotions);
for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc(); ++m)
{
Motion* motion = *m;
double distance;
solved = goal->isSatisfied(motion->state, &distance);
if (solved)
{
goalDist = distance;
solution = motion;
break;
}
// Check for approximate (best-so-far) solution
if (distance < goalDist)
{
goalDist = distance;
solution = motion;
}
const int newRegion = decomp_->locateRegion(motion->state);
graph_[boost::vertex(newRegion,graph_)].motions.push_back(motion);
++numMotions_;
Region& newRegionObj = graph_[boost::vertex(newRegion, graph_)];
improved |= updateCoverageEstimate(newRegionObj, motion->state);
if (newRegion != region)
{
// If this is the first time the tree has entered this region, add the region to avail
if (newRegionObj.motions.size() == 1)
availDist_.add(newRegion, newRegionObj.weight);
/* If the tree crosses an entire region and creates an edge (u,v) for which Proj(u) and Proj(v) are non-neighboring regions,
then we do not update connection estimates. This is because Syclop's shortest-path lead computation only considers neighboring regions. */
if (regionsToEdge_.count(std::pair<int,int>(region, newRegion)) > 0)
{
Adjacency* adj = regionsToEdge_[std::pair<int,int>(region,newRegion)];
adj->empty = false;
++adj->numSelections;
improved |= updateConnectionEstimate(graph_[boost::vertex(region,graph_)], newRegionObj, motion->state);
}
}
}
}
if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
break;
}
}
bool addedSolution = false;
if (solution != NULL)
{
std::vector<const 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);
goal->addSolutionPath(base::PathPtr(path), !solved, goalDist);
addedSolution = true;
}
return addedSolution;
}
ompl::base::PlannerStatus ompl::control::PID::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
while (const base::State *st = pis_.nextStart())
{
motions.push_back(new Motion(siC_));
si_->copyState(motions.back()->state, st);
siC_->nullControl(motions.back()->control);
}
if(motions.empty())
{
OMPL_ERROR("Invalid Start State");
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
{
sampler_ = si_->allocStateSampler();
}
if (!controlSampler_)
{
controlSampler_ = siC_->allocDirectedControlSampler();
}
bool solved = false;
double approxdif = std::numeric_limits<double>::infinity();
while (ptc == false)
{
/* create a motion */
// Add PID control action
Motion* new_motion = new Motion(siC_);
new_motion->state = si_->allocState();
siC_->nullControl(new_motion->control);
control_action(new_motion->control, motions.back()->state);
// Add State Propagation
unsigned int cd = siC_->propagateWhileValid(motions.back()->state, new_motion->control, siC_->getMinControlDuration(), new_motion->state);
motions.push_back(new_motion);
double dist = 0.0;
bool solv = goal->isSatisfied(new_motion->state, &dist);
if(solv)
{
solved = true;
}
if(dist < approxdif)
{
approxdif = dist;
}
}
/* set the solution path */
PathControl *path = new PathControl(si_);
for (unsigned i = 0 ; i < motions.size()-1; ++i)
{
path->append(motions[i]->state, motions[i]->control, siC_->getPropagationStepSize());
}
path->append(motions[motions.size()-1]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, approxdif);
return base::PlannerStatus(true, solved);
}
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::control::Syclop::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
if (!graphReady_)
{
numMotions_ = 0;
setupRegionEstimates();
setupEdgeEstimates();
graphReady_ = true;
}
while (const base::State *s = pis_.nextStart())
{
const int region = decomp_->locateRegion(s);
startRegions_.insert(region);
Motion *startMotion = addRoot(s);
graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
++numMotions_;
updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
}
if (startRegions_.empty())
{
OMPL_ERROR("%s: There are no valid start states", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
//We need at least one valid goal sample so that we can find the goal region
if (goalRegions_.empty())
{
if (const base::State *g = pis_.nextGoal(ptc))
goalRegions_.insert(decomp_->locateRegion(g));
else
{
OMPL_ERROR("%s: Unable to sample a valid goal state", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
}
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
std::vector<Motion*> newMotions;
const Motion *solution = NULL;
base::Goal *goal = pdef_->getGoal().get();
double goalDist = std::numeric_limits<double>::infinity();
bool solved = false;
while (!ptc && !solved)
{
const int chosenStartRegion = startRegions_.sampleUniform();
int chosenGoalRegion = -1;
// if we have not sampled too many goal regions already
if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_/2)
{
if (const base::State *g = pis_.nextGoal())
{
chosenGoalRegion = decomp_->locateRegion(g);
goalRegions_.insert(chosenGoalRegion);
}
}
if (chosenGoalRegion == -1)
chosenGoalRegion = goalRegions_.sampleUniform();
leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
computeAvailableRegions();
for (int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
{
const int region = selectRegion();
bool improved = false;
for (int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
{
newMotions.clear();
selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
for (std::vector<Motion*>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
{
Motion *motion = *m;
double distance;
solved = goal->isSatisfied(motion->state, &distance);
if (solved)
{
goalDist = distance;
solution = motion;
break;
}
// Check for approximate (best-so-far) solution
if (distance < goalDist)
{
goalDist = distance;
solution = motion;
}
const int newRegion = decomp_->locateRegion(motion->state);
graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
++numMotions_;
Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
improved |= updateCoverageEstimate(newRegionObj, motion->state);
/* If tree has just crossed from one region to its neighbor,
update the connection estimates. If the tree has crossed an entire region,
then region and newRegion are not adjacent, and so we do not update estimates. */
if (newRegion != region
&& regionsToEdge_.count(std::pair<int,int>(region,newRegion)) > 0)
{
Adjacency *adj = regionsToEdge_[std::pair<int,int>(region,newRegion)];
adj->empty = false;
++adj->numSelections;
improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj, motion->state);
}
/* If this region already exists in availDist, update its weight. */
if (newRegionObj.pdfElem != NULL)
availDist_.update(newRegionObj.pdfElem, newRegionObj.weight);
/* Otherwise, only add this region to availDist
if it already exists in the lead. */
else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
{
PDF<int>::Element *elem = availDist_.add(newRegion, newRegionObj.weight);
newRegionObj.pdfElem = elem;
}
}
}
if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
break;
}
}
bool addedSolution = false;
if (solution != NULL)
{
std::vector<const 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);
pdef_->addSolutionPath(base::PathPtr(path), !solved, goalDist, getName());
addedSolution = true;
}
return addedSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
BView *PrefsWindow::create_memory_pane(void)
{
char str[256], str2[256];
BView *pane = new BView(BRect(0, 0, top_frame.right-20, top_frame.bottom-80), GetString(STR_MEMORY_MISC_PANE_TITLE), B_FOLLOW_NONE, B_WILL_DRAW);
pane->SetViewColor(fill_color);
float right = pane->Bounds().right-10;
BEntry entry("/boot/var/swap");
off_t swap_space;
if (entry.GetSize(&swap_space) == B_NO_ERROR)
max_ramsize = swap_space / (1024 * 1024) - 8;
else
max_ramsize = sys_info.max_pages * B_PAGE_SIZE / (1024 * 1024) - 8;
int32 value = PrefsFindInt32("ramsize") / (1024 * 1024);
ramsize_slider = new RAMSlider(BRect(10, 5, right, 55), "ramsize", GetString(STR_RAMSIZE_SLIDER), new BMessage(MSG_RAMSIZE), 1, max_ramsize, B_TRIANGLE_THUMB);
ramsize_slider->SetValue(value);
ramsize_slider->UseFillColor(true, &slider_fill_color);
sprintf(str, GetString(STR_RAMSIZE_FMT), 1);
sprintf(str2, GetString(STR_RAMSIZE_FMT), max_ramsize);
ramsize_slider->SetLimitLabels(str, str2);
pane->AddChild(ramsize_slider);
BMenuField *menu_field;
BMenuItem *item;
BPopUpMenu *menu;
int id = PrefsFindInt32("modelid");
menu = new BPopUpMenu("");
menu_field = new BMenuField(BRect(10, 60, right, 75), "modelid", GetString(STR_MODELID_CTRL), menu);
menu_field->SetDivider(120);
menu->AddItem(item = new BMenuItem(GetString(STR_MODELID_5_LAB), new BMessage(MSG_MODELID_5)));
if (id == 5)
item->SetMarked(true);
menu->AddItem(item = new BMenuItem(GetString(STR_MODELID_14_LAB), new BMessage(MSG_MODELID_14)));
if (id == 14)
item->SetMarked(true);
pane->AddChild(menu_field);
int cpu = PrefsFindInt32("cpu");
bool fpu = PrefsFindBool("fpu");
menu = new BPopUpMenu("");
menu_field = new BMenuField(BRect(10, 82, right, 97), "cpu", GetString(STR_CPU_CTRL), menu);
menu_field->SetDivider(120);
menu->AddItem(item = new BMenuItem(GetString(STR_CPU_68020_LAB), new BMessage(MSG_CPU_68020)));
if (cpu == 2 && !fpu)
item->SetMarked(true);
menu->AddItem(item = new BMenuItem(GetString(STR_CPU_68020_FPU_LAB), new BMessage(MSG_CPU_68020_FPU)));
if (cpu == 2 && fpu)
item->SetMarked(true);
menu->AddItem(item = new BMenuItem(GetString(STR_CPU_68030_LAB), new BMessage(MSG_CPU_68030)));
if (cpu == 3 && !fpu)
item->SetMarked(true);
menu->AddItem(item = new BMenuItem(GetString(STR_CPU_68030_FPU_LAB), new BMessage(MSG_CPU_68030_FPU)));
if (cpu == 3 && fpu)
item->SetMarked(true);
menu->AddItem(item = new BMenuItem(GetString(STR_CPU_68040_LAB), new BMessage(MSG_CPU_68040)));
if (cpu == 4)
item->SetMarked(true);
pane->AddChild(menu_field);
rom_control = new PathControl(false, BRect(10, 104, right, 119), "rom", GetString(STR_ROM_FILE_CTRL), PrefsFindString("rom"), NULL);
rom_control->SetDivider(117);
pane->AddChild(rom_control);
return pane;
}
void PrefsWindow::read_volumes_prefs(void)
{
PrefsReplaceString("extfs", extfs_control->Text());
}
