Revision: 8748
http://playerstage.svn.sourceforge.net/playerstage/?rev=8748&view=rev
Author: natepak
Date: 2010-06-02 14:59:12 +0000 (Wed, 02 Jun 2010)
Log Message:
-----------
Update to the plugins
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/pioneer_circle.cc
code/gazebo/branches/simpar/plugins/pioneer_spiral.cc
code/gazebo/branches/simpar/plugins/pioneer_spiral.hh
Modified: code/gazebo/branches/simpar/plugins/pioneer_circle.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-06-02
07:04:20 UTC (rev 8747)
+++ code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-06-02
14:59:12 UTC (rev 8748)
@@ -1,10 +1,10 @@
#include <boost/bind.hpp>
+
#include <gazebo/gazeboserver.hh>
namespace gazebo
{
-
class PioneerCircle : public Handler
{
public: PioneerCircle() : Handler() {}
@@ -29,7 +29,6 @@
public: void Load()
{
-
std::string model_name = "pioneer";
this->robot = (Model*)World::Instance()->GetEntityByName(model_name);
@@ -48,24 +47,16 @@
public: void UpdateCB()
{
- if (this->leftWheel->GetAngularVel().z < 4.2)
- {
+ if (this->leftWheel->GetRelativeAngularVel().z < 2.2)
this->leftWheel->SetTorque(Vector3(0.0, 0.0, 0.2));
- }
- if (this->rightWheel->GetAngularVel().z > 4.0)
+ if (this->rightWheel->GetRelativeAngularVel().z > 2.1)
this->rightWheel->SetTorque(Vector3(0.0, 0.0, -0.2));
-
- //this->rightWheel->SetTorque(Vector3(0.0, 0.0, 0.01));
- //this->rightWheel->SetTorque(Vector3(0.0, 0.0, 0));
- //this->leftWheel->SetAngularVel(Vector3(0.0, 0.3, 0));
- //this->rightWheel->SetAngularVel(Vector3(0.0, 0.1, 0));
}
private: Model *robot;
private: Body *leftWheel;
private: Body *rightWheel;
-
};
GZ_REGISTER_HANDLER("PioneerCircle", PioneerCircle)
Modified: code/gazebo/branches/simpar/plugins/pioneer_spiral.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_spiral.cc 2010-06-02
07:04:20 UTC (rev 8747)
+++ code/gazebo/branches/simpar/plugins/pioneer_spiral.cc 2010-06-02
14:59:12 UTC (rev 8748)
@@ -1,3 +1,4 @@
+#include <stdlib.h>
#include <boost/bind.hpp>
#include "pioneer_spiral.hh"
@@ -7,10 +8,33 @@
PioneerSpiral::PioneerSpiral() : 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_spiral/");
+ system( (std::string("mkdir -p ") + this->path).c_str() );
+
+ this->count = 0;
+
+ this->indexFile = fopen(std::string(this->path + "index.txt").c_str(), "w");
+ fprintf(this->indexFile, "# index step_type step_time step_iterations\n");
}
PioneerSpiral::~PioneerSpiral()
{
+ fclose(this->indexFile);
+
World::Instance()->DisconnectWorldUpdateStartSignal(
boost::bind(&PioneerSpiral::UpdateCB, this));
@@ -29,32 +53,32 @@
void PioneerSpiral::Load()
{
-
std::string model_name = "pioneer";
this->robot = (Model*)World::Instance()->GetEntityByName(model_name);
+
if (this->robot)
{
+
this->robot->GetVisualNode()->SetRibbonTrail(true);
this->leftWheel = (Body*)this->robot->GetChild("left_wheel");
this->rightWheel = (Body*)this->robot->GetChild("right_wheel");
- Geom *leftWheelGeom = this->leftWheel->GetGeom("left_wheel_geom");
- Geom *rightWheelGeom = this->rightWheel->GetGeom("right_wheel_geom");
+ World::Instance()->ConnectWorldUpdateStartSignal(
+ boost::bind(&PioneerSpiral::UpdateCB, this));
- /*leftWheelGeom->SetContactsEnabled(true);
- rightWheelGeom->SetContactsEnabled(true);
+ this->prevTime = Simulator::Instance()->GetRealTime();
- leftWheelGeom->ConnectContactCallback(
- boost::bind(&PioneerSpiral::LeftContactCB, this, _1));
- rightWheelGeom->ConnectContactCallback(
- boost::bind(&PioneerSpiral::RightContactCB, this, _1));
- */
+ this->physics = World::Instance()->GetPhysicsEngine();
- World::Instance()->ConnectWorldUpdateStartSignal(
- boost::bind(&PioneerSpiral::UpdateCB, this));
+ Logger::Instance()->AddLog("pioneer");
- this->prevTime = Simulator::Instance()->GetRealTime();
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "]\n";
}
else
std::cerr << "Unable to find model[" << model_name << "]\n";
@@ -62,35 +86,56 @@
void PioneerSpiral::UpdateCB()
{
+ Time realTime = Simulator::Instance()->GetRealTime();
- Time currTime = Simulator::Instance()->GetRealTime();
- if (currTime - prevTime < Time(0,200000000))
+ double speedSum = this->robot->GetWorldLinearVel().GetLength();
+
+ if (realTime - prevTime < Time(0,100000000))
{
- this->leftWheel->SetTorque(Vector3(0.0,0.0,.01));
- this->rightWheel->SetTorque(Vector3(0.0,0.0,.01));
+ this->leftWheel->SetTorque(Vector3(0.0,0.0,.1));
+ this->rightWheel->SetTorque(Vector3(0.0,0.0,.1));
}
+ else if (speedSum < 1e-3)
+ {
+ Logger::Instance()->RemoveLog("pioneer");
-}
+ std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + this->path +
+ "pioneer_spiral_" + boost::lexical_cast<std::string>(this->count) +
+ ".data";
-/*void PioneerSpiral::LeftContactCB(const Contact &contact)
-{
- std::cout << "Left contact\n";
- for (unsigned int i=0; i < contact.positions.size(); i++)
- {
- std::cout << " Pos[" << contact.positions[i] << "] "
- << "Norm[" << contact.normals[i] << "] "
- << "Depth[" << contact.depths[i] << "]\n";
- }
-}
+ fprintf(this->indexFile,"%d %s %f %d\n",this->count,
(*this->stepTypesIter).c_str(), *this->stepTimesIter, *this->stepItersIter);
-void PioneerSpiral::RightContactCB(const Contact &contact)
-{
- std::cout << "Right contact\n";
- for (unsigned int i=0; i < contact.positions.size(); i++)
- {
- std::cout << " Pos[" << contact.positions[i] << "] "
- << "Norm[" << contact.normals[i] << "] "
- << "Depth[" << contact.depths[i] << "]\n";
+ 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++;
+ 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");
}
}
-*/
Modified: code/gazebo/branches/simpar/plugins/pioneer_spiral.hh
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_spiral.hh 2010-06-02
07:04:20 UTC (rev 8747)
+++ code/gazebo/branches/simpar/plugins/pioneer_spiral.hh 2010-06-02
14:59:12 UTC (rev 8748)
@@ -22,7 +22,21 @@
private: Model *robot;
private: Body *leftWheel;
private: Body *rightWheel;
+ private: PhysicsEngine *physics;
private: Time prevTime;
+
+ private: std::vector<std::string> stepTypes;
+ private: std::vector<std::string>::iterator stepTypesIter;
+
+ private: std::vector<double> stepTimes;
+ private: std::vector<double>::iterator stepTimesIter;
+
+ private: std::vector<unsigned int> stepIters;
+ private: std::vector<unsigned int>::iterator stepItersIter;
+
+ private: unsigned int count;
+ private: std::string path;
+ private: FILE *indexFile;
};
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
