diff --git a/src/System/include/BipedalLocomotion/System/VariablesHandler.h b/src/System/include/BipedalLocomotion/System/VariablesHandler.h index 1703fb8dec..d0ddb52cfe 100644 --- a/src/System/include/BipedalLocomotion/System/VariablesHandler.h +++ b/src/System/include/BipedalLocomotion/System/VariablesHandler.h @@ -8,6 +8,7 @@ #ifndef BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H #define BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H +#include #include #include #include @@ -19,6 +20,8 @@ namespace BipedalLocomotion namespace System { +class VariablesHandler; + /** * VariableHandler is useful to handle variables in an optimization problem, Their name, dimension * and position @@ -26,14 +29,26 @@ namespace System class VariablesHandler { public: - struct VariableDescription + class VariableDescription { - std::ptrdiff_t offset; - std::ptrdiff_t size; + public: + static constexpr std::ptrdiff_t InvalidIndex{-1}; + std::ptrdiff_t offset{InvalidIndex}; + std::ptrdiff_t size{InvalidIndex}; std::string name; bool isValid() const; + + std::ptrdiff_t getElementIndex(const std::string& name) const; + std::ptrdiff_t getElementIndex(std::ptrdiff_t localIndex) const; + static VariableDescription InvalidVariable(); + + private: + std::unordered_map elementsNameMap; + std::vector elementsName; + + friend class VariablesHandler; }; private: @@ -57,6 +72,7 @@ class VariablesHandler * |:----------------:|:----------------:|:--------------------------------------------------------------------------------------:|:---------:| * | `variables_name` | `vector` | List containing the name of the variables | Yes | * | `variables_size` | `vector` | List containing the size of the variables. The size must be a strictly positive number | Yes | + * | `_elements_name` | `vector` | List containing the name of the elements associated to a variable. | Yes | * @warning The previous content of the VariablesHandler is erased. * @return true/false in case of success/failure */ @@ -70,6 +86,26 @@ class VariablesHandler */ bool addVariable(const std::string& name, const std::size_t& size) noexcept; + /** + * Add a new variable to the list + * @param name of the variable + * @param size the size of the variable + * @param elementsName vector containing the name associated to each elements of the variable + * @return true/false in case of success/failure + */ + bool addVariable(const std::string& name, + const std::size_t& size, + const std::vector& elementsName) noexcept; + + /** + * Add a new variable to the list + * @param name of the variable + * @param elementsName vector containing the name associated to each elements of the variable + * @return true/false in case of success/failure + */ + bool addVariable(const std::string& name, + const std::vector& elementsName) noexcept; + /** * Get a variable from the list * @param name of the variable diff --git a/src/System/src/VariablesHandler.cpp b/src/System/src/VariablesHandler.cpp index 3963621864..8ec68cb3fd 100644 --- a/src/System/src/VariablesHandler.cpp +++ b/src/System/src/VariablesHandler.cpp @@ -7,22 +7,53 @@ #include #include +#include using namespace BipedalLocomotion::System; using namespace BipedalLocomotion::ParametersHandler; bool VariablesHandler::VariableDescription::isValid() const { - return (offset >= 0) && (size >= 0); + return (offset >= 0) && (size >= 0) && (elementsName.size() >= 0) && (elementsNameMap.size() >= 0); } VariablesHandler::VariableDescription VariablesHandler::VariableDescription::InvalidVariable() { VariablesHandler::VariableDescription tmp; - tmp.offset = tmp.size = -1; + tmp.offset = tmp.size = InvalidIndex; return tmp; } +std::ptrdiff_t VariablesHandler::VariableDescription::getElementIndex(const std::string& name) const +{ + // find the element index associated to the given name + auto element = elementsNameMap.find(name); + + if (element == elementsNameMap.end()) + { + log()->error("[VariableDescription::getElementIndex] Unable to find the element named: {}. " + "an InvalidIndex will be returned.", + name); + return InvalidIndex; + } + + return element->second + offset; +} + +std::ptrdiff_t +VariablesHandler::VariableDescription::getElementIndex(std::ptrdiff_t localIndex) const +{ + if (localIndex >= size) + { + log()->error("[VariableDescription::getElementIndex] The localIndex is greather than the " + "size of the variable. InvalidIndex will be returned."); + + return InvalidIndex; + } + + return localIndex + offset; +} + bool VariablesHandler::initialize(std::weak_ptr handler) noexcept { // clear the content of the handler @@ -72,15 +103,35 @@ bool VariablesHandler::initialize(std::weak_ptr handle return false; } + std::vector elementsNameVector; for (int i = 0; i < names.size(); i++) { - if (!this->addVariable(names[i], sizes[i])) + // check if the elements name vector has been provided + if (ptr->getParameter(names[i] + "_elements_name", elementsNameVector)) { - log()->error("{} Unable to add the variable named {} having a size equal to {}.", - logPrefix, - names[i], - sizes[i]); - return false; + + if (!this->addVariable(names[i], sizes[i], elementsNameVector)) + { + log()->error("{} Unable to add the variable named {} having a size equal to {}.", + logPrefix, + names[i], + sizes[i]); + return false; + } + } else + { + log()->info("{} The parameter {}_elements_name is not found. The default one is used", + logPrefix, + names[i]); + + if (!this->addVariable(names[i], sizes[i])) + { + log()->error("{} Unable to add the variable named {} having a size equal to {}.", + logPrefix, + names[i], + sizes[i]); + return false; + } } } @@ -88,6 +139,27 @@ bool VariablesHandler::initialize(std::weak_ptr handle } bool VariablesHandler::addVariable(const std::string& name, const std::size_t& size) noexcept +{ + std::vector elementsName(size); + for (int i = 0; i < size; i++) + { + elementsName[i] = name + "_" + std::to_string(i); + } + + return this->addVariable(name, elementsName.size(), elementsName); + + return true; +} + +bool VariablesHandler::addVariable(const std::string& name, + const std::vector& elementsName) noexcept +{ + return this->addVariable(name, elementsName.size(), elementsName); +} + +bool VariablesHandler::addVariable(const std::string& name, + const std::size_t& size, + const std::vector& elementsName) noexcept { // if the variable already exist cannot be added again. if (m_variables.find(name) != m_variables.end()) @@ -96,10 +168,33 @@ bool VariablesHandler::addVariable(const std::string& name, const std::size_t& s return false; } + if (elementsName.size() != size) + { + log()->error("[VariableHandler::addVariable] The size of the vector of the element is " + "different from the expected one. Expected: {}, Retrieved {}.", + size, + elementsName.size()); + return false; + } + VariablesHandler::VariableDescription description; description.size = size; description.offset = m_numberOfVariables; description.name = name; + description.elementsName = elementsName; + for (int i = 0; i < elementsName.size(); i++) + { + const auto& elementName = elementsName[i]; + auto outcome = description.elementsNameMap.insert({elementName, i}); + if (!outcome.second) + { + log()->error("[VariableHandler::addVariable] Unable to add the element {} in the " + "variable {}. The element already exists.", + elementName, + name); + return false; + } + } m_variables.emplace(name, description); m_numberOfVariables += size; @@ -138,7 +233,11 @@ std::string VariablesHandler::toString() const noexcept for (const auto& [key, variable] : m_variables) { out += key + " size: " + std::to_string(variable.size) - + ", offset: " + std::to_string(variable.offset) + ". "; + + ", offset: " + std::to_string(variable.offset) + " elements name:"; + for (const auto& name : variable.elementsName) + { + out += " " + name; + } } return out;