Example #1
0
DifferentialCPG::DifferentialCPG(
    const ::gazebo::physics::ModelPtr &_model,
    const sdf::ElementPtr _settings,
    const std::vector< revolve::gazebo::MotorPtr > &_motors,
    const std::vector< revolve::gazebo::SensorPtr > &_sensors)
    : nextState_(nullptr)
    , input_(new double[_sensors.size()])
    , output_(new double[_motors.size()])
{
  // Create transport node
  this->node_.reset(new gz::transport::Node());
  this->node_->Init();

  auto name = _model->GetName();
  // Listen to network modification requests
//  alterSub_ = node_->Subscribe(
//      "~/" + name + "/modify_diff_cpg", &DifferentialCPG::Modify,
//      this);

  if (not _settings->HasElement("rv:brain"))
  {
    std::cerr << "No robot brain detected, this is probably an error."
              << std::endl;
    throw std::runtime_error("DifferentialCPG brain did not receive settings");
  }

  std::cout << _settings->GetDescription() << std::endl;
  auto motor = _settings->HasElement("rv:motor")
               ? _settings->GetElement("rv:motor")
               : sdf::ElementPtr();
  while(motor)
  {
    if (not motor->HasAttribute("x") or not motor->HasAttribute("y"))
    {
      std::cerr << "Missing required motor attributes (x- and/or y- coordinate)"
                << std::endl;
      throw std::runtime_error("Robot brain error");
    }
    auto motorId = motor->GetAttribute("part_id")->GetAsString();
    auto coordX = std::atoi(motor->GetAttribute("x")->GetAsString().c_str());
    auto coordY = std::atoi(motor->GetAttribute("y")->GetAsString().c_str());

    this->positions_[motorId] = {coordX, coordY};
    this->neurons_[{coordX, coordY, 1}] = {1.f, 0.f, 0.f};
    this->neurons_[{coordX, coordY, -1}] = {1.f, 0.f, 0.f};

//    TODO: Add check for duplicate coordinates

    motor = motor->GetNextElement("rv:motor");
  }

  // Add connections between neighbouring neurons
  for (const auto &position : this->positions_)
  {
    auto name = position.first;
    int x, y; std::tie(x, y) = position.second;

    if (this->connections_.count({x, y, 1, x, y, -1}))
    {
      continue;
    }
    if (this->connections_.count({x, y, -1, x, y, 1}))
    {
      continue;
    }
    this->connections_[{x, y, 1, x, y, -1}] = 1.f;
    this->connections_[{x, y, -1, x, y, 1}] = 1.f;

    for (const auto &neighbour : this->positions_)
    {
      int nearX, nearY;
      std::tie(nearX, nearY) = neighbour.second;
      if ((x+1) == nearX or (y+1) == nearY or (x-1) == nearX or (y-1) == nearY)
      {
        this->connections_[{x, y, 1, nearX, nearY, 1}] = 1.f;
        this->connections_[{nearX, nearY, 1, x, y, 1}] = 1.f;
      }
    }
  }

  // Initialise array of neuron states for Update() method
  this->nextState_ = new double[this->neurons_.size()];
}
//Here we already have the model!
bool robotSim::gazeboConfigureHook(gazebo::physics::ModelPtr model) {
    if (model.get() == NULL) {
        RTT::log(RTT::Error) << "No model could be loaded" << RTT::endlog();
        return false;
    }

    if (!_models_loaded) {
        RTT::log(RTT::Error) << "URDF and SRDF models has not been passed. Call loadURDFAndSRDF(URDF_path, SRDF_path)" << RTT::endlog();
        return false;
    }

    // Get the joints
    gazebo_joints_ = model->GetJoints();
    model_links_ = model->GetLinks();

    RTT::log(RTT::Info) << "Model name "<< model->GetName() << RTT::endlog();
    RTT::log(RTT::Info) << "Model has " << gazebo_joints_.size() << " joints" << RTT::endlog();
    RTT::log(RTT::Info) << "Model has " << model_links_.size() << " links" << RTT::endlog();

    for(unsigned int i = 0; i < _xbotcore_model.get_chain_names().size(); ++i){
        std::string chain_name = _xbotcore_model.get_chain_names()[i];
        std::vector<std::string> enabled_joints_in_chain;
        _xbotcore_model.get_enabled_joints_in_chain(chain_name,enabled_joints_in_chain);

        kinematic_chains.insert(std::pair<std::string, boost::shared_ptr<KinematicChain>>(
            chain_name, boost::shared_ptr<KinematicChain>(
                new KinematicChain(chain_name, enabled_joints_in_chain, *(this->ports()), model
#ifdef USE_INTROSPECTION
                ,this
#endif
                ))));
    }

    RTT::log(RTT::Info) << "Kinematic Chains map created!" << RTT::endlog();

    for(std::map<std::string, boost::shared_ptr<KinematicChain>>::iterator it = kinematic_chains.begin();
        it != kinematic_chains.end(); it++){
        if(!(it->second->initKinematicChain(gains.Gains))){
            RTT::log(RTT::Warning) << "Problem Init Kinematic Chain" <<
                it->second->getKinematicChainName() << RTT::endlog();
            return false;
        }
    }
    RTT::log(RTT::Info) << "Kinematic Chains Initialized!" << RTT::endlog();



    RTT::log(RTT::Info) << "Checking Sensors (URDF/SRDF)"<<RTT::endlog();
    std::map<std::string,int> ft_srdf = _xbotcore_model.get_ft_sensors();
    std::map<std::string,int> imu_srdf = _xbotcore_model.get_imu_sensors();


    RTT::log(RTT::Info) << "Force Update of SensorManager (before accessing sensors)!" << RTT::endlog();
    gazebo::sensors::SensorManager::Instance()->Update(true);

    RTT::log(RTT::Info) << "Checking Sensors (Gazebo)"<<RTT::endlog();
    gazebo::sensors::Sensor_V sensors = gazebo::sensors::SensorManager::Instance()->GetSensors();

    gazebo::sensors::Sensor_V sensors_attached_to_robot;
    for(unsigned int i = 0; i < sensors.size(); ++i){
        if(sensors[i]->ScopedName().find("::"+model->GetName()+"::") != std::string::npos)
            sensors_attached_to_robot.push_back(sensors[i]);
    }

    for(std::map<std::string,int>::iterator i = ft_srdf.begin(); i != ft_srdf.end(); i++)
    {
        force_torque_sensor ft(i->first, model, _xbotcore_model.get_urdf_model(),
                               sensors_attached_to_robot,
                               *(this->ports())
    #ifdef USE_INTROSPECTION
                               ,this
    #endif
                               );
        if(ft.isInited())
            force_torque_sensors.push_back(ft);
    }

    for(std::map<std::string,int>::iterator i = imu_srdf.begin(); i != imu_srdf.end(); ++i){
        imu_sensor imu_(i->first,sensors_attached_to_robot,*(this->ports()));
        if(imu_.isInited())
            imu_sensors.push_back(imu_);
    }



    RTT::log(RTT::Warning) << "Done Configuring Component" << RTT::endlog();
    return true;
}