diff --git a/CMakeLists.txt b/CMakeLists.txt index c5cc2257d28836293362eff31a850cafaab12264..104b70538bca9b2393b93ef0c5fcefaf1c332a24 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,7 @@ SETUP_PROJECT() ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.2.0") ADD_REQUIRED_DEPENDENCY("eigenpy") ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0") +ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0") ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0") ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0") @@ -78,7 +79,7 @@ ADD_SUBDIRECTORY(include) ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(doc) ADD_SUBDIRECTORY(python) -#ADD_SUBDIRECTORY(unitTesting) +ADD_SUBDIRECTORY(unitTesting) SETUP_PROJECT_FINALIZE() SETUP_PROJECT_CPACK() diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt index df7d0d000d9aa48c528ad9e56e5458b7f5e941c7..864fee2896d5c44ba02455a01e71901cd8339b09 100644 --- a/unitTesting/CMakeLists.txt +++ b/unitTesting/CMakeLists.txt @@ -12,6 +12,7 @@ # General Lesser Public License for more details. You should have # received a copy of the GNU Lesser General Public License along with # sot-dynamic-pinocchio. If not, see <http://www.gnu.org/licenses/>. +INCLUDE(../cmake/python.cmake) ADD_DEFINITIONS(-DDEBUG=2) @@ -49,13 +50,17 @@ ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN) #SET(sampleinitconfig # ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleInitConfig.dat) +FINDPYTHON() + LIST(APPEND LOGGING_WATCHED_VARIABLES samplespec sampleljr) FOREACH(test ${tests}) SET(EXECUTABLE_NAME "${test}_exe") ADD_EXECUTABLE(${EXECUTABLE_NAME} ${test}.cpp) - + MESSAGE("PYTHON_INCLUDE_PATH: ${PYTHON_INCLUDE_PATH}") + INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH}) + TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} zmpreffromcom force-compensation @@ -71,7 +76,8 @@ FOREACH(test ${tests}) PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} pinocchio) PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} sot-core) PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph) - + PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph-python) + IF(${test}_plugins_dependencies) ADD_DEPENDENCIES(${EXECUTABLE_NAME} "${${test}_plugins_dependencies}") TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} "${${test}_plugins_dependencies}") diff --git a/unitTesting/test_constructor.cpp b/unitTesting/test_constructor.cpp index b821f42817a4ff4a7bda91bcefd137956341394c..ef62522da9fb14467f6775b0e5a812465aad0b43 100644 --- a/unitTesting/test_constructor.cpp +++ b/unitTesting/test_constructor.cpp @@ -29,25 +29,25 @@ using namespace dynamicgraph::sot; BOOST_AUTO_TEST_CASE (constructor) { /*-----------------------CONSTRUCTOR-----------------------------------------*/ - Dynamic dynamic_("sot_dynamic_test"); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointPositionSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::position"),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerPositionSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::ffposition"),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointVelocitySIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::velocity"),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerVelocitySIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::ffvelocity"),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointAccelerationSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::acceleration"),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerAccelerationSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::ffacceleration"),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.newtonEulerSINTERN.getName().c_str(),"sotDynamic(sot_dynamic_test)::intern(dummy)::newtoneuler" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.zmpSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::zmp" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.JcomSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(matrix)::Jcom" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.comSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::com" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.inertiaSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(matrix)::inertia" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.footHeightSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(double)::footHeight" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperJlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::upperJl" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.lowerJlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::lowerJl" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperVlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::upperVl" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperTlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::upperTl" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.inertiaRotorSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(matrix)::inertiaRotor" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.MomentaSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::momenta" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.AngularMomentumSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::angularmomentum" ),0); - BOOST_CHECK_EQUAL(std::strcmp(dynamic_.dynamicDriftSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::dynamicDrift" ),0); + DynamicPinocchio dynamic_("sot_dynamic_test"); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointPositionSIN.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::input(vector)::position"),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerPositionSIN.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::input(vector)::ffposition"),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointVelocitySIN.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::input(vector)::velocity"),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerVelocitySIN.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::input(vector)::ffvelocity"),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointAccelerationSIN.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::input(vector)::acceleration"),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerAccelerationSIN.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::input(vector)::ffacceleration"),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.newtonEulerSINTERN.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::intern(dummy)::newtoneuler" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.zmpSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::zmp" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.JcomSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(matrix)::Jcom" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.comSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::com" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.inertiaSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(matrix)::inertia" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.footHeightSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(double)::footHeight" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperJlSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::upperJl" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.lowerJlSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::lowerJl" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperVlSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::upperVl" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperTlSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::upperTl" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.inertiaRotorSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(matrix)::inertiaRotor" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.MomentaSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::momenta" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.AngularMomentumSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::angularmomentum" ),0); + BOOST_CHECK_EQUAL(std::strcmp(dynamic_.dynamicDriftSOUT.getName().c_str(),"sotDynamicPinocchio(sot_dynamic_test)::output(vector)::dynamicDrift" ),0); }