diff --git a/CMakeLists.txt b/CMakeLists.txt
index c2538cb479c738ed6becd5a484ed21ecd3c88e2f..061667e8a781d098083ee433deab18afa99fe810 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
# 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)
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index d2befdab4772515c5cea13acab834fe72e2a5a5c..f8f2a522c9a8dc174264fd1d10a8ce68b8632c8e 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/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
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index 87d38ddc109b16e1b520eb8c29a6d009a1a769e5..24e4e3dd1bc7e99a308e37fc57678a43551cb7c1 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/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)
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 2294c819aec95c1f2c6bed5c0e7b1d6cc857c614..9f9ee1dde0f2ca8afa670c1182945a933cf9cdc0 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,73 +1,15 @@
-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()
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index 63ad99f46f925db41b06038e8177a87c36e63885..6f59bb1da4c88707530a94cbcc2132ffabc1d45c 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/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++;
}
}