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);
}
