Revision: 8769
http://playerstage.svn.sourceforge.net/playerstage/?rev=8769&view=rev
Author: natepak
Date: 2010-06-22 02:19:53 +0000 (Tue, 22 Jun 2010)
Log Message:
-----------
Added ability to log contacts
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/ball_drop.cc
code/gazebo/branches/simpar/plugins/box_stack.cc
code/gazebo/branches/simpar/plugins/pioneer_gripper.cc
code/gazebo/branches/simpar/server/Entity.cc
code/gazebo/branches/simpar/server/Entity.hh
code/gazebo/branches/simpar/server/Logger.cc
code/gazebo/branches/simpar/server/World.cc
code/gazebo/branches/simpar/server/World.hh
code/gazebo/branches/simpar/server/physics/Body.cc
code/gazebo/branches/simpar/server/physics/Geom.cc
code/gazebo/branches/simpar/server/physics/Geom.hh
code/gazebo/branches/simpar/worlds/3shortstacks.world
code/gazebo/branches/simpar/worlds/empty.world
Modified: code/gazebo/branches/simpar/plugins/ball_drop.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/ball_drop.cc 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/plugins/ball_drop.cc 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -13,17 +13,17 @@
{
this->sphere = NULL;
this->model_name = "sphere";
- //this->SpawnBall(0,0,5);
+ this->SpawnBall(0,0,5);
- for (double i=0.001; i > 1e-5; i*=0.5)
- this->stepTimes.push_back(i);
+ //for (double i=0.001; i > 1e-5; i*=0.5)
+ this->stepTimes.push_back(0.1);
- for (unsigned int i=10; i<=100; i+=10)
- this->stepIters.push_back(i);
+ //for (unsigned int i=10; i<=100; i+=10)
+ this->stepIters.push_back(10);
- this->stepTypes.push_back("robust");
- this->stepTypes.push_back("world");
- this->stepTypes.push_back("quick");
+ //this->stepTypes.push_back("robust");
+ //this->stepTypes.push_back("world");
+ //this->stepTypes.push_back("quick");
this->stepTypesIter = this->stepTypes.begin();
this->stepTimesIter = this->stepTimes.begin();
@@ -74,13 +74,14 @@
model << " <body:sphere name='body'>";
model << " <geom:sphere name='geom'>";
model << " <size>0.25</size>";
- model << " <mass>1</mass>";
+ model << " <mass>10</mass>";
model << " <kp>100000000.0</kp>";
model << " <kd>1.0</kd>";
model << " <bounce>0</bounce>";
model << " <bounceVel>100000</bounceVel>";
model << " <slip1>0.01</slip1>";
model << " <slip2>0.01</slip2>";
+ model << " <enableContacts>true</enableContacts>";
model << " <visual>";
model << " <size>0.5 0.5 0.5</size>";
model << " <mesh>unit_sphere</mesh>";
@@ -101,6 +102,10 @@
this->count = 0;
this->prev_z = 0;
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&BallDrop::UpdateCB, this));
@@ -108,7 +113,7 @@
public: void UpdateCB()
{
- /*if (this->sphere == NULL)
+ if (this->sphere == NULL)
{
this->sphere =
(Model*)World::Instance()->GetEntityByName(this->model_name);
if (this->sphere)
@@ -118,7 +123,7 @@
this->prevTime = Simulator::Instance()->GetRealTime();
if (LOG)
- Logger::Instance()->AddLog("sphere","/tmp/sphere.log");
+
Logger::Instance()->AddLog("sphere::body::COM_Entity::geom","/tmp/sphere.log");
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&BallDrop::UpdateCB, this));
@@ -140,10 +145,9 @@
if (realTime - this->prevTime > 10.0)
{
- */
this->UpdateJob();
- //}
- //}
+ }
+ }
}
public: void UpdateJob()
@@ -180,10 +184,9 @@
}
}
- /*this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepType( *this->stepTypesIter );
this->physics->SetStepTime( *this->stepTimesIter );
this->physics->SetSORPGSIters( *this->stepItersIter );
- */
this->count++;
std::cout << "Type[" << *this->stepTypesIter << "] "
Modified: code/gazebo/branches/simpar/plugins/box_stack.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/box_stack.cc 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/plugins/box_stack.cc 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -15,13 +15,13 @@
//for (double i=0.001; i > 1e-5; i*=0.5)
//this->stepTimes.push_back(i);
- this->stepTimes.push_back(0.001);
+ this->stepTimes.push_back(0.01);
//for (unsigned int i=10; i<=100; i+=10)
this->stepIters.push_back(100);
+ //this->stepTypes.push_back("quick");
//this->stepTypes.push_back("world");
- //this->stepTypes.push_back("quick");
this->stepTypes.push_back("robust");
this->stepTypesIter = this->stepTypes.begin();
@@ -33,7 +33,7 @@
if (LOG)
{
- this->indexFile = fopen(std::string(this->path + "index.txt").c_str(),
"w");
+ this->indexFile = fopen(std::string(this->path + "index.txt").c_str(),
"a");
fprintf(this->indexFile, "# index step_type step_time
step_iterations\n");
}
}
@@ -61,7 +61,7 @@
this->prevTime = Simulator::Instance()->GetSimTime();
if (LOG)
- Logger::Instance()->AddLog("board","/tmp/board.log");
+
Logger::Instance()->AddLog("board::body::COM_Entity::geom","/tmp/board.log");
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&BoxStack::UpdateCB, this));
@@ -87,7 +87,7 @@
{
if (LOG)
{
- Logger::Instance()->RemoveLog("board");
+ Logger::Instance()->RemoveLog("board::body::COM_Entity::geom");
std::string mv_cmd = std::string("mv /tmp/board.log ") + this->path +
"box_stack_" + boost::lexical_cast<std::string>(this->count) +
@@ -133,7 +133,7 @@
this->prevTime = Simulator::Instance()->GetSimTime();
if (LOG)
- Logger::Instance()->AddLog("board","/tmp/board.log");
+
Logger::Instance()->AddLog("board::body::COM_Entity::geom","/tmp/board.log");
}
private: std::vector<unsigned int> stepIters;
Modified: code/gazebo/branches/simpar/plugins/pioneer_gripper.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_gripper.cc 2010-06-20
19:34:02 UTC (rev 8768)
+++ code/gazebo/branches/simpar/plugins/pioneer_gripper.cc 2010-06-22
02:19:53 UTC (rev 8769)
@@ -109,8 +109,8 @@
Vector3 leftVel = this->leftPaddle->GetWorldLinearVel();
Vector3 rightVel = this->rightPaddle->GetWorldLinearVel();
- this->leftForce = (0.08 - leftVel.y) * 0.5;
- this->rightForce = (-0.08 - rightVel.y) * 0.5;
+ this->leftForce = (0.08 - leftVel.y) * 10.5;
+ this->rightForce = (-0.08 - rightVel.y) * 10.5;
}
public: void Close()
@@ -124,8 +124,8 @@
public: void Grip()
{
- this->leftForce = -10.0;
- this->rightForce = 10.0;
+ this->leftForce = -500.0;
+ this->rightForce = 500.0;
}
public: void Lift()
Modified: code/gazebo/branches/simpar/server/Entity.cc
===================================================================
--- code/gazebo/branches/simpar/server/Entity.cc 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/server/Entity.cc 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -499,3 +499,15 @@
static std::string typenames[] = {"default", "model", "body", "geom",
"light"};
return typenames[this->type];
}
+
+
+////////////////////////////////////////////////////////////////////////////////
+void Entity::Print(std::string prefix)
+{
+ std::vector<Entity*>::iterator iter;
+ std::cout << prefix << this->GetName() << "\n";
+
+ prefix += " ";
+ for (iter = this->children.begin(); iter != this->children.end(); iter++)
+ (*iter)->Print(prefix);
+}
Modified: code/gazebo/branches/simpar/server/Entity.hh
===================================================================
--- code/gazebo/branches/simpar/server/Entity.hh 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/server/Entity.hh 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -179,6 +179,7 @@
/// pose of the parent has changed
protected: virtual void OnPoseChange() {}
+ public: void Print(std::string prefix);
/// \brief Returns true if the entities are the same. Checks only the name
public: bool operator==(const Entity &ent) const;
Modified: code/gazebo/branches/simpar/server/Logger.cc
===================================================================
--- code/gazebo/branches/simpar/server/Logger.cc 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/server/Logger.cc 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -2,6 +2,7 @@
#include "Time.hh"
#include "World.hh"
#include "Entity.hh"
+#include "Geom.hh"
#include "Simulator.hh"
#include "GazeboMessage.hh"
#include "Logger.hh"
@@ -30,10 +31,14 @@
void Logger::AddLog(const std::string &entity, const std::string &filename)
{
Logger::LogObj *newLog = new Logger::LogObj(entity, filename);
+
if (newLog->valid)
this->logObjects.push_back(newLog);
else
- delete newLog;
+ {
+ std::cerr << "Unable to create log\n";
+ delete newLog;
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -123,9 +128,26 @@
<< " " << pose3d.pos.x << " " << pose3d.pos.y << " " << pose3d.pos.z
<< " " << roll << " " << pitch << " " << yaw
<< " " << linearVel.x << " " << linearVel.y << " " << linearVel.z
- << " " << angularVel.x << " " << angularVel.y << " " << angularVel.z
- << "\n";
+ << " " << angularVel.x << " " << angularVel.y << " " << angularVel.z;
+
+ if (this->entity->GetType() == Entity::GEOM)
+ {
+ Geom *geom = (Geom*)(this->entity);
+ this->logFile << geom->GetContactCount() << " ";
+ std::cout << "Is A Geom[" << geom->GetContactCount() << "] ";
+ for (unsigned int i=0; i < geom->GetContactCount(); i++)
+ {
+ for (unsigned int j=0; j < geom->GetContact(i).depths.size(); j++)
+ std::cout << " D[" << geom->GetContact(i).depths[j] << "] ";
+ //this->logFile << geom->GetContact(i).depths[0] << " ";
+ }
+ std::cout << "\n";
+ }
}
+ else
+ this->logFile << "0 ";
+
+ this->logFile << "\n";
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/branches/simpar/server/World.cc
===================================================================
--- code/gazebo/branches/simpar/server/World.cc 2010-06-20 19:34:02 UTC (rev
8768)
+++ code/gazebo/branches/simpar/server/World.cc 2010-06-22 02:19:53 UTC (rev
8769)
@@ -231,6 +231,7 @@
this->worldStatesEndIter = this->worldStates.begin();
this->worldStatesCurrentIter = this->worldStatesInsertIter;
+ this->PrintEntityTree();
#ifdef USE_THREADPOOL
// start a thread pool with X threads
@@ -1787,3 +1788,14 @@
{
return this->selectedEntity;
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Print entity tree
+void World::PrintEntityTree()
+{
+ for (std::vector<Model*>::iterator iter = this->models.begin();
+ iter != this->models.end(); iter++)
+ {
+ (*iter)->Print("");
+ }
+}
Modified: code/gazebo/branches/simpar/server/World.hh
===================================================================
--- code/gazebo/branches/simpar/server/World.hh 2010-06-20 19:34:02 UTC (rev
8768)
+++ code/gazebo/branches/simpar/server/World.hh 2010-06-22 02:19:53 UTC (rev
8769)
@@ -233,6 +233,9 @@
/// \brief Get the selected entity
public: Entity *GetSelectedEntity() const;
+ /// \brief Print entity tree
+ public: void PrintEntityTree();
+
/// \brief Save the state of the world
private: void SaveState();
Modified: code/gazebo/branches/simpar/server/physics/Body.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/Body.cc 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/server/physics/Body.cc 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -57,7 +57,7 @@
{
this->type = Entity::BODY;
this->comEntity = new Entity(this);
- this->comEntity->SetName("COM Entity");
+ this->comEntity->SetName("COM_Entity");
this->physicsEngine = World::Instance()->GetPhysicsEngine();
Modified: code/gazebo/branches/simpar/server/physics/Geom.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/Geom.cc 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/server/physics/Geom.cc 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -76,6 +76,10 @@
this->laserFiducialIdP = new ParamT<int>("laserFiducialId",-1,0);
this->laserRetroP = new ParamT<float>("laserRetro",-1,0);
+
+ this->enableContactsP = new ParamT<bool>("enableContacts", false, 0);
+ //this->enableContactsP->Callback( &Geom::SetContactsEnabled, this );
+
Param::End();
World::Instance()->ConnectShowPhysicsSignal( boost::bind(&Geom::ShowPhysics,
this, _1) );
@@ -111,6 +115,7 @@
delete this->rpyP;
delete this->laserFiducialIdP;
delete this->laserRetroP;
+ delete this->enableContactsP;
if (this->shape)
delete this->shape;
@@ -152,7 +157,10 @@
this->rpyP->Load(node);
this->laserFiducialIdP->Load(node);
this->laserRetroP->Load(node);
+ this->enableContactsP->Load(node);
+ this->SetContactsEnabled(**this->enableContactsP);
+
this->SetRelativePose( Pose3d( **this->xyzP, **this->rpyP ) );
this->mass.SetMass( **this->massP );
@@ -467,7 +475,7 @@
////////////////////////////////////////////////////////////////////////////////
// Turn contact recording on or off
-void Geom::SetContactsEnabled(bool enable)
+void Geom::SetContactsEnabled(const bool &enable)
{
this->contactsEnabled = enable;
}
Modified: code/gazebo/branches/simpar/server/physics/Geom.hh
===================================================================
--- code/gazebo/branches/simpar/server/physics/Geom.hh 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/server/physics/Geom.hh 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -151,7 +151,7 @@
public: Shape *GetShape() const;
/// \brief Turn contact recording on or off
- public: void SetContactsEnabled(bool enable);
+ public: void SetContactsEnabled(const bool &enable);
/// \brief Return true of contact recording is on
public: bool GetContactsEnabled() const;
@@ -223,9 +223,9 @@
/// Mass as a double
private: ParamT<double> *massP;
-
protected: ParamT<Vector3> *xyzP;
protected: ParamT<Quatern> *rpyP;
+ protected: ParamT<bool> *enableContactsP;
/// Special bounding box visual
private: OgreVisual *bbVisual;
Modified: code/gazebo/branches/simpar/worlds/3shortstacks.world
===================================================================
--- code/gazebo/branches/simpar/worlds/3shortstacks.world 2010-06-20
19:34:02 UTC (rev 8768)
+++ code/gazebo/branches/simpar/worlds/3shortstacks.world 2010-06-22
02:19:53 UTC (rev 8769)
@@ -19,8 +19,8 @@
<gravity>0 0 -9.8</gravity>
<cfm>0.00000000001</cfm>
<erp>0.1</erp>
- <stepType>quick</stepType>
- <stepIters>100</stepIters>
+ <stepType>robust</stepType>
+ <stepIters>10</stepIters>
<stepW>1.3</stepW>
<contactSurfaceLayer>0.00</contactSurfaceLayer>
</physics:ode>
@@ -50,12 +50,14 @@
<body:box name="body">
<geom:box name="geom">
<size>0.5 2.5 0.1</size>
- <mass>100.0</mass>
+ <mass>10000.0</mass>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<kp>100000000.0</kp>
<kd>1.0</kd>
+ <enableContacts>true</enableContacts>
+
<visual>
<size>0.5 2.5 0.1</size>
<mesh>unit_box</mesh>
Modified: code/gazebo/branches/simpar/worlds/empty.world
===================================================================
--- code/gazebo/branches/simpar/worlds/empty.world 2010-06-20 19:34:02 UTC
(rev 8768)
+++ code/gazebo/branches/simpar/worlds/empty.world 2010-06-22 02:19:53 UTC
(rev 8769)
@@ -15,12 +15,12 @@
<verbosity>4</verbosity>
<physics:ode>
- <stepTime>0.001</stepTime>
+ <stepTime>0.1</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
- <stepType>robust</stepType>
+ <stepType>quick</stepType>
<stepIters>10</stepIters>
<stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
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
