Merge branch 'devel' into 'main'

Devel

See merge request !33

CMakeLists.txt

 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.16)
 
 # MAC OSX RPATH
 SET(CMAKE_MACOSX_RPATH 1)

include/bodydynamics/sensor/sensor_force_torque_inertial.h

@@ -43,6 +43,7 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
     double   gyro_drift_std;
     double   force_noise_std;
     double   torque_noise_std;
+    Vector3d acc_bias, gyro_bias;
     Vector3d com;
     Vector3d inertia;
     double   mass;
@@ -58,15 +59,27 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
         gyrob_initial_std = _server.getParam<double>(prefix + _unique_name + "/gyrob_initial_std");
         acc_drift_std     = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
         gyro_drift_std    = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
+        
         force_noise_std   = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
         torque_noise_std  = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
-        com               = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
-        inertia           = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
-        mass              = _server.getParam<double>(prefix + _unique_name + "/mass");
-        imu_bias_fix      = _server.getParam<bool>  (prefix + _unique_name + "/imu_bias_fix");
-        com_fix           = _server.getParam<bool>  (prefix + _unique_name + "/com_fix");
-        inertia_fix       = _server.getParam<bool>  (prefix + _unique_name + "/inertia_fix");
-        mass_fix          = _server.getParam<bool>  (prefix + _unique_name + "/mass_fix");
+        
+        if (_server.hasParam(prefix + _unique_name + "/acc_bias"))
+            acc_bias = _server.getParam<Vector3d>(prefix + _unique_name + "/acc_bias");
+        else
+            acc_bias.setZero();
+        if (_server.hasParam(prefix + _unique_name + "/gyro_bias"))
+            gyro_bias = _server.getParam<Vector3d>(prefix + _unique_name + "/gyro_bias");
+        else
+            gyro_bias.setZero();
+        
+        com          = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
+        inertia      = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
+        mass         = _server.getParam<double>(prefix + _unique_name + "/mass");
+        
+        imu_bias_fix = _server.getParam<bool>(prefix + _unique_name + "/imu_bias_fix");
+        com_fix      = _server.getParam<bool>(prefix + _unique_name + "/com_fix");
+        inertia_fix  = _server.getParam<bool>(prefix + _unique_name + "/inertia_fix");
+        mass_fix     = _server.getParam<bool>(prefix + _unique_name + "/mass_fix");
     }
     ~ParamsSensorForceTorqueInertial() override = default;
     std::string print() const override

src/sensor/sensor_force_torque_inertial.cpp

@@ -41,6 +41,10 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
                  false),
       params_fti_(_params)
 {
+    // set IMU bias here (it's complicated to do in the constructor above)
+    Vector6d imu_bias; imu_bias << params_fti_->acc_bias, params_fti_->gyro_bias;
+    getStateBlock('I')->setState(imu_bias);
+
     addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, params_fti_->com_fix));      // centre of mass
     addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, params_fti_->inertia_fix));  // inertial vector
     addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), params_fti_->mass_fix));  // total mass
@@ -58,6 +62,7 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
     setNoiseStd(noise_std_vector);
 }
 
+
 // TODO: Adapt this API to that of MR !448
 SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&             _states,
                                                      ParamsSensorForceTorqueInertialPtr _params)

test/gtest/CMakeLists.txt

-if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-  message("CMake version less than 3.11.0")
+include(FetchContent)
 
-  # Enable ExternalProject CMake module
-  include(ExternalProject)
+FetchContent_Declare(
+  googletest
+  GIT_REPOSITORY https://github.com/google/googletest.git 
+  GIT_TAG main)
 
-  set(GTEST_FORCE_SHARED_CRT ON)
-  set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+FetchContent_MakeAvailable(googletest)
 
-  # Download GoogleTest
-  ExternalProject_Add(googletest
-      GIT_REPOSITORY https://github.com/google/googletest.git
-      GIT_TAG        v1.8.x
-      # TIMEOUT 1 # We'll try this
-      CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
-      -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
-      -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
-      -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
-      -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
-      -DBUILD_GTEST=ON
-      PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
-      # Disable install step
-      INSTALL_COMMAND ""
-      UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-  )
-
-  # Get GTest source and binary directories from CMake project
-
-  # Specify include dir
-  ExternalProject_Get_Property(googletest source_dir)
-  set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-
-  # Specify MainTest's link libraries
-  ExternalProject_Get_Property(googletest binary_dir)
-  set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-
-  # Create a libgtest target to be used as a dependency by test programs
-  add_library(libgtest IMPORTED STATIC GLOBAL)
-  add_dependencies(libgtest googletest)
-
-  # Set libgtest properties
-  set_target_properties(libgtest PROPERTIES
-      "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
-      "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-  )
-
-else()
-
-  message("CMake version equal or greater than 3.11.0")
-
-  include(FetchContent)
-
-  FetchContent_Declare(
-    googletest
-    GIT_REPOSITORY https://github.com/google/googletest.git 
-    GIT_TAG main)
-
-  SET(INSTALL_GTEST OFF) # Disable installation of googletest
-  FetchContent_MakeAvailable(googletest)
-    
-endif()
-  
 function(wolf_add_gtest target)
   add_executable(${target} ${ARGN})
-  if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-    add_dependencies(${target} libgtest)
-    target_link_libraries(${target} PUBLIC libgtest ${PLUGIN_NAME})
-    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
-  else()
-    target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
-  endif()
+  target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
   add_test(NAME ${target} COMMAND ${target})
 endfunction()

test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp

@@ -91,7 +91,6 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
         char   delimiter = ',';
         std::getline(data_simulation, line_data);
 
-        int counter = 0;
 
         while (std::getline(data_simulation, line_data))
         {
@@ -199,8 +198,6 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
             a_meas_i = force_i/mass_true;
 
             a_meas.push_back(a_meas_i);
-            
-            counter++;
         }
     }