private: void worldUpdate(){
#if(PRINT_DEBUG)
       cout << "Updating the world for time: " << world->GetSimTime().Float() << endl;
#endif

       averageLinearVelocity += trunk->GetWorldCoGLinearVel() / 10.0;
       count++;
       if (world->GetSimTime().nsec % (10 * 1000000) == 0) {
          assert(count == 10);
          outputCSVX << averageLinearVelocity.x << ", ";
          outputCSVY << averageLinearVelocity.y << ", ";
          outputCSVZ << averageLinearVelocity.z << ", ";
          averageLinearVelocity = 0;
          count = 0;
       }

        if(world->GetSimTime().Float() >= MAX_TIME){
          #if(PRINT_DEBUG)
          cout << "Scenario completed. Updating results" << endl;
          #endif
          event::Events::DisconnectWorldUpdateBegin(this->connection);

          outputCSVX << endl;
          outputCSVX.close();

          outputCSVY << endl;
          outputCSVY.close();

          outputCSVZ << endl;
          outputCSVZ.close();
        }
    }
    private: void worldUpdate(){
        physics::LinkPtr head = model->GetLink("head_neck");
        #if(PRINT_DEBUG)
        cout << "Updating HIC for time " << world->GetSimTime() << endl;
        #endif
        updateMaximumHic(false);

        if(world->GetSimTime().Float() >= 2.0){
          #if(PRINT_DEBUG)
          cout << "Scenario completed. Updating results" << endl;
          #endif
          event::Events::DisconnectWorldUpdateBegin(this->connection);

           // Drain the HIC calculation
           for (unsigned int i = 0; i < ACC_HISTORY_MAX; ++i) {
              updateMaximumHic(true);
           }

          // Disconnect the sensors
          for(unsigned int i = 0; i < boost::size(contacts); ++i){
            sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + model->GetScopedName()
                                                       + "::" + contacts[i]);
            if(sensor == nullptr){
              cout << "Could not find sensor " << contacts[i] << endl;
              continue;
            }
            sensor->SetActive(false);
            sensor->DisconnectUpdated(sensorConnections[i]);
          }
          sensorConnections.clear();
          printResults();
          exit(0);
        }
    }
    private: void worldUpdate(){
#if(PRINT_DEBUG)
       cout << "Updating the world for time: " << world->GetSimTime().Float() << endl;
#endif
        outputCSV << world->GetSimTime().Float() << ", ";
        for(unsigned int i = 0; i < boost::size(contacts); ++i){
           outputCSV << contactForces[contacts[i]].GetLength() << ", ";
        }
        outputCSV << endl;

        if(world->GetSimTime().Float() >= MAX_TIME){
          #if(PRINT_DEBUG)
          cout << "Scenario completed. Updating results" << endl;
          #endif
          event::Events::DisconnectWorldUpdateBegin(this->connection);

          // Disconnect the sensors
          for(unsigned int i = 0; i < boost::size(contacts); ++i){
            sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + model->GetScopedName()
                                                       + "::" + contacts[i]);
            if(sensor == nullptr){
              cout << "Could not find sensor " << contacts[i] << endl;
              continue;
            }
            sensor->SetActive(false);
            sensor->DisconnectUpdated(sensorConnections[i]);
          }

          sensorConnections.clear();
          outputCSV << endl;
          outputCSV.close();
        }
    }
 public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
   world = _model->GetWorld();
   model = _model;
   #if(PRINT_DEBUG)
   cout << "Loading the contact forces plugin" << endl;
   #endif
   
   #if(PRINT_SENSORS)
   cout << "Listing all sensors: " << endl;
   vector<sensors::SensorPtr> sensors = sensors::SensorManager::Instance()->GetSensors();
   for(unsigned int i = 0; i < sensors.size(); ++i){
     cout << sensors[i]->GetScopedName() << endl;
   }
   cout << endl;
   #endif
   
   for(unsigned int i = 0; i < boost::size(contacts); ++i){
     sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + _model->GetScopedName()
                                                    + "::" + contacts[i]);
     if(sensor == nullptr){
         cout << "Could not find sensor " << contacts[i] << endl;
         continue;
     }
     sensor->SetActive(true);
     sensorConnections.push_back(sensor->ConnectUpdated(
     boost::bind(&ContactForcesPlugin::updateContacts, this, sensor, contacts[i])));
   }
   connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ContactForcesPlugin::worldUpdate, this));
 }
    private: void updateMaximumHic(bool drain) {
       // Get the current velocity for the head link.
       physics::LinkPtr head = model->GetLink("head_neck");
       assert(head);

       if (!drain) {
          headLinearAcc.push_back(head->GetWorldLinearAccel());
       }
       if(headLinearAcc.size() > ACC_HISTORY_MAX || drain) {
          headLinearAcc.erase(headLinearAcc.begin(), headLinearAcc.begin() + 1);
       }

       assert(headLinearAcc.size() <= ACC_HISTORY_MAX);

       // Now search for maximum value across all size dimensions
       for (unsigned int i = 0; i < headLinearAcc.size(); ++i) {
          for (unsigned int j = i + MIN_HIC_ACC_TIME; j < headLinearAcc.size(); ++j) {
             double hicCurrMax = hic(static_cast<double>(i) / SEC_TO_MSEC, static_cast<double>(j) / SEC_TO_MSEC, accMean(headLinearAcc.begin() + i, headLinearAcc.begin() + j + 1));
             if (hicCurrMax >= maximumHic) {
                maximumHic = hicCurrMax;
                maximumHicTime = world->GetSimTime();
             }
          }
       }
    }
    public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
      world = _model->GetWorld();
      model = _model;

      #if(PRINT_DEBUG)
      cout << "Loading the angular momentum over time plugin" << endl;
      #endif
      connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&AngularMomentumOverTimePlugin::worldUpdate, this));

      // Get the name of the folder to store the result in
      const char* resultsFolder = std::getenv("RESULTS_FOLDER");
      if(resultsFolder == nullptr){
         cout << "Results folder not set. Using current directory." << endl;
         resultsFolder = "./";
      }

      const string resultsFileName = string(resultsFolder) + "/" + "angular_momentum.csv";
      outputCSV.open(resultsFileName, ios::out);
      assert(outputCSV.is_open());
      writeHeader(outputCSV);

       math::Vector3 angularMomentum;
       for (unsigned int i = 0; i < boost::size(links); ++i) {
          const physics::LinkPtr link = model->GetLink(links[i]);
          angularMomentum += link->GetWorldInertiaMatrix() * link->GetWorldAngularVel();
       }

       // Write out t0
       outputCSV << world->GetSimTime().Double() << ", " << angularMomentum.x / 10.0
       << ", " << angularMomentum.y / 10.0 << ", " << angularMomentum.z / 10.0 << endl;
    }
Esempio n. 7
0
	void KeyboardController::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf) {
        printf("Loading plugin\n");
        this->updateConnection = event::Events::ConnectPreRender(
            boost::bind(&KeyboardController::Update, this));

        this->node = transport::NodePtr(new transport::Node());
  		this->node->Init(_world->GetName());
        this->publisher = this->node->Advertise<msgs::Vector3d>("~/camcontrol");
    }
    void worldUpdate() {
#if(PRINT_DEBUG)
        cout << "Updating the world for time: " << world->GetSimTime().Float() << endl;
#endif
        outputCSV << world->GetSimTime().Float() << ", ";
        for(unsigned int i = 0; i < boost::size(links); ++i) {
            outputCSV << model->GetLink(links[i])->GetWorldLinearAccel().GetLength() << ", ";
        }
        outputCSV << endl;

        if(world->GetSimTime().Float() >= MAX_TIME) {
#if(PRINT_DEBUG)
            cout << "Scenario completed. Updating results" << endl;
#endif
            event::Events::DisconnectWorldUpdateBegin(this->connection);

            outputCSV << endl;
            outputCSV.close();
        }
    }
 private: void worldUpdate(){
    if(this->model->GetWorld()->GetSimTime().nsec % 10000000 != 0){
       return;
    }
    
   csvFile << world->GetSimTime().Float() << ", ";
   // Iterate over model joints and print them
   for (unsigned int i = 0; i < boost::size(joints); ++i) {
     physics::JointPtr currJoint = model->GetJoint(joints[i]);
     for(unsigned int j = 0; j < currJoint->GetAngleCount(); ++j){
       csvFile << "   " << currJoint->GetForce(j) << ", ";
     }
   }
   csvFile << endl;
 }
    private: void worldUpdate(){
#if(PRINT_DEBUG)
       cout << "Updating the world for time: " << world->GetSimTime().Float() << endl;
#endif
       for (unsigned int i = 0; i < boost::size(links); ++i) {
          const physics::LinkPtr link = model->GetLink(links[i]);
          averageAngularMomentum +=  link->GetWorldInertiaMatrix() * link->GetWorldAngularVel();
       }
       if (world->GetSimTime().nsec % (10 * 1000000) == 0) {
          outputCSV << world->GetSimTime().Double() << ", " << averageAngularMomentum.x / 10.0
          << ", " << averageAngularMomentum.y / 10.0 << ", " << averageAngularMomentum.z / 10.0 << endl;
          averageAngularMomentum = math::Vector3();
       }

        if(world->GetSimTime().Float() >= MAX_TIME){
          #if(PRINT_DEBUG)
          cout << "Scenario completed. Updating results" << endl;
          #endif
          event::Events::DisconnectWorldUpdateBegin(this->connection);

          outputCSV << endl;
          outputCSV.close();
        }
    }
    public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
      world = _model->GetWorld();
      model = _model;
      #if(PRINT_DEBUG)
      cout << "Loading the contact forces per link over time plugin" << endl;
      #endif
      
      #if(PRINT_SENSORS)
      cout << "Listing all sensors: " << endl;
      vector<sensors::SensorPtr> sensors = sensors::SensorManager::Instance()->GetSensors();
      for(unsigned int i = 0; i < sensors.size(); ++i){
        cout << sensors[i]->GetScopedName() << endl;
      }
      cout << endl;
      #endif
      
      for(unsigned int i = 0; i < boost::size(contacts); ++i){
        sensors::SensorPtr sensor = sensors::SensorManager::Instance()->GetSensor(world->GetName() + "::" + _model->GetScopedName()
                                                       + "::" + contacts[i]);
        if(sensor == nullptr){
            cout << "Could not find sensor " << contacts[i] << endl;
            continue;
        }
        sensor->SetActive(true);
        sensorConnections.push_back(sensor->ConnectUpdated(
        boost::bind(&ContactForcesPerLinkOverTimePlugin
::contactForceForSensor, this, sensor, contacts[i])));
      }
      connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ContactForcesPerLinkOverTimePlugin
::worldUpdate, this));

      // Get the name of the folder to store the result in
      const char* resultsFolder = std::getenv("RESULTS_FOLDER");
      if(resultsFolder == nullptr){
         cout << "Results folder not set. Using current directory." << endl;
         resultsFolder = "./";
      }

      const string resultsFileName = string(resultsFolder) + "/" + "contact_forces_per_link.csv";
      outputCSV.open(resultsFileName, ios::out);
      assert(outputCSV.is_open());
      writeHeader(outputCSV);
    }
Esempio n. 12
0
bool doQueryGazeboVisibility(physics::WorldPtr world, sdf::ElementPtr sdf, gazebo_visibility_ros::QueryGazeboVisibility::Request &request, gazebo_visibility_ros::QueryGazeboVisibility::Response &response)
{
    ROS_INFO("Got QueryGazeboVisibility service request.");
    physics::ModelPtr model = world->GetModel(request.name);
    if((!model.get()))
    {
        ROS_INFO("Model name not loaded.");
        response.visible = false;
        return true;
    }
    ROS_INFO("Model name seems to make sense, and points to a model of type %d with %d children.", model->GetType(), model->GetChildCount());

    math::Box bbox = getModelBox(model);

    math::Vector3 start, cameraFwd, cameraSide, cameraUp;
    start.x = request.camera_pose.x;
    start.y = request.camera_pose.y;
    start.z = request.camera_pose.z;
    cameraFwd.x = request.camera_fwd.x;
    cameraFwd.y = request.camera_fwd.y;
    cameraFwd.z = request.camera_fwd.z;
    cameraFwd = cameraFwd.Normalize();
    cameraUp.x = request.camera_up.x;
    cameraUp.y = request.camera_up.y;
    cameraUp.z = request.camera_up.z;
    cameraUp = cameraUp.Normalize();
    cameraUp = cameraUp - (cameraUp*cameraFwd)*cameraFwd;
    cameraUp = cameraUp.Normalize();
    cameraSide = cameraUp.Cross(cameraFwd);

    double h2 = request.height*0.5;
    double w2 = request.width*0.5;

    math::Vector3 aux = request.focal_distance*cameraFwd + h2*cameraUp;
    aux = aux.Normalize();
    double upAngle = aux.Dot(cameraFwd);
    aux = request.focal_distance*cameraFwd + w2*cameraSide;
    aux = aux.Normalize();
    double sideAngle = aux.Dot(cameraFwd);

    std::vector<math::Vector3> points;

    getBBoxGrid(bbox, 8, points);
    ROS_INFO("Got a grid of points to test.");

    int maxK = points.size();
    int okPoints = 0;

    if(5 <= GAZEBO_MAJOR_VERSION)
    {
        gazebo::physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
        engine->InitForThread();

        gazebo::physics::ShapePtr ray = engine->CreateShape("ray", gazebo::physics::CollisionPtr());

        for(int k = 0; k < maxK; k++)
        {
            math::Vector3 end = getEndPoint(start, points[k], request.max_distance);
            if(inImage(start, end, cameraFwd, cameraSide, cameraUp, upAngle, sideAngle))
            {
                boost::dynamic_pointer_cast<gazebo::physics::RayShape>(ray)->SetPoints(start, end);
                double dist;
                std::string entityName;
                boost::dynamic_pointer_cast<gazebo::physics::RayShape>(ray)->GetIntersection(dist, entityName);
                if(request.name == trimSubEnts(entityName))
                    okPoints++;
            }
        }
    }
    else
    {
        for(int k = 0; k < maxK; k++)
        {
            /*A bit of a hack to get something running on older gazebo versions, even if a little.*/
            math::Vector3 end = getEndPoint(start, points[k], request.max_distance);
            if(inImage(start, end, cameraFwd, cameraSide, cameraUp, upAngle, sideAngle))
                okPoints++;
        }
    }

    ROS_INFO("Done with raytrace.");

    response.visible = false;
    if(request.threshold <= (okPoints*1.0)/(maxK*1.0))
        response.visible = true;

    return true;
}