From 1c80e93f3d3b4ae795f343d0069b7535e9556add Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Tue, 13 Feb 2024 09:20:19 +0900 Subject: [PATCH 1/4] [mc_rbdyn] Fix addition of BodySensor --- src/mc_rbdyn/Robot.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp index 94a687ec7b..55fabd2973 100644 --- a/src/mc_rbdyn/Robot.cpp +++ b/src/mc_rbdyn/Robot.cpp @@ -528,7 +528,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()); } From d0b43011218cfbbda20e915dc31a5f164834d212 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Tue, 13 Feb 2024 10:22:16 +0900 Subject: [PATCH 2/4] [mc_rbdyn] Implement `addForceSensor` This function allows to insert a new force sensor into a robot at run-time. This also updates the frames of every robot that uses the same data. This means that adding a force sensor to the control robot will not only update the frames of the control robot but also the real/observed robot and their outputs counterpart. The sensor calibration file is not searched, the usual scenario where this might be used implies that calibration files do not exist or do not reside in the usual location so searching for them would be wasteful. --- include/mc_rbdyn/Robot.h | 22 +++++++++++++--- include/mc_rbdyn/RobotData.h | 8 ++++++ include/mc_rbdyn/RobotFrame.h | 3 +++ src/mc_rbdyn/Robot.cpp | 48 +++++++++++++++++++++++++++-------- src/mc_rbdyn/RobotFrame.cpp | 6 +++++ 5 files changed, 72 insertions(+), 15 deletions(-) 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 55fabd2973..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; @@ -927,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); @@ -939,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()); } @@ -1481,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..1218051f49 100644 --- a/src/mc_rbdyn/RobotFrame.cpp +++ b/src/mc_rbdyn/RobotFrame.cpp @@ -29,6 +29,12 @@ RobotFrame::RobotFrame(NewRobotFrameToken tkn, { } +void RobotFrame::resetForceSensor() noexcept +{ + if(parent_) { sensor_ = (static_cast(parent_.get()))->sensor_; } + else { sensor_ = robot_.findBodyForceSensor(body()); } +} + const std::string & RobotFrame::body() const noexcept { return robot_.mb().body(static_cast(bodyMbcIdx_)).name(); From f4d09822e6b67e14e5a7256cfde30dd5a5120d25 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Tue, 13 Feb 2024 10:42:41 +0900 Subject: [PATCH 3/4] [tests] Add a test for addForceSensor --- .../TestCanonicalRobotController.cpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) 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); From b84298d99aff486f9a0600c42387acb1ce8e2f7c Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Tue, 13 Feb 2024 11:29:05 +0900 Subject: [PATCH 4/4] [mc_rbdyn] Fix frame sensor update The frames are not read in the order they are created so there's no reason that the parent would be updated before its children frames. --- src/mc_rbdyn/RobotFrame.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/mc_rbdyn/RobotFrame.cpp b/src/mc_rbdyn/RobotFrame.cpp index 1218051f49..d99016276b 100644 --- a/src/mc_rbdyn/RobotFrame.cpp +++ b/src/mc_rbdyn/RobotFrame.cpp @@ -31,8 +31,7 @@ RobotFrame::RobotFrame(NewRobotFrameToken tkn, void RobotFrame::resetForceSensor() noexcept { - if(parent_) { sensor_ = (static_cast(parent_.get()))->sensor_; } - else { sensor_ = robot_.findBodyForceSensor(body()); } + sensor_ = robot_.findBodyForceSensor(body()); } const std::string & RobotFrame::body() const noexcept