Revision: 8713
http://playerstage.svn.sourceforge.net/playerstage/?rev=8713&view=rev
Author: natepak
Date: 2010-05-25 22:16:16 +0000 (Tue, 25 May 2010)
Log Message:
-----------
Fixed double precision in ode
Modified Paths:
--------------
code/gazebo/branches/simpar/benchmarks/CMakeLists.txt
code/gazebo/branches/simpar/cmake/FindOde.cmake
code/gazebo/branches/simpar/server/physics/ode/CMakeLists.txt
code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
Added Paths:
-----------
code/gazebo/branches/simpar/benchmarks/box_drop_benchmark.cc
Modified: code/gazebo/branches/simpar/benchmarks/CMakeLists.txt
===================================================================
--- code/gazebo/branches/simpar/benchmarks/CMakeLists.txt 2010-05-25
17:31:59 UTC (rev 8712)
+++ code/gazebo/branches/simpar/benchmarks/CMakeLists.txt 2010-05-25
22:16:16 UTC (rev 8713)
@@ -28,6 +28,7 @@
camera_benchmark.cc
spinningbox_benchmark.cc
pendulum_benchmark.cc
+ box_drop_benchmark.cc
)
set_source_files_properties(${sources} PROPERTIES COMPILE_FLAGS "-ggdb -g2
-Wall")
@@ -39,6 +40,7 @@
add_executable(camera_benchmark camera_benchmark.cc)
add_executable(spinningbox_benchmark spinningbox_benchmark.cc)
add_executable(pendulum_benchmark pendulum_benchmark.cc)
+add_executable(box_drop_benchmark box_drop_benchmark.cc)
target_link_libraries(laser_benchmark gazebo ${LINK_LIBS} )
target_link_libraries(box_grid_benchmark gazebo ${LINK_LIBS} )
@@ -47,6 +49,7 @@
target_link_libraries(camera_benchmark gazebo ${LINK_LIBS} )
target_link_libraries(spinningbox_benchmark gazebo ${LINK_LIBS} )
target_link_libraries(pendulum_benchmark gazebo ${LINK_LIBS} )
+target_link_libraries(box_drop_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)
@@ -55,6 +58,7 @@
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(box_drop_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}")
@@ -63,5 +67,6 @@
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}")
+set_target_properties(box_drop_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)
+install (TARGETS laser_benchmark box_grid_benchmark box_pile_benchmark
joint_chain_benchmark camera_benchmark spinningbox_benchmark pendulum_benchmark
box_drop_benchmark DESTINATION ${CMAKE_INSTALL_PREFIX}/bin)
Added: code/gazebo/branches/simpar/benchmarks/box_drop_benchmark.cc
===================================================================
--- code/gazebo/branches/simpar/benchmarks/box_drop_benchmark.cc
(rev 0)
+++ code/gazebo/branches/simpar/benchmarks/box_drop_benchmark.cc
2010-05-25 22:16:16 UTC (rev 8713)
@@ -0,0 +1,119 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <libgazebo/gz.h>
+
+gazebo::Client *client = NULL;
+gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
+int laser_count = 0;
+
+std::string test_name="Box Pile 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 << " <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 gazebo::Client();
+ simIface = new gazebo::SimulationIface();
+ factoryIface = new gazebo::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;
+ }
+
+ FILE *out = fopen(data_filename.c_str(), "w");
+
+ spawn_box(0, 0);
+
+ gazebo::Vec3 linearVel, angularVel, linearAccel, angularAccel;
+ gazebo::Pose modelPose;
+
+ for (unsigned int j=0; j < 400; j++)
+ {
+ 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);
+ }
+
+ fclose(out);
+
+ //make_plot();
+
+ factoryIface->Close();
+ simIface->Close();
+ client->Disconnect();
+}
Modified: code/gazebo/branches/simpar/cmake/FindOde.cmake
===================================================================
--- code/gazebo/branches/simpar/cmake/FindOde.cmake 2010-05-25 17:31:59 UTC
(rev 8712)
+++ code/gazebo/branches/simpar/cmake/FindOde.cmake 2010-05-25 22:16:16 UTC
(rev 8713)
@@ -9,43 +9,43 @@
ENDIF (PKG_CONFIG_FOUND)
# if we didnt find it in pkg-config try ode-config
-IF (NOT ODE_FOUND)
- SET(ODE_CONFIG_PATH "ode-config")
- EXEC_PROGRAM(${ODE_CONFIG_PATH} ARGS --version RETURN_VALUE
ODE_CONFIG_RETURN OUTPUT_VARIABLE DETECTED_ODE_VERSION )
- STRING(REGEX REPLACE "[\r\n]" " " ${DETECTED_ODE_VERSION}
"${${DETECTED_ODE_VERSION}}")
+#IF (NOT ODE_FOUND)
+# SET(ODE_CONFIG_PATH "ode-config")
+# EXEC_PROGRAM(${ODE_CONFIG_PATH} ARGS --version RETURN_VALUE
ODE_CONFIG_RETURN OUTPUT_VARIABLE DETECTED_ODE_VERSION )
+# STRING(REGEX REPLACE "[\r\n]" " " ${DETECTED_ODE_VERSION}
"${${DETECTED_ODE_VERSION}}")
+#
+# # ode-config exists then get the details
+# IF(NOT ODE_CONFIG_RETURN)
+# IF(NOT ${REQUIRED_ODE_VERSION} VERSION_LESS ${DETECTED_ODE_VERSION} )
+# MESSAGE(STATUS " ode-config reports version ${DETECTED_ODE_VERSION}")
+# EXEC_PROGRAM(${ODE_CONFIG_PATH} ARGS --cflags RETURN_VALUE
ODE_CONFIG_RETURN OUTPUT_VARIABLE ODE_INCLUDE_DIRS )
+# STRING(REGEX REPLACE "[\r\n]" " " ${ODE_INCLUDE_DIRS}
"${${ODE_INCLUDE_DIRS}}")
+#
+# # we want to extract -DdSINGLE or -DdDOUBLE to compile with the correct
flag
+# STRING(REGEX REPLACE ".*-Dd([^ ]+).*" "-Dd\\1" ODE_CFLAGS_OTHER
"${ODE_INCLUDE_DIRS}")
+# MESSAGE(STATUS "${ODE_CFLAGS_OTHER} used to compile against the ODE
library")
+#
+# EXEC_PROGRAM(${ODE_CONFIG_PATH} ARGS --libs RETURN_VALUE
ODE_CONFIG_RETURN OUTPUT_VARIABLE ODE_LDFLAGS )
+# STRING(REGEX REPLACE "[\r\n]" " " ${ODE_LDFLAGS} "${${ODE_LDFLAGS}}")
+#
+# SET(ODE_FOUND ${DETECTED_ODE_VERSION})
+# ELSE(NOT ${REQUIRED_ODE_VERSION} VERSION_LESS ${DETECTED_ODE_VERSION} )
+# MESSAGE(STATUS "ode-config reports wrong version
(${DETECTED_ODE_VERSION} < ${REQUIRED_ODE_VERSION})")
+# ENDIF(NOT ${REQUIRED_ODE_VERSION} VERSION_LESS ${DETECTED_ODE_VERSION} )
+# ELSE(NOT ODE_CONFIG_RETURN)
+# MESSAGE(STATUS "no ode-config found")
+# ENDIF(NOT ODE_CONFIG_RETURN)
+#ENDIF (NOT ODE_FOUND)
- # ode-config exists then get the details
- IF(NOT ODE_CONFIG_RETURN)
- IF(NOT ${REQUIRED_ODE_VERSION} VERSION_LESS ${DETECTED_ODE_VERSION} )
- MESSAGE(STATUS " ode-config reports version ${DETECTED_ODE_VERSION}")
- EXEC_PROGRAM(${ODE_CONFIG_PATH} ARGS --cflags RETURN_VALUE
ODE_CONFIG_RETURN OUTPUT_VARIABLE ODE_INCLUDE_DIRS )
- STRING(REGEX REPLACE "[\r\n]" " " ${ODE_INCLUDE_DIRS}
"${${ODE_INCLUDE_DIRS}}")
-
- # we want to extract -DdSINGLE or -DdDOUBLE to compile with the correct
flag
- STRING(REGEX REPLACE ".*-Dd([^ ]+).*" "-Dd\\1" ODE_CFLAGS_OTHER
"${ODE_INCLUDE_DIRS}")
- MESSAGE(STATUS "${ODE_CFLAGS_OTHER} used to compile against the ODE
library")
-
- EXEC_PROGRAM(${ODE_CONFIG_PATH} ARGS --libs RETURN_VALUE
ODE_CONFIG_RETURN OUTPUT_VARIABLE ODE_LDFLAGS )
- STRING(REGEX REPLACE "[\r\n]" " " ${ODE_LDFLAGS} "${${ODE_LDFLAGS}}")
-
- SET(ODE_FOUND ${DETECTED_ODE_VERSION})
- ELSE(NOT ${REQUIRED_ODE_VERSION} VERSION_LESS ${DETECTED_ODE_VERSION} )
- MESSAGE(STATUS "ode-config reports wrong version
(${DETECTED_ODE_VERSION} < ${REQUIRED_ODE_VERSION})")
- ENDIF(NOT ${REQUIRED_ODE_VERSION} VERSION_LESS ${DETECTED_ODE_VERSION} )
- ELSE(NOT ODE_CONFIG_RETURN)
- MESSAGE(STATUS "no ode-config found")
- ENDIF(NOT ODE_CONFIG_RETURN)
-ENDIF (NOT ODE_FOUND)
-
IF (NOT ODE_FOUND)
BUILD_ERROR ("ODE and development files not found. See the following
website: http://www.ode.org";)
SET (INCLUDE_ODE FALSE CACHE BOOL "Include support for ODE")
ELSE (NOT ODE_FOUND)
SET (INCLUDE_ODE TRUE CACHE BOOL "Include support for ODE")
- APPEND_TO_CACHED_LIST(gazeboserver_cflags
- ${gazeboserver_cflags_desc}
- ${ODE_CFLAGS_OTHER})
+ #APPEND_TO_CACHED_LIST(gazeboserver_cflags
+ # ${gazeboserver_cflags_desc}
+ # ${ODE_CFLAGS_OTHER})
APPEND_TO_CACHED_LIST(gazeboserver_include_dirs
${gazeboserver_include_dirs_desc}
Modified: code/gazebo/branches/simpar/server/physics/ode/CMakeLists.txt
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/CMakeLists.txt
2010-05-25 17:31:59 UTC (rev 8712)
+++ code/gazebo/branches/simpar/server/physics/ode/CMakeLists.txt
2010-05-25 22:16:16 UTC (rev 8713)
@@ -39,6 +39,9 @@
${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE}})
endif (CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE})
-set_target_properties(gazebo_physics_ode PROPERTIES COMPILE_FLAGS "-fPIC")
+LIST_TO_STRING(MY_ODE_CFLAGS "${ODE_CFLAGS}")
+message (STATUS "ODE CFLAGS ${MY_ODE_CFLAGS}")
+
+set_target_properties(gazebo_physics_ode PROPERTIES COMPILE_FLAGS "-fPIC
${MY_ODE_CFLAGS}")
target_link_libraries( gazebo_physics_ode ${ODE_LIBRARIES})
install ( TARGETS gazebo_physics_ode DESTINATION ${CMAKE_INSTALL_PREFIX}/lib )
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-05-25
17:31:59 UTC (rev 8712)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-05-25
22:16:16 UTC (rev 8713)
@@ -196,6 +196,7 @@
// Set the rotation of the ODE body
dBodySetQuaternion(this->bodyId, q);
+
this->physicsEngine->UnlockMutex();
}
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
2010-05-25 17:31:59 UTC (rev 8712)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.hh
2010-05-25 22:16:16 UTC (rev 8713)
@@ -27,9 +27,6 @@
#ifndef ODEPHYSICS_HH
#define ODEPHYSICS_HH
-// Use single precision in ODE
-#define dSINGLE 1
-
#include <ode/ode.h>
#include "Param.hh"
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
