예제 #1
0
  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;

  }
예제 #2
0
  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!");
    }
  }