Ported force TestCoM

ADVRHumanoids · Feb 18, 2024 · 29fd3db · 29fd3db
1 parent b67ffdb
commit 29fd3db
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 41 deletions.
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -1,21 +1,6 @@
 include(ExternalProject)
 
-include(CheckCXXCompilerFlag)
-check_cxx_compiler_flag("-std=c++17" COMPILER_SUPPORTS_CXX17)
-check_cxx_compiler_flag("-std=c++14" COMPILER_SUPPORTS_CXX14)
-check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11)
-check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX17)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -Wmaybe-uninitialized -Wuninitialized")
-elseif(COMPILER_SUPPORTS_CXX14)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wmaybe-uninitialized -Wuninitialized")
-elseif(COMPILER_SUPPORTS_CXX11)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wmaybe-uninitialized -Wuninitialized")
-elseif(COMPILER_SUPPORTS_CXX0X)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -Wmaybe-uninitialized -Wuninitialized")
-else()
-    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 nor C++14 support. Please use a different C++ compiler.")
-endif()
+set(CMAKE_CXX_STANDARD 20)
 
 find_package(rviz_visual_tools QUIET)
 find_package(tf)
@@ -340,10 +325,10 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
 #     add_test(NAME OpenSoT_solvers_qpOases_AutoStack COMMAND testQPOases_AutoStack)
 # endif()
 
-# ADD_EXECUTABLE(testCoMForceTask tasks/force/TestCoM.cpp)
-# TARGET_LINK_LIBRARIES(testCoMForceTask ${TestLibs})
-# add_dependencies(testCoMForceTask  OpenSoT)
-# add_test(NAME OpenSoT_task_force_CoM COMMAND testCoMForceTask)
+ ADD_EXECUTABLE(testCoMForceTask tasks/force/TestCoM.cpp)
+ TARGET_LINK_LIBRARIES(testCoMForceTask ${TestLibs})
+ add_dependencies(testCoMForceTask  OpenSoT)
+ add_test(NAME OpenSoT_task_force_CoM COMMAND testCoMForceTask)
 
 # if(${qpSWIFT_FOUND})
 #     ADD_EXECUTABLE(testqpSWIFTSolver solvers/TestqpSWIFT.cpp)

diff --git a/tests/tasks/force/TestCoM.cpp b/tests/tasks/force/TestCoM.cpp
@@ -12,17 +12,16 @@
 #include <xbot2_interface/xbotinterface2.h>
 #include <OpenSoT/tasks/force/Force.h>
 #include <OpenSoT/utils/InverseDynamics.h>
+#include "../../common.h"
 
-std::string relative_path = OPENSOT_TEST_PATH "configs/coman/configs/config_coman_RBDL.yaml";
-std::string _path_to_cfg = relative_path;
 
 namespace{
 
-class testForceCoM : public ::testing::Test {
+class testForceCoM : public TestBase {
 
  protected:
 
-  testForceCoM()
+  testForceCoM() : TestBase("coman_floating_base")
   {
 
   }
@@ -39,11 +38,11 @@ class testForceCoM : public ::testing::Test {
 
 };
 
-class testWrench : public ::testing::Test {
+class testWrench : public TestBase {
 
  protected:
 
-  testWrench()
+  testWrench() : TestBase("coman_floating_base")
   {
 
   }
@@ -61,8 +60,7 @@ class testWrench : public ::testing::Test {
 };
 
 Eigen::VectorXd getGoodInitialPosition(XBot::ModelInterface::Ptr _model_ptr) {
-    Eigen::VectorXd _q(_model_ptr->getJointNum());
-    _q.setZero(_q.size());
+    Eigen::VectorXd _q = _model_ptr->getNeutralQ();
     _q[_model_ptr->getDofIndex("RHipSag")] = -25.0*M_PI/180.0;
     _q[_model_ptr->getDofIndex("RKneeSag")] = 50.0*M_PI/180.0;
     _q[_model_ptr->getDofIndex("RAnkSag")] = -25.0*M_PI/180.0;
@@ -82,14 +80,6 @@ Eigen::VectorXd getGoodInitialPosition(XBot::ModelInterface::Ptr _model_ptr) {
 
 
 TEST_F(testForceCoM, testForceCoM_StaticCase) {
-    XBot::ModelInterface::Ptr _model_ptr = XBot::ModelInterface::getModel(_path_to_cfg);
-
-    if(_model_ptr)
-        std::cout<<"pointer address: "<<_model_ptr.get()<<std::endl;
-    else
-        std::cout<<"pointer is NULL "<<_model_ptr.get()<<std::endl;
-
-
 
     Eigen::VectorXd q = getGoodInitialPosition(_model_ptr);
 
@@ -179,9 +169,6 @@ TEST_F(testForceCoM, testForceCoM_StaticCase) {
     wrench_reference(14) = 100.;
 
 
-
-
-
     OpenSoT::solvers::iHQP::Stack stack_of_tasks;
     stack_of_tasks.push_back(force_com_task);
 
@@ -195,8 +182,6 @@ TEST_F(testForceCoM, testForceCoM_StaticCase) {
     force_com_task->update(contact_wrenches_d);
 
 
-
-
     EXPECT_TRUE(sot->solve(contact_wrenches_d));
     std::cout<<"contact_wrenches_d = [ "<<contact_wrenches_d<<" ]"<<std::endl;