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));
}
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");
}
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);
}
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();
}
}