Revision: 8721
http://playerstage.svn.sourceforge.net/playerstage/?rev=8721&view=rev
Author: natepak
Date: 2010-05-28 16:20:21 +0000 (Fri, 28 May 2010)
Log Message:
-----------
Updated to fix a build issue
Modified Paths:
--------------
code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
code/gazebo/branches/simpar/cmake/FindOde.cmake
code/gazebo/branches/simpar/worlds/empty.world
Added Paths:
-----------
code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
Modified: code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
===================================================================
--- code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
2010-05-28 06:00:36 UTC (rev 8720)
+++ code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
2010-05-28 16:20:21 UTC (rev 8721)
@@ -173,15 +173,13 @@
simIface->StartLogEntity("pioneer");
simIface->Unpause();
- double left = 0.1/stepTime;
- if (left > 5)
- left = 5.0;
- double right = left * 0.5;
+ double left = 1.0;
+ double right = left*1.8;
std::cout << "Left[" << left << "] Right[" << right << "]\n";
- double time = simIface->data->realTime;
- while(simIface->data->realTime - time < 60)
+ double time = simIface->data->simTime;
+ while(simIface->data->simTime - time < 60)
{
simIface->SetAngularVel("pioneer::left_wheel", gazebo::Vec3(0,0,left));
simIface->SetAngularVel("pioneer::right_wheel", gazebo::Vec3(0,0, right));
@@ -252,11 +250,11 @@
std::vector<std::string > step_types;
std::vector<std::string >::iterator iter;
- step_types.push_back("robust");
+ //step_types.push_back("robust");
//step_types.push_back("world");
- //step_types.push_back("quick");
+ step_types.push_back("quick");
- /*for (iter = step_types.begin(); iter != step_types.end(); iter++)
+ for (iter = step_types.begin(); iter != step_types.end(); iter++)
{
std::string path =
std::string("/home/nate/work/simpar/data/pioneer_circle/") + *iter + "/";
system((std::string("mkdir -p ")+path).c_str());
@@ -268,7 +266,7 @@
int i = 0;
simIface->SetStepType(*iter);
- for (double step=0.01; step > 1e-5; step *= 0.5)
+ for (double step=0.0001; step > 1e-5; step *= 0.5)
{
unsigned int iterations = 10;
if (*iter == "world")
@@ -279,7 +277,8 @@
simIface->SetStepIterations(iterations);
fprintf(out,"%d %f %d\n",i,step, iterations);
- std::cout << "Type[" << *iter << "] Step[" << step << "] Iterations["
<< iterations << "]\n";
+ std::cout << "Type[" << *iter << "] Step[" << step
+ << "] Iterations[" << iterations << "]\n";
RunSim(step);
std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + path +
"pioneer_circle_benchmark_" + *iter + "_" + boost::lexical_cast<std::string>(i)
+ ".data";
@@ -289,16 +288,7 @@
fclose(out);
}
- */
- // This will make a spiral
- //for (int i=0; i < 50; i++)
- while(1)
- {
- simIface->ApplyTorque("pioneer::left_wheel",gazebo::Vec3(0,0,-50));
- usleep(100000);
- }
-
factoryIface->Close();
simIface->Close();
graphicsIface->Close();
Added: code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
===================================================================
--- code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
(rev 0)
+++ code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
2010-05-28 16:20:21 UTC (rev 8721)
@@ -0,0 +1,295 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <libgazebo/gz.h>
+#include <boost/lexical_cast.hpp>
+
+gazebo::Client *client = NULL;
+gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
+gazebo::Graphics3dIface *graphicsIface = NULL;
+
+std::string test_name="Pioneer Spiral Benchmark";
+std::string data_filename = "/tmp/pioneer_spiral_benchmark.data";
+
+void spawn_robot()
+{
+ std::ostringstream model;
+
+ model << "<model:physical name='pioneer'>";
+ model << " <xyz>0 0 0.15</xyz>";
+ model << " <rpy>0.0 0.0 0.0</rpy>";
+ model << " <canonicalBody>chassis_body</canonicalBody>";
+ model << " <body:box name='chassis_body'>";
+ model << " <xyz>0.0 0.0 0.0</xyz>";
+ model << " <rpy>0.0 0.0 0.0</rpy>";
+ model << " <geom:box name='chassis_geom'>";
+ model << " <xyz>0 0 0.0</xyz>";
+ model << " <size>0.445 0.277 0.17</size>";
+ model << " <mass>2.0</mass>";
+ model << " <mu1>1</mu1>";
+ model << " <visual>";
+ model << " <size>0.5 0.277 0.17</size>";
+ model << " <xyz>0 0 0.04</xyz>";
+ model << " <rpy>0 180 0</rpy>";
+ model << " <mesh>Pioneer2dx/chassis.mesh</mesh>";
+ model << " <material>Gazebo/Pioneer2Body</material>";
+ model << " </visual>";
+ model << " <visual>";
+ model << " <xyz>0.015 0 0.09</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <mesh>Pioneer2at/chassis_top.mesh</mesh>";
+ model << " <material>Gazebo/Black</material>";
+ model << " </visual>";
+ model << " <visual>";
+ model << " <xyz>0.21 0.0 0.068</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <size>0.12 0.29 0.034</size>";
+ model << " <mesh>Pioneer2at/sonarbank.mesh</mesh>";
+ model << " <material>Gazebo/Gold</material>";
+ model << " </visual>";
+ model << " <visual>";
+ model << " <xyz>-0.178 0.0 0.068</xyz>";
+ model << " <rpy>0 0 180</rpy>";
+ model << " <size>0.12 0.29 0.034</size>";
+ model << " <mesh>Pioneer2at/sonarbank.mesh</mesh>";
+ model << " <material>Gazebo/Gold</material>";
+ model << " </visual>";
+ model << " </geom:box>";
+ model << " </body:box>";
+ model << " <body:cylinder name='left_wheel'>";
+ model << " <xyz>0.1 -0.17 -0.0725</xyz>";
+ model << " <rpy>0 90 90</rpy>";
+ model << " <geom:cylinder name='left_wheel_geom'>";
+ model << " <size>0.075 0.05</size>";
+ model << " <mass>0.5</mass>";
+ model << " <mu1>0.5</mu1>";
+ model << " <visual>";
+ model << " <rpy>-90 0 0</rpy>";
+ model << " <size>0.15 0.05 0.15</size>";
+ model << " <mesh>Pioneer2dx/tire.mesh</mesh>";
+ model << " <material>Gazebo/Black</material>";
+ model << " </visual>";
+ model << " <visual>";
+ model << " <rpy>-90 0 0</rpy>";
+ model << " <size>0.088 0.05 0.088</size>";
+ model << " <mesh>Pioneer2at/wheel.mesh</mesh>";
+ model << " <material>Gazebo/Gold</material>";
+ model << " </visual>";
+ model << " <visual>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <xyz>0 0 0.015</xyz>";
+ model << " <size>0.04 0.04 0.08 </size>";
+ model << " <mesh>unit_cylinder</mesh>";
+ model << " <material>Gazebo/Black</material>";
+ model << " </visual>";
+ model << " </geom:cylinder>";
+ model << " </body:cylinder>";
+ model << " <body:cylinder name='right_wheel'>";
+ model << " <xyz>0.1 0.17 -0.0725</xyz>";
+ model << " <rpy>0 90 90</rpy>";
+ model << " <geom:cylinder name='right_wheel_geom'>";
+ model << " <size>0.075 0.05</size>";
+ model << " <mass>0.5</mass>";
+ model << " <mu1>0.5</mu1>";
+ model << " <visual>";
+ model << " <rpy>-90 0 0</rpy>";
+ model << " <size>0.15 0.05 0.15</size>";
+ model << " <mesh>Pioneer2dx/tire.mesh</mesh>";
+ model << " <material>Gazebo/Black</material>";
+ model << " </visual>";
+ model << " <visual>";
+ model << " <rpy>-90 0 0</rpy>";
+ model << " <size>0.088 0.05 0.088</size>";
+ model << " <mesh>Pioneer2at/wheel.mesh</mesh>";
+ model << " <material>Gazebo/Gold</material>";
+ model << " </visual>";
+ model << " <visual>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <xyz>0 0 -0.015</xyz>";
+ model << " <size>0.04 0.04 0.08</size>";
+ model << " <mesh>unit_cylinder</mesh>";
+ model << " <material>Gazebo/Black</material>";
+ model << " </visual>";
+ model << " </geom:cylinder>";
+ model << " </body:cylinder>";
+ model << " <body:sphere name='castor_body'>";
+ model << " <xyz>-0.200 0 -0.11</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <geom:sphere name='castor_geom'>";
+ model << " <size>0.04</size>";
+ model << " <mass>0.5</mass>";
+ model << " <mu1>0.5</mu1>";
+ model << " <visual>";
+ model << " <scale>0.04 0.04 0.04</scale>";
+ model << " <mesh>unit_sphere</mesh>";
+ model << " <material>Gazebo/Black</material>";
+ model << " </visual>";
+ model << " </geom:sphere>";
+ model << " </body:sphere>";
+ model << " <joint:hinge name='left_wheel_hinge'>";
+ model << " <body1>left_wheel</body1>";
+ model << " <body2>chassis_body</body2>";
+ model << " <anchor>left_wheel</anchor>";
+ model << " <anchorOffset>0 0 0.04</anchorOffset>";
+ model << " <axis>0 1 0</axis>";
+ model << " <erp>0.8</erp>";
+ model << " <cfm>10e-5</cfm>";
+ model << " </joint:hinge>";
+ model << " <joint:hinge name='right_wheel_hinge'>";
+ model << " <body1>right_wheel</body1>";
+ model << " <body2>chassis_body</body2>";
+ model << " <anchor>right_wheel</anchor>";
+ model << " <anchorOffset>0 0 -0.04</anchorOffset>";
+ model << " <axis>0 1 0</axis>";
+ model << " <erp>0.8</erp>";
+ model << " <cfm>10e-5</cfm>";
+ model << " </joint:hinge>";
+ model << " <joint:ball name='ball_joint'>";
+ model << " <body1>castor_body</body1>";
+ model << " <body2>chassis_body</body2>";
+ model << " <anchor>castor_body</anchor>";
+ model << " <erp>0.8</erp>";
+ model << " <cfm>10e-5</cfm>";
+ model << " </joint:ball>";
+ model << " <controller:differential_position2d name='controller1'>";
+ model << " <leftJoint>left_wheel_hinge</leftJoint>";
+ model << " <rightJoint>right_wheel_hinge</rightJoint>";
+ model << " <wheelSeparation>0.39</wheelSeparation>";
+ model << " <wheelDiameter>0.15</wheelDiameter>";
+ model << " <torque>5</torque>";
+ model << " <interface:position name='position_iface_0'/>";
+ model << " </controller:differential_position2d>";
+ model << "</model:physical>";
+
+ factoryIface->Lock(1);
+ strcpy( (char*)factoryIface->data->newModel, model.str().c_str() );
+ factoryIface->Unlock();
+}
+
+void RunSim(double stepTime)
+{
+ simIface->StartLogEntity("pioneer");
+ simIface->Unpause();
+
+ double left = 0.1/stepTime;
+ if (left > 5)
+ left = 5.0;
+ double right = left * 0.5;
+
+ std::cout << "Left[" << left << "] Right[" << right << "]\n";
+
+ double time = simIface->data->realTime;
+ while(simIface->data->realTime - time < 60)
+ {
+ simIface->ApplyTorque("pioneer::left_wheel",gazebo::Vec3(0,0,-50));
+ usleep(100000);
+ }
+
+ simIface->StopLogEntity("pioneer");
+ simIface->Pause();
+ simIface->Reset();
+}
+
+int main()
+{
+ client = new gazebo::Client();
+ simIface = new gazebo::SimulationIface();
+ factoryIface = new gazebo::FactoryIface();
+ graphicsIface = new gazebo::Graphics3dIface();
+
+ try
+ {
+ client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+ }
+ catch(std::string e)
+ {
+ std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
+ return -1;
+ }
+
+ /// Open the sim iface
+ try
+ {
+ simIface->Open(client, "default");
+ }
+ catch (std::string e)
+ {
+ std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
+ 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_robot();
+ usleep(100000);
+
+ /// Open the global graphics Interface
+ try
+ {
+ graphicsIface->Open(client, "pioneer");
+ }
+ catch (std::string e)
+ {
+ std::cerr << "Gazebo error: Unable to connect to an interface\n"
+ << e << "\n";
+ return -1;
+ }
+
+ graphicsIface->DrawRibbonTrail("trail");
+
+ std::vector<std::string > step_types;
+ std::vector<std::string >::iterator iter;
+ step_types.push_back("robust");
+ //step_types.push_back("world");
+ //step_types.push_back("quick");
+
+ for (iter = step_types.begin(); iter != step_types.end(); iter++)
+ {
+ std::string path =
std::string("/home/nate/work/simpar/data/pioneer_spiral/") + *iter + "/";
+ system((std::string("mkdir -p ")+path).c_str());
+
+ FILE *out = fopen(std::string(path+"index.txt").c_str(), "w");
+ fprintf(out,"# index step_time iterations\n");
+
+ int i = 0;
+
+ simIface->SetStepType(*iter);
+ for (double step=0.01; step > 1e-5; step *= 0.5)
+ {
+ unsigned int iterations = 10;
+ if (*iter == "world")
+ iterations = 199;
+ for (; iterations < 200; iterations +=20, i++)
+ {
+ simIface->SetStepTime(step);
+ simIface->SetStepIterations(iterations);
+
+ fprintf(out,"%d %f %d\n",i,step, iterations);
+ std::cout << "Type[" << *iter << "] Step[" << step << "] Iterations["
<< iterations << "]\n";
+ RunSim(step);
+
+ std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + path +
"pioneer_spiral_benchmark_" + *iter + "_" + boost::lexical_cast<std::string>(i)
+ ".data";
+ system(mv_cmd.c_str());
+ }
+ }
+
+ fclose(out);
+ }
+
+ factoryIface->Close();
+ simIface->Close();
+ graphicsIface->Close();
+ client->Disconnect();
+}
Modified: code/gazebo/branches/simpar/cmake/FindOde.cmake
===================================================================
--- code/gazebo/branches/simpar/cmake/FindOde.cmake 2010-05-28 06:00:36 UTC
(rev 8720)
+++ code/gazebo/branches/simpar/cmake/FindOde.cmake 2010-05-28 16:20:21 UTC
(rev 8721)
@@ -1,4 +1,5 @@
-INCLUDE (FindPkgConfig)
+include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+include (FindPkgConfig)
SET(REQUIRED_ODE_VERSION ${ODE_VERSION})
Modified: code/gazebo/branches/simpar/worlds/empty.world
===================================================================
--- code/gazebo/branches/simpar/worlds/empty.world 2010-05-28 06:00:36 UTC
(rev 8720)
+++ code/gazebo/branches/simpar/worlds/empty.world 2010-05-28 16:20:21 UTC
(rev 8721)
@@ -15,16 +15,16 @@
<verbosity>4</verbosity>
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
- <stepType>robust</stepType>
+ <stepType>quick</stepType>
<stepIters>100</stepIters>
<stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
- <contactSurfaceLayer>0.001</contactSurfaceLayer>
+ <contactSurfaceLayer>0.0</contactSurfaceLayer>
</physics:ode>
<rendering:gui>
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
