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