Exemple #1
0
  EReturn AICOProblem::initDerived(tinyxml2::XMLHandle & handle)
  {
    EReturn ret_value = SUCCESS;
    tinyxml2::XMLElement* xmltmp;
    bool hastime = false;
    XML_CHECK("T");
    XML_OK(getInt(*xmltmp, T));
    if (T <= 2)
    {
      INDICATE_FAILURE
      ;
      return PAR_ERR;
    }
    xmltmp = handle.FirstChildElement("duration").ToElement();
    if (xmltmp)
    {
      XML_OK(getDouble(*xmltmp, tau));
      tau = tau / ((double) T);
      hastime = true;
    }
    if (hastime)
    {
      xmltmp = handle.FirstChildElement("tau").ToElement();
      if (xmltmp)
        WARNING("Duration has already been specified, tau is ignored.");
    }
    else
    {
      XML_CHECK("tau");
      XML_OK(getDouble(*xmltmp, tau));
    }

    XML_CHECK("Qrate");
    XML_OK(getDouble(*xmltmp, Q_rate));
    XML_CHECK("Hrate");
    XML_OK(getDouble(*xmltmp, H_rate));
    XML_CHECK("Wrate");
    XML_OK(getDouble(*xmltmp, W_rate));
    {
      Eigen::VectorXd tmp;
      XML_CHECK("W");
      XML_OK(getVector(*xmltmp, tmp));
      W = Eigen::MatrixXd::Identity(tmp.rows(), tmp.rows());
      W.diagonal() = tmp;
    }
    for (TaskDefinition_map::const_iterator it = task_defs_.begin();
        it != task_defs_.end() and ok(ret_value); ++it)
    {
      if (it->second->type().compare(std::string("TaskSqrError")) == 0)
        ERROR("Task variable " << it->first << " is not an squared error!");
    }
    // Set number of time steps
    return setTime(T);
  }
Exemple #2
0
 void Identity::initDerived(tinyxml2::XMLHandle & handle)
 {
   // Load the goal
   if (handle.FirstChildElement("Ref").ToElement())
   {
       getVector(*(handle.FirstChildElement("Ref").ToElement()), jointRef);
       jointMap.resize(jointRef.rows());
       for (int i = 0; i < jointRef.rows(); i++)
           jointMap[i] = i;
       useRef = true;
   }
 }
  void TaskTerminationCriterion::initDerived(tinyxml2::XMLHandle & handle)
  {
    TaskSqrError::initDerived(handle);
      double thr;
      if (handle.FirstChildElement("Threshold").ToElement())
      {
        getDouble(*(handle.FirstChildElement("Threshold").ToElement()),thr);
        threshold0_(0) = thr;
      }
      else
      {
        throw_named("Threshold was not specified");
      }

    setTimeSteps(1);
  }
Exemple #4
0
 void IMesh::initDerived(tinyxml2::XMLHandle & handle)
 {
   EParam<std_msgs::String> ref;
   tinyxml2::XMLHandle tmp_handle = handle.FirstChildElement("ReferenceFrame");
   server_->registerParam(ns_, tmp_handle, ref);
   if (!tmp_handle.ToElement()) ref->data = "/world";
   initDebug(ref->data);
   eff_size_ = scene_->getMapSize(object_name_);
   weights_.setOnes(eff_size_, eff_size_);
   initialised_ = true;
 }
Exemple #5
0
 void IKProblem::initDerived(tinyxml2::XMLHandle & handle)
 {
   tinyxml2::XMLElement* xmltmp;
   xmltmp = handle.FirstChildElement("W").ToElement();
   if (xmltmp)
   {
     Eigen::VectorXd tmp;
     getVector(*xmltmp, tmp);
     config_w_ = Eigen::MatrixXd::Identity(tmp.rows(), tmp.rows());
     config_w_.diagonal() = tmp;
   }
   xmltmp = handle.FirstChildElement("Tolerance").ToElement();
   if (xmltmp)
   {
     getDouble(*xmltmp, tau_);
   }
   xmltmp = handle.FirstChildElement("T").ToElement();
   if (xmltmp)
   {
     try
     {
       getInt(*xmltmp, T_);
     }
     catch (Exception e)
     {
       T_ = 1;
     }
   }
   else
   {
     T_ = 1;
   }
   for (auto& it : task_defs_)
   {
     it.second->setTimeSteps(T_);
   }
 }
Exemple #6
0
void OMPLsolver::initDerived(tinyxml2::XMLHandle & handle)
{
    tinyxml2::XMLHandle tmp_handle = handle.FirstChildElement(
                                         "TrajectorySmooth");
    server_->registerParam<std_msgs::Bool>(ns_, tmp_handle, smooth_);

    tmp_handle = handle.FirstChildElement("Solver");
    server_->registerParam<std_msgs::String>(ns_, tmp_handle, solver_);

    tmp_handle = handle.FirstChildElement("SolverPackage");
    server_->registerParam<std_msgs::String>(ns_, tmp_handle,solver_package_);

    try
    {
        HIGHLIGHT_NAMED(object_name_,
                        "Using ["<<solver_->data<<"] from package ["<<solver_package_->data<<"].");
        base_solver_ = OMPLBaseSolver::base_solver_loader.createInstance(
                           "ompl_solver/" + solver_->data);
    } catch (pluginlib::PluginlibException& ex)
    {
        throw_named("EXOTica-OMPL plugin failed to load solver "<<solver_->data<<". Solver package: '"<<solver_package_->data<< "'. \nError: " << ex.what());
    }
    base_solver_->initialiseBaseSolver(handle, server_);
}
Exemple #7
0
 EReturn Initialiser::initialise(tinyxml2::XMLHandle root_handle,
     Server_ptr & server)
 {
   server = Server::Instance();
   tinyxml2::XMLHandle server_handle(root_handle.FirstChildElement("Server"));
   if (server_handle.ToElement())
   {
     if (ok(server->initialise(server_handle)))
     {
       return SUCCESS;
     }
     else
     {
       INDICATE_FAILURE
       return FAILURE;
     }
   }
   else
   {
Exemple #8
0
 void Server::initialise(tinyxml2::XMLHandle & handle)
 {
   static bool first = true;
   if (!singleton_server_)
   {
     name_ = handle.ToElement()->Attribute("name");
     std::string ns = name_;
   }
   tinyxml2::XMLHandle param_handle(handle.FirstChildElement("Parameters"));
   tinyxml2::XMLHandle tmp_handle = param_handle.FirstChild();
   while (tmp_handle.ToElement())
   {
     createParam(name_, tmp_handle);
     tmp_handle = tmp_handle.NextSibling();
   }
   if (first)
   {
     HIGHLIGHT_NAMED(name_, "EXOTica Server Initialised");
     first = false;
   }
 }
Exemple #9
0
  EReturn OMPLsolver::initDerived(tinyxml2::XMLHandle & handle)
  {
    tinyxml2::XMLHandle tmp_handle = handle.FirstChildElement(
        "TrajectorySmooth");
    server_->registerParam<std_msgs::Bool>(ns_, tmp_handle, smooth_);

    tmp_handle = handle.FirstChildElement("SaveResults");
    server_->registerParam<std_msgs::Bool>(ns_, tmp_handle, saveResults_);

    tinyxml2::XMLElement* xmltmp;
    std::string txt;
    XML_CHECK("algorithm");
    {
      txt = std::string(xmltmp->GetText());
      bool known = false;
      std::vector<std::string> nm = getPlannerNames();
      for (std::string s : nm)
      {
        if (txt.compare(s.substr(11)) == 0)
        {
          selected_planner_ = s;
          INFO(
              "Using planning algorithm: "<<selected_planner_<<". Trajectory smoother is "<<(smooth_->data?"Enabled":"Disabled"));
          known = true;
          break;
        }
      }
      if (!known)
      {
        ERROR("Unknown planning algorithm ["<<txt<<"]");
        HIGHLIGHT_NAMED(object_name_, "Available algorithms:");
        for (std::string s : nm)
          HIGHLIGHT_NAMED(object_name_, s);
        return FAILURE;
      }
    }

    XML_CHECK("max_goal_sampling_attempts");
    XML_OK(getInt(*xmltmp, goal_ampling_max_attempts_));

    projection_components_.clear();
    tmp_handle = handle.FirstChildElement("Projection");
    if (tmp_handle.ToElement())
    {
      projector_ = tmp_handle.ToElement()->Attribute("type");
      tinyxml2::XMLHandle jnt_handle = tmp_handle.FirstChildElement(
          "component");
      while (jnt_handle.ToElement())
      {
        const char* atr = jnt_handle.ToElement()->Attribute("name");
        if (atr)
        {
          projection_components_.push_back(std::string(atr));
        }
        else
        {
          INDICATE_FAILURE
          ;
          return FAILURE;
        }
        jnt_handle = jnt_handle.NextSiblingElement("component");
      }
    }

    tmp_handle = handle.FirstChildElement("Range");
    if (tmp_handle.ToElement())
    {
      server_->registerParam<std_msgs::String>(ns_, tmp_handle, range_);
    }

    if (saveResults_->data)
    {
      std::string path = ros::package::getPath("ompl_solver") + "/result/" + txt
          + ".txt";
      result_file_.open(path);
      if (!result_file_.is_open())
      {
        ERROR("Error open "<<path);
        return FAILURE;
      }
    }
    succ_cnt_ = 0;
    return SUCCESS;
  }
Exemple #10
0
 void Server::createParam(const std::string & ns,
     tinyxml2::XMLHandle & tmp_handle)
 {
   std::string name = ns + "/" + tmp_handle.ToElement()->Name();
   std::string type = tmp_handle.ToElement()->Attribute("type");
   if (type.compare("int") == 0)
   {
     double dou;
     getDouble(*tmp_handle.FirstChildElement("default").ToElement(), dou);
     std_msgs::Int64 val;
     val.data = dou;
     params_[name] = boost::shared_ptr<std_msgs::Int64>(
         new std_msgs::Int64(val));
   }
   else if (type.compare("double") == 0)
   {
     double dou;
     getDouble(*tmp_handle.FirstChildElement("default").ToElement(), dou);
     std_msgs::Float64 val;
     val.data = dou;
     params_[name] = boost::shared_ptr<std_msgs::Float64>(
         new std_msgs::Float64(val));
   }
   else if (type.compare("vector") == 0)
   {
     exotica::Vector vec;
     getStdVector(*tmp_handle.FirstChildElement("default").ToElement(),
             vec.data);
     params_[name] = boost::shared_ptr<exotica::Vector>(
         new exotica::Vector(vec));
   }
   else if (type.compare("bool") == 0)
   {
     bool val;
     getBool(*tmp_handle.FirstChildElement("default").ToElement(), val);
     std_msgs::Bool ros_bool;
     ros_bool.data = val;
     params_[name] = boost::shared_ptr<std_msgs::Bool>(
         new std_msgs::Bool(ros_bool));
   }
   else if (type.compare("boollist") == 0)
   {
     exotica::BoolList boollist;
     std::vector<bool> vec;
     getBoolVector(*tmp_handle.FirstChildElement("default").ToElement(),
             vec);
     boollist.data.resize(vec.size());
     for (int i = 0; i < vec.size(); i++)
       boollist.data[i] = vec[i];
     params_[name] = boost::shared_ptr<exotica::BoolList>(
         new exotica::BoolList(boollist));
   }
   else if (type.compare("string") == 0)
   {
     std::string str =
         tmp_handle.FirstChildElement("default").ToElement()->GetText();
     if (str.size() == 0)
     {
       throw_pretty("Invalid string!");
     }
     std_msgs::String ros_s;
     ros_s.data =
         tmp_handle.FirstChildElement("default").ToElement()->GetText();
     params_[name] = boost::shared_ptr<std_msgs::String>(
         new std_msgs::String(ros_s));
   }
   else if (type.compare("stringlist") == 0)
   {
     exotica::StringList list;
     getList(*tmp_handle.FirstChildElement("default").ToElement(),
             list.strings);
     params_[name] = boost::shared_ptr<exotica::StringList>(
         new exotica::StringList(list));
   }
   else
   {
     throw_pretty("Unknown type!");
   }
   INFO("Register new paramter "<<name)
 }