diff --git a/include/mc_rbdyn/ForceSensor.h b/include/mc_rbdyn/ForceSensor.h index 6cb0760b2b..be31096828 100644 --- a/include/mc_rbdyn/ForceSensor.h +++ b/include/mc_rbdyn/ForceSensor.h @@ -5,17 +5,13 @@ #pragma once #include +#include 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 calibration_; + detail::ForceSensorCalibData calibration_; }; inline bool operator==(const mc_rbdyn::ForceSensor & lhs, const mc_rbdyn::ForceSensor & rhs) diff --git a/include/mc_rbdyn/ForceSensorCalibData.h b/include/mc_rbdyn/ForceSensorCalibData.h new file mode 100644 index 0000000000..cc586fae3a --- /dev/null +++ b/include/mc_rbdyn/ForceSensorCalibData.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +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 diff --git a/include/mc_rtc/SchemaMacros.h b/include/mc_rtc/SchemaMacros.h index 90e79ebe1b..eb649fe633 100644 --- a/include/mc_rtc/SchemaMacros.h +++ b/include/mc_rtc/SchemaMacros.h @@ -1,4 +1,4 @@ -#define MC_RTC_PP_ID(X) X +#include #define MC_RTC_SCHEMA(SchemaT, BaseT) \ /** Tag to detect Schema objects */ \ diff --git a/include/mc_rtc/log/Logger.h b/include/mc_rtc/log/Logger.h index 98ec0b5839..8232b95e0c 100644 --- a/include/mc_rtc/log/Logger.h +++ b/include/mc_rtc/log/Logger.h @@ -109,6 +109,10 @@ struct MC_RTC_UTILS_DLLAPI Logger std::vector main_robot_module; /** Initial position of robots in the world */ std::map init; + /** Initial configuration of robots in the world */ + std::map>> init_q; + /** Calibration used for the force sensors */ + std::map> calibs; }; public: diff --git a/include/mc_rtc/macros/pp_id.h b/include/mc_rtc/macros/pp_id.h new file mode 100644 index 0000000000..d513ae08ff --- /dev/null +++ b/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 diff --git a/include/mc_rtc/pragma.h b/include/mc_rtc/pragma.h index 45c986b81b..5f09450bba 100644 --- a/include/mc_rtc/pragma.h +++ b/include/mc_rtc/pragma.h @@ -5,9 +5,7 @@ #pragma once #include - -/** Preprocessor identity, used to force expansion (mainly to work around MSVC "feature" */ -#define MC_RTC_PP_ID(x) x +#include /** Transform a expression to string*/ #define MC_RTC_STRINGIFY_(x) #x diff --git a/plugins/Replay/src/Replay.cpp b/plugins/Replay/src/Replay.cpp index a1a9aab02d..cfa84e8a38 100644 --- 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(std::min(iter, log_->size() - 1), 0); }, 0.0, static_cast(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(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 index d2c4a63f5c..888dacc46f 100644 --- a/src/mc_control/Ticker.cpp +++ b/src/mc_control/Ticker.cpp @@ -77,42 +77,57 @@ std::optional 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> 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 out; + for(size_t i = 0; i < robot.refJointOrder().size(); ++i) + { + auto mbcIdx = robot.jointIndexInMBC(i); + if(mbcIdx >= 0 && !q[static_cast(mbcIdx)].empty()) { out.push_back(q[static_cast(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 & 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> 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 get_robots(const mc_control::MCGlobalController & gc) -{ - std::vector 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(); } diff --git a/src/mc_control/mc_global_controller.cpp b/src/mc_control/mc_global_controller.cpp index 264a79afb3..e6e2c37ea0 100644 --- a/src/mc_control/mc_global_controller.cpp +++ b/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_; diff --git a/src/mc_rbdyn/ForceSensor.cpp b/src/mc_rbdyn/ForceSensor.cpp index f9759592de..f4fa9ce655 100644 --- a/src/mc_rbdyn/ForceSensor.cpp +++ b/src/mc_rbdyn/ForceSensor.cpp @@ -16,126 +16,79 @@ namespace mc_rbdyn namespace detail { -/** Structure used to hold the calibration data. This structure is *NOT* - * intended to be constructed directly, see @ForceSensorsCalibrator for usage. - * It is composed of 13 parameters (real numbers) that should be loaded - * from a calibration file. **/ -struct ForceSensorCalibData -{ -public: - /** Default constructor, always returns zero contribution */ - ForceSensorCalibData() {} - - /** Restore the calibrator default values such that it always returns zero - * contribution - */ - void reset() + +static std::string CalibDataRequirements = + R"(The file should contain 13 parameters in that order: + - mass (1) + - rpy of local rotation between the model force sensor and real one (3) + - translation from the parent body to the virtual link CoM (3) + - wrench offset (6).)"; + +void ForceSensorCalibData::reset() +{ + mass = 0.0; + worldForce = sva::ForceVecd(Eigen::Vector6d::Zero()); + X_f_ds = sva::PTransformd::Identity(); + X_p_vb = sva::PTransformd::Identity(); + offset = sva::ForceVecd(Eigen::Vector6d::Zero()); +} + +void ForceSensorCalibData::loadData(const std::string & filename, const Eigen::Vector3d & gravity) +{ + constexpr int nr_params = 13; + std::ifstream strm(filename); + if(!strm.is_open()) { - mass_ = 0.0; - worldForce_ = sva::ForceVecd(Eigen::Vector6d::Zero()); - X_f_ds_ = sva::PTransformd::Identity(); - X_p_vb_ = sva::PTransformd::Identity(); - offset_ = sva::ForceVecd(Eigen::Vector6d::Zero()); + mc_rtc::log::error("Could not open {}", filename); + return; } - /** 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) + // Vector 13d + Eigen::Matrix X; + double temp; + for(int i = 0; i < nr_params; ++i) { - constexpr int nr_params = 13; - std::ifstream strm(filename); - if(!strm.is_open()) + strm >> temp; + if(!strm.good()) { - mc_rtc::log::error("Could not open {}", filename); - return; + mc_rtc::log::error_and_throw("[ForceSensorCalibData] Invalid calibration file {}. {}", + filename, CalibDataRequirements); } - - // Vector 13d - Eigen::Matrix X; - double temp; - for(int i = 0; i < nr_params; ++i) + if(strm.eof()) { - strm >> temp; - if(!strm.good()) - { - mc_rtc::log::error_and_throw("[ForceSensorCalibData] Invalid calibration file {}. {}", - filename, dataRequirements()); - } - if(strm.eof()) - { - mc_rtc::log::error_and_throw( - "[ForceSensorCalibData] Calibration file {} is too short. {}", filename, dataRequirements()); - } - X(i) = temp; + mc_rtc::log::error_and_throw("[ForceSensorCalibData] Calibration file {} is too short. {}", + filename, CalibDataRequirements); } - - mass_ = X(0); - worldForce_ = sva::ForceVecd(Eigen::Vector3d(0., 0., 0.), -X(0) * gravity); - X_f_ds_ = mc_rbdyn::rpyToPT(X.segment<3>(1)); - X_p_vb_ = sva::PTransformd(Eigen::Matrix3d::Identity(), X.segment<3>(4)); - offset_ = sva::ForceVecd(X.segment<6>(7)); + X(i) = temp; } - static std::string dataRequirements() - { - return R"(The file should contain 13 parameters in that order: - - mass (1) - - rpy of local rotation between the model force sensor and real one (3) - - translation from the parent body to the virtual link CoM (3) - - wrench offset (6).)"; - } + mass = X(0); + worldForce = sva::ForceVecd(Eigen::Vector3d(0., 0., 0.), -X(0) * gravity); + X_f_ds = mc_rbdyn::rpyToPT(X.segment<3>(1)); + X_p_vb = sva::PTransformd(Eigen::Matrix3d::Identity(), X.segment<3>(4)); + offset = sva::ForceVecd(X.segment<6>(7)); +} - /** 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 - */ - inline sva::ForceVecd wfToSensor(const sva::PTransformd & X_0_p, const sva::PTransformd & X_p_f) const - { - sva::PTransformd X_0_ds = X_f_ds_ * X_p_f * X_0_p; - sva::PTransformd X_0_vb(X_0_ds.inv().rotation(), (X_p_vb_ * X_0_p * X_0_ds.inv()).translation()); - return offset_ + X_0_vb.transMul(worldForce_); - } +/** 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 ForceSensorCalibData::wfToSensor(const sva::PTransformd & X_0_p, + const sva::PTransformd & X_p_f) const noexcept +{ + sva::PTransformd X_0_ds = X_f_ds * X_p_f * X_0_p; + sva::PTransformd X_0_vb(X_0_ds.inv().rotation(), (X_p_vb * X_0_p * X_0_ds.inv()).translation()); + return offset + X_0_vb.transMul(worldForce); +} - /** Return X_f_ds, the pure rotation transform from the position given by - * the URDF and the one estimated by calibration - */ - inline const sva::PTransformd & X_f_ds() const { return X_f_ds_; } - - /** Return the mass of the link generating the wrench */ - inline double mass() const { return mass_; } - - inline const sva::ForceVecd & offset() const { return offset_; } - -private: - /** Mass of the link generating the wrench */ - double mass_ = 0.0; - /** Constant gravity wrench applied on the force sensor - * in the world frame: $\{0, mg \} */ - sva::ForceVecd worldForce_ = sva::ForceVecd(Eigen::Vector6d::Zero()); - /** Local rotation between the model force sensor and real one */ - sva::PTransformd X_f_ds_ = sva::PTransformd::Identity(); - /** Transform from the parent body to the virtual link CoM */ - sva::PTransformd X_p_vb_ = sva::PTransformd::Identity(); - /** Force/torque offset */ - sva::ForceVecd offset_ = sva::ForceVecd(Eigen::Vector6d::Zero()); -}; } // namespace detail ForceSensor::ForceSensor() : ForceSensor("", "", sva::PTransformd::Identity()) {} ForceSensor::ForceSensor(const std::string & name, const std::string & parentBodyName, const sva::PTransformd & X_p_f) -: Device(name, parentBodyName, X_p_f), wrench_(Eigen::Vector6d::Zero()), - calibration_(std::make_shared()) +: Device(name, parentBodyName, X_p_f), wrench_(Eigen::Vector6d::Zero()) { type_ = "ForceSensor"; } @@ -143,7 +96,7 @@ ForceSensor::ForceSensor(const std::string & name, const std::string & parentBod ForceSensor::ForceSensor(const ForceSensor & fs) : ForceSensor(fs.name_, fs.parentBody(), fs.X_p_f()) { wrench_ = fs.wrench_; - *calibration_ = *fs.calibration_; + calibration_ = fs.calibration_; } ForceSensor & ForceSensor::operator=(const ForceSensor & fs) @@ -152,13 +105,18 @@ ForceSensor & ForceSensor::operator=(const ForceSensor & fs) name_ = fs.name_; parent_ = fs.parent_; X_p_s_ = fs.X_p_s_; - *calibration_ = *fs.calibration_; + calibration_ = fs.calibration_; wrench_ = fs.wrench_; return *this; } ForceSensor::~ForceSensor() noexcept = default; +void ForceSensor::loadCalibrator(const detail::ForceSensorCalibData & data) noexcept +{ + calibration_ = data; +} + void ForceSensor::loadCalibrator(const std::string & calib_file, const Eigen::Vector3d & gravity) { if(!bfs::exists(calib_file)) @@ -166,22 +124,22 @@ void ForceSensor::loadCalibrator(const std::string & calib_file, const Eigen::Ve mc_rtc::log::warning("No calibration file {} found for force sensor {}", calib_file, name()); return; } - else { calibration_->loadData(calib_file, gravity); } + calibration_.loadData(calib_file, gravity); } void ForceSensor::copyCalibrator(const mc_rbdyn::ForceSensor & other) { - calibration_ = std::make_shared(*other.calibration_); + calibration_ = other.calibration_; } void ForceSensor::resetCalibrator() { - calibration_->reset(); + calibration_.reset(); } const sva::PTransformd & ForceSensor::X_fsmodel_fsactual() const { - return calibration_->X_f_ds(); + return calibration_.X_f_ds; } const sva::PTransformd ForceSensor::X_fsactual_parent() const @@ -191,18 +149,18 @@ const sva::PTransformd ForceSensor::X_fsactual_parent() const double ForceSensor::mass() const { - return calibration_->mass(); + return calibration_.mass; } const sva::ForceVecd & ForceSensor::offset() const { - return calibration_->offset(); + return calibration_.offset; } sva::ForceVecd ForceSensor::wrenchWithoutGravity(const mc_rbdyn::Robot & robot) const { sva::PTransformd X_0_p = robot.mbc().bodyPosW[robot.bodyIndexByName(parent_)]; - auto w = wrench_ - calibration_->wfToSensor(X_0_p, X_p_s_); + auto w = wrench_ - calibration_.wfToSensor(X_0_p, X_p_s_); return w; } diff --git a/src/mc_rtc/Logger.cpp b/src/mc_rtc/Logger.cpp index 30b53c6e33..11d32ea9ef 100644 --- a/src/mc_rtc/Logger.cpp +++ b/src/mc_rtc/Logger.cpp @@ -297,12 +297,14 @@ void Logger::log() } else if constexpr(std::is_same_v) { - builder.start_array(5); + builder.start_array(7); builder.write(static_cast(3)); builder.write(meta_.timestep); builder.write(meta_.main_robot); builder.write(meta_.main_robot_module); builder.write(meta_.init); + builder.write(meta_.init_q); + builder.write(meta_.calibs); builder.finish_array(); } else { static_assert(!std::is_same_v, "non-exhaustive visitor"); } diff --git a/src/mc_rtc/internals/LogEntry.h b/src/mc_rtc/internals/LogEntry.h index 154ff24866..ddb6cffbf7 100644 --- a/src/mc_rtc/internals/LogEntry.h +++ b/src/mc_rtc/internals/LogEntry.h @@ -511,9 +511,9 @@ struct LogEntry : mpack_tree_t else if(event_t == 3) { // StartEvent event - if(event_size != 5) + if(event_size < 5) { - log::error("Start event should have five entries"); + log::error("Start event should have at least five entries"); valid_ = false; return; } @@ -523,6 +523,8 @@ struct LogEntry : mpack_tree_t meta.main_robot = data[2].operator std::string(); meta.main_robot_module = data[3]; meta.init = data[4]; + if(data.size() > 5) { meta.init_q = data[5]; } + if(data.size() > 6) { meta.calibs = data[6]; } metaOut = meta; } else diff --git a/tests/controllers/replay/CMakeLists.txt b/tests/controllers/replay/CMakeLists.txt index 81e1c91681..568effd36a 100644 --- 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/$") +target_include_directories(TestReplayController PRIVATE ${CONFIG_HEADER_INCLUDE_DIR}) add_global_controller_test_run( TestReplayController_Record ${CMAKE_CURRENT_BINARY_DIR}/$/mc_rtc.yaml 50 diff --git a/tests/controllers/replay/TestReplayController.cpp b/tests/controllers/replay/TestReplayController.cpp index e03598d6b2..7d95b25204 100644 --- a/tests/controllers/replay/TestReplayController.cpp +++ b/tests/controllers/replay/TestReplayController.cpp @@ -12,11 +12,24 @@ #include +#include + 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 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(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 index 3dda8ea659..38ccc542d9 100644 --- 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"