EReturn AICOProblem::reinitialise(rapidjson::Document& document,
boost::shared_ptr<PlanningProblem> problem)
{
clear();
if (document.IsArray())
{
for (rapidjson::SizeType i = 0; i < document.Size(); i++)
{
rapidjson::Value& obj = document[i];
if (obj.IsObject())
{
std::string constraintClass;
if (ok(getJSON(obj["class"], constraintClass)))
{
if (knownMaps_.find(constraintClass) != knownMaps_.end())
{
TaskMap_ptr taskmap;
if (ok(
TaskMap_fac::Instance().createObject(
knownMaps_[constraintClass], taskmap)))
{
EReturn ret = taskmap->initialise(obj, server_, scenes_,
problem);
if (ok(ret))
{
if (ret != CANCELLED)
{
std::string name = taskmap->getObjectName();
task_maps_[name] = taskmap;
TaskDefinition_ptr task;
if (ok(
TaskDefinition_fac::Instance().createObject(
"TaskSqrError", task)))
{
TaskSqrError_ptr sqr = boost::static_pointer_cast<
TaskSqrError>(task);
sqr->setTaskMap(taskmap);
int dim;
taskmap->taskSpaceDim(dim);
sqr->y_star0_.resize(dim);
sqr->rho0_(0) = 0.0;
sqr->rho1_(0) = 1e4;
sqr->object_name_ = name
+ std::to_string((unsigned long) sqr.get());
// TODO: Better implementation of stting goals from JSON
sqr->y_star0_.setZero();
sqr->setTimeSteps(T + 2);
Eigen::VectorXd tspan(2);
Eigen::VectorXi tspani(2);
if (obj["tspan"]["__ndarray__"].IsArray())
{
getJSON(obj["tspan"]["__ndarray__"], tspan);
}
else
{
getJSON(obj["tspan"], tspan);
}
if (tspan(0) <= 0.0) tspan(0) = 0.0;
if (tspan(1) >= 1.0) tspan(1) = 1.0;
tspani(0) = (int) (T * tspan(0));
tspani(1) = (int) (T * tspan(1));
for (int t = tspani(0); t <= tspani(1); t++)
{
sqr->registerRho(
Eigen::VectorXdRef_ptr(sqr->rho1_.segment(0, 1)),
t);
}
sqr->wasFullyInitialised_ = true;
task_defs_[name] = task;
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
}
else
{
ROS_WARN_STREAM(
"Creation of '"<<constraintClass<<"' cancelled!");
}
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
}
else
{
WARNING("Ignoring unknown constraint '"<<constraintClass<<"'");
}
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
else
{
INDICATE_FAILURE
;
return FAILURE;
}
return SUCCESS;
}
void IKProblem::reinitialise(rapidjson::Document& document,
boost::shared_ptr<PlanningProblem> problem)
{
clear();
if (document.IsArray())
{
for (rapidjson::SizeType i = 0; i < document.Size(); i++)
{
rapidjson::Value& obj = document[i];
if (obj.IsObject())
{
std::string constraintClass;
getJSON(obj["class"], constraintClass);
if (knownMaps_.find(constraintClass) != knownMaps_.end())
{
TaskMap_ptr taskmap = Initialiser::createMap(knownMaps_[constraintClass]);
taskmap->initialise(obj, server_, scene_,problem);
std::string name = taskmap->getObjectName();
task_maps_[name] = taskmap;
TaskDefinition_ptr task = Initialiser::createDefinition("TaskSqrError");
TaskSqrError_ptr sqr = boost::static_pointer_cast<TaskSqrError>(task);
sqr->setTaskMap(taskmap);
int dim;
taskmap->taskSpaceDim(dim);
sqr->y_star0_.resize(dim);
sqr->rho0_(0) = 0.0;
sqr->rho1_(0) = 1.0;
sqr->object_name_ = name+ std::to_string((unsigned long) sqr.get());
// TODO: Better implementation of stting goals from JSON
sqr->y_star0_.setZero();
sqr->setTimeSteps(T_);
Eigen::VectorXd tspan(2);
Eigen::VectorXi tspani(2);
// TODO fix ndarray problem
getJSON(obj["tspan"], tspan);
if (tspan(0) <= 0.0) tspan(0) = 0.0;
if (tspan(1) >= 1.0) tspan(1) = 1.0;
tspani(0) = (int) ((T_ - 1) * tspan(0));
tspani(1) = (int) ((T_ - 1) * tspan(1));
for (int t = tspani(0); t <= tspani(1); t++)
{
sqr->registerRho(Eigen::VectorXdRef_ptr(sqr->rho1_.segment(0, 1)),t);
}
sqr->wasFullyInitialised_ = true;
task_defs_[name] = task;
}
else
{
// WARNING("Ignoring unknown constraint '"<<constraintClass<<"'");
}
}
{
throw_named("Invalid JSON document object!");
}
}
}
else
{
throw_named("Invalid JSON array!");
}
}