Revision: 8751
http://playerstage.svn.sourceforge.net/playerstage/?rev=8751&view=rev
Author: natepak
Date: 2010-06-06 12:49:34 +0000 (Sun, 06 Jun 2010)
Log Message:
-----------
Fixing the ERP values
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/pioneer_circle.cc
code/gazebo/branches/simpar/plugins/pioneer_line.cc
code/gazebo/branches/simpar/server/World.cc
code/gazebo/branches/simpar/server/physics/SurfaceParams.cc
code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc
code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
Modified: code/gazebo/branches/simpar/plugins/pioneer_circle.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-06-03
00:34:23 UTC (rev 8750)
+++ code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-06-06
12:49:34 UTC (rev 8751)
@@ -7,10 +7,35 @@
class PioneerCircle : public Handler
{
- public: PioneerCircle() : Handler() {}
+ public: PioneerCircle() : Handler()
+ {
+ for (double i=0.001; i > 1e-5; i*=0.5)
+ this->stepTimes.push_back(i);
+ for (unsigned int i=10; i<=100; i+=10)
+ this->stepIters.push_back(i);
+
+ this->stepTypes.push_back("world");
+ this->stepTypes.push_back("quick");
+ //this->stepTypes.push_back("robust");
+
+ this->stepTypesIter = this->stepTypes.begin();
+ this->stepTimesIter = this->stepTimes.begin();
+ this->stepItersIter = this->stepIters.begin();
+
+ this->path = std::string("/home/nate/work/simpar/data/pioneer_circle/");
+ system( (std::string("mkdir -p ") + this->path).c_str() );
+
+ this->circleCount = 4;
+
+ this->indexFile = fopen(std::string(this->path + "index.txt").c_str(),
"w");
+ fprintf(this->indexFile, "# index step_type step_time
step_iterations\n");
+ }
+
public: ~PioneerCircle()
{
+ fclose(this->indexFile);
+
World::Instance()->DisconnectWorldUpdateStartSignal(
boost::bind(&PioneerCircle::UpdateCB, this));
@@ -25,6 +50,7 @@
body->SetAngularVel(Vector3(0,0,0));
}
}
+ this->robot->Reset();
}
public: void Load()
@@ -38,8 +64,20 @@
this->leftWheel = (Body*)this->robot->GetChild("left_wheel");
this->rightWheel = (Body*)this->robot->GetChild("right_wheel");
+ this->physics = World::Instance()->GetPhysicsEngine();
+
+ this->prevTime = Simulator::Instance()->GetRealTime();
+ this->count = 0;
+ this->circleCount = 0;
+
+ Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
+
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&PioneerCircle::UpdateCB, this));
+
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "]\n";
}
else
std::cerr << "Unable to find model[" << model_name << "]\n";
@@ -47,16 +85,92 @@
public: void UpdateCB()
{
- if (this->leftWheel->GetRelativeAngularVel().z < 2.2)
- this->leftWheel->SetTorque(Vector3(0.0, 0.0, 0.2));
+ Time realTime = Simulator::Instance()->GetRealTime();
+ Pose3d pose = this->robot->GetWorldPose();
- if (this->rightWheel->GetRelativeAngularVel().z > 2.1)
- this->rightWheel->SetTorque(Vector3(0.0, 0.0, -0.2));
+ if (this->circleCount <= 1)
+ {
+ if (this->leftWheel->GetRelativeAngularVel().z < 2.2)
+ this->leftWheel->SetTorque(Vector3(0.0, 0.0, 0.2));
+
+ if (this->rightWheel->GetRelativeAngularVel().z > 2.1)
+ this->rightWheel->SetTorque(Vector3(0.0, 0.0, -0.2));
+
+ if (realTime - this->prevTime > 5.0 && fabs(pose.pos.x) < 0.1 )
+ {
+ this->prevTime = realTime;
+ this->circleCount++;
+ }
+ }
+ else
+ this->UpdateJob();
}
+ public: void UpdateJob()
+ {
+ Logger::Instance()->RemoveLog("pioneer");
+
+ std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + this->path +
+ "pioneer_circle_" + boost::lexical_cast<std::string>(this->count) +
+ ".data";
+
+ fprintf(this->indexFile,"%d %s %f %d\n",this->count,
+ (*this->stepTypesIter).c_str(), *this->stepTimesIter,
+ *this->stepItersIter);
+
+ system(mv_cmd.c_str());
+
+ this->stepItersIter++;
+ if (this->stepItersIter == this->stepIters.end())
+ {
+ this->stepItersIter = this->stepIters.begin();
+ this->stepTimesIter++;
+ if (this->stepTimesIter == this->stepTimes.end())
+ {
+ this->stepTimesIter = this->stepTimes.begin();
+ this->stepTypesIter++;
+ if (this->stepTypesIter == this->stepTypes.end())
+ {
+ Simulator::Instance()->RemoveHandler(this->name);
+ return;
+ }
+ }
+ }
+
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+
+ this->count++;
+ this->circleCount = 0;
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "] "
+ << "Count[" << this->count << "]\n";
+
+ this->robot->Reset();
+ this->prevTime = Simulator::Instance()->GetRealTime();
+ Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
+ }
+ private: std::vector<unsigned int> stepIters;
+ private: std::vector<unsigned int>::iterator stepItersIter;
+
+ private: std::vector<double> stepTimes;
+ private: std::vector<double>::iterator stepTimesIter;
+
+ private: std::vector<std::string> stepTypes;
+ private: std::vector<std::string>::iterator stepTypesIter;
+ private: std::string path;
+
private: Model *robot;
private: Body *leftWheel;
private: Body *rightWheel;
+
+ private: FILE *indexFile;
+ private: Time prevTime;
+ private: PhysicsEngine *physics;
+ private: unsigned int circleCount;
+ private: unsigned int count;
};
GZ_REGISTER_HANDLER("PioneerCircle", PioneerCircle)
Modified: code/gazebo/branches/simpar/plugins/pioneer_line.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_line.cc 2010-06-03 00:34:23 UTC
(rev 8750)
+++ code/gazebo/branches/simpar/plugins/pioneer_line.cc 2010-06-06 12:49:34 UTC
(rev 8751)
@@ -88,61 +88,62 @@
public: void UpdateCB()
{
- if (Simulator::Instance()->IsPaused())
- return;
-
Time realTime = Simulator::Instance()->GetRealTime();
- //double speedSum = this->robot->GetWorldLinearVel().GetLength();
-
if (realTime - prevTime < Time(0,100000000))
{
this->leftWheel->SetTorque(Vector3(0.0,0.0,.1));
this->rightWheel->SetTorque(Vector3(0.0,0.0,.1));
}
else if (realTime - prevTime >= Time(20,0))
- {
- Logger::Instance()->RemoveLog("pioneer");
+ this->UpdateJob();
+ }
- std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + this->path
+
- "pioneer_line_" + boost::lexical_cast<std::string>(this->count) +
- ".data";
- fprintf(this->indexFile,"%d %s %f %d\n",this->count,
(*this->stepTypesIter).c_str(), *this->stepTimesIter, *this->stepItersIter);
+ public: void UpdateJob()
+ {
+ Logger::Instance()->RemoveLog("pioneer");
- system(mv_cmd.c_str());
+ std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + this->path +
+ "pioneer_line_" + boost::lexical_cast<std::string>(this->count) +
+ ".data";
- this->stepItersIter++;
- if (this->stepItersIter == this->stepIters.end())
+ fprintf(this->indexFile,"%d %s %f %d\n",this->count,
+ (*this->stepTypesIter).c_str(), *this->stepTimesIter,
+ *this->stepItersIter);
+
+ system(mv_cmd.c_str());
+
+ this->stepItersIter++;
+ if (this->stepItersIter == this->stepIters.end())
+ {
+ this->stepItersIter = this->stepIters.begin();
+ this->stepTimesIter++;
+ if (this->stepTimesIter == this->stepTimes.end())
{
- this->stepItersIter = this->stepIters.begin();
- this->stepTimesIter++;
- if (this->stepTimesIter == this->stepTimes.end())
+ this->stepTimesIter = this->stepTimes.begin();
+ this->stepTypesIter++;
+ if (this->stepTypesIter == this->stepTypes.end())
{
- this->stepTimesIter = this->stepTimes.begin();
- this->stepTypesIter++;
- if (this->stepTypesIter == this->stepTypes.end())
- {
- Simulator::Instance()->RemoveHandler(this->name);
- return;
- }
+ Simulator::Instance()->RemoveHandler(this->name);
+ return;
}
}
+ }
- this->physics->SetStepType( *this->stepTypesIter );
- this->physics->SetStepTime( *this->stepTimesIter );
- this->physics->SetSORPGSIters( *this->stepItersIter );
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
- this->count++;
- std::cout << "Type[" << *this->stepTypesIter << "] "
- << "Time[" << *this->stepTimesIter << "] "
- << "Iters[" << *this->stepItersIter << "] "
- << "Count[" << this->count << "]\n";
+ this->count++;
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "] "
+ << "Count[" << this->count << "]\n";
- this->robot->Reset();
- this->prevTime = Simulator::Instance()->GetRealTime();
- Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
- }
+ this->robot->Reset();
+ this->prevTime = Simulator::Instance()->GetRealTime();
+ Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
}
private: std::vector<unsigned int> stepIters;
Modified: code/gazebo/branches/simpar/server/World.cc
===================================================================
--- code/gazebo/branches/simpar/server/World.cc 2010-06-03 00:34:23 UTC (rev
8750)
+++ code/gazebo/branches/simpar/server/World.cc 2010-06-06 12:49:34 UTC (rev
8751)
@@ -215,12 +215,13 @@
else
this->physicsEngine = PhysicsFactory::NewPhysicsEngine("ode");
+ // This should come before loading of entities
+ this->physicsEngine->Load(rootNode);
+
// last bool is initModel, init model is not needed as Init()
// is called separately from main.cc
this->LoadEntities(rootNode, NULL, false, false);
- this->physicsEngine->Load(rootNode);
-
this->threadsP->Load(rootNode);
this->saveStateTimeoutP->Load(rootNode);
this->saveStateBufferSizeP->Load(rootNode);
Modified: code/gazebo/branches/simpar/server/physics/SurfaceParams.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/SurfaceParams.cc 2010-06-03
00:34:23 UTC (rev 8750)
+++ code/gazebo/branches/simpar/server/physics/SurfaceParams.cc 2010-06-06
12:49:34 UTC (rev 8751)
@@ -54,8 +54,8 @@
this->enableFriction = true;
- this->useGlobalERP = false;
- this->useGlobalCFM = false;
+ //this->useGlobalERP = false;
+ //this->useGlobalCFM = false;
}
//////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc 2010-06-03
00:34:23 UTC (rev 8750)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc 2010-06-06
12:49:34 UTC (rev 8751)
@@ -52,7 +52,6 @@
/// Load a joint
void ODEJoint::Load(XMLConfigNode *node)
{
- Joint::Load(node);
double h = this->physics->GetStepTime().Double();
double stopErp = h * (**this->stopKpP) / (h * (**this->stopKpP) +
(**this->stopKdP));
@@ -73,6 +72,8 @@
this->feedback = new dJointFeedback;
dJointSetFeedback(this->jointId, this->feedback);
}
+
+ Joint::Load(node);
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-06-03 00:34:23 UTC (rev 8750)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-06-06 12:49:34 UTC (rev 8751)
@@ -173,6 +173,17 @@
// Reset the contact pointer
this->contactFeedbackIter = this->contactFeedbacks.begin();
+
+ Vector3 g = this->gravityP->GetValue();
+ dWorldSetGravity(this->worldId, g.x, g.y, g.z);
+
+ dWorldSetCFM(this->worldId, this->globalCFMP->GetValue());
+ dWorldSetERP(this->worldId, this->globalERPP->GetValue());
+
+ dWorldSetQuickStepNumIterations(this->worldId, **this->stepItersP );
+ dWorldSetQuickStepW(this->worldId, **this->stepWP );
+
+ dWorldSetRobustStepMaxIterations(this->worldId, **this->stepItersP );
}
////////////////////////////////////////////////////////////////////////////////
@@ -197,16 +208,6 @@
// Initialize the ODE engine
void ODEPhysics::Init()
{
- Vector3 g = this->gravityP->GetValue();
- dWorldSetGravity(this->worldId, g.x, g.y, g.z);
-
- dWorldSetCFM(this->worldId, this->globalCFMP->GetValue());
- dWorldSetERP(this->worldId, this->globalERPP->GetValue());
-
- dWorldSetQuickStepNumIterations(this->worldId, **this->stepItersP );
- dWorldSetQuickStepW(this->worldId, **this->stepWP );
-
- dWorldSetRobustStepMaxIterations(this->worldId, **this->stepItersP );
}
////////////////////////////////////////////////////////////////////////////////
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
ThinkGeek and WIRED's GeekDad team up for the Ultimate
GeekDad Father's Day Giveaway. ONE MASSIVE PRIZE to the
lucky parental unit. See the prize list and enter to win:
http://p.sf.net/sfu/thinkgeek-promo
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
