Step by step simulation c++ with gazebo

[osrf-migration]

Original report (archived issue) by Brice Clement (Bitbucket: Brice_clement).

---

HI,

We are trying to use gazebo in a step by step way, because we have a controller running at some frequency, and we want to avoid asynchronous issue.

So we looked at the example of your Custom main, and we found that your are using :

#!c++

gazebo::runWorld(world, iterations);

Where iteration is the number of iteration to perform. But this function can't be used like this :

#!c++
 while (!stop) {
    gazebo::runWorld(world, 1);
    //  do some stuff
  }

Because it seems like it's reseting everythings from scratch.

So we would like to use the world->Step() function which is private, to run step by step the simulation.

Best regards,

[osrf-migration]

Original comment by Shane Loretz (Bitbucket: Shane Loretz, GitHub: sloretz).

---

I think you are looking for this version of World::Step()

      /// \brief Step the world forward in time.
      /// \param[in] _steps The number of steps the World should take.
      public: void Step(const unsigned int _steps);

https://github.com/osrf/gazebo/blob/05340e7388b8bb8eb26fe7dfb3fd7279ebe6c87e/gazebo/physics/World.hh#L449

The integration tests have examples of using it.

https://github.com/osrf/gazebo/blob/05340e7388b8bb8eb26fe7dfb3fd7279ebe6c87e/test/integration/plugin.cc#L107

I will close this issue since I think that answers the question. Feel free to reopen if there is more you would like to discuss.

[osrf-migration]

Original comment by Shane Loretz (Bitbucket: Shane Loretz, GitHub: sloretz).

---

• changed state from "new" to "resolved"

[osrf-migration]

Original comment by Brice Clement (Bitbucket: Brice_clement).

---

Hi,

This function does nothing, here are the implementation :

#!c++

void World::Step(const unsigned int _steps)
{
  if (!this->IsPaused())
  {
    gzwarn << "Calling World::Step(steps) while world is not paused\n";
    this->SetPaused(true);
  }

  {
    std::lock_guard<std::recursive_mutex> lock(this->dataPtr->worldUpdateMutex);
    this->dataPtr->stepInc = _steps;
  }

  // block on completion
  bool wait = true;
  while (wait)
  {
    common::Time::NSleep(1);
    std::lock_guard<std::recursive_mutex> lock(this->dataPtr->worldUpdateMutex);
    if (this->dataPtr->stepInc == 0 || this->dataPtr->stop)
      wait = false;
  }
}

And this is the Step() :

#!c++

void World::Step()
{
  DIAG_TIMER_START("World::Step");

  /// need this because ODE does not call dxReallocateWorldProcessContext()
  /// until dWorld.*Step
  /// Plugins that manipulate joints (and probably other properties) require
  /// one iteration of the physics engine. Do not remove this.
  if (!this->dataPtr->pluginsLoaded && this->SensorsInitialized())
  {
    this->LoadPlugins();
    this->dataPtr->pluginsLoaded = true;
  }

  DIAG_TIMER_LAP("World::Step", "loadPlugins");

  // Send statistics about the world simulation
  this->PublishWorldStats();

  DIAG_TIMER_LAP("World::Step", "publishWorldStats");

  double updatePeriod = this->dataPtr->physicsEngine->GetUpdatePeriod();
  // sleep here to get the correct update rate
  common::Time tmpTime = common::Time::GetWallTime();
  common::Time sleepTime = this->dataPtr->prevStepWallTime +
    common::Time(updatePeriod) - tmpTime - this->dataPtr->sleepOffset;

  common::Time actualSleep;
  if (sleepTime > 0)
  {
    common::Time::Sleep(sleepTime);
    actualSleep = common::Time::GetWallTime() - tmpTime;
  }
  else
    sleepTime = 0;

  // exponentially avg out
  this->dataPtr->sleepOffset = (actualSleep - sleepTime) * 0.01 +
                      this->dataPtr->sleepOffset * 0.99;

  DIAG_TIMER_LAP("World::Step", "sleepOffset");

  // throttling update rate, with sleepOffset as tolerance
  // the tolerance is needed as the sleep time is not exact
  if (common::Time::GetWallTime() - this->dataPtr->prevStepWallTime +
      this->dataPtr->sleepOffset >= common::Time(updatePeriod))
  {
    std::lock_guard<std::recursive_mutex> lock(this->dataPtr->worldUpdateMutex);

    DIAG_TIMER_LAP("World::Step", "worldUpdateMutex");

    this->dataPtr->prevStepWallTime = common::Time::GetWallTime();

    double stepTime = this->dataPtr->physicsEngine->GetMaxStepSize();

    if (!this->IsPaused() || this->dataPtr->stepInc > 0
        || this->dataPtr->needsReset)
    {
      // query timestep to allow dynamic time step size updates
      this->dataPtr->simTime += stepTime;
      this->dataPtr->iterations++;
      this->Update();

      DIAG_TIMER_LAP("World::Step", "update");

      if (this->IsPaused() && this->dataPtr->stepInc > 0)
        this->dataPtr->stepInc--;
    }
    else
    {
      // Flush the log record buffer, if there is data in it.
      if (util::LogRecord::Instance()->BufferSize() > 0)
        util::LogRecord::Instance()->Notify();
      this->dataPtr->pauseTime += stepTime;
    }
  }

  gazebo::util::IntrospectionManager::Instance()->NotifyUpdates();

  this->ProcessMessages();

  DIAG_TIMER_STOP("World::Step");

  if (g_clearModels)
    this->ClearModels();
}

Best regards,

[osrf-migration]

Original comment by Brice Clement (Bitbucket: Brice_clement).

---

• changed state from "resolved" to "open"

[osrf-migration]

Original comment by Shane Loretz (Bitbucket: Shane Loretz, GitHub: sloretz).

---

It assumes the world is updating in another thread. The ServerFixture used by the integration tests shows how to set that up.

https://github.com/osrf/gazebo/blob/05340e7388b8bb8eb26fe7dfb3fd7279ebe6c87e/gazebo/test/ServerFixture.cc#L200

It sounds like you might be interested in fewer API calls to get to that point. Feel free to update this issue with a proposal for what you would that API to look like.

[osrf-migration]

Original comment by Brice Clement (Bitbucket: Brice_clement).

---

Hi,

I'm not sure if i understood what you wanted to show me about the ServerFixture, but we would like to avoid to have the world in a thread. Just something like your Step() function (the one implemented without parameters), but if it's possible to make it public instead of private. So we can access it.

Best regards

[osrf-migration]

Original comment by Ignacio Abadia (Bitbucket: iAbaTer).

---

Hi Brice,

I am trying to use the function world->Step() too, but I'm facing some troubles doing so. Did you manage to use it?

Best regards