Add import and export of iDynTree::Model from custom iDynTree format idyntree-model-json

cmake/iDynTreeDependencies.cmake

@@ -96,6 +96,7 @@ idyntree_handle_dependency(WORHP DO_NOT_SILENTLY_SEARCH)
 # NO_MODULE passed to avoid that the Findassimp of YCM is used instead, https://github.com/robotology/idyntree/pull/832
 idyntree_handle_dependency(assimp DO_NOT_SILENTLY_SEARCH NO_MODULE MAIN_TARGET assimp::assimp)
 idyntree_handle_dependency(sdformat DO_NOT_SILENTLY_SEARCH MAIN_TARGET sdformat::sdformat)
+idyntree_handle_dependency(nlohmann_json)
 # Workaround for https://github.com/robotology/idyntree/issues/693
 if(TARGET assimp::assimp)
   get_property(assimp_INTERFACE_INCLUDE_DIRECTORIES

doc/build-from-source.md

@@ -18,14 +18,15 @@ In case they are disabled, tipically some functionality of iDynTree is not provi
 | [glfw](https://www.glfw.org/) | No | `IDYNTREE_USES_IRRLICHT` | ✔️ | ✔️ |
 | [osqp-eigen](https://github.com/robotology/osqp-eigen) | No | `IDYNTREE_USES_OSQPEIGEN` | ✔️ | ✔️ |
 | [sdformat](http://sdformat.org/) | No | `IDYNTREE_USES_SDFORMAT` | ✔️ | ❌ |
+| [nlohmann_json](https://github.com/nlohmann/json) | No | `IDYNTREE_USES_NLOHMANN_JSON` | ✔️ | ❌ |
 | [meshcat-cpp](https://github.com/ami-iit/meshcat-cpp) | No | `IDYNTREE_USES_MESHCATCPP` | ❌ | ❌ |
 
 
 ### Install dependencies with conda-forge
 
 If you are using conda, the dependencies of iDynTree can be installed with:
 ~~~
-conda install -c conda-forge cmake compilers make ninja pkg-config eigen libxml2-devel assimp ipopt irrlicht osqp-eigen swig python glfw libsdformat>=16.0
+conda install -c conda-forge cmake compilers make ninja pkg-config eigen libxml2-devel assimp ipopt irrlicht osqp-eigen swig python glfw libsdformat>=16.0 nlohmann_json
 ~~~
 
 ### Install dependencies with apt

doc/model_loading.md

@@ -1,9 +1,41 @@
 # How to load Models in iDynTree
 
-Most of the classes and function related to Model loading/unloading are documented in the [`iDynTree ModelIO`](http://wiki.icub.org/codyco/dox/html/idyntree/html/group__iDynTreeModelIO.html) doxygen module.
+The main two classes related to model import and export in iDynTree are:
+
+* [`iDynTree::ModelLoader`](https://gbionics.github.io/idyntree/classiDynTree_1_1ModelLoader.html): to convert models from external formats to `iDynTree::Model`
+* [`iDynTree::ModelExporter`](https://gbionics.github.io/idyntree/classiDynTree_1_1ModelExporter.html): to export `iDynTree::Model` to external formats
+
 In this document we will discuss the particular aspects of each file format supported by iDynTree for loading multibody systems information.
 
+iDynTree also provides the `idyntree-model-convert` command-line tool to convert models across supported formats.
+
+Basic usage:
+
+```bash
+idyntree-model-convert -i input_model.urdf -o output_model.json
+```
+
+You can optionally force input/output formats (for example when file extensions are ambiguous):
+
+```bash
+idyntree-model-convert \
+  -i input_model \
+  -o output_model \
+  --input-format urdf \
+  --output-format idyntree-model-json
+```
+
+Currently, input formats are `urdf`, `sdf`, and `idyntree-model-json`, while output formats are `urdf` and `idyntree-model-json`.
+
+
+| Format | Section |
+| --- | --- |
+| `urdf` | [URDF models](#urdf-models) |
+| `sdf` | [SDFormat models](#sdformat-models) |
+| `idyntree-model-json` | [iDynTree model JSON](#idyntree-model-json) |
+
 ## URDF models
+
 The main format used by iDynTree to load multibody models is the [URDF format](http://wiki.ros.org/urdf), originally developed in the ROS project.
 
 iDynTree follows the [URDF specification](http://wiki.ros.org/urdf/XML/model) as much as possible.
@@ -105,7 +137,7 @@ iDynTree also supports loading robot models from the [SDFormat (Simulation Descr
 
 ### Usage
 
-The `ModelLoader` class automatically detects SDFormat files based on file extension (`.sdf` or `.world`):
+The `ModelLoader` class automatically detects SDFormat files based on file extension (`.sdf` or `.world`), using the canonical format name `sdf`:
 
 ```cpp
 iDynTree::ModelLoader loader;
@@ -213,3 +245,35 @@ The following SDFormat features are not yet supported:
 - Additional joint types (ball, universal, screw)
 - Nested model hierarchies
 - Visual and material properties (colors, textures)
+
+## iDynTree model JSON
+
+iDynTree also supports loading and exporting models via a iDynTree-specific JSON format named `idyntree-model-json`.
+
+As this format is mainly meant for dumping the memory state of an `iDynTree::Model`, it is not meant to provide backward compatibility, so every change in the `.json` format will be indicated by a bump in the integer value the `idyntree_model_json_version` top-level json element, and so model exported in older versions of iDynTree may not be loaded in future versions of iDynTree.
+
+This support requires `nlohmann_json` and must be enabled at build time with
+the `IDYNTREE_USES_NLOHMANN_JSON` CMake option.
+
+### Usage
+
+For files, `ModelLoader` auto-detects this format from the `.json` extension:
+
+```cpp
+iDynTree::ModelLoader loader;
+bool ok = loader.loadModelFromFile("robot.json"); // Auto-detected as idyntree-model-json
+```
+
+You can also explicitly specify the format:
+
+```cpp
+bool ok = loader.loadModelFromFile("robot.json", "idyntree-model-json");
+```
+
+For export:
+
+```cpp
+iDynTree::ModelExporter exporter;
+exporter.init(model);
+bool ok = exporter.exportModelToFile("robot.json", "idyntree-model-json");
+```

pixi.toml

@@ -62,6 +62,7 @@ numpy = "*"
 yarp = "*"
 osqp-eigen = "*"
 libsdformat = ">=16.0"
+nlohmann_json = "*"
 clang-format = ">=21.1.5"
 parallel = "*"
 # Find a way to add this only for supported envs

src/model_io/codecs/CMakeLists.txt

@@ -8,6 +8,7 @@ set(IDYNTREE_MODELIO_HEADERS include/iDynTree/URDFDofsImport.h
 
 set(IDYNTREE_MODELIO_PRIVATE_HEADERS include/private/URDFDocument.h
                                           include/private/SDFormatDocument.h
+                                          include/private/ModelIOFormatUtils.h
                                           include/private/InertialElement.h
                                           include/private/JointElement.h
                                           include/private/LinkElement.h
@@ -40,6 +41,11 @@ set(IDYNTREE_MODELIO_SOURCES src/URDFDofsImport.cpp
                                   src/ModelCalibrationHelper.cpp
                                   src/URDFModelExport.cpp)
 
+if(IDYNTREE_USES_NLOHMANN_JSON)
+    list(APPEND IDYNTREE_MODELIO_SOURCES src/ModelJSONImportExport.cpp)
+    list(APPEND IDYNTREE_MODELIO_PRIVATE_HEADERS include/private/ModelJSONImportExport.h)
+endif()
+
 list(APPEND IDYNTREE_MODELIO_SOURCES ${IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES})
 
 set(libraryname idyntree-modelio)
@@ -62,6 +68,12 @@ if(IDYNTREE_USES_SDFORMAT)
     target_compile_definitions(${libraryname} PRIVATE IDYNTREE_USES_SDFORMAT)
 endif()
 
+# Add nlohmann_json support if available
+if(IDYNTREE_USES_NLOHMANN_JSON)
+    target_link_libraries(${libraryname} PRIVATE nlohmann_json::nlohmann_json)
+    target_compile_definitions(${libraryname} PRIVATE IDYNTREE_USES_NLOHMANN_JSON)
+endif()
+
 # See https://stackoverflow.com/questions/38832528/transitive-target-include-directories-on-object-libraries
 # Can be removed with CMake 3.12
 target_include_directories(${libraryname} PRIVATE $<TARGET_PROPERTY:idyntree-private-fpconv,INTERFACE_INCLUDE_DIRECTORIES>)

src/model_io/codecs/include/iDynTree/ModelExporter.h

@@ -108,8 +108,9 @@ struct ModelExporterOptions
  *
  * Helper class to export a model to the supported textual formats.
  *
- * Currently the only format supported for export is the URDF format,
- * as it is described in http://wiki.ros.org/urdf/XML .
+ * Supported formats for export are:
+ * - URDF, as described in http://wiki.ros.org/urdf/XML
+ * - iDynTree native JSON (`idyntree-model-json`)
  *
  * Only iDynTree::Model classes that represent multibody system with no loops
  * can be exported.
@@ -127,6 +128,7 @@ struct ModelExporterOptions
  * | Format | Extendend Name | Website |  String for filetype argument  |
  * |:-----------------------------:|:-------:|:-------:|:--------:|
  * | URDF | Unified Robot Description Format  | http://wiki.ros.org/urdf | `urdf` |
+ * | idyntree-model-json | iDynTree native JSON format | N/A | `idyntree-model-json` |
  *
  * ## URDF
  *
@@ -223,7 +225,8 @@ class ModelExporter
      * Export the model of the robot to a string.
      *
      * @param modelString string containg the model of the robot.
-     * @param filetype type of the file to load, currently supporting only urdf type.
+     * @param filetype type of the model format to export.
+     *                 Supported values: `urdf`, `idyntree-model-json`.
      *
      */
     bool exportModelToString(std::string& modelString, const std::string filetype = "urdf");
@@ -233,8 +236,9 @@ class ModelExporter
      *
      * @param filename path to the file to export.
      *                 It can be either a relative filename with respect to the current working
-     * directory, or an absolute filename.
-     * @param filetype type of the file to load, currently supporting only urdf type.
+     *                 directory, or an absolute filename.
+     * @param filetype type of the model format to export.
+     *                 Supported values: `urdf`, `idyntree-model-json`.
      *
      */
     bool exportModelToFile(const std::string& filename, const std::string filetype = "urdf");

src/model_io/codecs/include/iDynTree/ModelLoader.h

@@ -125,16 +125,18 @@ class ModelLoader
      * Load the model of the robot  from a string.
      *
      * @param modelString string containg the model of the robot.
-     * @param filetype type of the file to load, currently supporting only urdf
-     * type
+     * @param filetype type of the model to load. Supported values are:
+     *                 `urdf`, `sdf`, `idyntree-model-json`.
+     *                 If empty, the format is auto-detected from content when possible.
      * @param packageDirs a vector containing the different directories where to
-     * search for model meshes
+     *                   search for model meshes
      * @note In case no package is specified ModelLoader will look for the meshes
-     * in `GZ_SIM_RESOURCE_PATH`, `GAZEBO_MODEL_PATH`, `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH`
+     *       in `GZ_SIM_RESOURCE_PATH`, `GAZEBO_MODEL_PATH`, `ROS_PACKAGE_PATH` and
+     *       `AMENT_PREFIX_PATH`
      * @note If a given model searches for the meshes in
-     * `package://StrangeModel/Nested/mesh.stl`, and the actual mesh is in
-     * `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` should
-     * contain `/usr/local/share`.
+     *       `package://StrangeModel/Nested/mesh.stl`, and the actual mesh is in
+     *       `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` should
+     *       contain `/usr/local/share`.
      */
     bool loadModelFromString(const std::string& modelString,
                              const std::string& filetype = "",
@@ -144,16 +146,20 @@ class ModelLoader
      * Load the model of the robot from an external file.
      *
      * @param filename path to the file to load
-     * @param filetype type of the file to load, currently supporting only urdf
-     * type.
+     * @param filetype type of the model to load. Supported values are:
+     *                 `urdf`, `sdf`, `idyntree-model-json`.
+     *                 If empty, the format is auto-detected from file extension
+     *                 (`.urdf` -> `urdf`, `.sdf`/`.world` -> `sdf`, `.json` ->
+     * `idyntree-model-json`).
      * @param packageDirs a vector containing the different directories where to
-     * search for model meshes
+     *                   search for model meshes
      * @note In case no package is specified ModelLoader will look for the meshes
-     * in `GZ_SIM_RESOURCE_PATH`, `GAZEBO_MODEL_PATH`, `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH`
+     *       in `GZ_SIM_RESOURCE_PATH`, `GAZEBO_MODEL_PATH`, `ROS_PACKAGE_PATH` and
+     *       `AMENT_PREFIX_PATH`
      * @note If a given model searches for the meshes in
-     * `package://StrangeModel/Nested/mesh.stl`, and the actual mesh is in
-     * `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` should
-     * contain `/usr/local/share`.
+     *       `package://StrangeModel/Nested/mesh.stl`, and the actual mesh is in
+     *       `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` should
+     *       contain `/usr/local/share`.
      */
     bool loadModelFromFile(const std::string& filename,
                            const std::string& filetype = "",

src/model_io/codecs/include/private/ModelIOFormatUtils.h

@@ -0,0 +1,67 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#ifndef IDYNTREE_MODEL_IO_FORMAT_UTILS_H
+#define IDYNTREE_MODEL_IO_FORMAT_UTILS_H
+
+#include <algorithm>
+#include <cctype>
+#include <string>
+
+namespace iDynTree
+{
+
+inline std::string modelFormatToLower(std::string value)
+{
+    std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) {
+        return static_cast<char>(std::tolower(c));
+    });
+    return value;
+}
+
+inline std::string normalizeModelFormatName(const std::string& format)
+{
+    return modelFormatToLower(format);
+}
+
+inline std::string inferModelFormatFromFilename(const std::string& filePath)
+{
+    const size_t dotPos = filePath.rfind('.');
+    if (dotPos == std::string::npos)
+    {
+        return "";
+    }
+
+    const std::string extension = modelFormatToLower(filePath.substr(dotPos + 1));
+
+    if (extension == "urdf")
+    {
+        return "urdf";
+    }
+
+    if (extension == "sdf" || extension == "world")
+    {
+        return "sdf";
+    }
+
+    if (extension == "json")
+    {
+        return "idyntree-model-json";
+    }
+
+    return "";
+}
+
+inline bool isSupportedImportModelFormat(const std::string& format)
+{
+    return format == "urdf" || format == "sdf" || format == "idyntree-model-json";
+}
+
+inline bool isSupportedExportModelFormat(const std::string& format)
+{
+    return format == "urdf" || format == "idyntree-model-json";
+}
+
+} // namespace iDynTree
+
+#endif // IDYNTREE_MODEL_IO_FORMAT_UTILS_H