Merge pull request #429 from ami-iit/variables_handler

Add the possibility to set the name of each element of a variable stored in the variables handler

CHANGELOG.md

@@ -4,6 +4,7 @@ All notable changes to this project are documented in this file.
 ## [Unreleased]
 ### Added
 - Implement Python bindings for the TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/428)
+- Add the possibility to set the name of each element of a variable stored in the variables handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/429)
 
 ### Changed
 ### Fix

bindings/python/System/src/VariablesHandler.cpp

@@ -35,7 +35,22 @@ void CreateVariablesHandler(pybind11::module& module)
         .def_static("invalid_variable", &VariablesHandler::VariableDescription::InvalidVariable);
 
     variablesHandler.def(py::init())
-        .def("add_variable", &VariablesHandler::addVariable, py::arg("name"), py::arg("size"))
+        .def("add_variable",
+             py::overload_cast<const std::string&, const std::size_t&>(
+                 &VariablesHandler::addVariable),
+             py::arg("name"),
+             py::arg("size"))
+        .def("add_variable",
+             py::overload_cast<const std::string&, const std::vector<std::string>&>(
+                 &VariablesHandler::addVariable),
+             py::arg("name"),
+             py::arg("elements_name"))
+        .def("add_variable",
+             py::overload_cast<const std::string&, const std::size_t&, const std::vector<std::string>& >(
+                 &VariablesHandler::addVariable),
+             py::arg("name"),
+             py::arg("size"),
+             py::arg("elements_name"))
         .def("get_variable",
              py::overload_cast<const std::string&>(&VariablesHandler::getVariable, py::const_),
              py::arg("name"))

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 <cstddef>
 #include <string>
 #include <memory>
 #include <unordered_map>
@@ -19,21 +20,35 @@ namespace BipedalLocomotion
 namespace System
 {
 
+class VariablesHandler;
+
 /**
  * VariableHandler is useful to handle variables in an optimization problem, Their name, dimension
  * and position
  */
 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<std::string, std::ptrdiff_t> m_elementsNameMap;
+        std::vector<std::string> m_elementsName;
+
+        friend class VariablesHandler;
     };
 
 private:
@@ -57,6 +72,7 @@ class VariablesHandler
      * |:----------------:|:----------------:|:--------------------------------------------------------------------------------------:|:---------:|
      * | `variables_name` | `vector<string>` |                        List containing the name of the variables                       |    Yes    |
      * | `variables_size` |   `vector<int>`  | List containing the size of the variables. The size must be a strictly positive number |    Yes    |
+     * | `<variable_name>_elements_name` |   `vector<string>`  |  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<std::string>& 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<std::string>& elementsName) noexcept;
+
     /**
      * Get a variable from the list
      * @param name of the variable

src/System/src/VariablesHandler.cpp

@@ -7,22 +7,54 @@
 
 #include <BipedalLocomotion/System/VariablesHandler.h>
 #include <BipedalLocomotion/TextLogging/Logger.h>
+#include <cstddef>
 
 using namespace BipedalLocomotion::System;
 using namespace BipedalLocomotion::ParametersHandler;
 
 bool VariablesHandler::VariableDescription::isValid() const
 {
-    return (offset >= 0) && (size >= 0);
+    return (offset >= 0) && (size >= 0) && (m_elementsName.size() >= 0)
+           && (m_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 = m_elementsNameMap.find(name);
+
+    if (element == m_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<const IParametersHandler> handler) noexcept
 {
     // clear the content of the handler
@@ -72,22 +104,63 @@ bool VariablesHandler::initialize(std::weak_ptr<const IParametersHandler> handle
         return false;
     }
 
+    std::vector<std::string> 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))
+        {
+
+            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()->error("{} Unable to add the variable named {} having a size equal to {}.",
+            log()->debug("{} The parameter {}_elements_name is not found. The default one is used",
                          logPrefix,
-                         names[i],
-                         sizes[i]);
-            return false;
+                         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;
+            }
         }
     }
 
     return true;
 }
 
 bool VariablesHandler::addVariable(const std::string& name, const std::size_t& size) noexcept
+{
+    std::vector<std::string> 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<std::string>& elementsName) noexcept
+{
+    return this->addVariable(name, elementsName.size(), elementsName);
+}
+
+bool VariablesHandler::addVariable(const std::string& name,
+                                   const std::size_t& size,
+                                   const std::vector<std::string>& elementsName) noexcept
 {
     // if the variable already exist cannot be added again.
     if (m_variables.find(name) != m_variables.end())
@@ -96,10 +169,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.m_elementsName = elementsName;
+    for (int i = 0; i < elementsName.size(); i++)
+    {
+        const auto& elementName = elementsName[i];
+        auto outcome = description.m_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 +234,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.m_elementsName)
+        {
+            out += " " + name;
+        }
     }
 
     return out;

src/System/tests/VariablesHandlerTest.cpp

@@ -23,6 +23,7 @@ TEST_CASE("Test Variable Handler")
     {
         constexpr std::size_t variable1Size = 42;
         constexpr std::size_t variable2Size = 35;
+        constexpr std::size_t variable3Size = 3;
 
         REQUIRE(handler.addVariable("variable_1", variable1Size));
         REQUIRE(handler.addVariable("variable_2", variable2Size));
@@ -34,8 +35,15 @@ TEST_CASE("Test Variable Handler")
         REQUIRE(handler.getVariable("variable_2").size == variable2Size);
 
         REQUIRE(handler.getNumberOfVariables() == variable1Size + variable2Size);
-
         REQUIRE_FALSE(handler.getVariable("variable_3").isValid());
+
+        auto variable2 = handler.getVariable("variable_2");
+        REQUIRE(variable2.getElementIndex(10) == 10 + variable1Size);
+        REQUIRE(variable2.getElementIndex("variable_2_10") == 10 + variable1Size);
+
+        REQUIRE(handler.addVariable("variable_3", variable3Size, {"foo", "bar", "ok"}));
+        auto variable3 = handler.getVariable("variable_3");
+        REQUIRE(variable3.getElementIndex("bar") == variable1Size + variable2Size + 1);
     }
 
     SECTION("Add variables - ParametersHandler")