Introduce the Variables Handler

src/System/CMakeLists.txt

@@ -4,7 +4,9 @@
 
 # set target name
 add_bipedal_locomotion_library(
- NAME System
- PUBLIC_HEADERS include/BipedalLocomotion/System/Advanceable.h
- IS_INTERFACE #This will be removed when adding also sources.
- )
+ NAME System
+ PUBLIC_HEADERS include/BipedalLocomotion/System/Advanceable.h include/BipedalLocomotion/System/VariablesHandler.h
+ SOURCES src/VariablesHandler.cpp
+ PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core
+ SUBDIRECTORIES tests
+ )

src/System/include/BipedalLocomotion/System/VariablesHandler.h

@@ -0,0 +1,56 @@
+/**
+ * @file VariablesHandler.h
+ * @authors Giulio Romualdi
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <string>
+#include <unordered_map>
+
+#include <iDynTree/Core/Utils.h>
+
+#ifndef BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H
+#define BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H
+
+namespace BipedalLocomotion
+{
+namespace System
+{
+/**
+ * VariableHandler is useful to handle variables in an optimization problem, Their name, dimension
+ * and position
+ */
+class VariablesHandler
+{
+
+ std::unordered_map<std::string, iDynTree::IndexRange> m_variables; /**< Map containing the name
+ of a variable and its
+ index range */
+ std::size_t m_numberOfVariables{0}; /**< Total number of Variable seen as scalar */
+
+public:
+ /**
+ * Add a new variable to the list
+ * @param name of the variable
+ * @param size the size of the variable
+ * @return true/false in case of success/failure
+ */
+ bool addVariable(const std::string& name, const std::size_t& size) noexcept;
+
+ /**
+ * Get a variable from the list
+ * @param name of the variable
+ * @return the index range associated to the variable
+ */
+ iDynTree::IndexRange getVariable(const std::string& name) const noexcept;
+
+ /**
+ * Get the number of variables
+ * @return the total number of variables
+ */
+ const std::size_t& getNumberOfVariables() const noexcept;
+};
+} // namespace System
+} // namespace BipedalLocomotion
+#endif // BIPEDAL_LOCOMOTION_SYSTEM_VARIABLES_HANDLER_H

src/System/src/VariablesHandler.cpp

@@ -0,0 +1,48 @@
+/**
+ * @file VariablesHandler.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <iostream>
+#include <BipedalLocomotion/System/VariablesHandler.h>
+
+using namespace BipedalLocomotion::System;
+
+bool VariablesHandler::addVariable(const std::string& name, const std::size_t& size) noexcept
+{
+ // if the variable already exist cannot be added again.
+ if (m_variables.find(name) != m_variables.end())
+ {
+ std::cerr << "[VariableHandler::addVariable] The variable name " << name
+ << " already exists";
+ return false;
+ }
+
+ iDynTree::IndexRange indexRange;
+ indexRange.size = size;
+ indexRange.offset = m_numberOfVariables;
+ m_variables.emplace(name, indexRange);
+ m_numberOfVariables += size;
+
+ return true;
+}
+
+iDynTree::IndexRange VariablesHandler::getVariable(const std::string& name) const noexcept
+{
+ auto variable = m_variables.find(name);
+
+ // if the variable is present its IndexRange is returned otherwise an
+ // invalid IndexRange is provided to the user
+
+ if (variable != m_variables.end())
+ return variable->second;
+ else
+ return iDynTree::IndexRange::InvalidRange();
+}
+
+const std::size_t& VariablesHandler::getNumberOfVariables() const noexcept
+{
+ return m_numberOfVariables;
+}

src/System/tests/CMakeLists.txt

@@ -0,0 +1,8 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+add_bipedal_test(
+ NAME VariablesHandler
+ SOURCES VariablesHandlerTest.cpp
+ LINKS BipedalLocomotion::System)

src/System/tests/VariablesHandlerTest.cpp

@@ -0,0 +1,35 @@
+/**
+ * @file VariablesHandlerTest.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2019 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+// Catch2
+#include <catch2/catch.hpp>
+
+#include <BipedalLocomotion/System/VariablesHandler.h>
+
+using namespace BipedalLocomotion::System;
+
+TEST_CASE("Test Variable Handler")
+{
+ // Instantiate the handler
+ VariablesHandler handler;
+
+ constexpr std::size_t variable1Size = 42;
+ constexpr std::size_t variable2Size = 35;
+
+ REQUIRE(handler.addVariable("variable_1", variable1Size));
+ REQUIRE(handler.addVariable("variable_2", variable2Size));
+
+ REQUIRE(handler.getVariable("variable_1").offset == 0);
+ REQUIRE(handler.getVariable("variable_1").size == variable1Size);
+
+ REQUIRE(handler.getVariable("variable_2").offset == variable1Size);
+ REQUIRE(handler.getVariable("variable_2").size == variable2Size);
+
+ REQUIRE(handler.getNumberOfVariables() == variable1Size + variable2Size);
+
+ REQUIRE_FALSE(handler.getVariable("variable_3").isValid());
+}