Revision: 8762
http://playerstage.svn.sourceforge.net/playerstage/?rev=8762&view=rev
Author: natepak
Date: 2010-06-11 20:54:20 +0000 (Fri, 11 Jun 2010)
Log Message:
-----------
Added box_stack plugin
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/CMakeLists.txt
code/gazebo/branches/simpar/worlds/3stacks.world
Added Paths:
-----------
code/gazebo/branches/simpar/plugins/box_stack.cc
code/gazebo/branches/simpar/worlds/3shortstacks.world
Modified: code/gazebo/branches/simpar/plugins/CMakeLists.txt
===================================================================
--- code/gazebo/branches/simpar/plugins/CMakeLists.txt 2010-06-11 18:12:50 UTC
(rev 8761)
+++ code/gazebo/branches/simpar/plugins/CMakeLists.txt 2010-06-11 20:54:20 UTC
(rev 8762)
@@ -20,6 +20,7 @@
pioneer_gripper
ball_drop
box_push
+ box_stack
)
Added: code/gazebo/branches/simpar/plugins/box_stack.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/box_stack.cc
(rev 0)
+++ code/gazebo/branches/simpar/plugins/box_stack.cc 2010-06-11 20:54:20 UTC
(rev 8762)
@@ -0,0 +1,158 @@
+#include <boost/bind.hpp>
+
+#include <gazebo/gazeboserver.hh>
+
+#define LOG true
+
+namespace gazebo
+{
+
+ class BoxStack : public Handler
+ {
+ public: BoxStack() : Handler()
+ {
+ this->box = NULL;
+
+ //for (double i=0.001; i > 1e-5; i*=0.5)
+ //this->stepTimes.push_back(i);
+ this->stepTimes.push_back(0.001);
+
+ //for (unsigned int i=10; i<=100; i+=10)
+ this->stepIters.push_back(100);
+
+ 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/box_stack/");
+ system( (std::string("mkdir -p ") + this->path).c_str() );
+
+ if (LOG)
+ {
+ this->indexFile = fopen(std::string(this->path + "index.txt").c_str(),
"w");
+ fprintf(this->indexFile, "# index step_type step_time
step_iterations\n");
+ }
+ }
+
+ public: ~BoxStack()
+ {
+ if (LOG)
+ fclose(this->indexFile);
+
+ World::Instance()->DisconnectWorldUpdateStartSignal(
+ boost::bind(&BoxStack::UpdateCB, this));
+ }
+
+ public: void Load()
+ {
+ this->box = (Model*)World::Instance()->GetEntityByName("board");
+ this->physics = World::Instance()->GetPhysicsEngine();
+ this->count = 0;
+
+ World::Instance()->ConnectWorldUpdateStartSignal(
+ boost::bind(&BoxStack::UpdateCB, this));
+
+ if (this->box)
+ {
+ this->prevTime = Simulator::Instance()->GetSimTime();
+
+ if (LOG)
+ Logger::Instance()->AddLog("board","/tmp/board.log");
+
+ World::Instance()->ConnectWorldUpdateStartSignal(
+ boost::bind(&BoxStack::UpdateCB, this));
+
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "]\n";
+ }
+ Simulator::Instance()->SetPaused(false);
+ }
+
+ public: void UpdateCB()
+ {
+ if (Simulator::Instance()->GetSimTime() - this->prevTime > 20.0)
+ this->UpdateJob();
+ }
+
+ public: void UpdateJob()
+ {
+ if (LOG)
+ {
+ Logger::Instance()->RemoveLog("board");
+
+ std::string mv_cmd = std::string("mv /tmp/board.log ") + this->path +
+ "box_stack_" + 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()->SetUserQuit();
+ Simulator::Instance()->RemoveHandler(this->name);
+ return;
+ }
+ }
+ }
+
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+
+ World::Instance()->Reset();
+
+ this->count++;
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "] "
+ << "Count[" << this->count << "]\n";
+
+ this->prevTime = Simulator::Instance()->GetSimTime();
+
+ if (LOG)
+ Logger::Instance()->AddLog("board","/tmp/board.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 *box;
+
+ private: FILE *indexFile;
+ private: Time prevTime;
+ private: PhysicsEngine *physics;
+ private: unsigned int count;
+ };
+
+ GZ_REGISTER_HANDLER("BoxStack", BoxStack)
+}
Added: code/gazebo/branches/simpar/worlds/3shortstacks.world
===================================================================
--- code/gazebo/branches/simpar/worlds/3shortstacks.world
(rev 0)
+++ code/gazebo/branches/simpar/worlds/3shortstacks.world 2010-06-11
20:54:20 UTC (rev 8762)
@@ -0,0 +1,456 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz";
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model";
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor";
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body";
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom";
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint";
+
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface";
+
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering";
+
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller";
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics";
>
+
+ <verbosity>5</verbosity>
+
+ <physics:ode>
+ <stepTime>0.001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.00000000001</cfm>
+ <erp>0.1</erp>
+ <stepType>quick</stepType>
+ <stepIters>100</stepIters>
+ <stepW>1.3</stepW>
+ <contactSurfaceLayer>0.00</contactSurfaceLayer>
+ </physics:ode>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <imageSize>1024 800</imageSize>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>1.0 1.5 101</xyz>
+ <rpy>0 90 0</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+ <rendering:ogre>
+ <ambient>0.5 0.5 0.5 1.0</ambient>
+ <shadows>false</shadows>
+ </rendering:ogre>
+
+ <model:physical name="board">
+ <xyz>1 1.5 5.05</xyz>
+ <static>false</static>
+ <body:box name="body">
+ <geom:box name="geom">
+ <size>0.5 2.5 0.1</size>
+ <mass>100.0</mass>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 2.5 0.1</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="box1_model">
+ <xyz>1 1.5 0.5</xyz>
+ <canonicalBody>box1_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box1_body">
+ <geom:box name="box1_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box3_model">
+ <xyz>1 1.5 1.5</xyz>
+ <canonicalBody>box3_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box3_body">
+ <geom:box name="box3_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box4_model">
+ <xyz>1 1.5 2.5</xyz>
+ <canonicalBody>box4_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box4_body">
+ <geom:box name="box4_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box5_model">
+ <xyz>1 1.5 3.5</xyz>
+ <canonicalBody>box5_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box5_body">
+ <geom:box name="box5_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box6_model">
+ <xyz>1 1.5 4.5</xyz>
+ <canonicalBody>box6_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box6_body">
+ <geom:box name="box6_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+
+
+
+ <model:physical name="box_b_1_model">
+ <xyz>1 0.5 0.5</xyz>
+ <canonicalBody>box1_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box1_body">
+ <geom:box name="box1_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_b_3_model">
+ <xyz>1 0.5 1.5</xyz>
+ <canonicalBody>box3_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box3_body">
+ <geom:box name="box3_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_b_4_model">
+ <xyz>1 0.5 2.5</xyz>
+ <canonicalBody>box4_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box4_body">
+ <geom:box name="box4_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_b_5_model">
+ <xyz>1 0.5 3.5</xyz>
+ <canonicalBody>box5_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box5_body">
+ <geom:box name="box5_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_b_6_model">
+ <xyz>1 0.5 4.5</xyz>
+ <canonicalBody>box6_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box6_body">
+ <geom:box name="box6_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+ <model:physical name="box_a_1_model">
+ <xyz>1 2.5 0.5</xyz>
+ <canonicalBody>box1_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box1_body">
+ <geom:box name="box1_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_a_3_model">
+ <xyz>1 2.5 1.5</xyz>
+ <canonicalBody>box3_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box3_body">
+ <geom:box name="box3_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_a_4_model">
+ <xyz>1 2.5 2.5</xyz>
+ <canonicalBody>box4_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box4_body">
+ <geom:box name="box4_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_a_5_model">
+ <xyz>1 2.5 3.5</xyz>
+ <canonicalBody>box5_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box5_body">
+ <geom:box name="box5_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="box_a_6_model">
+ <xyz>1 2.5 4.5</xyz>
+ <canonicalBody>box6_body</canonicalBody>
+ <static>false</static>
+
+ <body:box name="box6_body">
+ <geom:box name="box6_geom1">
+ <size>0.5 0.5 1.0</size>
+ <mass>0.1</mass>
+
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+
+ <visual>
+ <size>0.5 0.5 1.0</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- Ground Plane -->
+ <model:physical name="plane1_model">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane1_body">
+ <geom:plane name="plane1_geom">
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ <segments>10 10</segments>
+ <uvTile>100 100</uvTile>
+ <material>Gazebo/GrayGrid</material>
+ <mu1>1000.0</mu1>
+ <mu2>1000.0</mu2>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <!-- White Point light -->
+ <model:renderable name="point_white">
+ <xyz>1 1 100</xyz>
+ <enableGravity>false</enableGravity>
+
+ <light>
+ <type>point</type>
+ <diffuseColor>0.9 0.9 0.9</diffuseColor>
+ <specularColor>0.9 0.9 0.9</specularColor>
+ <range>200</range>
+
+ <!-- Constant(0-1) Linear(0-1) Quadratic -->
+ <attenuation>0.8 0.001 0.0</attenuation>
+ </light>
+ </model:renderable>
+
+</gazebo:world>
Modified: code/gazebo/branches/simpar/worlds/3stacks.world
===================================================================
--- code/gazebo/branches/simpar/worlds/3stacks.world 2010-06-11 18:12:50 UTC
(rev 8761)
+++ code/gazebo/branches/simpar/worlds/3stacks.world 2010-06-11 20:54:20 UTC
(rev 8762)
@@ -19,10 +19,10 @@
<gravity>0 0 -9.8</gravity>
<cfm>0.00000000001</cfm>
<erp>0.1</erp>
- <stepType>robust</stepType>
+ <stepType>quick</stepType>
<stepIters>10</stepIters>
<stepW>1.3</stepW>
- <contactSurfaceLayer>0.00</contactSurfaceLayer>
+ <contactSurfaceLayer>0.001</contactSurfaceLayer>
</physics:ode>
<rendering:gui>
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
