EReturn Initialiser::initialiseProblemJSON(PlanningProblem_ptr problem,
const std::string& constraints)
{
{
Document document;
if (!document.Parse<0>(constraints.c_str()).HasParseError())
{
if (ok(problem->reinitialise(document, problem)))
{
// Everythinh is fine
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
}
else
{
ERROR(
"Can't parse constraints from JSON string!\n"<<document.GetParseError() <<"\n"<<constraints.substr(document.GetErrorOffset(),50));
return FAILURE;
}
}
return SUCCESS;
}
bool OMPLsolver::isSolvable(const PlanningProblem_ptr & prob)
{
if (prob->type().compare("exotica::OMPLProblem") == 0) return true;
return false;
}
EReturn OMPLsolver::specifyProblem(PlanningProblem_ptr goals,
PlanningProblem_ptr costs, PlanningProblem_ptr goalBias,
PlanningProblem_ptr samplingBias)
{
if (goals->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR("This solver can't use problem of type '" << goals->type() << "'!");
return PAR_INV;
}
if (costs
&& costs->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR("This solver can't use problem of type '" << costs->type() << "'!");
return PAR_INV;
}
if (goalBias
&& goalBias->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR(
"This solver can't use problem of type '" << goalBias->type() << "'!");
return PAR_INV;
}
if (samplingBias
&& samplingBias->type().compare(std::string("exotica::OMPLProblem"))
!= 0)
{
ERROR(
"This solver can't use problem of type '" << samplingBias->type() << "'!");
return PAR_INV;
}
MotionSolver::specifyProblem(goals);
prob_ = boost::static_pointer_cast<OMPLProblem>(goals);
costs_ = boost::static_pointer_cast<OMPLProblem>(costs);
goalBias_ = boost::static_pointer_cast<OMPLProblem>(goalBias);
samplingBias_ = boost::static_pointer_cast<OMPLProblem>(samplingBias);
for (auto & it : prob_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
if (costs)
{
for (auto & it : costs_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
if (goalBias_)
{
for (auto & it : goalBias_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
if (samplingBias_)
{
for (auto & it : samplingBias_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
compound_ = prob_->isCompoundStateSpace();
if (compound_)
{
state_space_ = ob::StateSpacePtr(
OMPLSE3RNCompoundStateSpace::FromProblem(prob_, server_));
}
else
{
state_space_ = OMPLStateSpace::FromProblem(prob_);
}
HIGHLIGHT_NAMED(object_name_,
"Using state space: "<<state_space_->getName());
ompl_simple_setup_.reset(new og::SimpleSetup(state_space_));
ompl_simple_setup_->setStateValidityChecker(
ob::StateValidityCheckerPtr(new OMPLStateValidityChecker(this)));
ompl_simple_setup_->setPlannerAllocator(
boost::bind(known_planners_[selected_planner_], _1,
this->getObjectName() + "_" + selected_planner_));
if (compound_)
ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
boost::bind(&allocSE3RNStateSampler,
(ob::StateSpace*) state_space_.get()));
ompl_simple_setup_->getSpaceInformation()->setup();
std::vector<std::string> jnts;
prob_->getScenes().begin()->second->getJointNames(jnts);
if (projector_.compare("Joints") == 0)
{
bool projects_ok_ = true;
std::vector<int> vars(projection_components_.size());
for (int i = 0; i < projection_components_.size(); i++)
{
bool found = false;
for (int j = 0; j < jnts.size(); j++)
{
if (projection_components_[i].compare(jnts[j]) == 0)
{
vars[i] = j;
found = true;
break;
}
}
if (!found)
{
WARNING(
"Projection joint ["<<projection_components_[i]<<"] does not exist, OMPL Projection Evaluator not used");
projects_ok_ = false;
break;
}
}
if (projects_ok_)
{
ompl_simple_setup_->getStateSpace()->registerDefaultProjection(
ompl::base::ProjectionEvaluatorPtr(
new exotica::OMPLProjection(state_space_, vars)));
std::string tmp;
for (int i = 0; i < projection_components_.size(); i++)
tmp = tmp + "[" + projection_components_[i] + "] ";
HIGHLIGHT_NAMED(object_name_, " Using projection joints "<<tmp);
}
}
else if (projector_.compare("DMesh") == 0)
{
// Construct default DMesh projection relationship
std::vector<std::pair<int, int> > tmp_index(
projection_components_.size() - 1);
for (int i = 0; i < tmp_index.size(); i++)
{
tmp_index[i].first = i;
tmp_index[i].second = tmp_index.size();
}
ompl_simple_setup_->getStateSpace()->registerDefaultProjection(
ompl::base::ProjectionEvaluatorPtr(
new exotica::DMeshProjection(state_space_, projection_components_,
tmp_index, prob_->getScenes().begin()->second)));
std::string tmp;
for (int i = 0; i < projection_components_.size(); i++)
tmp = tmp + "[" + projection_components_[i] + "] ";
HIGHLIGHT_NAMED(object_name_, " Using DMesh Projection links "<<tmp);
}
else
{
WARNING_NAMED(object_name_,
"Unknown projection type "<<projector_<<". Setting ProjectionEvaluator failed.");
}
ompl_simple_setup_->setGoal(constructGoal());
ompl_simple_setup_->setup();
if (isFlexiblePlanner())
{
INFO_NAMED(object_name_,
"Setting up FRRT Local planner from file\n"<<prob_->local_planner_config_);
if (!ompl_simple_setup_->getPlanner()->as<ompl::geometric::FlexiblePlanner>()->setUpLocalPlanner(
prob_->local_planner_config_, prob_->scenes_.begin()->second))
{
INDICATE_FAILURE
return FAILURE;
}
}
