From 88bc1b6f313a22182f7a029170c4097b44db3c37 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 20 Mar 2024 14:59:41 +0100 Subject: [PATCH] [PressureObserver] Add PressureForceSensorObserver --- .../PressureForceSensorObserver.h | 45 +++++++++ src/CMakeLists.txt | 1 + src/PressureForceSensorObserver.cpp | 94 +++++++++++++++++++ 3 files changed, 140 insertions(+) create mode 100644 include/mc_state_observation/PressureForceSensorObserver.h create mode 100644 src/PressureForceSensorObserver.cpp diff --git a/include/mc_state_observation/PressureForceSensorObserver.h b/include/mc_state_observation/PressureForceSensorObserver.h new file mode 100644 index 0000000..7cdf6b2 --- /dev/null +++ b/include/mc_state_observation/PressureForceSensorObserver.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +namespace mc_state_observation +{ + +struct PressureForceSensorObserver : public mc_observers::Observer +{ + PressureForceSensorObserver(const std::string & type, double dt); + + void configure(const mc_control::MCController & ctl, const mc_rtc::Configuration &) override; + + void reset(const mc_control::MCController & ctl) override; + + bool run(const mc_control::MCController & ctl) override; + + void update(mc_control::MCController & ctl) override; + +protected: + /*! \brief Add observer information the GUI. + * + * @param category Category in which to add this observer + */ + void addToGUI(const mc_control::MCController &, + mc_rtc::gui::StateBuilder &, + const std::vector & /* category */) override; + +protected: + std::string robot_ = ""; + std::string forceSensor_ = ""; + std::string FSR_FL = ""; + std::string FSR_FR = ""; + std::string FSR_RL = ""; + std::string FSR_RR = ""; + double FSR_FL_V = 0.0; + double FSR_FR_V = 0.0; + double FSR_RL_V = 0.0; + double FSR_RR_V = 0.0; + bool manualOverride_ = false; + std::vector pressureSensors_; + sva::ForceVecd estimatedWrench_ = sva::ForceVecd::Zero(); +}; + +} // namespace mc_state_observation diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5e270e8..f2669a4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -39,6 +39,7 @@ macro(add_observer_with_filter observer_name) endmacro() add_so_observer(AttitudeObserver) +add_simple_observer(PressureForceSensorObserver) if(WITH_ROS_OBSERVERS) add_simple_observer(MocapObserver) diff --git a/src/PressureForceSensorObserver.cpp b/src/PressureForceSensorObserver.cpp new file mode 100644 index 0000000..4324da8 --- /dev/null +++ b/src/PressureForceSensorObserver.cpp @@ -0,0 +1,94 @@ +#include +#include +#include +#include + +namespace mc_state_observation +{ + +PressureForceSensorObserver::PressureForceSensorObserver(const std::string & type, double dt) +: mc_observers::Observer(type, dt) +{ +} + +void PressureForceSensorObserver::configure(const mc_control::MCController & ctl, const mc_rtc::Configuration & config) +{ + robot_ = config("robot", ctl.robot().name()); + if(config.has("estimatedSensor")) { forceSensor_ = static_cast(config("estimatedSensor")); } + else { mc_rtc::log::error_and_throw("[{}] forceSensor configuration is mandatory.", name()); } + + FSR_FL = static_cast(config("pressureFrontLeft")); + FSR_FR = static_cast(config("pressureFrontRight")); + FSR_RL = static_cast(config("pressureRearLeft")); + FSR_RR = static_cast(config("pressureRearRight")); + pressureSensors_ = {FSR_FL, FSR_FR, FSR_RL, FSR_RR}; + + desc_ = fmt::format("{} (forceSensor={}, FSR_FL={}, FSR_FR={}, FSR_RL={}, FSR_RR={})", name_, forceSensor_, FSR_FL, + FSR_FR, FSR_RL, FSR_RR); +} + +void PressureForceSensorObserver::reset(const mc_control::MCController & ctl) {} + +bool PressureForceSensorObserver::run(const mc_control::MCController & ctl) +{ + auto & robot = ctl.robot(robot_); + double DX_FL = robot.forceSensor(FSR_FL).X_p_s().translation().x(); + double DY_FL = robot.forceSensor(FSR_FL).X_p_s().translation().y(); + double DX_FR = robot.forceSensor(FSR_FR).X_p_s().translation().x(); + double DY_FR = robot.forceSensor(FSR_FR).X_p_s().translation().y(); + double DX_RL = robot.forceSensor(FSR_RL).X_p_s().translation().x(); + double DY_RL = robot.forceSensor(FSR_RL).X_p_s().translation().y(); + double DX_RR = robot.forceSensor(FSR_RR).X_p_s().translation().x(); + double DY_RR = robot.forceSensor(FSR_RR).X_p_s().translation().y(); + + if(!manualOverride_) + { + FSR_FL_V = robot.forceSensor(FSR_FL).wrench().force().z(); + FSR_FR_V = robot.forceSensor(FSR_FR).wrench().force().z(); + FSR_RL_V = robot.forceSensor(FSR_RL).wrench().force().z(); + FSR_RR_V = robot.forceSensor(FSR_RR).wrench().force().z(); + } + + estimatedWrench_.couple() = {DY_FL * FSR_FL_V + DY_FR * FSR_FR_V + DY_RL * FSR_RL_V + DY_RR * FSR_RR_V, + -DX_FL * FSR_FL_V - DX_FR * FSR_FR_V - DX_RL * FSR_RL_V - DX_RR * FSR_RR_V, 0.0}; + estimatedWrench_.force().z() = FSR_FL_V + FSR_FR_V + FSR_RL_V + FSR_RR_V; + + return true; +} + +void PressureForceSensorObserver::update(mc_control::MCController & ctl) +{ + auto & robot = ctl.robot(robot_); + auto & data = *robot.data(); + auto & sensor = data.forceSensors.at(data.forceSensorsIndex.at(forceSensor_)); + sensor.wrench(estimatedWrench_); +} + +void PressureForceSensorObserver::addToGUI(const mc_control::MCController & ctl, + mc_rtc::gui::StateBuilder & gui, + const std::vector & category) +{ + gui.addElement(this, category, + mc_rtc::gui::ArrayLabel("Estimated Force", {"x [N]", "y [N]", "z [N]"}, + [this]() -> Eigen::Vector3d { return estimatedWrench_.force(); })); + gui.addElement(this, category, + mc_rtc::gui::ArrayLabel("Estimated Couple", {"x [N.m]", "y [N.m]", "z [N.m]"}, + [this]() -> Eigen::Vector3d { return estimatedWrench_.couple(); })); + + gui.addElement(this, category, mc_rtc::gui::Input("Manual override", manualOverride_)); + auto addPressureSensor = [this, &category, &gui](const std::string & pressureSensorName, double & pressureValue) + { + gui.addElement(this, category, + mc_rtc::gui::NumberInput( + pressureSensorName, [&pressureValue]() -> double { return pressureValue; }, + [&pressureValue](double f) { pressureValue = f; })); + }; + addPressureSensor(FSR_FL, FSR_FL_V); + addPressureSensor(FSR_FR, FSR_FR_V); + addPressureSensor(FSR_RL, FSR_RL_V); + addPressureSensor(FSR_RR, FSR_RR_V); +} + +} // namespace mc_state_observation + +EXPORT_OBSERVER_MODULE("PressureForceSensor", mc_state_observation::PressureForceSensorObserver)