Implement contactListMapFromJson() and contactListMapToJson() methods

.github/workflows/ci.yml

@@ -92,8 +92,8 @@ jobs:
         ./bootstrap-vcpkg.bat
         ./vcpkg.exe install catch2:x64-windows
 
-        # Install pybind11
-        ./vcpkg.exe install pybind11:x64-windows
+        # Install pybind11 and nlohmann_json
+        ./vcpkg.exe install pybind11:x64-windows nlohmann-json:x64-windows
 
     - name: Dependencies [macOS]
       if: matrix.os == 'macOS-latest'
@@ -107,7 +107,7 @@ jobs:
         brew unlink gcc@9
 
         brew update
-        brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt cppad pkg-config pybind11 libmatio spdlog opencv pcl librealsense
+        brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt cppad pkg-config pybind11 libmatio spdlog opencv pcl librealsense nlohmann-json
 
     - name: Dependencies [Ubuntu]
       if: startsWith(matrix.os, 'ubuntu')
@@ -117,7 +117,7 @@ jobs:
         sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev  libboost-system-dev libboost-filesystem-dev \
                              libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \
                              libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python3-dev valgrind coinor-libipopt-dev \
-                             libmatio-dev python3-pytest python3-numpy python3-scipy python3-setuptools libspdlog-dev libopencv-dev  libpcl-dev python3-pybind11
+                             libmatio-dev python3-pytest python3-numpy python3-scipy python3-setuptools libspdlog-dev libopencv-dev  libpcl-dev python3-pybind11 nlohmann-json3-dev
         # install realsense from apt (see https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages)
         sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
         sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo focal main" -u

.github/workflows/conda-forge-ci.yml

@@ -36,7 +36,7 @@ jobs:
         # Compilation related dependencies
         mamba install cmake compilers make ninja pkg-config
         # Actual dependencies
-        mamba install -c robotology idyntree yarp libmatio matio-cpp lie-group-controllers eigen qhull casadi cppad spdlog catch2
+        mamba install -c robotology idyntree yarp libmatio matio-cpp lie-group-controllers eigen qhull casadi cppad spdlog catch2 nlohmann_json
 
     - name: Dependencies [manif - Linux&macOS]
       if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu')

CHANGELOG.md

@@ -37,6 +37,8 @@ All notable changes to this project are documented in this file.
 - Implement `YarpImplementation::setFromFile()` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/307)
 - Implement `CoMTask` in TSID (https://github.com/dic-iit/bipedal-locomotion-framework/pull/304)
 - Implement `YarpParametersHandler` bindings (https://github.com/dic-iit/bipedal-locomotion-framework/pull/309)
+- Implement `contactListMapFromJson()` and `contactListMapToJson()` methods and python bindings (https://github.com/dic-iit/bipedal-locomotion-framework/issues/316)
+
 
 ### Changed
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)

README.md

@@ -48,7 +48,7 @@ The **bipedal-locomotion-framework** project consists of several components. The
 | :------------------------------------------: | :----------------------------------------------------------: | :----------------------------------------------------------: |
 |          [AutoDiff](./src/Autodiff)          |                Bridge between CppAD and Eigen                |   [`CppAD`](https://coin-or.github.io/CppAD/doc/cppad.htm)   |
 |     [ContactModels](./src/ContactModels)     | Models to describe the contact between robot and enviroment  |                              -                               |
-|          [Contacts](./src/Contacts)          |              Syntactic description of a contact              |         [`manif`](https://github.com/artivis/manif)          |
+|          [Contacts](./src/Contacts)          |              Syntactic description of a contact              |  [`manif`](https://github.com/artivis/manif) [nlohmann json](https://github.com/nlohmann/json/) |
 |    [CommonConversions](./src/Conversions)    |      Common conversion utilities used in the framework       |                              -                               |
 |    [ManifConversions](./src/Conversions)     | `manif` library related conversion utilities used in the framework |         [`manif`](https://github.com/artivis/manif)          |
 |        [Estimators](./src/Estimators)        |                 Library containing observers                 |                              -                               |

bindings/python/Contacts/include/BipedalLocomotion/bindings/Contacts/Contacts.h

@@ -21,6 +21,7 @@ void CreateContact(pybind11::module& module);
 void CreateContactList(pybind11::module& module);
 void CreateContactPhase(pybind11::module& module);
 void CreateContactPhaseList(pybind11::module& module);
+void CreateContactListJsonParser(pybind11::module& module);
 
 } // namespace Contacts
 } // namespace bindings

bindings/python/Contacts/src/Contacts.cpp

@@ -13,6 +13,7 @@
 #include <BipedalLocomotion/Contacts/ContactList.h>
 #include <BipedalLocomotion/Contacts/ContactPhase.h>
 #include <BipedalLocomotion/Contacts/ContactPhaseList.h>
+#include <BipedalLocomotion/Contacts/ContactListJsonParser.h>
 
 #include <BipedalLocomotion/bindings/Contacts/Contacts.h>
 
@@ -170,6 +171,19 @@ void CreateContactPhaseList(pybind11::module& module)
         .def("lists", &ContactPhaseList::lists);
 }
 
+void CreateContactListJsonParser(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+
+    module.def("contact_list_map_from_json",
+               BipedalLocomotion::Contacts::contactListMapFromJson,
+               py::arg("filename"));
+    module.def("contact_list_map_to_json",
+               BipedalLocomotion::Contacts::contactListMapToJson,
+               py::arg("map"),
+               py::arg("filename"));
+}
+
 } // namespace contatcs
 } // namespace bindings
 } // namespace BipedalLocomotion

bindings/python/Contacts/src/Module.cpp

@@ -25,6 +25,7 @@ void CreateModule(pybind11::module& module)
     CreateContactList(module);
     CreateContactPhase(module);
     CreateContactPhaseList(module);
+    CreateContactListJsonParser(module);
 
     CreateContactDetector(module);
     CreateSchmittTriggerUnit(module);

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -69,6 +69,10 @@ find_package(realsense2 QUIET)
 checkandset_dependency(realsense2)
 dependency_classifier(realsense2 IS_USED ${FRAMEWORK_USE_realsense2} PUBLIC)
 
+find_package(nlohmann_json 3.7.3 QUIET)
+checkandset_dependency(nlohmann_json)
+dependency_classifier(nlohmann_json MINIMUM_VERSION 3.7.3 IS_USED ${FRAMEWORK_USE_nlohmann_json})
+
 find_package(Catch2 QUIET)
 checkandset_dependency(Catch2)
 
@@ -102,7 +106,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_Estimators
 
 framework_dependent_option(FRAMEWORK_COMPILE_Contact
   "Compile Contact libraries?" ON
-  "FRAMEWORK_USE_manif" OFF)
+  "FRAMEWORK_USE_manif;FRAMEWORK_USE_nlohmann_json" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_Planners
   "Compile Planners libraries?" ON

src/Contacts/CMakeLists.txt

@@ -8,9 +8,10 @@ if (FRAMEWORK_COMPILE_Contact)
   add_bipedal_locomotion_library(
     NAME                  Contacts
     SUBDIRECTORIES        tests/Contacts
-    PUBLIC_HEADERS        ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h
-    SOURCES               src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp
+    PUBLIC_HEADERS        ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ContactListJsonParser.h
+    SOURCES               src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ContactListJsonParser.cpp
     PUBLIC_LINK_LIBRARIES MANIF::manif BipedalLocomotion::Math BipedalLocomotion::TextLogging
+    PRIVATE_LINK_LIBRARIES nlohmann_json::nlohmann_json
     INSTALLATION_FOLDER   Contacts)
 
   set(H_PREFIX include/BipedalLocomotion/ContactDetectors)

src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h

@@ -14,10 +14,11 @@
 //iDynTree
 #include <manif/manif.h>
 
-//std
+// std
+#include <functional>
 #include <set>
 #include <string>
-#include <functional>
+#include <unordered_map>
 
 namespace BipedalLocomotion
 {
@@ -228,9 +229,13 @@ class ContactList
 
 };
 
-} //namespace Contacts
-} //namespace BipedalLocomotion
-
+/**
+ * @brief Utility alias to a map of ContacLists.
+ * @note The key of the map is a user define label representing the contact name.
+ */
+using ContactListMap = std::unordered_map<std::string, ContactList>;
 
+} // namespace Contacts
+} // namespace BipedalLocomotion
 
 #endif // BIPEDAL_LOCOMOTION_CONTACTS_CONTACTLIST_H

src/Contacts/include/BipedalLocomotion/Contacts/ContactListJsonParser.h

@@ -0,0 +1,101 @@
+/**
+ * @file ContactListJsonParser.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 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.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_CONTACTS_CONTACTLIST_JSON_PARSER_H
+#define BIPEDAL_LOCOMOTION_CONTACTS_CONTACTLIST_JSON_PARSER_H
+
+#include <string>
+
+// BipedalLocomotion
+#include <BipedalLocomotion/Contacts/ContactList.h>
+
+namespace BipedalLocomotion
+{
+namespace Contacts
+{
+
+/**
+ * Parse a ContactListMap from a JSON file.
+ * @param filename the name of the file that should be loaded.
+ * @return return a ContactListMap containing all the contacts.
+ * @note The JSON file should have a structure similar to the following example.
+ * ```json
+ * {
+ *  "my_left_foot": [
+ *    {
+ *      "activation_time": 0.0,
+ *      "deactivation_time": 1.0,
+ *      "index": 42,
+ *      "name": "left_foot",
+ *      "position": [
+ *        0.5968800669521466,
+ *        0.8232947158735686,
+ *        -0.6048972614132321
+ *      ],
+ *      "quaternion": [
+ *        -0.3149241877569222,
+ *        -0.8968666065772184,
+ *        0.18925645870451685,
+ *        0.24624182993742819
+ *      ]
+ *    },
+ *    {
+ *      "activation_time": 2.0,
+ *      "deactivation_time": 5.0,
+ *      "index": 42,
+ *      "name": "left_foot",
+ *      "position": [
+ *        0.10793991159086103,
+ *        -0.0452058962756795,
+ *        0.2577418495238488
+ *      ],
+ *      "quaternion": [
+ *        0.09318476783085411,
+ *        0.5701901104695768,
+ *        -0.10052857125007243,
+ *        -0.8099961995771466
+ *      ]
+ *    }
+ *  ],
+ *  "my_right_foot": [
+ *    {
+ *      "activation_time": 0.0,
+ *      "deactivation_time": 3.0,
+ *      "index": 33,
+ *      "name": "right_foot",
+ *      "position": [
+ *        0.8323901360074013,
+ *        0.2714234559198019,
+ *        0.43459385886536617
+ *      ],
+ *      "quaternion": [
+ *        -0.794180500610814,
+ *        -0.17857274481750596,
+ *        0.5769716176729885,
+ *        -0.06702879722711332
+ *      ]
+ *    }
+ *  ]
+ *}
+ * ```
+ * @note The structure of each step replicates the attributes of PlannedContact
+ */
+ContactListMap contactListMapFromJson(const std::string& filename);
+
+/**
+ * Store a ContactListMap to a JSON file.
+ * @param map a ContactListMap containing the list of the contacts.
+ * @param filename the name of the file that should be loaded.
+ * @return return true in case of success, failure otherwise.
+ * @note The function generates a JSON file readable by contactListMapFromJson()
+ */
+bool contactListMapToJson(const ContactListMap& map, const std::string& filename);
+
+} // namespace Contacts
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_CONTACTS_CONTACTLIST_JSON_PARSER_H

src/Contacts/include/BipedalLocomotion/Contacts/ContactPhaseList.h

@@ -12,18 +12,13 @@
 #include <BipedalLocomotion/Contacts/ContactList.h>
 #include <BipedalLocomotion/Contacts/ContactPhase.h>
 
-#include <unordered_map>
 #include <initializer_list>
 #include <vector>
 
 namespace BipedalLocomotion
 {
 namespace Contacts
 {
-/**
- * @brief Utility alias to a map of ContacLists.
- */
-using ContactListMap = std::unordered_map<std::string, ContactList>;
 
 /**
  * @brief The ContactPhaseList class computes the contact phases according to a bunch of input contact lists.