Revision: 7759
http://playerstage.svn.sourceforge.net/playerstage/?rev=7759&view=rev
Author: natepak
Date: 2009-06-02 20:35:01 +0000 (Tue, 02 Jun 2009)
Log Message:
-----------
Added throttling of the physics loop
Modified Paths:
--------------
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/Simulator.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/physics/PhysicsEngine.cc
code/gazebo/trunk/worlds/pioneer2dx.world
code/gazebo/trunk/worlds/simpleshapes.world
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2009-06-02 19:46:15 UTC (rev
7758)
+++ code/gazebo/trunk/server/Simulator.cc 2009-06-02 20:35:01 UTC (rev
7759)
@@ -60,7 +60,6 @@
gazeboConfig(NULL),
loaded(false),
pause(false),
- iterations(0),
simTime(0.0),
pauseTime(0.0),
startTime(0.0),
@@ -411,13 +410,6 @@
}
////////////////////////////////////////////////////////////////////////////////
-/// Get the number of iterations of this simulation session
-unsigned long Simulator::GetIterations() const
-{
- return this->iterations;
-}
-
-////////////////////////////////////////////////////////////////////////////////
// Get the simulation time
double Simulator::GetSimTime() const
{
@@ -618,33 +610,31 @@
double step = world->GetPhysicsEngine()->GetStepTime();
double physicsUpdateRate = world->GetPhysicsEngine()->GetUpdateRate();
- //double renderUpdateRate = OgreAdaptor::Instance()->GetUpdateRate();
double physicsUpdatePeriod = 1.0 / physicsUpdateRate;
- //double renderUpdatePeriod = 1.0 / renderUpdateRate;
-
+
+ double diffTime;
double currTime;
+ double lastTime = this->GetRealTime();
+ struct timespec req, rem;
- this->prevPhysicsTime = this->GetRealTime();
- this->prevRenderTime = this->GetRealTime();
-
while (!this->userQuit)
{
#ifdef TIMING
double tmpT1 = this->GetWallTime();
#endif
+
currTime = this->GetRealTime();
- if (physicsUpdateRate == 0 ||
- currTime - this->prevPhysicsTime >= physicsUpdatePeriod)
+ //if (physicsUpdateRate == 0 ||
+ //currTime - lastTime >= physicsUpdatePeriod)
{
// Update the physics engine
//if (!this->GetUserPause() && !this->IsPaused() ||
- // (this->GetUserPause() && this->GetUserStepInc()))
+ // (this->GetUserPause() && this->GetUserStepInc()))
if (!this->IsPaused())
{
this->simTime += step;
- this->iterations++;
this->SetUserStepInc(!this->GetUserStepInc());
}
else
@@ -653,13 +643,42 @@
// this->pause=true;
}
- this->prevPhysicsTime = this->GetRealTime();
+ lastTime = this->GetRealTime();
{
boost::recursive_mutex::scoped_lock lock(*this->mutex);
world->Update();
}
- usleep(1);
+
+ currTime = this->GetRealTime();
+
+ // Set a default sleep time
+ req.tv_sec = 0;
+ req.tv_nsec = 10000;
+
+ // If the physicsUpdateRate < 0, then we should try to match the
+ // update rate to real time
+ if ( physicsUpdateRate < 0 &&
+ (this->GetSimTime() + this->GetPauseTime()) >
+ this->GetRealTime())
+ {
+ diffTime = (this->GetSimTime() + this->GetPauseTime()) -
+ this->GetRealTime();
+ req.tv_sec = (int) floor(diffTime);
+ req.tv_nsec = (int) (fmod(diffTime, 1.0) * 1e9);
+ }
+ // Otherwise try to match the update rate to the one specified in
+ // the xml file
+ else if (physicsUpdateRate > 0 &&
+ currTime - lastTime < physicsUpdatePeriod)
+ {
+ diffTime = physicsUpdatePeriod - (currTime - lastTime);
+
+ req.tv_sec = (int) floor(diffTime);
+ req.tv_nsec = (int) (fmod(diffTime, 1.0) * 1e9);
+ }
+
+ nanosleep(&req, &rem);
}
// Process all incoming messages from simiface
@@ -670,9 +689,11 @@
this->userQuit = true;
break;
}
+
#ifdef TIMING
double tmpT2 = this->GetWallTime();
- std::cout << " Simulator::PhysicsLoop() DT(" << tmpT2-tmpT1 << ")" <<
std::endl;
+ std::cout << " Simulator::PhysicsLoop() DT(" << tmpT2-tmpT1
+ << ")" << std::endl;
#endif
}
}
Modified: code/gazebo/trunk/server/Simulator.hh
===================================================================
--- code/gazebo/trunk/server/Simulator.hh 2009-06-02 19:46:15 UTC (rev
7758)
+++ code/gazebo/trunk/server/Simulator.hh 2009-06-02 20:35:01 UTC (rev
7759)
@@ -97,15 +97,6 @@
/// \brief Set whether the simulation is paused
public: void SetPaused(bool p);
- /// \brief Get the number of iterations
- public: unsigned long GetIterations() const;
-/*
- /// \brief Set the number of iterations
- public: static void SetIterations(unsigned long count);
-
- /// \brief Increment the number of iterations
- public: static void IncIterations();
-*/
/// Get the simulation time
/// \return The simulation time
public: double GetSimTime() const;
@@ -200,12 +191,8 @@
/// Flag set if simulation is paused
private: bool pause;
- /// Count of the number of iterations
- private: unsigned long iterations;
-
/// Current simulation time
private: double simTime, pauseTime, startTime;
- private: double prevPhysicsTime, prevRenderTime;
//upper limits on updating
//how many updates we have done in this slot
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-06-02 19:46:15 UTC (rev 7758)
+++ code/gazebo/trunk/server/World.cc 2009-06-02 20:35:01 UTC (rev 7759)
@@ -431,7 +431,7 @@
this->SetModelPose(model, model->GetInitPose());
// Add the model to our list
- if (Simulator::Instance()->GetIterations() == 0)
+ if (Simulator::Instance()->GetSimTime() == 0)
this->models.push_back(model);
else
{
Modified: code/gazebo/trunk/server/physics/PhysicsEngine.cc
===================================================================
--- code/gazebo/trunk/server/physics/PhysicsEngine.cc 2009-06-02 19:46:15 UTC
(rev 7758)
+++ code/gazebo/trunk/server/physics/PhysicsEngine.cc 2009-06-02 20:35:01 UTC
(rev 7759)
@@ -36,7 +36,7 @@
{
Param::Begin(&this->parameters);
this->gravityP = new ParamT<Vector3>("gravity",Vector3(0.0, -9.80665, 0.0),
0);
- this->updateRateP = new ParamT<double>("maxUpdateRate", 0.0, 0);
+ this->updateRateP = new ParamT<double>("updateRate", 0.0, 0);
this->stepTimeP = new ParamT<double>("stepTime",0.025,0);
Param::End();
Modified: code/gazebo/trunk/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/trunk/worlds/pioneer2dx.world 2009-06-02 19:46:15 UTC (rev
7758)
+++ code/gazebo/trunk/worlds/pioneer2dx.world 2009-06-02 20:35:01 UTC (rev
7759)
@@ -23,6 +23,10 @@
<gravity>0 0 -9.8</gravity>
<cfm>10e-5</cfm>
<erp>0.3</erp>
+ <!-- updateRate: <0 == throttle simTime to match realTime.
+ 0 == No throttling
+ >0 == Frequency at which to throttle the sim -->
+ <updateRate>-1</updateRate>
</physics:ode>
<rendering:gui>
Modified: code/gazebo/trunk/worlds/simpleshapes.world
===================================================================
--- code/gazebo/trunk/worlds/simpleshapes.world 2009-06-02 19:46:15 UTC (rev
7758)
+++ code/gazebo/trunk/worlds/simpleshapes.world 2009-06-02 20:35:01 UTC (rev
7759)
@@ -19,6 +19,11 @@
<gravity>0 0 -9.8</gravity>
<cfm>10e-2</cfm>
<erp>0.2</erp>
+
+ <!-- updateRate: <0 == throttle simTime to match realTime.
+ 0 == No throttling
+ >0 == Frequency at which to throttle the sim -->
+ <updateRate>-1</updateRate>
</physics:ode>
<rendering:gui>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
OpenSolaris 2009.06 is a cutting edge operating system for enterprises
looking to deploy the next generation of Solaris that includes the latest
innovations from Sun and the OpenSource community. Download a copy and
enjoy capabilities such as Networking, Storage and Virtualization.
Go to: http://p.sf.net/sfu/opensolaris-get
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
