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!");
}
}
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 SvgElementVisitor::visit(clan::DomElement &e)
{
if (e.get_namespace_uri() != Svg::svg_ns) return;
// To do: do a more efficient search for the correct handler
auto local_name = e.get_local_name();
if (local_name == "a") a(e);
else if (local_name == "altGlyph") altGlyph(e);
else if (local_name == "altGlyphDef") altGlyphDef(e);
else if (local_name == "altGlyphItem") altGlyphItem(e);
else if (local_name == "animate") animate(e);
else if (local_name == "animateColor") animateColor(e);
else if (local_name == "animateMotion") animateMotion(e);
else if (local_name == "animateTransform") animateTransform(e);
else if (local_name == "circle") circle(e);
else if (local_name == "clipPath") clipPath(e);
else if (local_name == "color-profile") color_profile(e);
else if (local_name == "cursor") cursor(e);
else if (local_name == "defs") defs(e);
else if (local_name == "desc") desc(e);
else if (local_name == "ellipse") ellipse(e);
else if (local_name == "feBlend") feBlend(e);
else if (local_name == "feColorMatrix") feColorMatrix(e);
else if (local_name == "feComponentTransfer") feComponentTransfer(e);
else if (local_name == "feComposite") feComposite(e);
else if (local_name == "feConvolveMatrix") feConvolveMatrix(e);
else if (local_name == "feDiffuseLighting") feDiffuseLighting(e);
else if (local_name == "feDisplacementMap") feDisplacementMap(e);
else if (local_name == "feDistantLight") feDistantLight(e);
else if (local_name == "feFlood") feFlood(e);
else if (local_name == "feFuncA") feFuncA(e);
else if (local_name == "feFuncB") feFuncB(e);
else if (local_name == "feFuncG") feFuncG(e);
else if (local_name == "feFuncR") feFuncR(e);
else if (local_name == "feGaussianBlur") feGaussianBlur(e);
else if (local_name == "feImage") feImage(e);
else if (local_name == "feMerge") feMerge(e);
else if (local_name == "feMergeNode") feMergeNode(e);
else if (local_name == "feMorphology") feMorphology(e);
else if (local_name == "feOffset") feOffset(e);
else if (local_name == "fePointLight") fePointLight(e);
else if (local_name == "feSpecularLighting") feSpecularLighting(e);
else if (local_name == "feSpotLight") feSpotLight(e);
else if (local_name == "feTile") feTile(e);
else if (local_name == "feTurbulence") feTurbulence(e);
else if (local_name == "filter") filter(e);
else if (local_name == "font") font(e);
else if (local_name == "font-face") font_face(e);
else if (local_name == "font-face-format") font_face_format(e);
else if (local_name == "font-face-name") font_face_name(e);
else if (local_name == "font-face-src") font_face_src(e);
else if (local_name == "font-face-uri") font_face_uri(e);
else if (local_name == "foreignObject") foreignObject(e);
else if (local_name == "g") g(e);
else if (local_name == "glyph") glyph(e);
else if (local_name == "glyphRef") glyphRef(e);
else if (local_name == "hkern") hkern(e);
else if (local_name == "image") image(e);
else if (local_name == "line") line(e);
else if (local_name == "linearGradient") linearGradient(e);
else if (local_name == "marker") marker(e);
else if (local_name == "mask") mask(e);
else if (local_name == "metadata") metadata(e);
else if (local_name == "missing-glyph") missing_glyph(e);
else if (local_name == "mpath") mpath(e);
else if (local_name == "path") path(e);
else if (local_name == "pattern") pattern(e);
else if (local_name == "polygon") polygon(e);
else if (local_name == "polyline") polyline(e);
else if (local_name == "radialGradient") radialGradient(e);
else if (local_name == "rect") rect(e);
else if (local_name == "script") script(e);
else if (local_name == "set") set(e);
else if (local_name == "stop") stop(e);
else if (local_name == "style") style(e);
else if (local_name == "svg") svg(e);
else if (local_name == "switch") switch_(e);
else if (local_name == "symbol") symbol(e);
else if (local_name == "text") text(e);
else if (local_name == "textPath") textPath(e);
else if (local_name == "title") title(e);
else if (local_name == "tref") tref(e);
else if (local_name == "tspan") tspan(e);
else if (local_name == "use") use(e);
else if (local_name == "view") view(e);
else if (local_name == "vkern") vkern(e);
}