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