Revision: 8776
http://playerstage.svn.sourceforge.net/playerstage/?rev=8776&view=rev
Author: natepak
Date: 2010-06-22 22:40:10 +0000 (Tue, 22 Jun 2010)
Log Message:
-----------
Updated the benchmarks
Modified Paths:
--------------
code/gazebo/trunk/benchmarks/CMakeLists.txt
code/gazebo/trunk/benchmarks/box_grid_benchmark.cc
code/gazebo/trunk/benchmarks/box_pile_benchmark.cc
code/gazebo/trunk/benchmarks/camera_benchmark.cc
code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc
code/gazebo/trunk/benchmarks/laser_benchmark.cc
code/gazebo/trunk/benchmarks/pendulum_benchmark.cc
code/gazebo/trunk/benchmarks/spinningbox_benchmark.cc
Added Paths:
-----------
code/gazebo/trunk/benchmarks/box_drop_benchmark.cc
code/gazebo/trunk/benchmarks/pioneer_circle_benchmark.cc
code/gazebo/trunk/benchmarks/pioneer_spiral_benchmark.cc
code/gazebo/trunk/benchmarks/table_benchmark.cc
Modified: code/gazebo/trunk/benchmarks/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/benchmarks/CMakeLists.txt 2010-06-22 22:39:28 UTC (rev
8775)
+++ code/gazebo/trunk/benchmarks/CMakeLists.txt 2010-06-22 22:40:10 UTC (rev
8776)
@@ -1,16 +1,5 @@
project(benchmarks)
-INCLUDE (${gazebo_cmake_dir}/GazeboUtils.cmake)
-STRING (REPLACE " " ";" boost_include_dirs_split "${boost_include_dirs}")
-STRING (REPLACE " " ";" boost_library_dirs_split "${boost_library_dirs}")
-INCLUDE_DIRECTORIES(
- .
- ${CMAKE_SOURCE_DIR}/libgazebo
- ${LINK_DIRS}
- ${boost_include_dirs_split}
-)
-
-
cmake_minimum_required( VERSION 2.4 FATAL_ERROR )
if(COMMAND cmake_policy)
@@ -18,58 +7,38 @@
cmake_policy(SET CMP0004 NEW)
endif(COMMAND cmake_policy)
-include (FindPkgConfig)
-# if (PKG_CONFIG_FOUND)
-# pkg_check_modules(PKG_CFGS REQUIRED libgazebo)
-# set (INCLUDE_DIRS ${PKG_CFGS_INCLUDE_DIRS})
-# set (LINK_DIRS ${PKG_CFGS_LIBRARY_DIRS})
-# set (LINK_LIBS ${PKG_CFGS_LINK_LIBS} ${PKG_CFGS_LIBRARIES})
-# endif (PKG_CONFIG_FOUND)
+include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+string (REPLACE " " ";" boost_include_dirs_split "${boost_include_dirs}")
+string (REPLACE " " ";" boost_library_dirs_split "${boost_library_dirs}")
+include_directories(
+ .
+ ${CMAKE_SOURCE_DIR}/libgazebo
+ ${LINK_DIRS}
+ ${boost_include_dirs_split}
+)
+
link_directories(../libgazebo ${INCLUDE_DIRS})
-set (sources laser_benchmark.cc
- box_grid_benchmark.cc
- box_pile_benchmark.cc
- joint_chain_benchmark.cc
- camera_benchmark.cc
- spinningbox_benchmark.cc
- pendulum_benchmark.cc
- )
+set (benchmarks laser
+ box_grid
+ box_pile
+ joint_chain
+ camera
+ spinningbox
+ pendulum
+ box_drop
+ table
+ pioneer_circle
+ pioneer_spiral
+ )
-set_source_files_properties(${sources} PROPERTIES COMPILE_FLAGS "-ggdb -g2
-Wall")
-
-add_executable(laser_benchmark laser_benchmark.cc)
-add_executable(box_pile_benchmark box_pile_benchmark.cc)
-add_executable(box_grid_benchmark box_grid_benchmark.cc)
-add_executable(joint_chain_benchmark joint_chain_benchmark.cc)
-add_executable(camera_benchmark camera_benchmark.cc)
-add_executable(spinningbox_benchmark spinningbox_benchmark.cc)
-add_executable(pendulum_benchmark pendulum_benchmark.cc)
-
-target_link_libraries(laser_benchmark gazebo ${LINK_LIBS} )
-target_link_libraries(box_grid_benchmark gazebo ${LINK_LIBS} )
-target_link_libraries(box_pile_benchmark gazebo ${LINK_LIBS} )
-target_link_libraries(joint_chain_benchmark gazebo ${LINK_LIBS} )
-target_link_libraries(camera_benchmark gazebo ${LINK_LIBS} )
-target_link_libraries(spinningbox_benchmark gazebo ${LINK_LIBS} )
-target_link_libraries(pendulum_benchmark gazebo ${LINK_LIBS} )
-
-set_target_properties(laser_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
-set_target_properties(box_grid_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
-set_target_properties(box_pile_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
-set_target_properties(joint_chain_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
-set_target_properties(camera_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
-set_target_properties(spinningbox_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
-set_target_properties(pendulum_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
-
-set_target_properties(laser_benchmark PROPERTIES LINK_FLAGS
"${LINK_FLAGS} ${gazebo_lflags}")
-set_target_properties(box_grid_benchmark PROPERTIES LINK_FLAGS
"${LINK_FLAGS} ${gazebo_lflags}")
-set_target_properties(box_pile_benchmark PROPERTIES LINK_FLAGS
"${LINK_FLAGS} ${gazebo_lflags}")
-set_target_properties(joint_chain_benchmark PROPERTIES LINK_FLAGS
"${LINK_FLAGS} ${gazebo_lflags}")
-set_target_properties(camera_benchmark PROPERTIES LINK_FLAGS
"${LINK_FLAGS} ${gazebo_lflags}")
-set_target_properties(spinningbox_benchmark PROPERTIES LINK_FLAGS
"${LINK_FLAGS} ${gazebo_lflags}")
-set_target_properties(pendulum_benchmark PROPERTIES LINK_FLAGS
"${LINK_FLAGS} ${gazebo_lflags}")
-
-install (TARGETS laser_benchmark box_grid_benchmark box_pile_benchmark
joint_chain_benchmark camera_benchmark spinningbox_benchmark pendulum_benchmark
DESTINATION ${CMAKE_INSTALL_PREFIX}/bin)
+foreach (src ${benchmarks})
+ set_source_files_properties(${src}_benchmark.cc PROPERTIES COMPILE_FLAGS
"-Wall")
+ add_executable(${src}_benchmark ${src}_benchmark.cc)
+ target_link_libraries(${src}_benchmark gazebo ${LINK_LIBS} )
+ set_target_properties(${src}_benchmark PROPERTIES SKIP_BUILD_RPATH TRUE)
+ set_target_properties(${src}_benchmark PROPERTIES LINK_FLAGS "${LINK_FLAGS}
${gazebo_lflags}")
+ install (TARGETS ${src}_benchmark DESTINATION ${CMAKE_INSTALL_PREFIX}/bin)
+endforeach (src ${benchmarks})
Copied: code/gazebo/trunk/benchmarks/box_drop_benchmark.cc (from rev 8771,
code/gazebo/branches/simpar/benchmarks/box_drop_benchmark.cc)
===================================================================
--- code/gazebo/trunk/benchmarks/box_drop_benchmark.cc
(rev 0)
+++ code/gazebo/trunk/benchmarks/box_drop_benchmark.cc 2010-06-22 22:40:10 UTC
(rev 8776)
@@ -0,0 +1,179 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <libgazebo/gz.h>
+#include <boost/lexical_cast.hpp>
+
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
+int laser_count = 0;
+
+std::string test_name="Box Drop Benchmark";
+std::string xlabel = "Box Count";
+std::string ylabel = "Simtime / Realtime";
+std::string data_filename = "/tmp/" + test_name + ".data";
+
+void make_plot()
+{
+ std::ostringstream cmd;
+
+ cmd << "echo \"";
+ cmd << "set xlabel '" << xlabel << "'\n";
+ cmd << "set ylabel '" << ylabel << "'\n";
+ cmd << "set title '" << test_name << "'\n";
+ cmd << "set terminal png\n";
+ cmd << "set output '" << test_name << ".png'\n";
+ cmd << "plot '" << data_filename << "' with lines\n";
+ cmd << "\" | gnuplot";
+
+ if (system(cmd.str().c_str() ) < 0)
+ std::cerr << "Error\n";
+}
+
+void spawn_box(double x, double y, double z=1)
+{
+ std::ostringstream model;
+
+ model << "<model:physical name='box'>";
+ model << " <xyz>" << x << " " << y << " " << z << "</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <body:box name='body'>";
+ model << " <geom:box name='geom'>";
+ model << " <size>0.5 0.5 0.5</size>";
+ model << " <mass>1</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 << " <visual>";
+ model << " <size>0.5 0.5 0.5</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();
+}
+
+void RunSim()
+{
+ simIface->StartLogEntity("box", "/tmp/box.log");
+ simIface->Unpause();
+ /*libgazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
+ libgazebo::Pose modelPose;
+
+ double time = simIface->data->realTime;
+ while ( simIface->data->realTime - time < 10.0 )
+ {
+ //simIface->GetState("box", modelPose, linearVel, angularVel, linearAccel,
angularAccel);
+
+ //if ( fabs(prev_z - modelPose.pos.z) >= 1e-5)
+ //time = simIface->data->realTime;
+
+ //prev_z = modelPose.pos.z;
+ }
+ */
+ usleep(10000000);
+
+ simIface->StopLogEntity("box");
+ simIface->Pause();
+ simIface->Reset();
+}
+
+
+int main()
+{
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
+
+ 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;
+ }
+ double prev_z = 5;
+ spawn_box(0, 0, prev_z);
+ usleep(10000);
+
+
+ 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/box_drop/") +
*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\n");
+
+ int i = 0;
+
+ simIface->SetStepType(*iter);
+ for (double step=0.1; 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();
+ std::string mv_cmd = std::string("mv /tmp/box.log ") + path +
"box_drop_benchmark_" + *iter + "_" + boost::lexical_cast<std::string>(i) +
".data";
+ system(mv_cmd.c_str());
+ }
+ }
+ fclose(out);
+ }
+
+ factoryIface->Close();
+ simIface->Close();
+ client->Disconnect();
+}
+
+
Modified: code/gazebo/trunk/benchmarks/box_grid_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/box_grid_benchmark.cc 2010-06-22 22:39:28 UTC
(rev 8775)
+++ code/gazebo/trunk/benchmarks/box_grid_benchmark.cc 2010-06-22 22:40:10 UTC
(rev 8776)
@@ -4,9 +4,9 @@
#include <vector>
#include <libgazebo/gz.h>
-gazebo::Client *client = NULL;
-gazebo::SimulationIface *simIface = NULL;
-gazebo::FactoryIface *factoryIface = NULL;
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
int laser_count = 0;
std::string test_name="Box Grid Benchmark";
@@ -60,9 +60,9 @@
int main()
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- factoryIface = new gazebo::FactoryIface();
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
try
{
Modified: code/gazebo/trunk/benchmarks/box_pile_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/box_pile_benchmark.cc 2010-06-22 22:39:28 UTC
(rev 8775)
+++ code/gazebo/trunk/benchmarks/box_pile_benchmark.cc 2010-06-22 22:40:10 UTC
(rev 8776)
@@ -4,9 +4,9 @@
#include <vector>
#include <libgazebo/gz.h>
-gazebo::Client *client = NULL;
-gazebo::SimulationIface *simIface = NULL;
-gazebo::FactoryIface *factoryIface = NULL;
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
int laser_count = 0;
std::string test_name="Box Pile Benchmark";
@@ -60,9 +60,9 @@
int main()
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- factoryIface = new gazebo::FactoryIface();
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
try
{
Modified: code/gazebo/trunk/benchmarks/camera_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/camera_benchmark.cc 2010-06-22 22:39:28 UTC
(rev 8775)
+++ code/gazebo/trunk/benchmarks/camera_benchmark.cc 2010-06-22 22:40:10 UTC
(rev 8776)
@@ -4,9 +4,9 @@
#include <vector>
#include <libgazebo/gz.h>
-gazebo::Client *client = NULL;
-gazebo::SimulationIface *simIface = NULL;
-gazebo::FactoryIface *factoryIface = NULL;
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
int camera_count = 0;
std::string test_name="Camera Benchmark";
@@ -68,9 +68,9 @@
int main()
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- factoryIface = new gazebo::FactoryIface();
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
try
{
Modified: code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc 2010-06-22
22:39:28 UTC (rev 8775)
+++ code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc 2010-06-22
22:40:10 UTC (rev 8776)
@@ -4,9 +4,9 @@
#include <vector>
#include <libgazebo/gz.h>
-gazebo::Client *client = NULL;
-gazebo::SimulationIface *simIface = NULL;
-gazebo::FactoryIface *factoryIface = NULL;
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
int joint_count = 0;
std::string test_name="Joint Chain Benchmark";
@@ -89,9 +89,9 @@
int main()
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- factoryIface = new gazebo::FactoryIface();
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
try
{
Modified: code/gazebo/trunk/benchmarks/laser_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/laser_benchmark.cc 2010-06-22 22:39:28 UTC
(rev 8775)
+++ code/gazebo/trunk/benchmarks/laser_benchmark.cc 2010-06-22 22:40:10 UTC
(rev 8776)
@@ -4,9 +4,9 @@
#include <vector>
#include <libgazebo/gz.h>
-gazebo::Client *client = NULL;
-gazebo::SimulationIface *simIface = NULL;
-gazebo::FactoryIface *factoryIface = NULL;
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
int laser_count = 0;
std::string test_name="Laser Benchmark";
@@ -72,9 +72,9 @@
int main()
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- factoryIface = new gazebo::FactoryIface();
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
try
{
Modified: code/gazebo/trunk/benchmarks/pendulum_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/pendulum_benchmark.cc 2010-06-22 22:39:28 UTC
(rev 8775)
+++ code/gazebo/trunk/benchmarks/pendulum_benchmark.cc 2010-06-22 22:40:10 UTC
(rev 8776)
@@ -4,8 +4,8 @@
#include <vector>
#include <libgazebo/gz.h>
-gazebo::Client *client = NULL;
-gazebo::SimulationIface *simIface = NULL;
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
std::string test_name="Pendulum Benchmark";
std::string xlabel = "Pendulum Count";
@@ -31,8 +31,8 @@
int main(int argc, char **argv)
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
try
{
@@ -64,13 +64,13 @@
//double simTime = 0;
//double realTime = 0;
- gazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
- gazebo::Pose modelPose;
+ libgazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
+ libgazebo::Pose modelPose;
bool done = false;
while (!done)
{
- //simIface->SetAngularVel("box_model", gazebo::Vec3(0,0,vel));
+ //simIface->SetAngularVel("box_model", libgazebo::Vec3(0,0,vel));
simIface->GetState("base_model::swing_body", modelPose, linearVel,
angularVel,
linearAccel, angularAccel);
Copied: code/gazebo/trunk/benchmarks/pioneer_circle_benchmark.cc (from rev
8771, code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc)
===================================================================
--- code/gazebo/trunk/benchmarks/pioneer_circle_benchmark.cc
(rev 0)
+++ code/gazebo/trunk/benchmarks/pioneer_circle_benchmark.cc 2010-06-22
22:40:10 UTC (rev 8776)
@@ -0,0 +1,317 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <libgazebo/gz.h>
+#include <boost/lexical_cast.hpp>
+#include <math.h>
+
+#define DTOR(x) ( (x) * M_PI / 180.0)
+
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
+libgazebo::Graphics3dIface *graphicsIface = NULL;
+
+std::string test_name="Pioneer Circle Benchmark";
+std::string data_filename = "/tmp/pioneer_circle_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", "/tmp/pioneer.log");
+ simIface->Unpause();
+
+ double angle = 10;
+ double speed = 0.2;
+ std::cout << "Radians[" << DTOR(angle) << "]\n";
+ double left = speed + DTOR(angle) * 0.39/2;
+ double right = speed - DTOR(angle) * 0.39/2;
+
+ left = left / (.15 *0.5);
+ right = right / (.15 *0.5);
+
+ //std::cout << "Left[" << left << "] Right[" << right << "]\n";
+
+ libgazebo::Pose pose;
+ std::cout << "X[" << pose.pos.x << "] Y[" << pose.pos.y << "]\n";
+
+ double time = simIface->data->realTime;
+ int circles = 0;
+
+ while (circles < 4)
+ {
+ simIface->SetAngularVel("pioneer::left_wheel", libgazebo::Vec3(0,0,left));
+ simIface->SetAngularVel("pioneer::right_wheel", libgazebo::Vec3(0,0,
right));
+ simIface->GetPose2d("pioneer", pose);
+
+ if (simIface->data->realTime - time > 5.0 && fabs(pose.pos.x) < 0.1 )
+ {
+ time = simIface->data->realTime;
+ circles++;
+ std::cout << "Found a circle[" << circles << "]\n";
+ }
+ }
+
+ simIface->StopLogEntity("pioneer");
+ simIface->Pause();
+ simIface->Reset();
+}
+
+int main()
+{
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
+ graphicsIface = new libgazebo::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_circle/") + *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.001; step > 1e-5; step *= 0.5)
+ {
+ unsigned int iterations = 100;
+ 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_circle_benchmark_" + *iter + "_" + boost::lexical_cast<std::string>(i)
+ ".data";
+ system(mv_cmd.c_str());
+ }
+ }
+
+ fclose(out);
+ }
+
+ factoryIface->Close();
+ simIface->Close();
+ graphicsIface->Close();
+ client->Disconnect();
+}
Copied: code/gazebo/trunk/benchmarks/pioneer_spiral_benchmark.cc (from rev
8771, code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc)
===================================================================
--- code/gazebo/trunk/benchmarks/pioneer_spiral_benchmark.cc
(rev 0)
+++ code/gazebo/trunk/benchmarks/pioneer_spiral_benchmark.cc 2010-06-22
22:40:10 UTC (rev 8776)
@@ -0,0 +1,298 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <libgazebo/gz.h>
+#include <boost/lexical_cast.hpp>
+
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
+libgazebo::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()
+{
+ simIface->StartLogEntity("pioneer", "/tmp/pioneer.log");
+ simIface->Unpause();
+
+ libgazebo::Pose pose;
+ libgazebo::Vec3 lvel, avel, lacc, aacc;
+
+ double time = simIface->data->realTime;
+ while(simIface->data->realTime - time < 60)
+ {
+ simIface->GetState("pioneer::left_wheel",pose, lvel, avel, lacc, aacc);
+ printf("Linear[%f %f %f]\n",lvel.x, lvel.y, lvel.z);
+ printf("Angular[%f %f %f]\n",aacc.x, aacc.y, aacc.z);
+
+ simIface->ApplyTorque("pioneer::left_wheel",libgazebo::Vec3(0,0,-30));
+ //simIface->ApplyForce("pioneer::chassis_body",libgazebo::Vec3(5,0,0));
+ usleep(2000);
+ }
+
+ simIface->StopLogEntity("pioneer");
+ simIface->Pause();
+ simIface->Reset();
+}
+
+int main()
+{
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
+ graphicsIface = new libgazebo::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();
+/*
+ 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/trunk/benchmarks/spinningbox_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/spinningbox_benchmark.cc 2010-06-22
22:39:28 UTC (rev 8775)
+++ code/gazebo/trunk/benchmarks/spinningbox_benchmark.cc 2010-06-22
22:40:10 UTC (rev 8776)
@@ -4,9 +4,9 @@
#include <vector>
#include <libgazebo/gz.h>
-gazebo::Client *client = NULL;
-gazebo::SimulationIface *simIface = NULL;
-gazebo::FactoryIface *factoryIface = NULL;
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
std::string test_name="Spinning Box Benchmark";
std::string xlabel = "Spinning Box Count";
@@ -60,9 +60,9 @@
int main(int argc, char **argv)
{
- client = new gazebo::Client();
- simIface = new gazebo::SimulationIface();
- factoryIface = new gazebo::FactoryIface();
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
float vel = 1.0;
if (argc > 1)
@@ -113,14 +113,14 @@
//double realTime = 0;
- gazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
- gazebo::Pose modelPose;
+ libgazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
+ libgazebo::Pose modelPose;
bool done = false;
while (!done)
{
- simIface->SetAngularVel("box", gazebo::Vec3(0,0,vel));
+ simIface->SetAngularVel("box", libgazebo::Vec3(0,0,vel));
simIface->GetState("box", modelPose, linearVel, angularVel,
linearAccel, angularAccel);
Copied: code/gazebo/trunk/benchmarks/table_benchmark.cc (from rev 8771,
code/gazebo/branches/simpar/benchmarks/table_benchmark.cc)
===================================================================
--- code/gazebo/trunk/benchmarks/table_benchmark.cc
(rev 0)
+++ code/gazebo/trunk/benchmarks/table_benchmark.cc 2010-06-22 22:40:10 UTC
(rev 8776)
@@ -0,0 +1,216 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <libgazebo/gz.h>
+
+libgazebo::Client *client = NULL;
+libgazebo::SimulationIface *simIface = NULL;
+libgazebo::FactoryIface *factoryIface = NULL;
+int laser_count = 0;
+
+std::string test_name="Table Benchmark";
+std::string data_filename = "/tmp/table_benchmark.data";
+
+void spawn_table()
+{
+ std::ostringstream model;
+
+ model << "<model:physical name='table'>";
+ model << " <xyz>0 0 0.51</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <static>false</static>";
+ model << " <body:box name='body'>";
+
+ model << " <geom:box name='surface'>";
+ model << " <size>1.02 1.02 0.01</size>";
+ model << " <mass>1</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>0</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>1.02 1.02 .01</size>";
+ model << " <mesh>unit_box</mesh>";
+ model << " <material>Gazebo/Rocky</material>";
+ model << " </visual>";
+ model << " </geom:box>";
+
+ model << " <geom:box name='leg1'>";
+ model << " <xyz>-0.5 -0.5 -0.25</xyz>";
+ model << " <size>.02 .02 0.5</size>";
+ model << " <mass>1</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>0</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>0.01 0.01 0.5</size>";
+ model << " <mesh>unit_box</mesh>";
+ model << " <material>Gazebo/Rocky</material>";
+ model << " </visual>";
+ model << " </geom:box>";
+
+ model << " <geom:box name='leg2'>";
+ model << " <xyz>-0.5 0.5 -0.25</xyz>";
+ model << " <size>.02 .02 0.5</size>";
+ model << " <mass>1</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>0</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>0.01 0.01 0.5</size>";
+ model << " <mesh>unit_box</mesh>";
+ model << " <material>Gazebo/Rocky</material>";
+ model << " </visual>";
+ model << " </geom:box>";
+
+ model << " <geom:box name='leg3'>";
+ model << " <xyz>0.5 -0.5 -0.25</xyz>";
+ model << " <size>.02 .02 0.5</size>";
+ model << " <mass>1</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>0</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>0.01 0.01 0.5</size>";
+ model << " <mesh>unit_box</mesh>";
+ model << " <material>Gazebo/Rocky</material>";
+ model << " </visual>";
+ model << " </geom:box>";
+
+ model << " <geom:box name='leg4'>";
+ model << " <xyz>0.5 0.5 -0.25</xyz>";
+ model << " <size>.02 .02 0.5</size>";
+ model << " <mass>1</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>0</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>0.01 0.01 0.5</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();
+}
+
+void spawn_box(double x, double y, double z=1)
+{
+ std::ostringstream model;
+
+ model << "<model:physical name='box'>";
+ model << " <xyz>" << x << " " << y << " " << z << "</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <body:box name='body'>";
+ model << " <geom:box name='geom'>";
+ model << " <size>0.5 0.5 0.5</size>";
+ model << " <mass>1</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>0</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>0.5 0.5 0.5</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()
+{
+ client = new libgazebo::Client();
+ simIface = new libgazebo::SimulationIface();
+ factoryIface = new libgazebo::FactoryIface();
+
+ 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;
+ }
+
+ double prev_z = 5;
+
+ spawn_table();
+ usleep(100000);
+ spawn_box(0, 0, prev_z);
+ usleep(1000000);
+ simIface->StartLogEntity("box","/tmp/box.log");
+ simIface->StartLogEntity("table","/tmp/table.log");
+ simIface->Unpause();
+
+ libgazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
+ libgazebo::Pose modelPose;
+
+ double time = simIface->data->realTime;
+ while ( simIface->data->realTime - time < 2.0 )
+ {
+ simIface->GetState("box", modelPose, linearVel, angularVel, linearAccel,
angularAccel);
+
+ if ( fabs(prev_z - modelPose.pos.z) >= 1e-5)
+ time = simIface->data->realTime;
+
+ prev_z = modelPose.pos.z;
+ }
+ simIface->StopLogEntity("box");
+ simIface->StopLogEntity("table");
+
+ factoryIface->Close();
+ simIface->Close();
+ client->Disconnect();
+}
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
