Merge pull request #398 from gergondet/topic/ImproveReplayAccuracy

include/mc_rbdyn/ForceSensor.h

@@ -5,17 +5,13 @@
 #pragma once
 
 #include <mc_rbdyn/Device.h>
+#include <mc_rbdyn/ForceSensorCalibData.h>
 
 namespace mc_rbdyn
 {
 
 struct Robot;
 
-namespace detail
-{
-struct ForceSensorCalibData;
-}
-
 /** This struct is intended to hold static information about a force sensor
  * and the current reading of said sensor. If the appropriate data is
  * provided, a gravity-free reading can be provided.
@@ -114,6 +110,8 @@ struct MC_RBDYN_DLLAPI ForceSensor : public Device
    * @{
    */
 
+  void loadCalibrator(const detail::ForceSensorCalibData & data) noexcept;
+
   /** Load data from a file, using a gravity vector. The file should
    * contain 13 parameters in that order: mass (1), rpy for X_f_ds (3),
    * position for X_p_vb (3), wrench offset (6).
@@ -153,6 +151,9 @@ struct MC_RBDYN_DLLAPI ForceSensor : public Device
   /** Return the sensor offset */
   const sva::ForceVecd & offset() const;
 
+  /** Access the calibration object */
+  inline const detail::ForceSensorCalibData & calib() const noexcept { return calibration_; }
+
   /** @} */
   /* End of Calibration group */
 
@@ -161,7 +162,7 @@ struct MC_RBDYN_DLLAPI ForceSensor : public Device
 private:
   sva::ForceVecd wrench_;
 
-  std::shared_ptr<detail::ForceSensorCalibData> calibration_;
+  detail::ForceSensorCalibData calibration_;
 };
 
 inline bool operator==(const mc_rbdyn::ForceSensor & lhs, const mc_rbdyn::ForceSensor & rhs)

include/mc_rbdyn/ForceSensorCalibData.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <mc_rtc/Schema.h>
+
+namespace mc_rbdyn::detail
+{
+
+struct ForceSensorCalibData
+{
+  MC_RTC_NEW_SCHEMA(ForceSensorCalibData)
+#define MEMBER(...) MC_RTC_PP_ID(MC_RTC_SCHEMA_REQUIRED_DEFAULT_MEMBER(ForceSensorCalibData, __VA_ARGS__))
+  MEMBER(double, mass, "Mass of the link generating the wrench")
+  MEMBER(sva::ForceVecd, worldForce, "Constant gravity wrench applied on the force sensor in the world frame")
+  MEMBER(sva::PTransformd, X_f_ds, "Local rotation between the model force sensor and the real one")
+  MEMBER(sva::PTransformd, X_p_vb, "Transform from the parent body to the virtual link CoM")
+  MEMBER(sva::ForceVecd, offset, "Force/torque offset")
+#undef MEMBER
+
+  /** Restore the calibrator default values such that it always returns zero
+   * contribution
+   */
+  void reset();
+
+  /** Load data from a file, using a gravity vector. The file
+   * should contain 13 parameters in that order: mass (1),
+   * rpy for X_f_ds (3), position for X_p_vb (3), wrench
+   * offset (6).
+   *
+   * If the file does not exist, default calibration parameters that do nothing
+   * will be used. If the file exists but its parameters are invalid, an exception will be thrown.
+   *
+   * @throws std::invalid_argument if the file is ill-formed.
+   **/
+  void loadData(const std::string & filename, const Eigen::Vector3d & gravity);
+
+  /** Return the gravity wrench applied on the force sensor in the sensor
+   * frame, i.e. the external force $f_{ext}$ is:
+   * $f_{ext} = f_{mes} - wfToSensor$.
+   * @param X_0_p the transform in the world frame of the parent body of the sensor
+   * @param X_p_f the transform from the parent body to the sensor itself
+   */
+  sva::ForceVecd wfToSensor(const sva::PTransformd & X_0_p, const sva::PTransformd & X_p_f) const noexcept;
+};
+
+} // namespace mc_rbdyn::detail

include/mc_rtc/SchemaMacros.h

@@ -1,4 +1,4 @@
-#define MC_RTC_PP_ID(X) X
+#include <mc_rtc/macros/pp_id.h>
 
 #define MC_RTC_SCHEMA(SchemaT, BaseT)                                                                        \
   /** Tag to detect Schema objects */                                                                        \

include/mc_rtc/log/Logger.h

@@ -109,6 +109,10 @@ struct MC_RTC_UTILS_DLLAPI Logger
     std::vector<std::string> main_robot_module;
     /** Initial position of robots in the world */
     std::map<std::string, sva::PTransformd> init;
+    /** Initial configuration of robots in the world */
+    std::map<std::string, std::vector<std::vector<double>>> init_q;
+    /** Calibration used for the force sensors */
+    std::map<std::string, std::map<std::string, mc_rtc::Configuration>> calibs;
   };
 
 public:

include/mc_rtc/macros/pp_id.h

@@ -0,0 +1,8 @@
+/*
+ * Copyright 2015-2023 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+/** Preprocessor identity, used to force expansion (mainly to work around MSVC "feature") */
+#define MC_RTC_PP_ID(x) x

include/mc_rtc/pragma.h

@@ -5,9 +5,7 @@
 #pragma once
 
 #include <mc_rtc/macros/deprecated.h>
-
-/** Preprocessor identity, used to force expansion (mainly to work around MSVC "feature" */
-#define MC_RTC_PP_ID(x) x
+#include <mc_rtc/macros/pp_id.h>
 
 /** Transform a expression to string*/
 #define MC_RTC_STRINGIFY_(x) #x

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;

src/mc_control/Ticker.cpp

@@ -77,42 +77,57 @@ std::optional<sva::PTransformd> get_posW(const mc_rtc::log::FlatLog & log,
                                          bool is_main,
                                          size_t idx)
 {
-  auto entry = log_entry("ff", robot, is_main);
   if(log.meta())
   {
     auto it = log.meta()->init.find(robot);
     if(it != log.meta()->init.end()) { return it->second; }
   }
+  auto entry = log_entry("ff", robot, is_main);
   if(!log.has(entry)) { return std::nullopt; }
   return log.get(entry, idx, sva::PTransformd::Identity());
 }
 
+std::optional<std::vector<double>> get_initial_encoders(const mc_rtc::log::FlatLog & log,
+                                                        const mc_rbdyn::Robot & robot,
+                                                        bool is_main)
+{
+  if(log.meta())
+  {
+    auto it = log.meta()->init_q.find(robot.name());
+    if(it != log.meta()->init_q.end())
+    {
+      const auto & q = it->second;
+      std::vector<double> out;
+      for(size_t i = 0; i < robot.refJointOrder().size(); ++i)
+      {
+        auto mbcIdx = robot.jointIndexInMBC(i);
+        if(mbcIdx >= 0 && !q[static_cast<size_t>(mbcIdx)].empty()) { out.push_back(q[static_cast<size_t>(mbcIdx)][0]); }
+        else { out.push_back(0.0); }
+      }
+      return out;
+    }
+  }
+  if(log.has(log_entry("qIn", robot.name(), is_main))) { return get_encoders(log, robot.name(), is_main, 0); }
+  return std::nullopt;
+}
+
 /** Get the initial state of all robots from the given log */
-auto get_initial_state(const mc_rtc::log::FlatLog & log,
-                       const std::vector<std::string> & robots,
-                       const std::string & main)
+auto get_initial_state(const mc_rtc::log::FlatLog & log, const mc_rbdyn::Robots & robots, const std::string & main)
 {
   std::pair<std::map<std::string, std::vector<double>>, std::map<std::string, sva::PTransformd>> out;
   auto & encoders = out.first;
   auto & bases = out.second;
   for(const auto & r : robots)
   {
-    bool is_main = r == main;
-    if(log.has(log_entry("qIn", r, is_main))) { encoders[r] = get_encoders(log, r, is_main, 0); }
-    auto posW = get_posW(log, r, is_main, 0);
-    if(posW) { bases[r] = *posW; }
+    bool is_main = r.name() == main;
+    auto r_encoders = get_initial_encoders(log, r, is_main);
+    if(r_encoders) { encoders[r.name()] = *r_encoders; }
+    auto posW = get_posW(log, r.name(), is_main, 0);
+    if(posW) { bases[r.name()] = *posW; }
   }
   return out;
 }
 
-/** Get the names of the robot currently in the controller */
-std::vector<std::string> get_robots(const mc_control::MCGlobalController & gc)
-{
-  std::vector<std::string> out;
-  for(const auto & r : gc.controller().robots()) { out.push_back(r.name()); }
-  return out;
-}
-
 auto get_gc_configuration = [](const Ticker::Configuration & config)
 {
   mc_control::MCGlobalController::GlobalConfiguration out(config.mc_rtc_configuration);
@@ -165,7 +180,7 @@ Ticker::Ticker(const Configuration & config) : config_(config), gc_(get_gc_confi
       }
     }
     // Do the initialization
-    auto [encoders, attitudes] = get_initial_state(*log_, get_robots(gc_), gc_.controller().robot().name());
+    auto [encoders, attitudes] = get_initial_state(*log_, gc_.robots(), gc_.controller().robot().name());
     gc_.init(encoders, attitudes);
   }
   else { gc_.init(); }
@@ -189,7 +204,7 @@ bool Ticker::step()
     simulate_sensors();
     if(log_)
     {
-      auto [encoders, attitudes] = get_initial_state(*log_, get_robots(gc_), gc_.controller().robot().name());
+      auto [encoders, attitudes] = get_initial_state(*log_, gc_.robots(), gc_.controller().robot().name());
       gc_.reset(encoders, attitudes);
     }
     else { gc_.reset(); }

src/mc_control/mc_global_controller.cpp

@@ -1047,7 +1047,14 @@ void MCGlobalController::setup_log()
   meta.main_robot = controller_->robot().name();
   meta.main_robot_module = controller_->robot().module().parameters();
   meta.init.clear();
-  for(const auto & r : controller_->robots()) { meta.init[r.name()] = r.posW(); }
+  meta.init_q.clear();
+  meta.calibs.clear();
+  for(const auto & r : controller_->robots())
+  {
+    meta.init[r.name()] = r.posW();
+    meta.init_q[r.name()] = r.mbc().q;
+    for(const auto & fs : r.forceSensors()) { meta.calibs[r.name()][fs.name()] = fs.calib().toConfiguration(); }
+  }
   if(setup_logger_.count(current_ctrl)) { return; }
   // Copy controller pointer to avoid lambda issue
   MCController * controller = controller_;