diff --git a/include/mc_rbdyn/Robot.h b/include/mc_rbdyn/Robot.h index d98150b341..f3dd837fe1 100644 --- a/include/mc_rbdyn/Robot.h +++ b/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_; - /** Correspondance between bodies' names and attached force sensors */ - std::map bodyForceSensors_; /** Frames in this robot */ std::unordered_map frames_; /** Mass of this robot */ diff --git a/include/mc_rbdyn/RobotData.h b/include/mc_rbdyn/RobotData.h index 39653be9ac..fd46d65683 100644 --- a/include/mc_rbdyn/RobotData.h +++ b/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 forceSensors; /** Correspondance between force sensor's name and force sensor index */ std::unordered_map forceSensorsIndex; + /** Correspondance between bodies' names and attached force sensors */ + std::unordered_map 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 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 robots; }; using RobotDataPtr = std::shared_ptr; diff --git a/include/mc_rbdyn/RobotFrame.h b/include/mc_rbdyn/RobotFrame.h index 8008160a84..4c9a2ff827 100644 --- a/include/mc_rbdyn/RobotFrame.h +++ b/include/mc_rbdyn/RobotFrame.h @@ -155,6 +155,9 @@ struct MC_RBDYN_DLLAPI RobotFrame : public mc_rtc::shared const ForceSensor * sensor_ = nullptr; void init_tvm_frame() const final; + + /** Search for a force sensor inside Robot again */ + void resetForceSensor() noexcept; }; } // namespace mc_rbdyn diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp index 94a687ec7b..02c9826b2a 100644 --- a/src/mc_rbdyn/Robot.cpp +++ b/src/mc_rbdyn/Robot.cpp @@ -258,6 +258,7 @@ Robot::Robot(NewRobotToken, { if(params.data_) { data_ = params.data_; } else { data_ = std::make_shared(); } + 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 & 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; } diff --git a/src/mc_rbdyn/RobotFrame.cpp b/src/mc_rbdyn/RobotFrame.cpp index 65af2bf6f5..d99016276b 100644 --- a/src/mc_rbdyn/RobotFrame.cpp +++ b/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(bodyMbcIdx_)).name(); diff --git a/tests/controllers/TestCanonicalRobotController.cpp b/tests/controllers/TestCanonicalRobotController.cpp index 68b61c9ade..69161178f1 100644 --- a/tests/controllers/TestCanonicalRobotController.cpp +++ b/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);