Revision: 8711
http://playerstage.svn.sourceforge.net/playerstage/?rev=8711&view=rev
Author: natepak
Date: 2010-05-25 01:37:27 +0000 (Tue, 25 May 2010)
Log Message:
-----------
Allow for different step functions
Modified Paths:
--------------
code/gazebo/branches/simpar/benchmarks/spinningbox_benchmark.cc
code/gazebo/branches/simpar/server/Simulator.cc
code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
code/gazebo/branches/simpar/worlds/coffee_cup.world
code/gazebo/branches/simpar/worlds/empty.world
code/gazebo/branches/simpar/worlds/pendulum.world
code/gazebo/branches/simpar/worlds/pr2.world
code/gazebo/branches/simpar/worlds/simpleshapes.world
code/gazebo/branches/simpar/worlds/single_box.world
code/gazebo/branches/simpar/worlds/test_stacks.world
code/gazebo/branches/simpar/worlds/test_stacks_with_rays.world
code/gazebo/branches/simpar/worlds/trimesh.world
code/gazebo/branches/simpar/worlds/willowgarage.world
Modified: code/gazebo/branches/simpar/benchmarks/spinningbox_benchmark.cc
===================================================================
--- code/gazebo/branches/simpar/benchmarks/spinningbox_benchmark.cc
2010-05-25 01:00:29 UTC (rev 8710)
+++ code/gazebo/branches/simpar/benchmarks/spinningbox_benchmark.cc
2010-05-25 01:37:27 UTC (rev 8711)
@@ -6,6 +6,7 @@
gazebo::Client *client = NULL;
gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
std::string test_name="Spinning Box Benchmark";
std::string xlabel = "Spinning Box Count";
@@ -29,10 +30,39 @@
std::cerr << "Error\n";
}
+void spawn_box()
+{
+ std::ostringstream model;
+
+ model << "<model:physical name='box'>";
+ model << " <static>false</static>";
+ model << " <xyz>0 0 0.5</xyz>";
+ model << " <rpy>00 00 0</rpy>";
+ model << " <body:box name='body'>";
+ model << " <geom:box name='geom'>";
+ model << " <size>1 1 1</size>";
+ model << " <mass>1</mass>";
+ model << " <mu1>.1</mu1>";
+ model << " <mu2>.1</mu2>";
+ model << " <visual>";
+ model << " <size>1 1 1</size>";
+ model << " <mesh>unit_box</mesh>";
+ model << " <material>Gazebo/Rocky</material>";
+ model << " </visual>";
+ model << " </geom:box>";
+ model << " </body:box>";
+ model << "</model:physical>";
+
+ factoryIface->Lock(1);
+ strcpy( (char*)factoryIface->data->newModel, model.str().c_str() );
+ factoryIface->Unlock();
+}
+
int main(int argc, char **argv)
{
client = new gazebo::Client();
simIface = new gazebo::SimulationIface();
+ factoryIface = new gazebo::FactoryIface();
float vel = 1.0;
if (argc > 1)
@@ -59,6 +89,20 @@
return -1;
}
+ // Open the factory iface
+ try
+ {
+ factoryIface->Open(client, "default");
+ }
+ catch( std::string e)
+ {
+ std::cerr << "Gazebo error: Unable to connect to the factory Iface:"
+ << e << "\n";
+ return -1;
+ }
+ spawn_box();
+
+ usleep(5000000);
//FILE *out = fopen(data_filename.c_str(), "w");
/*if (!out)
@@ -72,12 +116,13 @@
gazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
gazebo::Pose modelPose;
+
bool done = false;
while (!done)
{
- simIface->SetAngularVel("box_model", gazebo::Vec3(0,0,vel));
+ simIface->SetAngularVel("box", gazebo::Vec3(0,0,vel));
- simIface->GetState("box_model", modelPose, linearVel, angularVel,
+ simIface->GetState("box", modelPose, linearVel, angularVel,
linearAccel, angularAccel);
printf("Pos[%4.2f %4.2f %4.2f] RPY[%4.2f %4.2f %4.2f] ", modelPose.pos.x,
modelPose.pos.y, modelPose.pos.z, modelPose.roll, modelPose.pitch,
modelPose.yaw);
printf("LV[%4.2f %4.2f %4.2f] ",linearVel.x, linearVel.y, linearVel.z );
Modified: code/gazebo/branches/simpar/server/Simulator.cc
===================================================================
--- code/gazebo/branches/simpar/server/Simulator.cc 2010-05-25 01:00:29 UTC
(rev 8710)
+++ code/gazebo/branches/simpar/server/Simulator.cc 2010-05-25 01:37:27 UTC
(rev 8711)
@@ -57,9 +57,9 @@
<gravity>0 0 -9.8</gravity>\
<cfm>0.0000000001</cfm>\
<erp>0.2</erp>\
- <quickStep>true</quickStep>\
- <quickStepIters>10</quickStepIters>\
- <quickStepW>1.3</quickStepW>\
+ <stepType>quick</stepType>\
+ <stepIters>10</stepIters>\
+ <stepW>1.3</stepW>\
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>\
<contactSurfaceLayer>0.001</contactSurfaceLayer>\
</physics:ode>\
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-05-25 01:00:29 UTC (rev 8710)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-05-25 01:37:27 UTC (rev 8711)
@@ -90,9 +90,9 @@
Param::Begin(&this->parameters);
this->globalCFMP = new ParamT<double>("cfm", 10e-5, 0);
this->globalERPP = new ParamT<double>("erp", 0.2, 0);
- this->quickStepP = new ParamT<bool>("quickStep", false, 0);
- this->quickStepItersP = new ParamT<int>("quickStepIters", 20, 0);
- this->quickStepWP = new ParamT<double>("quickStepW", 1.3, 0); ///
over_relaxation value for SOR
+ this->stepTypeP = new ParamT<std::string>("stepType", "quick", 0);
+ this->stepItersP = new ParamT<int>("stepIters", 20, 0);
+ this->stepWP = new ParamT<double>("stepW", 1.3, 0); /// over_relaxation
value for SOR
this->contactMaxCorrectingVelP = new
ParamT<double>("contactMaxCorrectingVel", 10.0, 0);
this->contactSurfaceLayerP = new ParamT<double>("contactSurfaceLayer", 0.01,
0);
this->autoDisableBodyP = new ParamT<bool>("autoDisableBody", false, 0);
@@ -119,9 +119,9 @@
delete this->globalCFMP;
delete this->globalERPP;
- delete this->quickStepP;
- delete this->quickStepItersP;
- delete this->quickStepWP;
+ delete this->stepTypeP;
+ delete this->stepItersP;
+ delete this->stepWP;
delete this->contactMaxCorrectingVelP;
delete this->contactSurfaceLayerP;
}
@@ -140,9 +140,9 @@
this->updateRateP->Load(cnode);
this->globalCFMP->Load(cnode);
this->globalERPP->Load(cnode);
- this->quickStepP->Load(cnode);
- this->quickStepItersP->Load(cnode);
- this->quickStepWP->Load(cnode);
+ this->stepTypeP->Load(cnode);
+ this->stepItersP->Load(cnode);
+ this->stepWP->Load(cnode);
this->contactMaxCorrectingVelP->Load(cnode);
this->contactSurfaceLayerP->Load(cnode);
this->autoDisableBodyP->Load(cnode);
@@ -179,9 +179,9 @@
stream << prefix << " " << *(this->updateRateP) << "\n";
stream << prefix << " " << *(this->globalCFMP) << "\n";
stream << prefix << " " << *(this->globalERPP) << "\n";
- stream << prefix << " " << *(this->quickStepP) << "\n";
- stream << prefix << " " << *(this->quickStepItersP) << "\n";
- stream << prefix << " " << *(this->quickStepWP) << "\n";
+ stream << prefix << " " << *(this->stepTypeP) << "\n";
+ stream << prefix << " " << *(this->stepItersP) << "\n";
+ stream << prefix << " " << *(this->stepWP) << "\n";
stream << prefix << " " << *(this->contactMaxCorrectingVelP) << "\n";
stream << prefix << " " << *(this->contactSurfaceLayerP) << "\n";
stream << prefix << "</physics:ode>\n";
@@ -193,10 +193,14 @@
{
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->quickStepItersP->GetValue() );
- dWorldSetQuickStepW(this->worldId, this->quickStepWP->GetValue() );
+
+ dWorldSetQuickStepNumIterations(this->worldId, **this->stepItersP );
+ dWorldSetQuickStepW(this->worldId, **this->stepWP );
+
+ dWorldSetRobustStepMaxIterations(this->worldId, **this->stepItersP );
}
////////////////////////////////////////////////////////////////////////////////
@@ -270,10 +274,14 @@
//DiagnosticTimer timer("ODEPhysics Step Update");
// Update the dynamical model
- if (**this->quickStepP)
+ if (**this->stepTypeP == "quick")
dWorldQuickStep(this->worldId, (**this->stepTimeP).Double());
+ else if (**this->stepTypeP == "world")
+ dWorldStep( this->worldId, (**this->stepTimeP).Double() );
+ else if (**this->stepTypeP == "robust")
+ dWorldRobustStep( this->worldId, (**this->stepTimeP).Double() );
else
- dWorldStep( this->worldId, (**this->stepTimeP).Double() );
+ gzthrow(std::string("Invalid step type[") + **this->stepTypeP);
// Very important to clear out the contact group
dJointGroupEmpty( this->contactGroup );
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
2010-05-25 01:00:29 UTC (rev 8710)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
2010-05-25 01:37:27 UTC (rev 8711)
@@ -154,9 +154,9 @@
private: ParamT<double> *globalCFMP;
private: ParamT<double> *globalERPP;
- private: ParamT<bool> *quickStepP;
- private: ParamT<int> *quickStepItersP;
- private: ParamT<double> *quickStepWP;
+ private: ParamT<std::string> *stepTypeP;
+ private: ParamT<int> *stepItersP;
+ private: ParamT<double> *stepWP;
private: ParamT<double> *contactMaxCorrectingVelP;
private: ParamT<double> *contactSurfaceLayerP;
private: ParamT<bool> *autoDisableBodyP;
Modified: code/gazebo/branches/simpar/worlds/coffee_cup.world
===================================================================
--- code/gazebo/branches/simpar/worlds/coffee_cup.world 2010-05-25 01:00:29 UTC
(rev 8710)
+++ code/gazebo/branches/simpar/worlds/coffee_cup.world 2010-05-25 01:37:27 UTC
(rev 8711)
@@ -21,9 +21,9 @@
<gravity>0 0 -9.80665</gravity>
<cfm>10e-8</cfm>
<erp>0.3</erp>
- <quickStep>true</quickStep>
- <quickStepIters>50</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>50</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
Modified: code/gazebo/branches/simpar/worlds/empty.world
===================================================================
--- code/gazebo/branches/simpar/worlds/empty.world 2010-05-25 01:00:29 UTC
(rev 8710)
+++ code/gazebo/branches/simpar/worlds/empty.world 2010-05-25 01:37:27 UTC
(rev 8711)
@@ -20,16 +20,11 @@
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
- <quickStep>true</quickStep>
- <quickStepIters>10</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>quick</stepType>
+ <stepIters>10</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
-
- <!-- updateRate: <0 == throttle simTime to match realTime.
- 0 == No throttling
- >0 == Frequency at which to throttle the sim -->
- <!--<updateRate>0</updateRate>-->
</physics:ode>
<rendering:gui>
@@ -39,9 +34,9 @@
</rendering:gui>
<rendering:ogre>
- <ambient>0 0 0 1</ambient>
+ <ambient>.2 .2 .2 1</ambient>
<shadows>true</shadows>
- <grid>false</grid>
+ <grid>false</grid>
</rendering:ogre>
<!-- Ground Plane -->
@@ -54,7 +49,7 @@
<geom:plane name="plane1_geom">
<normal>0 0 1</normal>
<size>100 100</size>
- <segments>10 10</segments>
+ <segments>1 1</segments>
<uvTile>100 100</uvTile>
<mu1>109999.0</mu1>
<mu2>1000.0</mu2>
Modified: code/gazebo/branches/simpar/worlds/pendulum.world
===================================================================
--- code/gazebo/branches/simpar/worlds/pendulum.world 2010-05-25 01:00:29 UTC
(rev 8710)
+++ code/gazebo/branches/simpar/worlds/pendulum.world 2010-05-25 01:37:27 UTC
(rev 8711)
@@ -19,9 +19,9 @@
<gravity>0 0 -9.8</gravity>
<cfm>10e-10</cfm>
<erp>0.2</erp>
- <quickStep>true</quickStep>
- <quickStepIters>50</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>50</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
Modified: code/gazebo/branches/simpar/worlds/pr2.world
===================================================================
--- code/gazebo/branches/simpar/worlds/pr2.world 2010-05-25 01:00:29 UTC
(rev 8710)
+++ code/gazebo/branches/simpar/worlds/pr2.world 2010-05-25 01:37:27 UTC
(rev 8711)
@@ -21,9 +21,9 @@
<cfm>10e-10</cfm>
<erp>0.2</erp>
<updateRate>0</updateRate>
- <quickStep>true</quickStep>
- <quickStepIters>50</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>50</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>
Modified: code/gazebo/branches/simpar/worlds/simpleshapes.world
===================================================================
--- code/gazebo/branches/simpar/worlds/simpleshapes.world 2010-05-25
01:00:29 UTC (rev 8710)
+++ code/gazebo/branches/simpar/worlds/simpleshapes.world 2010-05-25
01:37:27 UTC (rev 8711)
@@ -19,9 +19,9 @@
<gravity>0 0 -9.8</gravity>
<cfm>10e-10</cfm>
<erp>0.2</erp>
- <quickStep>true</quickStep>
- <quickStepIters>50</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>50</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
Modified: code/gazebo/branches/simpar/worlds/single_box.world
===================================================================
--- code/gazebo/branches/simpar/worlds/single_box.world 2010-05-25 01:00:29 UTC
(rev 8710)
+++ code/gazebo/branches/simpar/worlds/single_box.world 2010-05-25 01:37:27 UTC
(rev 8711)
@@ -19,9 +19,9 @@
<gravity>0 0 -9.8</gravity>
<cfm>10e-10</cfm>
<erp>0.2</erp>
- <quickStep>true</quickStep>
- <quickStepIters>50</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>50</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
Modified: code/gazebo/branches/simpar/worlds/test_stacks.world
===================================================================
--- code/gazebo/branches/simpar/worlds/test_stacks.world 2010-05-25
01:00:29 UTC (rev 8710)
+++ code/gazebo/branches/simpar/worlds/test_stacks.world 2010-05-25
01:37:27 UTC (rev 8711)
@@ -27,9 +27,9 @@
<gravity>0 0 -9.8</gravity>
<cfm>0.001</cfm>
<erp>0.1</erp>
- <quickStep>true</quickStep>
- <quickStepIters>30</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>30</stepIters>
+ <stepW>1.3</stepW>
</physics:ode>
<rendering:gui>
Modified: code/gazebo/branches/simpar/worlds/test_stacks_with_rays.world
===================================================================
--- code/gazebo/branches/simpar/worlds/test_stacks_with_rays.world
2010-05-25 01:00:29 UTC (rev 8710)
+++ code/gazebo/branches/simpar/worlds/test_stacks_with_rays.world
2010-05-25 01:37:27 UTC (rev 8711)
@@ -27,9 +27,9 @@
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
- <quickStep>true</quickStep>
- <quickStepIters>10</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>10</stepIters>
+ <stepW>1.3</stepW>
</physics:ode>
<rendering:gui>
Modified: code/gazebo/branches/simpar/worlds/trimesh.world
===================================================================
--- code/gazebo/branches/simpar/worlds/trimesh.world 2010-05-25 01:00:29 UTC
(rev 8710)
+++ code/gazebo/branches/simpar/worlds/trimesh.world 2010-05-25 01:37:27 UTC
(rev 8711)
@@ -21,9 +21,9 @@
<gravity>0 0 -9.80665</gravity>
<cfm>10e-8</cfm>
<erp>0.3</erp>
- <quickStep>true</quickStep>
- <quickStepIters>20</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>true</stepType>
+ <stepIters>20</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
Modified: code/gazebo/branches/simpar/worlds/willowgarage.world
===================================================================
--- code/gazebo/branches/simpar/worlds/willowgarage.world 2010-05-25
01:00:29 UTC (rev 8710)
+++ code/gazebo/branches/simpar/worlds/willowgarage.world 2010-05-25
01:37:27 UTC (rev 8711)
@@ -24,9 +24,9 @@
<updateRate>0</updateRate>
<cfm>1e-07</cfm>
<erp>0.2</erp>
- <quickStep>1</quickStep>
- <quickStepIters>10</quickStepIters>
- <quickStepW>1.3</quickStepW>
+ <stepType>1</stepType>
+ <stepIters>10</stepIters>
+ <stepW>1.3</stepW>
<contactMaxCorrectingVel>100</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>
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
