Merge pull request #433 from gergondet/topic/AddForceSensor

[mc_rbdyn] Implement addForceSensor

include/mc_rbdyn/Robot.h

@@ -663,6 +663,19 @@ struct MC_RBDYN_DLLAPI Robot
    * @{
    */
 
+  /** Add a new force sensor to the robot
+   *
+   * This also updates frames' sensors.
+   *
+   * This does not load the calibration file for this sensor. It is up to the caller to do so if they have a file
+   * availble.
+   *
+   * \param sensor Sensor to be added to the robot
+   *
+   * \throws If a sensor with the same name already exists within this robot
+   */
+  void addForceSensor(const mc_rbdyn::ForceSensor & fs);
+
   /** Check if a force sensor exists
    *
    * @param name Name of the sensor
@@ -684,7 +697,10 @@ struct MC_RBDYN_DLLAPI Robot
    * @returns True if the body has a force sensor attached to it, false
    * otherwise
    */
-  inline bool bodyHasForceSensor(const std::string & body) const noexcept { return bodyForceSensors_.count(body) != 0; }
+  inline bool bodyHasForceSensor(const std::string & body) const noexcept
+  {
+    return data_->bodyForceSensors_.count(body) != 0;
+  }
 
   /**
    * @brief Checks if the surface has a force sensor directly attached to it
@@ -715,7 +731,7 @@ struct MC_RBDYN_DLLAPI Robot
    */
   inline bool bodyHasIndirectForceSensor(const std::string & body) const
   {
-    return bodyHasForceSensor(body) || findIndirectForceSensorBodyName(body).size() != 0;
+    return bodyHasForceSensor(body) || findIndirectForceSensorBodyName(body).empty();
   }
 
   /** Check if the surface has a force sensor attached to it (directly or
@@ -1129,8 +1145,6 @@ struct MC_RBDYN_DLLAPI Robot
   Springs springs_;
   /** Flexibility in this instance */
   std::vector<Flexibility> flexibility_;
-  /** Correspondance between bodies' names and attached force sensors */
-  std::map<std::string, size_t> bodyForceSensors_;
   /** Frames in this robot */
   std::unordered_map<std::string, RobotFramePtr> frames_;
   /** Mass of this robot */

include/mc_rbdyn/RobotData.h

@@ -17,6 +17,8 @@
 namespace mc_rbdyn
 {
 
+struct Robot;
+
 /** Hold data and objects that are shared among different views of a robot (control/real/output)
  *
  * The framework enforce consistency between these views
@@ -42,6 +44,8 @@ struct RobotData
   std::vector<ForceSensor> forceSensors;
   /** Correspondance between force sensor's name and force sensor index */
   std::unordered_map<std::string, size_t> forceSensorsIndex;
+  /** Correspondance between bodies' names and attached force sensors */
+  std::unordered_map<std::string, size_t> bodyForceSensors_;
   /** Hold all body sensors */
   BodySensorVector bodySensors;
   /** Correspondance between body sensor's name and body sensor index*/
@@ -60,6 +64,10 @@ struct RobotData
   DevicePtrVector devices;
   /** Correspondance between a device's name and a device index */
   std::unordered_map<std::string, size_t> devicesIndex;
+  /** A list of robots that share this data
+   *
+   * This is used to communicate changes to the data to all instances that share this data */
+  std::vector<Robot *> robots;
 };
 
 using RobotDataPtr = std::shared_ptr<RobotData>;

include/mc_rbdyn/RobotFrame.h

@@ -155,6 +155,9 @@ struct MC_RBDYN_DLLAPI RobotFrame : public mc_rtc::shared<RobotFrame, Frame>
   const ForceSensor * sensor_ = nullptr;
 
   void init_tvm_frame() const final;
+
+  /** Search for a force sensor inside Robot again */
+  void resetForceSensor() noexcept;
 };
 
 } // namespace mc_rbdyn

src/mc_rbdyn/Robot.cpp

@@ -258,6 +258,7 @@ Robot::Robot(NewRobotToken,
 {
   if(params.data_) { data_ = params.data_; }
   else { data_ = std::make_shared<RobotData>(); }
+  data_->robots.push_back(this);
   const auto & module_ = module();
 
   sva::PTransformd base_tf = params.base_tf_.value_or(sva::PTransformd::Identity());
@@ -394,6 +395,7 @@ Robot::Robot(NewRobotToken,
     {
       const auto & fs = data_->forceSensors[i];
       data_->forceSensorsIndex[fs.name()] = i;
+      data_->bodyForceSensors_[fs.parentBody()] = i;
     }
   }
   auto & forceSensors_ = data_->forceSensors;
@@ -412,11 +414,6 @@ Robot::Robot(NewRobotToken,
       else { fs.loadCalibrator(calib_file.string(), mbc().gravity); }
     }
   }
-  for(size_t i = 0; i < forceSensors_.size(); ++i)
-  {
-    const auto & fs = forceSensors_[i];
-    bodyForceSensors_[fs.parentBody()] = i;
-  }
 
   for(const auto & b : mb().bodies())
   {
@@ -500,7 +497,14 @@ Robot::Robot(NewRobotToken,
   zmp_ = Eigen::Vector3d::Zero();
 }
 
-Robot::~Robot() = default;
+Robot::~Robot()
+{
+  if(data_)
+  {
+    auto it = std::find(data_->robots.begin(), data_->robots.end(), this);
+    if(it != data_->robots.end()) { data_->robots.erase(it); }
+  }
+}
 
 Robot::Robot(Robot &&) = default;
 
@@ -528,7 +532,8 @@ void Robot::addBodySensor(const BodySensor & sensor)
     data_->bodySensors.push_back(sensor);
     data_->bodySensorsIndex.insert({sensor.name(), data_->bodySensors.size() - 1});
 
-    if(!bodyHasBodySensor(sensor.name())) data_->bodyBodySensors.insert({sensor.name(), data_->bodySensors.size() - 1});
+    if(!bodyHasBodySensor(sensor.parentBody()))
+      data_->bodyBodySensors.insert({sensor.parentBody(), data_->bodySensors.size() - 1});
   }
 
   else { mc_rtc::log::error_and_throw("Body sensor named {} already attached to {}", sensor.name(), this->name()); }
@@ -926,6 +931,28 @@ std::vector<Flexibility> & Robot::flexibility()
   return flexibility_;
 }
 
+void Robot::addForceSensor(const mc_rbdyn::ForceSensor & fs)
+{
+  auto it = data_->forceSensorsIndex.find(fs.name());
+  if(it != data_->forceSensorsIndex.end())
+  {
+    mc_rtc::log::error_and_throw("Cannot add a force sensor named {} since {} already has one", fs.name(),
+                                 this->name());
+  }
+  data_->forceSensors.push_back(fs);
+  data_->forceSensorsIndex[fs.name()] = data_->forceSensors.size() - 1;
+  auto bfs_it = data_->bodyForceSensors_.find(fs.parentBody());
+  if(bfs_it == data_->bodyForceSensors_.end())
+  {
+    data_->bodyForceSensors_[fs.parentBody()] = data_->forceSensors.size() - 1;
+  }
+  auto updateFrames = [](const mc_rbdyn::Robot & robot)
+  {
+    for(const auto & f : robot.frames_) { f.second->resetForceSensor(); }
+  };
+  for(auto & r : data_->robots) { updateFrames(*r); }
+}
+
 const ForceSensor & Robot::forceSensor(const std::string & name) const
 {
   auto it = data_->forceSensorsIndex.find(name);
@@ -938,8 +965,8 @@ const ForceSensor & Robot::forceSensor(const std::string & name) const
 
 const ForceSensor & Robot::bodyForceSensor(const std::string & body) const
 {
-  auto it = bodyForceSensors_.find(body);
-  if(it == bodyForceSensors_.end())
+  auto it = data_->bodyForceSensors_.find(body);
+  if(it == data_->bodyForceSensors_.end())
   {
     mc_rtc::log::error_and_throw("No force sensor directly attached to {} in {}", body, name());
   }
@@ -1480,10 +1507,10 @@ RobotFramePtr Robot::makeTemporaryFrame(const std::string & name,
 
 const ForceSensor * Robot::findBodyForceSensor(const std::string & body) const
 {
-  auto it = bodyForceSensors_.find(body);
-  if(it != bodyForceSensors_.end()) { return &data_->forceSensors[it->second]; }
+  auto it = data_->bodyForceSensors_.find(body);
+  if(it != data_->bodyForceSensors_.end()) { return &data_->forceSensors[it->second]; }
   auto bodyName = findIndirectForceSensorBodyName(body);
-  if(bodyName.size()) { return &data_->forceSensors[bodyForceSensors_.find(bodyName)->second]; }
+  if(!bodyName.empty()) { return &data_->forceSensors[data_->bodyForceSensors_.find(bodyName)->second]; }
   return nullptr;
 }
 

src/mc_rbdyn/RobotFrame.cpp

@@ -29,6 +29,11 @@ RobotFrame::RobotFrame(NewRobotFrameToken tkn,
 {
 }
 
+void RobotFrame::resetForceSensor() noexcept
+{
+  sensor_ = robot_.findBodyForceSensor(body());
+}
+
 const std::string & RobotFrame::body() const noexcept
 {
   return robot_.mb().body(static_cast<int>(bodyMbcIdx_)).name();

tests/controllers/TestCanonicalRobotController.cpp

@@ -34,6 +34,44 @@ struct MC_CONTROL_DLLAPI TestCanonicalRobotController : public MCController
       BOOST_REQUIRE(r.data().get() == outputRobot(r.name()).data().get());
       BOOST_REQUIRE(r.data().get() == outputRealRobot(r.name()).data().get());
     }
+    auto for_all_robots = [this](auto && callback)
+    {
+      callback(robot());
+      callback(realRobot());
+      callback(outputRobot());
+      callback(outputRealRobot());
+    };
+    auto require_for_all_robots = [this](auto && callback, const char * desc)
+    {
+      auto do_throw = [&]()
+      {
+        mc_rtc::log::error_and_throw(desc);
+        return true;
+      };
+      BOOST_REQUIRE(callback(robot()) || do_throw());
+      BOOST_REQUIRE(callback(realRobot()) || do_throw());
+      BOOST_REQUIRE(callback(outputRobot()) || do_throw());
+      BOOST_REQUIRE(callback(outputRealRobot()) || do_throw());
+    };
+    // Check that we can create frames at runtime
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return !r.hasFrame("CameraFrame"); }, "has no CameraFrame");
+    for_all_robots([](mc_rbdyn::Robot & r)
+                   { r.makeFrame("CameraFrame", r.frame("NECK_P_S"), sva::PTransformd::Identity()); });
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return r.hasFrame("CameraFrame"); }, "has CameraFrame");
+    // Checks that we can add a new force sensor at run-time
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return !r.hasForceSensor("HeadForceSensor"); },
+                           "has no HeadForceSensor");
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return !r.bodyHasForceSensor("NECK_P_S"); },
+                           "NECK_P_S has no force sensor");
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return !r.frame("CameraFrame").hasForceSensor(); },
+                           "CameraFrame has no force sensor");
+    robot().addForceSensor(mc_rbdyn::ForceSensor{"HeadForceSensor", "NECK_P_S", sva::PTransformd::Identity()});
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return r.hasForceSensor("HeadForceSensor"); },
+                           "has HeadForceSensor");
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return r.bodyHasForceSensor("NECK_P_S"); },
+                           "NECK_P_S has force sensor");
+    require_for_all_robots([](const mc_rbdyn::Robot & r) { return r.frame("CameraFrame").hasForceSensor(); },
+                           "CameraFrame has force sensor");
     solver().addConstraintSet(kinematicsConstraint);
     postureTask->stiffness(1);
     postureTask->weight(1);