Improve ForceSensors definition

jrl-umi3218 · Mar 20, 2024 · dc8e1f6 · dc8e1f6
1 parent 7395ff7
commit dc8e1f6
Showing 1 changed file with 33 additions and 17 deletions.
diff --git a/src/nao.cpp b/src/nao.cpp
@@ -3,8 +3,6 @@
 
 #include <RBDyn/parsers/urdf.h>
 
-#include <fstream>
-
 #include <boost/filesystem.hpp>
 namespace bfs = boost::filesystem;
 
@@ -88,21 +86,39 @@ NAOCommonRobotModule::NAOCommonRobotModule()
   halfSitting["RThumb2"] = {0.0};
 
   // Foot force sensors.
-  // XXX parse position from URDF
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("LFsrFR", "l_ankle", sva::PTransformd(Eigen::Vector3d(0.07025, -0.0231, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("LFsrRR", "l_ankle", sva::PTransformd(Eigen::Vector3d(-0.02965, -0.0191, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("LFsrFL", "l_ankle", sva::PTransformd(Eigen::Vector3d(0.07025, 0.0299, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("LFsrRL", "l_ankle", sva::PTransformd(Eigen::Vector3d(-0.03025, 0.0299, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("RFsrFL", "r_ankle", sva::PTransformd(Eigen::Vector3d(0.07025, 0.0231, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("RFsrRL", "r_ankle", sva::PTransformd(Eigen::Vector3d(-0.03025, 0.0191, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("RFsrRR", "r_ankle", sva::PTransformd(Eigen::Vector3d(-0.02965, -0.0299, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("RFsrFR", "r_ankle", sva::PTransformd(Eigen::Vector3d(0.07025, -0.0299, -0.04511))));
-  // XXX fix position
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("LF_TOTAL_WEIGHT", "l_ankle", sva::PTransformd(Eigen::Vector3d(0.07025, -0.0299, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("RF_TOTAL_WEIGHT", "r_ankle", sva::PTransformd(Eigen::Vector3d(0.07025, -0.0299, -0.04511))));
-  // TODO: Write an observer to convert the pressure sensor measurements above to a wrench in the virtual Left/RightFootForceSensor
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("LeftFootForceSensor", "l_ankle", sva::PTransformd(Eigen::Vector3d(0.0, 0.0, -0.04511))));
-  _forceSensors.push_back(mc_rbdyn::ForceSensor("RightFootForceSensor", "l_ankle", sva::PTransformd(Eigen::Vector3d(0.0, 0.0, -0.04511))));
+
+  auto createForceSensors = [this](const std::string & prefix, const std::string & prefixL, const std::string & link,
+      double DX_FL, double DY_FL, double DX_FR, double DY_FR, double DX_RL, double DY_RL, double DX_RR, double DY_RR)
+  {
+    constexpr double ankleZ = -0.04511;
+    _forceSensors.push_back(mc_rbdyn::ForceSensor(prefix + "srFR", link, sva::PTransformd(Eigen::Vector3d(DX_FR, DY_FR, ankleZ))));
+    _forceSensors.push_back(mc_rbdyn::ForceSensor(prefix + "srRR", link, sva::PTransformd(Eigen::Vector3d(DX_RR, DY_RR, ankleZ))));
+    _forceSensors.push_back(mc_rbdyn::ForceSensor(prefix + "srFL", link, sva::PTransformd(Eigen::Vector3d(DX_FL, DY_FL, ankleZ))));
+    _forceSensors.push_back(mc_rbdyn::ForceSensor(prefix + "srRL", link, sva::PTransformd(Eigen::Vector3d(DX_RL, DY_RL, ankleZ))));
+    _forceSensors.push_back(mc_rbdyn::ForceSensor("LF_TOTAL_WEIGHT", link, sva::PTransformd(Eigen::Vector3d(0, 0, ankleZ))));
+    // TODO: Write an observer to convert the pressure sensor measurements above to a wrench in the virtual Left/RightFootForceSensor
+    _forceSensors.push_back(mc_rbdyn::ForceSensor(prefixL + "FootForceSensor", link, sva::PTransformd(Eigen::Vector3d(0, 0, ankleZ))));
+  };
+
+  auto LF_DX_FL = 0.07025;
+  auto LF_DY_FL = 0.0299;
+  auto LF_DX_FR = 0.07025;
+  auto LF_DY_FR = -0.0231;
+  auto LF_DX_RL = -0.03025;
+  auto LF_DY_RL = 0.0299;
+  auto LF_DX_RR = -0.02965;
+  auto LF_DY_RR = -0.0191;
+  createForceSensors("LF", "Left", "l_ankle", LF_DX_FL, LF_DY_FL, LF_DX_FR, LF_DY_FR, LF_DX_RL, LF_DY_RL, LF_DX_RR, LF_DY_RR);
+
+  auto RF_DX_FL = 0.07025;
+  auto RF_DY_FL = 0.0231;
+  auto RF_DX_FR = 0.07025;
+  auto RF_DY_FR = -0.0299;
+  auto RF_DX_RL = -0.03025;
+  auto RF_DY_RL = 0.0191;
+  auto RF_DX_RR = -0.02965;
+  auto RF_DY_RR = -0.0299;
+  createForceSensors("RF", "Right", "r_ankle", RF_DX_FL, RF_DY_FL, RF_DX_FR, RF_DY_FR, RF_DX_RL, RF_DY_RL, RF_DX_RR, RF_DY_RR);
 
   _minimalSelfCollisions = {
       mc_rbdyn::Collision("Head", "l_wrist", 0.02, 0.01, 0.),