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