world.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 902 行 · 第 1/2 页

        PRINT_ERR1("error loading plugin: %s", dlerror());

      i=j+1;
    }
  }
  
  // try to load it from the directory where the config file is
  if (!handle && worldfilepath)
  {
    // Note that dirname() modifies the contents, so
    // we need to make a copy of the filename.
    tmp = strdup(worldfilepath);
    memset(fullpath,0,PATH_MAX);
    cfgdir = dirname(tmp);
    if(cfgdir[0] != '/' && cfgdir[0] != '~')
    {
      getcwd(fullpath, PATH_MAX);
      strcat(fullpath,"/");
    }
    strcat(fullpath,cfgdir);
    strcat(fullpath,"/");
    strcat(fullpath,pluginname);
    free(tmp);

    PRINT_MSG1(1, "loading %s...", fullpath);      
    if(!(handle = dlopen(fullpath, RTLD_NOW)))
      PRINT_ERR1("error loading plugin: %s", dlerror());
  }

  // works in more than one version of autotools.  Fix this later.
  // try to load it from prefix/lib/player/plugins
  if(!handle)
  {
    memset(fullpath,0,PATH_MAX);
    strcpy(fullpath,GAZEBO_PLUGIN_PATH);
    strcat(fullpath,"/");
    strcat(fullpath,pluginname);

    PRINT_MSG1(1, "loading %s...", fullpath);      
    if(!(handle = dlopen(fullpath, RTLD_NOW)))
      PRINT_ERR1("error loading plugin: %s", dlerror());
  }

  // Now invoke the initialization function
  if(handle)
  {
    fflush(stdout);
    initfunc = (gz_plugin_init_fn_t) dlsym(handle, "gazebo_plugin_init");
    if((error = dlerror()) != NULL)
    {
      PRINT_ERR1("plugin init failed [%s]", error);
      return -1;
    }    
    if((*initfunc)() != 0)
    {
      PRINT_ERR("plugin init failed");
      return -1;
    }
    return 0;
  }
  else
    return -1;

  return 0;
  
#else
  PRINT_ERR("Sorry, no support for shared libraries, so can't load plugins");
  return 0;
#endif
}


//////////////////////////////////////////////////////////////////////////////
// Save the world
// This save only changes to pose, and only for top-level objects
int World::Save()
{
  int i;
  Model *model;
  
  for (i = 0; i < this->modelCount; i++)
  {
    model = this->models[i];
    if (model->parent != NULL)
      continue;
    model->initPose = model->GetPose();
    model->node->SetPosition("xyz", model->GetPose().pos);
    model->node->SetRotation("rpy", model->GetPose().rot);
  }

  PRINT_MSG1(1, "\nsaving: %s\n", this->worldFile->filename);
  this->worldFile->Save(NULL);
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Set the model pose and the pose of its attached children.  
void World::SetModelPose( Model *model, GzPose pose )
{
  int i;
  Model *child;
  GzPose opose, npose, cpose;

  // Get the current global pose of the model
  opose = model->GetPose();

  // Compute the new global pose of the model
  if (model->parent)
  {
    cpose = model->parentBody->GetPose();
    npose = GzCoordPoseAdd( pose, cpose );
  }
  else
  {
    npose = pose;
  }

  // Recursively move children
  for (i = 0; i < this->modelCount; i++)
  {
    child = this->models[i];
    if (child->parent != model)
      continue;

    // Compute current relative pose of child
    cpose = GzCoordPoseSub(child->GetPose(), opose);

    // Compute the new global pose of the child
    cpose = GzCoordPoseAdd( cpose, npose );

    // Compute the new child pose relative to the current model pose
    cpose = GzCoordPoseSub( cpose, opose );

    // Move child 
    this->SetModelPose( child, cpose );
  }

  // Set the new model pose
  model->SetPose( npose );

  // Set our relative pose
  if (model->parentBody && model->joint == NULL)
    model->relPose = GzCoordPoseSub(model->GetPose(), model->parentBody->GetPose());

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Reset the world
void World::ResetModelPoses()
{
  int i;
  Model *model;
  
  // Update models
  for (i = 0; i < this->modelCount; i++)
  {
    model = this->models[i];
    this->SetModelPose(model, model->initPose);
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Single-step the world
void World::Step()
{
  int i;
  Model *model;
  GzPose pose;
  gz_sim_data_t *data;
  bool reset, save, pause;

  data = this->gz_sim->data;
  
  // Check for commands from the external interface
  gz_sim_lock(this->gz_sim, 1);
  pause = data->pause;
  reset = data->reset;
  data->reset = 0;
  save = data->save;
  data->save = 0;
  gz_sim_unlock(this->gz_sim);

  // Reset poses on request
  if (reset)
    this->ResetModelPoses();

  // Save poses on request
  if (save)
    this->Save();

  // If we are paused, we do something a bit sneaky: we update the
  // models, but dont advance the simulator time.  This allows models
  // like the ObserverCam to continue to provide feedback.
  if (pause)
  {
    for (i = 0; i < this->modelCount; i++)
    {
      model = this->models[i];
      model->MasterUpdate( 0.0 );
    }

    // Increment the pause time
    this->pauseTime += this->stepTime;
  }

  else
  {
    // Update models
    for (i = 0; i < this->modelCount; i++)
    {
      model = this->models[i];
      model->MasterUpdate( this->stepTime );
    }
  
    // Increment the time
    this->simTime += this->stepTime;

    // Do collision detection; this will add contacts to the contact
    // group
    dSpaceCollide( this->spaceId, this, CollisionCallback );
    
    // Update the dynamical model
    dWorldStep( this->worldId, this->stepTime );
    //dWorldStepFast1( this->worldId, step, 10 );

    // Very important to clear out the contact group
    dJointGroupEmpty( this->contactGroup );
  
    // Move models with dummy bodies, since ODE wont do it
    for (i = 0; i < this->modelCount; i++)
    {
      model = this->models[i];
      if (model->parentBody != NULL && model->joint == NULL)
      {
        pose = GzCoordPoseAdd(model->relPose, model->parentBody->GetPose());
        model->SetPose(pose);
      }
    }
  }

  // Update the external interface
  gz_sim_lock(this->gz_sim, 1);
  data->sim_time = this->GetSimTime();
  data->pause_time = this->GetPauseTime();
  data->real_time = this->GetRealTime();
  gz_sim_unlock(this->gz_sim);

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Do collision detection
void World::CollisionCallback( void *data, dGeomID o1, dGeomID o2 )
{
  int i,n;
  World *self;
  Geom *geom1, *geom2;
  dContactGeom contactGeoms[10];
  dContact contactInfo;
  dJointID joint;
  int num;

  self = (World*) data;
  
  // Maximum number of contacts
  num = sizeof(contactGeoms) / sizeof(contactGeoms[0]);
    
  // If either geom is a space...
  if (dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ))
  {
    // If the spaces/geoms belong to different spaces, collide them
    if (dGeomGetSpace(o1) != dGeomGetSpace(o2))
      dSpaceCollide2( o1, o2, self, &CollisionCallback );

    // If the spaces belong the world space, collide them
    else if (dGeomGetSpace(o1) == self->spaceId || dGeomGetSpace(o2) == self->spaceId)
      dSpaceCollide2( o1, o2, self, &CollisionCallback );
  }
  else
  {
    // There should be no geoms in the world space
    assert(dGeomGetSpace(o1) != self->spaceId);
    assert(dGeomGetSpace(o2) != self->spaceId);

    // We should never test two geoms in the same space
    assert(dGeomGetSpace(o1) != dGeomGetSpace(o2));

    // Get pointers to the underlying geoms
    geom1 = NULL;
    if (dGeomGetClass(o1) == dGeomTransformClass)
      geom1 = (Geom*) dGeomGetData(dGeomTransformGetGeom(o1));
    else
      geom1 = (Geom*) dGeomGetData(o1);

    geom2 = NULL;
    if (dGeomGetClass(o2) == dGeomTransformClass)
      geom2 = (Geom*) dGeomGetData(dGeomTransformGetGeom(o2));
    else
      geom2 = (Geom*) dGeomGetData(o2);

    assert(geom1 && geom2);
    
    // Detect collisions betweed geoms
    n = dCollide(o1, o2, num, contactGeoms, sizeof(contactGeoms[0]));

    for (i=0; i < n; i++)
    {
      dBodyID body1 = dGeomGetBody(contactGeoms[i].g1);
      dBodyID body2 = dGeomGetBody(contactGeoms[i].g2);

      // Dont add contact joints between already connected bodies.
      // Sometimes the body is unspecified; should probably figure out
      // what this means
      if (body1 && body2 &&
          dAreConnectedExcluding(body1, body2, dJointTypeContact))
          continue;
         
      contactInfo.geom = contactGeoms[i];
      contactInfo.surface.mode = 0;
      
      // Compute the CFM and ERP by assuming the two bodies form a
      // spring-damper system.
      double h, kp, kd;
      h = self->stepTime;
      kp = 1 / (1 / geom1->contact->kp + 1 / geom2->contact->kp);
      kd = geom1->contact->kd + geom2->contact->kd;
      contactInfo.surface.mode |= dContactSoftERP | dContactSoftCFM;
      contactInfo.surface.soft_erp = h * kp / (h * kp + kd);
      contactInfo.surface.soft_cfm = 1 / (h * kp + kd);

      /*
      printf("%f %f %f %f \n",
             kp, kd,
             contactInfo.surface.soft_erp,
             contactInfo.surface.soft_cfm);
      */

      // Compute friction effects; this is standard Coulomb friction
      contactInfo.surface.mode |= dContactApprox1;
      contactInfo.surface.mu = Min(geom1->contact->mu1, geom2->contact->mu1);

      // Compute slipping effects
      //contactInfo.surface.slip1 = (geom1->contact->slip1 + geom2->contact->slip1)/2.0;
      //contactInfo.surface.slip2 = (geom1->contact->slip2 + geom2->contact->slip2)/2.0;
      
      // Construct a contact joint between the two bodies
      joint = dJointCreateContact(self->worldId, self->contactGroup, &contactInfo);
      dJointAttach(joint, body1, body2);
    }
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get the current sim time (elapsed time)
double World::GetSimTime()
{
  return this->simTime;
}


//////////////////////////////////////////////////////////////////////////////
// Get the current pause time (elapsed time)
double World::GetPauseTime()
{
  return this->pauseTime;
}


//////////////////////////////////////////////////////////////////////////////
// Get the current real time (elapsed time)
double World::GetRealTime()
{
  return this->GetWallTime() - this->startTime;
}


//////////////////////////////////////////////////////////////////////////////
// Get the wall clock time
double World::GetWallTime()
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return tv.tv_sec + tv.tv_usec * 1e-6;
}

//////////////////////////////////////////////////////////////////////////////
// Get the time relative to 12am (seconds)
double World::GetDayTime()
{
  if (this->dayTime >=0)
  {
    return this->dayTime + this->GetRealTime();
  } else {
    double wallTime = this->GetWallTime();
    time_t wallTimeWhole = (time_t)(wallTime);
    wallTime -= wallTimeWhole;

    tm *dayTm = localtime(&wallTimeWhole);

    return dayTm->tm_hour*3600 + dayTm->tm_min*60 + dayTm->tm_sec + wallTime;
  }
}

//////////////////////////////////////////////////////////////////////////////
// Add a model
void World::AddModel( Model *model )
{
  // Re-size array as needed
  if (this->modelCount >= this->modelMaxCount)
  {
    this->modelMaxCount += 10;
    this->models = (Model**) realloc( this->models, this->modelMaxCount * sizeof(this->models[0]));
    assert( this->models );
  }

  this->models[this->modelCount] = model;
  this->modelCount++;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get the number of models
int World::GetNumModels() const
{
  return this->modelCount;
}


//////////////////////////////////////////////////////////////////////////////
// Get the list of models
Model **World::GetModels() const
{
  return this->models;
}