[mc_rtc] Load calibration from the replay plugin

This allows us to test the loading of the calibration files from the replay
jrl-umi3218 · Oct 6, 2023 · 9d413a7 · 9d413a7
1 parent c1fc6d6
commit 9d413a7
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 14 deletions.
diff --git a/plugins/Replay/src/Replay.cpp b/plugins/Replay/src/Replay.cpp
@@ -234,6 +234,21 @@ void Replay::reset(mc_control::MCGlobalController & gc)
             iters_ = std::max<size_t>(std::min<size_t>(iter, log_->size() - 1), 0);
           },
           0.0, static_cast<double>(log_->size()) * gc.timestep()));
+  // Use calibration from the replay
+  if(with_inputs_ && log_->meta())
+  {
+    const auto & calibs = log_->meta()->calibs;
+    for(const auto & [r, fs_calibs] : calibs)
+    {
+      if(!gc.robots().hasRobot(r)) { continue; }
+      auto & robot = gc.robots().robot(r);
+      for(const auto & [fs_name, calib] : fs_calibs)
+      {
+        auto & fs = const_cast<mc_rbdyn::ForceSensor &>(robot.forceSensor(fs_name));
+        fs.loadCalibrator(mc_rbdyn::detail::ForceSensorCalibData::fromConfiguration(calib));
+      }
+    }
+  }
   // Run once to fill the initial sensors
   before(gc);
   iters_ = 0;

diff --git a/src/mc_control/Ticker.cpp b/src/mc_control/Ticker.cpp
@@ -181,19 +181,6 @@ Ticker::Ticker(const Configuration & config) : config_(config), gc_(get_gc_confi
     }
     // Do the initialization
     auto [encoders, attitudes] = get_initial_state(*log_, gc_.robots(), gc_.controller().robot().name());
-    if(log_->meta())
-    {
-      const auto & calibs = log_->meta()->calibs;
-      for(const auto & [r, fs_calibs] : calibs)
-      {
-        auto & robot = gc_.robots().robot(r);
-        for(const auto & [fs_name, calib] : fs_calibs)
-        {
-          auto & fs = const_cast<mc_rbdyn::ForceSensor &>(robot.forceSensor(fs_name));
-          fs.loadCalibrator(mc_rbdyn::detail::ForceSensorCalibData::fromConfiguration(calib));
-        }
-      }
-    }
     gc_.init(encoders, attitudes);
   }
   else { gc_.init(); }

diff --git a/tests/controllers/replay/CMakeLists.txt b/tests/controllers/replay/CMakeLists.txt
@@ -28,6 +28,9 @@ set_target_properties(
              LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/TestReplayController
              RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/TestReplayController
 )
+target_include_directories(TestReplayController PRIVATE ${CMAKE_SOURCE_DIR}/tests)
+set(CONFIG_HEADER_INCLUDE_DIR "${CMAKE_BINARY_DIR}/tests/include/$<CONFIG>")
+target_include_directories(TestReplayController PRIVATE ${CONFIG_HEADER_INCLUDE_DIR})
 
 add_global_controller_test_run(
   TestReplayController_Record ${CMAKE_CURRENT_BINARY_DIR}/$<CONFIG>/mc_rtc.yaml 50

diff --git a/tests/controllers/replay/TestReplayController.cpp b/tests/controllers/replay/TestReplayController.cpp
@@ -12,11 +12,24 @@
 
 #include <boost/test/unit_test.hpp>
 
+#include <utils.h>
+
 namespace mc_control
 {
 
 static std::string REPLAY_PATH = "";
 
+static auto calib = []()
+{
+  mc_rbdyn::detail::ForceSensorCalibData calib;
+  calib.mass = 42.42;
+  calib.worldForce = random_fv();
+  calib.X_f_ds = random_pt();
+  calib.X_p_vb = random_pt();
+  calib.offset = random_fv();
+  return calib;
+}();
+
 template<bool Play>
 struct MC_CONTROL_DLLAPI TestReplayController : public MCController
 {
@@ -30,12 +43,17 @@ struct MC_CONTROL_DLLAPI TestReplayController : public MCController
     solver().addConstraintSet(compoundJointConstraint);
     postureTask->stiffness(200.0);
     qpsolver->setContacts({});
+    if constexpr(!Play)
+    {
+      const_cast<mc_rbdyn::ForceSensor &>(robot().forceSensor("LeftFootForceSensor")).loadCalibrator(calib);
+    }
   }
 
   bool run() override
   {
     if constexpr(Play)
     {
+      BOOST_REQUIRE(robot().forceSensor("LeftFootForceSensor").calib() == calib);
       BOOST_REQUIRE(datastore().has("Replay::Log"));
       BOOST_REQUIRE(!datastore().has("NOT_IN_DATASTORE"));
     }

diff --git a/tests/controllers/replay/TestReplayController_Play.in.yaml b/tests/controllers/replay/TestReplayController_Play.in.yaml
@@ -1,6 +1,6 @@
 Plugins: [Replay]
 Replay:
-  with-inputs: false
+  with-inputs: true
   with-gui-inputs: false
   with-outputs: false
   with-datastore-config: "@CMAKE_CURRENT_SOURCE_DIR@/datastore-replay.yaml"