diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake index 281615295f..0ee7bd808f 100644 --- a/cmake/iDynTreeDependencies.cmake +++ b/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 diff --git a/doc/build-from-source.md b/doc/build-from-source.md index 9a45cfe4ca..4678cdda8b 100644 --- a/doc/build-from-source.md +++ b/doc/build-from-source.md @@ -18,6 +18,7 @@ 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` | ❌ | ❌ | @@ -25,7 +26,7 @@ In case they are disabled, tipically some functionality of iDynTree is not provi 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 diff --git a/doc/model_loading.md b/doc/model_loading.md index ac0c4723d3..ed11dd5220 100644 --- a/doc/model_loading.md +++ b/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"); +``` diff --git a/pixi.lock b/pixi.lock index d505b23097..a388ce12b0 100644 --- a/pixi.lock +++ b/pixi.lock @@ -3,6 +3,8 @@ environments: default: channels: - url: https://conda.anaconda.org/conda-forge/ + options: + pypi-prerelease-mode: if-necessary-or-explicit packages: linux-64: - conda: https://conda.anaconda.org/conda-forge/linux-64/_libgcc_mutex-0.1-conda_forge.tar.bz2 @@ -1224,6 +1226,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/win-64/mkl-2025.3.0-hac47afa_454.conda - conda: https://conda.anaconda.org/conda-forge/win-64/mumps-seq-5.8.1-hd297af6_4.conda - conda: https://conda.anaconda.org/conda-forge/win-64/ninja-1.13.1-h477610d_0.conda + - conda: https://conda.anaconda.org/conda-forge/win-64/nlohmann_json-3.12.0-h5112557_1.conda - conda: https://conda.anaconda.org/conda-forge/win-64/numpy-1.21.6-py37h2830a78_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/win-64/openssl-3.6.0-h725018a_0.conda - conda: https://conda.anaconda.org/conda-forge/win-64/osqp-eigen-0.11.0-h5205572_0.conda @@ -12308,6 +12311,15 @@ packages: license_family: MIT size: 136912 timestamp: 1758194464430 +- conda: https://conda.anaconda.org/conda-forge/win-64/nlohmann_json-3.12.0-h5112557_1.conda + sha256: 045edd5d571c235de67472ad8fe03d9706b8426c4ba9a73f408f946034b6bc5e + md5: 24a9dde77833cc48289ef92b4e724da4 + constrains: + - nlohmann_json-abi ==3.12.0 + license: MIT + license_family: MIT + size: 134870 + timestamp: 1758194302226 - conda: https://conda.anaconda.org/conda-forge/linux-64/nspr-4.38-h29cc59b_0.conda sha256: e3664264bd936c357523b55c71ed5a30263c6ba278d726a75b1eb112e6fb0b64 md5: e235d5566c9cc8970eb2798dd4ecf62f diff --git a/pixi.toml b/pixi.toml index 4a3a6bb3c6..5d2b07668c 100644 --- a/pixi.toml +++ b/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 diff --git a/src/model_io/codecs/CMakeLists.txt b/src/model_io/codecs/CMakeLists.txt index a584b80830..5ae486177f 100644 --- a/src/model_io/codecs/CMakeLists.txt +++ b/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 $) diff --git a/src/model_io/codecs/include/iDynTree/ModelExporter.h b/src/model_io/codecs/include/iDynTree/ModelExporter.h index 22b0b24ed1..2bcfd142d6 100644 --- a/src/model_io/codecs/include/iDynTree/ModelExporter.h +++ b/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"); diff --git a/src/model_io/codecs/include/iDynTree/ModelLoader.h b/src/model_io/codecs/include/iDynTree/ModelLoader.h index 374ca7f44c..0846d4c7f8 100644 --- a/src/model_io/codecs/include/iDynTree/ModelLoader.h +++ b/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 = "", diff --git a/src/model_io/codecs/include/private/ModelIOFormatUtils.h b/src/model_io/codecs/include/private/ModelIOFormatUtils.h new file mode 100644 index 0000000000..da37e8d4b6 --- /dev/null +++ b/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 +#include +#include + +namespace iDynTree +{ + +inline std::string modelFormatToLower(std::string value) +{ + std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { + return static_cast(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 diff --git a/src/model_io/codecs/include/private/ModelJSONImportExport.h b/src/model_io/codecs/include/private/ModelJSONImportExport.h new file mode 100644 index 0000000000..5eb7a691b4 --- /dev/null +++ b/src/model_io/codecs/include/private/ModelJSONImportExport.h @@ -0,0 +1,218 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_JSON_IMPORT_EXPORT_H +#define IDYNTREE_MODEL_JSON_IMPORT_EXPORT_H + +#include + +#include + +namespace iDynTree +{ + +/** + * @brief Current version of the idyntree-model-json format. + * + * This integer is incremented whenever the format changes in a way that breaks + * backward or forward compatibility. + * + * **No backward compatibility is expected across different versions of the format.** + * Each iDynTree release documents explicitly which version(s) it supports. + * + * Supported version: 1 + * + * --- + * **idyntree-model-json format schema (version 1)** + * + * Top-level object: + * ```json + * { + * "idyntree_model_json_version": , + * "default_base_link": , + * "links": [ , ... ], + * "joints": [ , ... ], + * "additional_frames": [ , ... ], + * "visual_solid_shapes": [ , ... ], + * "collision_solid_shapes": [ , ... ], + * "sensors": [ , ... ] + * } + * ``` + * + * Link object: + * ```json + * { + * "name": , + * "inertia": { + * "mass": , + * "center_of_mass": [x, y, z], + * "rotational_inertia_wrt_frame_origin": [[r00,r01,r02],[r10,r11,r12],[r20,r21,r22]] + * } + * } + * ``` + * + * Joint object (common fields): + * ```json + * { + * "name": , + * "type": "fixed"|"revolute"|"prismatic"|"revolute_so2"|"spherical", + * "parent_link": , // getFirstAttachedLink() + * "child_link": , // getSecondAttachedLink() + * "rest_transform": // link1_X_link2 at rest position + * } + * ``` + * Plus type-specific fields for non-fixed joints (see below). + * + * Transform object: + * ```json + * { + * "position": [x, y, z], + * "rotation_matrix": [[r00,r01,r02],[r10,r11,r12],[r20,r21,r22]] + * } + * ``` + * + * Axis object: + * ```json + * { + * "direction": [x, y, z], + * "origin": [x, y, z] + * } + * ``` + * + * Limits object (1-DOF joints): + * ```json + * { + * "has_position_limits": , + * "min_position": , // present only if has_position_limits == true + * "max_position": , // present only if has_position_limits == true + * "has_effort_limits": , + * "effort_limit": , // present only if has_effort_limits == true + * "has_velocity_limits": , + * "velocity_limit": // present only if has_velocity_limits == true + * } + * ``` + * + * Dynamics object (1-DOF joints): + * ```json + * { + * "type": "NoJointDynamics"|"URDFJointDynamics", + * "damping": , + * "static_friction": + * } + * ``` + * + * Revolute / Prismatic / RevoluteSO2 extra fields: + * ```json + * { + * "axis": , + * "limits": , + * "dynamics": + * } + * ``` + * + * Spherical extra fields: + * ```json + * { + * "joint_center_wrt_parent": [x, y, z], + * "limits": { + * "has_position_limits": , + * "min_position": [min0, min1, min2], // present only if has_position_limits == true + * "max_position": [max0, max1, max2], // present only if has_position_limits == true + * "has_effort_limits": , + * "effort_limit": [e0, e1, e2], // present only if has_effort_limits == true + * "has_velocity_limits": , + * "velocity_limit": [v0, v1, v2] // present only if has_velocity_limits == true + * }, + * "dynamics": { + * "type": "NoJointDynamics"|"URDFJointDynamics", + * "damping": [d0, d1, d2], + * "static_friction": [sf0, sf1, sf2] + * } + * } + * ``` + * + * Additional frame object: + * ```json + * { + * "name": , + * "parent_link": , + * "link_H_frame": + * } + * ``` + * + * Link shapes object: + * ```json + * { + * "link_name": , + * "shapes": [ , ... ] + * } + * ``` + * + * Shape object: + * ```json + * { + * "type": "sphere"|"box"|"cylinder"|"external_mesh", + * "has_name": , + * "name": , // present only if has_name == true + * "link_H_geometry": , + * "has_material": , + * "material": , // present only if has_material == true + * ... type-specific fields ... + * } + * ``` + * + * For "external_mesh" shapes, only the mesh filename is serialized. The + * package_dirs information is intentionally omitted to keep the JSON artifact + * self-contained and independent from local package search paths. + * + * Sensor object: + * ```json + * { + * "name": , + * "type": "six_axis_force_torque"|"accelerometer"|"gyroscope"| + * "three_axis_angular_accelerometer"|"three_axis_force_torque_contact", + * ... type-specific fields ... + * } + * ``` + */ +static constexpr int IDYNTREE_MODEL_JSON_FORMAT_VERSION = 1; + +/** + * Export an iDynTree::Model to a JSON string in the idyntree-model-json format. + * + * @param model the model to export + * @param jsonString the output JSON string + * @return true on success, false on failure + */ +bool modelToJSONString(const Model& model, std::string& jsonString); + +/** + * Export an iDynTree::Model to a JSON file in the idyntree-model-json format. + * + * @param model the model to export + * @param filename the output file path + * @return true on success, false on failure + */ +bool modelToJSONFile(const Model& model, const std::string& filename); + +/** + * Load an iDynTree::Model from a JSON string in the idyntree-model-json format. + * + * @param jsonString the input JSON string + * @param model the output model + * @return true on success, false on failure + */ +bool modelFromJSONString(const std::string& jsonString, Model& model); + +/** + * Load an iDynTree::Model from a JSON file in the idyntree-model-json format. + * + * @param filename the input file path + * @param model the output model + * @return true on success, false on failure + */ +bool modelFromJSONFile(const std::string& filename, Model& model); + +} // namespace iDynTree + +#endif // IDYNTREE_MODEL_JSON_IMPORT_EXPORT_H diff --git a/src/model_io/codecs/src/ModelExporter.cpp b/src/model_io/codecs/src/ModelExporter.cpp index 15bd7faaf0..ceb350306c 100644 --- a/src/model_io/codecs/src/ModelExporter.cpp +++ b/src/model_io/codecs/src/ModelExporter.cpp @@ -3,8 +3,13 @@ #include "iDynTree/ModelExporter.h" +#include "ModelIOFormatUtils.h" #include "URDFModelExport.h" +#ifdef IDYNTREE_USES_NLOHMANN_JSON +#include "ModelJSONImportExport.h" +#endif + #include #include @@ -94,16 +99,32 @@ bool ModelExporter::init(const Model& model, bool ModelExporter::exportModelToString(std::string& modelString, const std::string filetype) { - if (filetype != "urdf") + const std::string normalizedFiletype = normalizeModelFormatName(filetype); + + if (normalizedFiletype == "urdf") + { + return URDFStringFromModel(m_pimpl->m_model, modelString, m_pimpl->m_options); + } + + if (normalizedFiletype == "idyntree-model-json") { - std::stringstream error_msg; - error_msg << "Filetype " << filetype - << " not supported. Only urdf format is currently supported."; - reportError("ModelExporter", "exportModelToString", error_msg.str().c_str()); +#ifdef IDYNTREE_USES_NLOHMANN_JSON + return modelToJSONString(m_pimpl->m_model, modelString); +#else + reportError("ModelExporter", + "exportModelToString", + "idyntree-model-json format requested but iDynTree was not compiled with " + "nlohmann_json support. " + "Please rebuild iDynTree with IDYNTREE_USES_NLOHMANN_JSON option enabled."); return false; +#endif } - return URDFStringFromModel(m_pimpl->m_model, modelString, m_pimpl->m_options); + std::stringstream error_msg; + error_msg << "Filetype " << filetype + << " not supported. Supported export formats: urdf, idyntree-model-json."; + reportError("ModelExporter", "exportModelToString", error_msg.str().c_str()); + return false; } /** @@ -115,16 +136,32 @@ bool ModelExporter::exportModelToString(std::string& modelString, const std::str */ bool ModelExporter::exportModelToFile(const std::string& filename, const std::string filetype) { - if (filetype != "urdf") + const std::string normalizedFiletype = normalizeModelFormatName(filetype); + + if (normalizedFiletype == "urdf") + { + return URDFFromModel(m_pimpl->m_model, filename, m_pimpl->m_options); + } + + if (normalizedFiletype == "idyntree-model-json") { - std::stringstream error_msg; - error_msg << "Filetype " << filetype - << " not supported. Only urdf format is currently supported."; - reportError("ModelExporter", "exportModelToFile", error_msg.str().c_str()); +#ifdef IDYNTREE_USES_NLOHMANN_JSON + return modelToJSONFile(m_pimpl->m_model, filename); +#else + reportError("ModelExporter", + "exportModelToFile", + "idyntree-model-json format requested but iDynTree was not compiled with " + "nlohmann_json support. " + "Please rebuild iDynTree with IDYNTREE_USES_NLOHMANN_JSON option enabled."); return false; +#endif } - return URDFFromModel(m_pimpl->m_model, filename, m_pimpl->m_options); + std::stringstream error_msg; + error_msg << "Filetype " << filetype + << " not supported. Supported export formats: urdf, idyntree-model-json."; + reportError("ModelExporter", "exportModelToFile", error_msg.str().c_str()); + return false; } } // namespace iDynTree diff --git a/src/model_io/codecs/src/ModelJSONImportExport.cpp b/src/model_io/codecs/src/ModelJSONImportExport.cpp new file mode 100644 index 0000000000..8d3b3567e0 --- /dev/null +++ b/src/model_io/codecs/src/ModelJSONImportExport.cpp @@ -0,0 +1,1042 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include "ModelJSONImportExport.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace iDynTree +{ + +// ─── serialization helpers ─────────────────────────────────────────────────── + +static nlohmann::json positionToJSON(const Position& pos) +{ + return nlohmann::json::array({pos(0), pos(1), pos(2)}); +} + +static Position positionFromJSON(const nlohmann::json& j) +{ + return Position(j.at(0).get(), j.at(1).get(), j.at(2).get()); +} + +static nlohmann::json directionToJSON(const Direction& dir) +{ + return nlohmann::json::array({dir(0), dir(1), dir(2)}); +} + +static Direction directionFromJSON(const nlohmann::json& j) +{ + return Direction(j.at(0).get(), j.at(1).get(), j.at(2).get()); +} + +static nlohmann::json rotation3x3ToJSON(const Rotation& rot) +{ + nlohmann::json m = nlohmann::json::array(); + for (int r = 0; r < 3; r++) + { + nlohmann::json row = nlohmann::json::array(); + for (int c = 0; c < 3; c++) + row.push_back(rot(r, c)); + m.push_back(row); + } + return m; +} + +static Rotation rotation3x3FromJSON(const nlohmann::json& j) +{ + Rotation rot; + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + rot(r, c) = j.at(r).at(c).get(); + return rot; +} + +static nlohmann::json transformToJSON(const Transform& T) +{ + return {{"position", positionToJSON(T.getPosition())}, + {"rotation_matrix", rotation3x3ToJSON(T.getRotation())}}; +} + +static Transform transformFromJSON(const nlohmann::json& j) +{ + return Transform(rotation3x3FromJSON(j.at("rotation_matrix")), + positionFromJSON(j.at("position"))); +} + +static nlohmann::json axisToJSON(const Axis& ax) +{ + return {{"direction", directionToJSON(ax.getDirection())}, + {"origin", positionToJSON(ax.getOrigin())}}; +} + +static Axis axisFromJSON(const nlohmann::json& j) +{ + return Axis(directionFromJSON(j.at("direction")), positionFromJSON(j.at("origin"))); +} + +static nlohmann::json vector3ToJSON(const Vector3& vec) +{ + return nlohmann::json::array({vec(0), vec(1), vec(2)}); +} + +static Vector3 vector3FromJSON(const nlohmann::json& j) +{ + Vector3 vec; + for (int i = 0; i < 3; ++i) + { + vec(i) = j.at(i).get(); + } + return vec; +} + +static nlohmann::json vector4ToJSON(const Vector4& vec) +{ + return nlohmann::json::array({vec(0), vec(1), vec(2), vec(3)}); +} + +static Vector4 vector4FromJSON(const nlohmann::json& j) +{ + Vector4 vec; + for (int i = 0; i < 4; ++i) + { + vec(i) = j.at(i).get(); + } + return vec; +} + +static nlohmann::json materialToJSON(const Material& material) +{ + nlohmann::json materialJSON; + materialJSON["name"] = material.name(); + materialJSON["has_color"] = material.hasColor(); + if (material.hasColor()) + { + materialJSON["color"] = vector4ToJSON(material.color()); + } + materialJSON["has_texture"] = material.hasTexture(); + if (material.hasTexture()) + { + materialJSON["texture"] = material.texture(); + } + return materialJSON; +} + +static Material materialFromJSON(const nlohmann::json& j) +{ + Material material(j.at("name").get()); + if (j.at("has_color").get()) + { + material.setColor(vector4FromJSON(j.at("color"))); + } + if (j.at("has_texture").get()) + { + material.setTexture(j.at("texture").get()); + } + return material; +} + +static nlohmann::json solidShapeToJSON(const SolidShape& shape) +{ + nlohmann::json shapeJSON; + shapeJSON["has_name"] = shape.isNameValid(); + if (shape.isNameValid()) + { + shapeJSON["name"] = shape.getName(); + } + shapeJSON["link_H_geometry"] = transformToJSON(shape.getLink_H_geometry()); + shapeJSON["has_material"] = shape.isMaterialSet(); + if (shape.isMaterialSet()) + { + shapeJSON["material"] = materialToJSON(shape.getMaterial()); + } + + if (shape.isSphere()) + { + shapeJSON["type"] = "sphere"; + shapeJSON["radius"] = shape.asSphere()->getRadius(); + } else if (shape.isBox()) + { + shapeJSON["type"] = "box"; + shapeJSON["size"] = nlohmann::json::array( + {shape.asBox()->getX(), shape.asBox()->getY(), shape.asBox()->getZ()}); + } else if (shape.isCylinder()) + { + shapeJSON["type"] = "cylinder"; + shapeJSON["length"] = shape.asCylinder()->getLength(); + shapeJSON["radius"] = shape.asCylinder()->getRadius(); + } else if (shape.isExternalMesh()) + { + shapeJSON["type"] = "external_mesh"; + shapeJSON["filename"] = shape.asExternalMesh()->getFilename(); + shapeJSON["scale"] = vector3ToJSON(shape.asExternalMesh()->getScale()); + } + + return shapeJSON; +} + +static bool addSolidShapeToModel(const nlohmann::json& shapeJSON, + ModelSolidShapes& modelShapes, + LinkIndex linkIndex) +{ + const std::string type = shapeJSON.at("type").get(); + + SolidShape* shape = nullptr; + if (type == "sphere") + { + Sphere sphere; + sphere.setRadius(shapeJSON.at("radius").get()); + shape = sphere.clone(); + } else if (type == "box") + { + Box box; + box.setX(shapeJSON.at("size").at(0).get()); + box.setY(shapeJSON.at("size").at(1).get()); + box.setZ(shapeJSON.at("size").at(2).get()); + shape = box.clone(); + } else if (type == "cylinder") + { + Cylinder cylinder; + cylinder.setLength(shapeJSON.at("length").get()); + cylinder.setRadius(shapeJSON.at("radius").get()); + shape = cylinder.clone(); + } else if (type == "external_mesh") + { + ExternalMesh mesh; + mesh.setFilename(shapeJSON.at("filename").get()); + mesh.setScale(vector3FromJSON(shapeJSON.at("scale"))); + shape = mesh.clone(); + } else + { + reportError("ModelJSONExport", "addSolidShapeToModel", "Unknown solid shape type."); + return false; + } + + shape->setLink_H_geometry(transformFromJSON(shapeJSON.at("link_H_geometry"))); + if (shapeJSON.at("has_name").get()) + { + shape->setName(shapeJSON.at("name").get()); + } + if (shapeJSON.at("has_material").get()) + { + shape->setMaterial(materialFromJSON(shapeJSON.at("material"))); + } + + modelShapes.addSingleLinkSolidShape(linkIndex, *shape); + delete shape; + return true; +} + +static nlohmann::json +modelSolidShapesToJSON(const Model& model, const ModelSolidShapes& modelShapes) +{ + nlohmann::json linkShapesJSON = nlohmann::json::array(); + const auto& allLinkShapes = modelShapes.getLinkSolidShapes(); + + for (LinkIndex linkIndex = 0; linkIndex < model.getNrOfLinks(); ++linkIndex) + { + nlohmann::json linkJSON; + linkJSON["link_name"] = model.getLinkName(linkIndex); + linkJSON["shapes"] = nlohmann::json::array(); + + for (const SolidShape* shape : allLinkShapes[linkIndex]) + { + if (shape == nullptr) + { + reportError("ModelJSONExport", + "modelSolidShapesToJSON", + "Encountered null solid shape pointer."); + return nlohmann::json(); + } + linkJSON["shapes"].push_back(solidShapeToJSON(*shape)); + } + + linkShapesJSON.push_back(linkJSON); + } + + return linkShapesJSON; +} + +static bool modelSolidShapesFromJSON(const nlohmann::json& linkShapesJSON, + Model& model, + ModelSolidShapes& modelShapes) +{ + for (const auto& linkJSON : linkShapesJSON) + { + const std::string linkName = linkJSON.at("link_name").get(); + const LinkIndex linkIndex = model.getLinkIndex(linkName); + if (linkIndex == LINK_INVALID_INDEX) + { + reportError("ModelJSONExport", + "modelSolidShapesFromJSON", + ("Unknown link for solid shapes: " + linkName).c_str()); + return false; + } + + for (const auto& shapeJSON : linkJSON.at("shapes")) + { + if (!addSolidShapeToModel(shapeJSON, modelShapes, linkIndex)) + { + return false; + } + } + } + + return true; +} + +static nlohmann::json sensorToJSON(const Sensor& sensor, const Model& model) +{ + nlohmann::json sensorJSON; + sensorJSON["name"] = sensor.getName(); + + switch (sensor.getSensorType()) + { + case SIX_AXIS_FORCE_TORQUE: { + const auto* ftSensor = dynamic_cast(&sensor); + sensorJSON["type"] = "six_axis_force_torque"; + sensorJSON["parent_joint"] = ftSensor->getParentJoint(); + sensorJSON["first_link"] = ftSensor->getFirstLinkName(); + sensorJSON["second_link"] = ftSensor->getSecondLinkName(); + sensorJSON["applied_wrench_link"] = model.getLinkName(ftSensor->getAppliedWrenchLink()); + + Transform firstLink_H_sensor; + Transform secondLink_H_sensor; + ftSensor->getLinkSensorTransform(ftSensor->getFirstLinkIndex(), firstLink_H_sensor); + ftSensor->getLinkSensorTransform(ftSensor->getSecondLinkIndex(), secondLink_H_sensor); + sensorJSON["first_link_H_sensor"] = transformToJSON(firstLink_H_sensor); + sensorJSON["second_link_H_sensor"] = transformToJSON(secondLink_H_sensor); + break; + } + case ACCELEROMETER: { + const auto* accelerometer = dynamic_cast(&sensor); + sensorJSON["type"] = "accelerometer"; + sensorJSON["parent_link"] = accelerometer->getParentLink(); + sensorJSON["link_H_sensor"] = transformToJSON(accelerometer->getLinkSensorTransform()); + break; + } + case GYROSCOPE: { + const auto* gyroscope = dynamic_cast(&sensor); + sensorJSON["type"] = "gyroscope"; + sensorJSON["parent_link"] = gyroscope->getParentLink(); + sensorJSON["link_H_sensor"] = transformToJSON(gyroscope->getLinkSensorTransform()); + break; + } + case THREE_AXIS_ANGULAR_ACCELEROMETER: { + const auto* angularAccelerometer + = dynamic_cast(&sensor); + sensorJSON["type"] = "three_axis_angular_accelerometer"; + sensorJSON["parent_link"] = angularAccelerometer->getParentLink(); + sensorJSON["link_H_sensor"] + = transformToJSON(angularAccelerometer->getLinkSensorTransform()); + break; + } + case THREE_AXIS_FORCE_TORQUE_CONTACT: { + const auto* contactSensor = dynamic_cast(&sensor); + sensorJSON["type"] = "three_axis_force_torque_contact"; + sensorJSON["parent_link"] = contactSensor->getParentLink(); + sensorJSON["link_H_sensor"] = transformToJSON(contactSensor->getLinkSensorTransform()); + sensorJSON["load_cell_locations"] = nlohmann::json::array(); + for (const Position& loadCellLocation : contactSensor->getLoadCellLocations()) + { + sensorJSON["load_cell_locations"].push_back(positionToJSON(loadCellLocation)); + } + break; + } + default: + reportError("ModelJSONExport", "sensorToJSON", "Unknown sensor type."); + return nlohmann::json(); + } + + return sensorJSON; +} + +static bool addSensorToModel(const nlohmann::json& sensorJSON, Model& model) +{ + const std::string sensorType = sensorJSON.at("type").get(); + + if (sensorType == "six_axis_force_torque") + { + const std::string parentJointName = sensorJSON.at("parent_joint").get(); + const std::string firstLinkName = sensorJSON.at("first_link").get(); + const std::string secondLinkName = sensorJSON.at("second_link").get(); + const std::string appliedWrenchLinkName + = sensorJSON.at("applied_wrench_link").get(); + + const JointIndex parentJointIndex = model.getJointIndex(parentJointName); + const LinkIndex firstLinkIndex = model.getLinkIndex(firstLinkName); + const LinkIndex secondLinkIndex = model.getLinkIndex(secondLinkName); + const LinkIndex appliedWrenchLinkIndex = model.getLinkIndex(appliedWrenchLinkName); + if (parentJointIndex == JOINT_INVALID_INDEX || firstLinkIndex == LINK_INVALID_INDEX + || secondLinkIndex == LINK_INVALID_INDEX + || appliedWrenchLinkIndex == LINK_INVALID_INDEX) + { + reportError("ModelJSONExport", + "addSensorToModel", + "Failed to resolve joint or link referenced by six axis force torque " + "sensor."); + return false; + } + + SixAxisForceTorqueSensor sensor; + sensor.setName(sensorJSON.at("name").get()); + sensor.setParentJoint(parentJointName); + sensor.setParentJointIndex(parentJointIndex); + sensor.setFirstLinkName(firstLinkName); + sensor.setSecondLinkName(secondLinkName); + sensor.setFirstLinkSensorTransform(firstLinkIndex, + transformFromJSON(sensorJSON.at("first_link_H_sensor"))); + sensor.setSecondLinkSensorTransform(secondLinkIndex, + transformFromJSON(sensorJSON.at("second_link_H_" + "sensor"))); + sensor.setAppliedWrenchLink(appliedWrenchLinkIndex); + return model.sensors().addSensor(sensor) >= 0; + } + + const std::string parentLinkName = sensorJSON.at("parent_link").get(); + const LinkIndex parentLinkIndex = model.getLinkIndex(parentLinkName); + if (parentLinkIndex == LINK_INVALID_INDEX) + { + reportError("ModelJSONExport", + "addSensorToModel", + ("Unknown parent link referenced by sensor: " + parentLinkName).c_str()); + return false; + } + + if (sensorType == "accelerometer") + { + AccelerometerSensor sensor; + sensor.setName(sensorJSON.at("name").get()); + sensor.setParentLink(parentLinkName); + sensor.setParentLinkIndex(parentLinkIndex); + sensor.setLinkSensorTransform(transformFromJSON(sensorJSON.at("link_H_sensor"))); + return model.sensors().addSensor(sensor) >= 0; + } + if (sensorType == "gyroscope") + { + GyroscopeSensor sensor; + sensor.setName(sensorJSON.at("name").get()); + sensor.setParentLink(parentLinkName); + sensor.setParentLinkIndex(parentLinkIndex); + sensor.setLinkSensorTransform(transformFromJSON(sensorJSON.at("link_H_sensor"))); + return model.sensors().addSensor(sensor) >= 0; + } + if (sensorType == "three_axis_angular_accelerometer") + { + ThreeAxisAngularAccelerometerSensor sensor; + sensor.setName(sensorJSON.at("name").get()); + sensor.setParentLink(parentLinkName); + sensor.setParentLinkIndex(parentLinkIndex); + sensor.setLinkSensorTransform(transformFromJSON(sensorJSON.at("link_H_sensor"))); + return model.sensors().addSensor(sensor) >= 0; + } + if (sensorType == "three_axis_force_torque_contact") + { + ThreeAxisForceTorqueContactSensor sensor; + sensor.setName(sensorJSON.at("name").get()); + sensor.setParentLink(parentLinkName); + sensor.setParentLinkIndex(parentLinkIndex); + sensor.setLinkSensorTransform(transformFromJSON(sensorJSON.at("link_H_sensor"))); + + std::vector loadCellLocations; + for (const auto& loadCellLocationJSON : sensorJSON.at("load_cell_locations")) + { + loadCellLocations.push_back(positionFromJSON(loadCellLocationJSON)); + } + sensor.setLoadCellLocations(loadCellLocations); + return model.sensors().addSensor(sensor) >= 0; + } + + reportError("ModelJSONExport", "addSensorToModel", "Unknown sensor type."); + return false; +} + +// Limits for 1-DOF joints +template static nlohmann::json oneDOFLimitsToJSON(const JointT& joint) +{ + nlohmann::json lim; + lim["has_position_limits"] = joint.hasPosLimits(); + if (joint.hasPosLimits()) + { + lim["min_position"] = joint.getMinPosLimit(0); + lim["max_position"] = joint.getMaxPosLimit(0); + } + lim["has_effort_limits"] = joint.hasEffortLimits(); + if (joint.hasEffortLimits()) + lim["effort_limit"] = joint.getEffortLimit(0); + lim["has_velocity_limits"] = joint.hasVelocityLimits(); + if (joint.hasVelocityLimits()) + lim["velocity_limit"] = joint.getVelocityLimit(0); + return lim; +} + +// Dynamics for 1-DOF joints +template static nlohmann::json oneDOFDynamicsToJSON(const JointT& joint) +{ + nlohmann::json dyn; + dyn["type"] = (joint.getJointDynamicsType() == URDFJointDynamics) ? "URDFJointDynamics" + : "NoJointDynamics"; + dyn["damping"] = joint.getDamping(0); + dyn["static_friction"] = joint.getStaticFriction(0); + return dyn; +} + +// Limits for SphericalJoint (3-DOF, per-joint booleans / per-DOF values) +static nlohmann::json sphericalLimitsToJSON(const SphericalJoint& joint) +{ + nlohmann::json lim; + lim["has_position_limits"] = joint.hasPosLimits(); + if (joint.hasPosLimits()) + { + nlohmann::json minArr = nlohmann::json::array(); + nlohmann::json maxArr = nlohmann::json::array(); + for (size_t i = 0; i < 3; i++) + { + minArr.push_back(joint.getMinPosLimit(i)); + maxArr.push_back(joint.getMaxPosLimit(i)); + } + lim["min_position"] = minArr; + lim["max_position"] = maxArr; + } + lim["has_effort_limits"] = joint.hasEffortLimits(); + if (joint.hasEffortLimits()) + { + nlohmann::json effortArr = nlohmann::json::array(); + for (size_t i = 0; i < 3; i++) + effortArr.push_back(joint.getEffortLimit(i)); + lim["effort_limit"] = effortArr; + } + lim["has_velocity_limits"] = joint.hasVelocityLimits(); + if (joint.hasVelocityLimits()) + { + nlohmann::json velArr = nlohmann::json::array(); + for (size_t i = 0; i < 3; i++) + velArr.push_back(joint.getVelocityLimit(i)); + lim["velocity_limit"] = velArr; + } + return lim; +} + +// Dynamics for SphericalJoint (per-DOF damping/friction) +static nlohmann::json sphericalDynamicsToJSON(const SphericalJoint& joint) +{ + nlohmann::json dyn; + dyn["type"] = (joint.getJointDynamicsType() == URDFJointDynamics) ? "URDFJointDynamics" + : "NoJointDynamics"; + nlohmann::json dampingArr = nlohmann::json::array(); + nlohmann::json frictionArr = nlohmann::json::array(); + for (size_t i = 0; i < 3; i++) + { + dampingArr.push_back(joint.getDamping(i)); + frictionArr.push_back(joint.getStaticFriction(i)); + } + dyn["damping"] = dampingArr; + dyn["static_friction"] = frictionArr; + return dyn; +} + +// ─── export ────────────────────────────────────────────────────────────────── + +bool modelToJSONString(const Model& model, std::string& jsonString) +{ + nlohmann::json root; + root["idyntree_model_json_version"] = IDYNTREE_MODEL_JSON_FORMAT_VERSION; + root["default_base_link"] = model.getLinkName(model.getDefaultBaseLink()); + + // Links + nlohmann::json linksJSON = nlohmann::json::array(); + for (LinkIndex lnkIdx = 0; lnkIdx < model.getNrOfLinks(); lnkIdx++) + { + const Link* link = model.getLink(lnkIdx); + const SpatialInertia& inertia = link->getInertia(); + + nlohmann::json inertiaJSON; + inertiaJSON["mass"] = inertia.getMass(); + inertiaJSON["center_of_mass"] = positionToJSON(inertia.getCenterOfMass()); + + // Store raw rotational inertia wrt frame origin (what is directly stored internally) + const RotationalInertia& rotI = inertia.getRotationalInertiaWrtFrameOrigin(); + nlohmann::json rotIJSON = nlohmann::json::array(); + for (int r = 0; r < 3; r++) + { + nlohmann::json row = nlohmann::json::array(); + for (int c = 0; c < 3; c++) + row.push_back(rotI(r, c)); + rotIJSON.push_back(row); + } + inertiaJSON["rotational_inertia_wrt_frame_origin"] = rotIJSON; + + nlohmann::json linkJSON; + linkJSON["name"] = model.getLinkName(lnkIdx); + linkJSON["inertia"] = inertiaJSON; + linksJSON.push_back(linkJSON); + } + root["links"] = linksJSON; + + // Joints + nlohmann::json jointsJSON = nlohmann::json::array(); + for (JointIndex jntIdx = 0; jntIdx < model.getNrOfJoints(); jntIdx++) + { + const IJointConstPtr joint = model.getJoint(jntIdx); + const LinkIndex link1 = joint->getFirstAttachedLink(); + const LinkIndex link2 = joint->getSecondAttachedLink(); + + nlohmann::json jointJSON; + jointJSON["name"] = model.getJointName(jntIdx); + jointJSON["parent_link"] = model.getLinkName(link1); + jointJSON["child_link"] = model.getLinkName(link2); + // rest_transform = link1_X_link2: getRestTransform(child=link1, parent=link2) + // means p_link1 = T * p_link2, i.e. link1_X_link2 + jointJSON["rest_transform"] = transformToJSON(joint->getRestTransform(link1, link2)); + + const auto* fixedJoint = dynamic_cast(joint); + const auto* revoluteJoint = dynamic_cast(joint); + const auto* prismaticJoint = dynamic_cast(joint); + const auto* revoluteSO2Joint = dynamic_cast(joint); + const auto* sphericalJoint = dynamic_cast(joint); + + if (fixedJoint) + { + jointJSON["type"] = "fixed"; + } else if (revoluteJoint) + { + jointJSON["type"] = "revolute"; + // axis expressed in child (link2) frame + jointJSON["axis"] = axisToJSON(revoluteJoint->getAxis(link2, link1)); + jointJSON["limits"] = oneDOFLimitsToJSON(*revoluteJoint); + jointJSON["dynamics"] = oneDOFDynamicsToJSON(*revoluteJoint); + } else if (prismaticJoint) + { + jointJSON["type"] = "prismatic"; + jointJSON["axis"] = axisToJSON(prismaticJoint->getAxis(link2, link1)); + jointJSON["limits"] = oneDOFLimitsToJSON(*prismaticJoint); + jointJSON["dynamics"] = oneDOFDynamicsToJSON(*prismaticJoint); + } else if (revoluteSO2Joint) + { + jointJSON["type"] = "revolute_so2"; + jointJSON["axis"] = axisToJSON(revoluteSO2Joint->getAxis(link2, link1)); + jointJSON["limits"] = oneDOFLimitsToJSON(*revoluteSO2Joint); + jointJSON["dynamics"] = oneDOFDynamicsToJSON(*revoluteSO2Joint); + } else if (sphericalJoint) + { + jointJSON["type"] = "spherical"; + // joint center relative to link1 (parent) + jointJSON["joint_center_wrt_parent"] + = positionToJSON(sphericalJoint->getJointCenter(link1)); + jointJSON["limits"] = sphericalLimitsToJSON(*sphericalJoint); + jointJSON["dynamics"] = sphericalDynamicsToJSON(*sphericalJoint); + } else + { + reportError("ModelJSONExport", + "modelToJSONString", + "Unknown joint type encountered during export."); + return false; + } + + jointsJSON.push_back(jointJSON); + } + root["joints"] = jointsJSON; + + // Additional frames + nlohmann::json framesJSON = nlohmann::json::array(); + for (FrameIndex frmIdx = model.getNrOfLinks(); frmIdx < model.getNrOfFrames(); frmIdx++) + { + nlohmann::json frameJSON; + frameJSON["name"] = model.getFrameName(frmIdx); + const LinkIndex parentLinkIdx = model.getFrameLink(frmIdx); + frameJSON["parent_link"] = model.getLinkName(parentLinkIdx); + frameJSON["link_H_frame"] = transformToJSON(model.getFrameTransform(frmIdx)); + framesJSON.push_back(frameJSON); + } + root["additional_frames"] = framesJSON; + + const nlohmann::json visualSolidShapesJSON + = modelSolidShapesToJSON(model, model.visualSolidShapes()); + if (visualSolidShapesJSON.is_null()) + { + return false; + } + root["visual_solid_shapes"] = visualSolidShapesJSON; + + const nlohmann::json collisionSolidShapesJSON + = modelSolidShapesToJSON(model, model.collisionSolidShapes()); + if (collisionSolidShapesJSON.is_null()) + { + return false; + } + root["collision_solid_shapes"] = collisionSolidShapesJSON; + + nlohmann::json sensorsJSON = nlohmann::json::array(); + for (SensorsList::const_iterator sensorIterator = model.sensors().allSensorsIterator(); + sensorIterator.isValid(); + ++sensorIterator) + { + const Sensor* sensor = *sensorIterator; + if (sensor == nullptr) + { + reportError("ModelJSONExport", + "modelToJSONString", + "Encountered null sensor pointer during export."); + return false; + } + + const nlohmann::json sensorJSON = sensorToJSON(*sensor, model); + if (sensorJSON.is_null()) + { + return false; + } + sensorsJSON.push_back(sensorJSON); + } + root["sensors"] = sensorsJSON; + + jsonString = root.dump(2); + return true; +} + +bool modelToJSONFile(const Model& model, const std::string& filename) +{ + std::string jsonString; + if (!modelToJSONString(model, jsonString)) + return false; + + std::ofstream ofs(filename); + if (!ofs.is_open()) + { + reportError("ModelJSONExport", + "modelToJSONFile", + ("Failed to open file for writing: " + filename).c_str()); + return false; + } + ofs << jsonString; + return ofs.good(); +} + +// ─── import ────────────────────────────────────────────────────────────────── + +// Helper: deserialize 1-DOF limits +template static bool applyOneDOFLimits(const nlohmann::json& limJ, JointT& joint) +{ + if (limJ.at("has_position_limits").get()) + { + joint.enablePosLimits(true); + + const double minPosition = limJ.at("min_position").get(); + const double maxPosition = limJ.at("max_position").get(); + + if (!joint.setPosLimits(0, minPosition, maxPosition)) + { + return false; + } + } + if (limJ.at("has_effort_limits").get()) + { + joint.enableEffortLimits(true); + joint.setEffortLimit(0, limJ.at("effort_limit").get()); + } + if (limJ.at("has_velocity_limits").get()) + { + joint.enableVelocityLimits(true); + joint.setVelocityLimit(0, limJ.at("velocity_limit").get()); + } + return true; +} + +// Helper: deserialize 1-DOF dynamics +template +static void applyOneDOFDynamics(const nlohmann::json& dynJ, JointT& joint) +{ + const std::string dynType = dynJ.at("type").get(); + joint.setJointDynamicsType(dynType == "URDFJointDynamics" ? URDFJointDynamics + : NoJointDynamics); + joint.setDamping(0, dynJ.at("damping").get()); + joint.setStaticFriction(0, dynJ.at("static_friction").get()); +} + +bool modelFromJSONString(const std::string& jsonString, Model& model) +{ + nlohmann::json root; + try + { + root = nlohmann::json::parse(jsonString); + } catch (const nlohmann::json::parse_error& e) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("JSON parse error: " + std::string(e.what())).c_str()); + return false; + } + + // Version check + if (!root.contains("idyntree_model_json_version")) + { + reportError("ModelJSONExport", + "modelFromJSONString", + "Missing field 'idyntree_model_json_version'."); + return false; + } + const int version = root["idyntree_model_json_version"].get(); + if (version != IDYNTREE_MODEL_JSON_FORMAT_VERSION) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Unsupported idyntree_model_json_version: " + std::to_string(version) + + ". Supported version: " + std::to_string(IDYNTREE_MODEL_JSON_FORMAT_VERSION)) + .c_str()); + return false; + } + + model = Model(); + + // ── Links ──────────────────────────────────────────────────────────────── + for (const auto& linkJSON : root.at("links")) + { + const std::string name = linkJSON.at("name").get(); + const auto& inertiaJSON = linkJSON.at("inertia"); + + const double mass = inertiaJSON.at("mass").get(); + const Position com = positionFromJSON(inertiaJSON.at("center_of_mass")); + + const auto& rotIJSON = inertiaJSON.at("rotational_inertia_wrt_frame_origin"); + double data[9]; + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + data[r * 3 + c] = rotIJSON.at(r).at(c).get(); + RotationalInertia rotI(data, 3, 3); + + // The SpatialInertia constructor stores rotInertia as-is (wrt frame origin). + SpatialInertia spatialInertia(mass, com, rotI); + + Link link; + link.setInertia(spatialInertia); + model.addLink(name, link); + } + + // ── Joints ─────────────────────────────────────────────────────────────── + for (const auto& jointJSON : root.at("joints")) + { + const std::string name = jointJSON.at("name").get(); + const std::string type = jointJSON.at("type").get(); + const std::string parentLinkName = jointJSON.at("parent_link").get(); + const std::string childLinkName = jointJSON.at("child_link").get(); + + const LinkIndex parentIdx = model.getLinkIndex(parentLinkName); + if (parentIdx == LINK_INVALID_INDEX) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Joint '" + name + "': parent link not found: " + parentLinkName).c_str()); + return false; + } + const LinkIndex childIdx = model.getLinkIndex(childLinkName); + if (childIdx == LINK_INVALID_INDEX) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Joint '" + name + "': child link not found: " + childLinkName).c_str()); + return false; + } + + // rest_transform is link1_X_link2 (parent_X_child) + const Transform restTransform = transformFromJSON(jointJSON.at("rest_transform")); + + if (type == "fixed") + { + FixedJoint fixedJoint(parentIdx, childIdx, restTransform); + model.addJoint(name, &fixedJoint); + } else if (type == "revolute") + { + RevoluteJoint revJoint; + revJoint.setAttachedLinks(parentIdx, childIdx); + revJoint.setRestTransform(restTransform); + const Axis axis = axisFromJSON(jointJSON.at("axis")); + revJoint.setAxis(axis, childIdx, parentIdx); + if (!applyOneDOFLimits(jointJSON.at("limits"), revJoint)) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Unable to set limits for revolute joint: " + name).c_str()); + return false; + } + applyOneDOFDynamics(jointJSON.at("dynamics"), revJoint); + model.addJoint(name, &revJoint); + } else if (type == "prismatic") + { + PrismaticJoint prismJoint; + prismJoint.setAttachedLinks(parentIdx, childIdx); + prismJoint.setRestTransform(restTransform); + const Axis axis = axisFromJSON(jointJSON.at("axis")); + prismJoint.setAxis(axis, childIdx, parentIdx); + if (!applyOneDOFLimits(jointJSON.at("limits"), prismJoint)) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Unable to set limits for prismatic joint: " + name).c_str()); + return false; + } + applyOneDOFDynamics(jointJSON.at("dynamics"), prismJoint); + model.addJoint(name, &prismJoint); + } else if (type == "revolute_so2") + { + RevoluteSO2Joint revSO2Joint; + revSO2Joint.setAttachedLinks(parentIdx, childIdx); + revSO2Joint.setRestTransform(restTransform); + const Axis axis = axisFromJSON(jointJSON.at("axis")); + revSO2Joint.setAxis(axis, childIdx, parentIdx); + if (!applyOneDOFLimits(jointJSON.at("limits"), revSO2Joint)) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Unable to set limits for revolute_so2 joint: " + name).c_str()); + return false; + } + applyOneDOFDynamics(jointJSON.at("dynamics"), revSO2Joint); + model.addJoint(name, &revSO2Joint); + } else if (type == "spherical") + { + SphericalJoint sphericalJoint; + sphericalJoint.setAttachedLinks(parentIdx, childIdx); + sphericalJoint.setRestTransform(restTransform); + const Position center = positionFromJSON(jointJSON.at("joint_center_wrt_parent")); + sphericalJoint.setJointCenter(parentIdx, center); + + const auto& limJSON = jointJSON.at("limits"); + if (limJSON.at("has_position_limits").get()) + { + sphericalJoint.enablePosLimits(true); + const auto& minArr = limJSON.at("min_position"); + const auto& maxArr = limJSON.at("max_position"); + for (size_t i = 0; i < 3; i++) + sphericalJoint.setPosLimits(i, + minArr.at(i).get(), + maxArr.at(i).get()); + } + if (limJSON.at("has_effort_limits").get()) + { + sphericalJoint.enableEffortLimits(true); + const auto& effortArr = limJSON.at("effort_limit"); + for (size_t i = 0; i < 3; i++) + sphericalJoint.setEffortLimit(i, effortArr.at(i).get()); + } + if (limJSON.at("has_velocity_limits").get()) + { + sphericalJoint.enableVelocityLimits(true); + const auto& velArr = limJSON.at("velocity_limit"); + for (size_t i = 0; i < 3; i++) + sphericalJoint.setVelocityLimit(i, velArr.at(i).get()); + } + + const auto& dynJSON = jointJSON.at("dynamics"); + const std::string dynType = dynJSON.at("type").get(); + sphericalJoint.setJointDynamicsType(dynType == "URDFJointDynamics" ? URDFJointDynamics + : NoJointDynamics); + const auto& dampingArr = dynJSON.at("damping"); + const auto& frictionArr = dynJSON.at("static_friction"); + for (size_t i = 0; i < 3; i++) + { + sphericalJoint.setDamping(i, dampingArr.at(i).get()); + sphericalJoint.setStaticFriction(i, frictionArr.at(i).get()); + } + + model.addJoint(name, &sphericalJoint); + } else + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Unknown joint type: " + type).c_str()); + return false; + } + } + + // ── Additional frames ──────────────────────────────────────────────────── + for (const auto& frameJSON : root.at("additional_frames")) + { + const std::string frameName = frameJSON.at("name").get(); + const std::string parentLinkName = frameJSON.at("parent_link").get(); + const Transform link_H_frame = transformFromJSON(frameJSON.at("link_H_frame")); + model.addAdditionalFrameToLink(parentLinkName, frameName, link_H_frame); + } + + if (root.contains("visual_solid_shapes") + && !modelSolidShapesFromJSON(root.at("visual_solid_shapes"), + model, + model.visualSolidShapes())) + { + return false; + } + + if (root.contains("collision_solid_shapes") + && !modelSolidShapesFromJSON(root.at("collision_solid_shapes"), + model, + model.collisionSolidShapes())) + { + return false; + } + + if (root.contains("sensors")) + { + for (const auto& sensorJSON : root.at("sensors")) + { + if (!addSensorToModel(sensorJSON, model)) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Failed to import sensor: " + + sensorJSON.value("name", std::string(""))) + .c_str()); + return false; + } + } + } + + // ── Default base link ──────────────────────────────────────────────────── + const std::string defaultBaseLinkName = root.at("default_base_link").get(); + const LinkIndex defaultBaseIdx = model.getLinkIndex(defaultBaseLinkName); + if (defaultBaseIdx == LINK_INVALID_INDEX) + { + reportError("ModelJSONExport", + "modelFromJSONString", + ("Default base link not found: " + defaultBaseLinkName).c_str()); + return false; + } + model.setDefaultBaseLink(defaultBaseIdx); + + return true; +} + +bool modelFromJSONFile(const std::string& filename, Model& model) +{ + std::ifstream ifs(filename); + if (!ifs.is_open()) + { + reportError("ModelJSONExport", + "modelFromJSONFile", + ("Failed to open file for reading: " + filename).c_str()); + return false; + } + std::stringstream ss; + ss << ifs.rdbuf(); + return modelFromJSONString(ss.str(), model); +} + +} // namespace iDynTree diff --git a/src/model_io/codecs/src/ModelLoader.cpp b/src/model_io/codecs/src/ModelLoader.cpp index c0568360bb..70d824e3a4 100644 --- a/src/model_io/codecs/src/ModelLoader.cpp +++ b/src/model_io/codecs/src/ModelLoader.cpp @@ -3,13 +3,17 @@ #include "iDynTree/ModelLoader.h" +#include "ModelIOFormatUtils.h" #include "SDFormatDocument.h" #include "URDFDocument.h" +#ifdef IDYNTREE_USES_NLOHMANN_JSON +#include "ModelJSONImportExport.h" +#endif + #include #include -#include #include #include @@ -93,26 +97,44 @@ bool ModelLoader::loadModelFromFile(const std::string& filename, const std::string& filetype, const std::vector& packageDirs /* = {} */) { - // Determine the file type - either from explicit parameter or from file - // extension - std::string actualFileType = filetype; + std::string actualFileType = normalizeModelFormatName(filetype); if (actualFileType.empty()) { - // Try to determine from extension - size_t dotPos = filename.rfind('.'); - if (dotPos != std::string::npos) + actualFileType = inferModelFormatFromFilename(filename); + } + + if (!actualFileType.empty() && !isSupportedImportModelFormat(actualFileType)) + { + reportError("ModelLoader", + "loadModelFromFile", + "Unsupported filetype. Supported import formats are: urdf, sdf, " + "idyntree-model-json."); + return false; + } + + if (actualFileType == "idyntree-model-json") + { +#ifdef IDYNTREE_USES_NLOHMANN_JSON + Model parsedModel; + if (!modelFromJSONFile(filename, parsedModel)) { - actualFileType = filename.substr(dotPos + 1); - // Convert to lowercase for comparison - std::transform(actualFileType.begin(), - actualFileType.end(), - actualFileType.begin(), - ::tolower); + reportError("ModelLoader", + "loadModelFromFile", + "Error in parsing model from idyntree-model-json file."); + return false; } + return m_pimpl->setModel(parsedModel); +#else + reportError("ModelLoader", + "loadModelFromFile", + "idyntree-model-json format detected but iDynTree was not compiled with " + "nlohmann_json support. " + "Please rebuild iDynTree with IDYNTREE_USES_NLOHMANN_JSON option enabled."); + return false; +#endif } - // Check if this is an SDF file - if (actualFileType == "sdf" || actualFileType == "world") + if (actualFileType == "sdf") { #ifdef IDYNTREE_USES_SDFORMAT // Use SDFormat parser @@ -172,12 +194,23 @@ bool ModelLoader::loadModelFromString(const std::string& modelString, const std::string& filetype, const std::vector& packageDirs /* = {} */) { + const std::string normalizedFiletype = normalizeModelFormatName(filetype); + + if (!normalizedFiletype.empty() && !isSupportedImportModelFormat(normalizedFiletype)) + { + reportError("ModelLoader", + "loadModelFromString", + "Unsupported filetype. Supported import formats are: urdf, sdf, " + "idyntree-model-json."); + return false; + } + // Check if this is an SDF string (look for tag) bool isSDF = false; - if (filetype == "sdf" || filetype == "world") + if (normalizedFiletype == "sdf") { isSDF = true; - } else if (filetype.empty()) + } else if (normalizedFiletype.empty()) { // Try to detect from content size_t sdfPos = modelString.find("setModel(parsedModel); +#else + reportError("ModelLoader", + "loadModelFromString", + "idyntree-model-json format requested but iDynTree was not compiled with " + "nlohmann_json support. " + "Please rebuild iDynTree with IDYNTREE_USES_NLOHMANN_JSON option enabled."); + return false; +#endif + } + if (isSDF) { #ifdef IDYNTREE_USES_SDFORMAT diff --git a/src/model_io/codecs/tests/CMakeLists.txt b/src/model_io/codecs/tests/CMakeLists.txt index e34de8d12f..0280c8df9a 100644 --- a/src/model_io/codecs/tests/CMakeLists.txt +++ b/src/model_io/codecs/tests/CMakeLists.txt @@ -24,6 +24,21 @@ add_modelio_urdf_unit_test(URDFGenericSensorImport) add_modelio_urdf_unit_test(PredictSensorsMeasurement) add_modelio_urdf_unit_test(icubSensorURDF) +# Add idyntree-model-json round-trip test (only if nlohmann_json support is enabled) +if(IDYNTREE_USES_NLOHMANN_JSON) + set(testsrc JSONModelRoundTripTest.cpp) + set(testbinary JSONModelRoundTripTest) + set(testname UnitTestJSONModelRoundTrip) + add_executable(${testbinary} ${testsrc}) + target_link_libraries(${testbinary} PRIVATE idyntree-modelio idyntree-testmodels Eigen3::Eigen) + target_compile_definitions(${testbinary} PRIVATE IDYNTREE_USES_NLOHMANN_JSON) + add_test(NAME ${testname} COMMAND ${testbinary}) + + if(IDYNTREE_RUN_VALGRIND_TESTS) + add_test(NAME memcheck_${testname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $) + endif() +endif() + # Add SDFormat test (only if SDFormat support is enabled) if(IDYNTREE_USES_SDFORMAT) set(testsrc SDFormatModelImportUnitTest.cpp) diff --git a/src/model_io/codecs/tests/JSONModelRoundTripTest.cpp b/src/model_io/codecs/tests/JSONModelRoundTripTest.cpp new file mode 100644 index 0000000000..53f77bbebd --- /dev/null +++ b/src/model_io/codecs/tests/JSONModelRoundTripTest.cpp @@ -0,0 +1,155 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +/** + * Round-trip tests for the idyntree-model-json format. + * + * For each random model generated with iDynTree::getRandomModel: + * 1. Export the model to an idyntree-model-json string. + * 2. Import the model back. + * 3. Verify that all attributes of the original and round-tripped model match. + */ + +#include "JSONModelRoundTripTestUtils.h" +#include "ModelJSONImportExport.h" +#include "testModels.h" + +#include +#include +#include +#include + +#include +#include +#include + +using namespace iDynTree; + +// ─── test cases ─────────────────────────────────────────────────────────────── + +static void testRoundTripWithJointTypes(unsigned int nrOfJoints, + unsigned int allowedJointTypes, + const std::string& label) +{ + std::cout << "Testing round-trip: " << label << " (" << nrOfJoints << " joints)" << std::endl; + + const Model originalModel = getRandomModel(nrOfJoints, 5, allowedJointTypes); + + std::string jsonString; + ASSERT_IS_TRUE(modelToJSONString(originalModel, jsonString)); + + Model loadedModel; + ASSERT_IS_TRUE(modelFromJSONString(jsonString, loadedModel)); + + assertModelsAreEqualIncludingShapesAndSensors(originalModel, loadedModel); +} + +static void testVersionMismatchIsRejected() +{ + // Manually craft a JSON string with a wrong version number + std::string badJSON = R"({"idyntree_model_json_version": 9999, "default_base_link": "base", + "links": [], "joints": [], "additional_frames": []})"; + Model model; + // Suppress expected error output + ASSERT_IS_FALSE(modelFromJSONString(badJSON, model)); +} + +static void testMissingVersionIsRejected() +{ + std::string badJSON = R"({"default_base_link": "base", + "links": [], "joints": [], "additional_frames": []})"; + Model model; + ASSERT_IS_FALSE(modelFromJSONString(badJSON, model)); +} + +static void testEmptyModelRoundTrip() +{ + // A model with a single link and no joints is the minimal valid model + Model original; + Link baseLink; + SpatialInertia inertia(1.0, Position(0, 0, 0), RotationalInertia()); + inertia.zero(); + // Override with identity inertia for single link + baseLink.setInertia(inertia); + original.addLink("base", baseLink); + original.setDefaultBaseLink(0); + + std::string jsonString; + ASSERT_IS_TRUE(modelToJSONString(original, jsonString)); + + Model loaded; + ASSERT_IS_TRUE(modelFromJSONString(jsonString, loaded)); + + ASSERT_EQUAL_DOUBLE(loaded.getNrOfLinks(), 1); + ASSERT_EQUAL_DOUBLE(loaded.getNrOfJoints(), 0); + ASSERT_EQUAL_STRING(loaded.getLinkName(0), "base"); + ASSERT_EQUAL_STRING(loaded.getLinkName(loaded.getDefaultBaseLink()), "base"); +} + +static void testRoundTripAfterChangingBase() +{ + Model originalModel = getRandomModel(10, 5, ALL_JOINT_TYPES); + + std::string jsonString; + ASSERT_IS_TRUE(modelToJSONString(originalModel, jsonString)); + + Model loadedModel; + ASSERT_IS_TRUE(modelFromJSONString(jsonString, loadedModel)); + + const std::string newBaseLinkName = getRandomLinkOfModel(originalModel); + + const LinkIndex originalNewBaseIdx = originalModel.getLinkIndex(newBaseLinkName); + const LinkIndex loadedNewBaseIdx = loadedModel.getLinkIndex(newBaseLinkName); + + ASSERT_IS_TRUE(originalNewBaseIdx != LINK_INVALID_INDEX); + ASSERT_IS_TRUE(loadedNewBaseIdx != LINK_INVALID_INDEX); + + originalModel.setDefaultBaseLink(originalNewBaseIdx); + loadedModel.setDefaultBaseLink(loadedNewBaseIdx); + + assertModelsAreEqualIncludingShapesAndSensors(originalModel, loadedModel); +} + +static void testRoundTripFromURDFFixture(const std::string& urdfFilename) +{ + std::cout << "Testing round-trip fixture: " << urdfFilename << std::endl; + const std::string urdfPath = getAbsModelPath(urdfFilename); + + ModelLoader loader; + ASSERT_IS_TRUE(loader.loadModelFromFile(urdfPath, "urdf")); + + const Model originalModel = loader.model(); + + std::string jsonString; + ASSERT_IS_TRUE(modelToJSONString(originalModel, jsonString)); + + Model loadedModel; + ASSERT_IS_TRUE(modelFromJSONString(jsonString, loadedModel)); + + assertModelsAreEqualIncludingShapesAndSensors(originalModel, loadedModel); +} + +int main() +{ + // ── All joint types including Spherical ─────────────────────────────────── + for (unsigned int nrJoints : {1u, 5u, 10u}) + { + testRoundTripWithJointTypes(nrJoints, ALL_JOINT_TYPES, "ALL_JOINT_TYPES"); + } + + // ── Edge cases ──────────────────────────────────────────────────────────── + testEmptyModelRoundTrip(); + testRoundTripAfterChangingBase(); + + // ── Deterministic fixture-based tests (non-random models) ────────────── + for (unsigned int modelIndex = 0; modelIndex < IDYNTREE_TESTS_URDFS_NR; ++modelIndex) + { + testRoundTripFromURDFFixture(IDYNTREE_TESTS_URDFS[modelIndex]); + } + + testVersionMismatchIsRejected(); + testMissingVersionIsRejected(); + + std::cout << "All idyntree-model-json round-trip tests passed." << std::endl; + return EXIT_SUCCESS; +} diff --git a/src/model_io/codecs/tests/JSONModelRoundTripTestUtils.h b/src/model_io/codecs/tests/JSONModelRoundTripTestUtils.h new file mode 100644 index 0000000000..cce676376a --- /dev/null +++ b/src/model_io/codecs/tests/JSONModelRoundTripTestUtils.h @@ -0,0 +1,501 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_JSON_MODEL_ROUND_TRIP_TEST_UTILS_H +#define IDYNTREE_JSON_MODEL_ROUND_TRIP_TEST_UTILS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace iDynTree +{ + +inline void assertJSONRoundTripTransformsAreEqual(const Transform& lhs, + const Transform& rhs, + double tol = 1e-10) +{ + ASSERT_EQUAL_MATRIX_TOL(lhs.asHomogeneousTransform(), rhs.asHomogeneousTransform(), tol); +} + +inline void +assertJSONRoundTripBaseModelAreEqual(const Model& expected, const Model& actual, double tol = 1e-10) +{ + ASSERT_EQUAL_DOUBLE(expected.getNrOfLinks(), actual.getNrOfLinks()); + ASSERT_EQUAL_DOUBLE(expected.getNrOfJoints(), actual.getNrOfJoints()); + ASSERT_EQUAL_DOUBLE(expected.getNrOfDOFs(), actual.getNrOfDOFs()); + ASSERT_EQUAL_DOUBLE(expected.getNrOfPosCoords(), actual.getNrOfPosCoords()); + ASSERT_EQUAL_DOUBLE(expected.getNrOfFrames(), actual.getNrOfFrames()); + + ASSERT_EQUAL_STRING(expected.getLinkName(expected.getDefaultBaseLink()), + actual.getLinkName(actual.getDefaultBaseLink())); + + for (LinkIndex expectedLinkIdx = 0; expectedLinkIdx < expected.getNrOfLinks(); + ++expectedLinkIdx) + { + const std::string& linkName = expected.getLinkName(expectedLinkIdx); + const LinkIndex actualLinkIdx = actual.getLinkIndex(linkName); + + ASSERT_IS_TRUE(actualLinkIdx != LINK_INVALID_INDEX); + ASSERT_EQUAL_STRING(linkName, actual.getLinkName(actualLinkIdx)); + ASSERT_EQUAL_MATRIX_TOL(expected.getLink(expectedLinkIdx)->getInertia().asMatrix(), + actual.getLink(actualLinkIdx)->getInertia().asMatrix(), + tol); + } + + for (JointIndex expectedJointIdx = 0; expectedJointIdx < expected.getNrOfJoints(); + ++expectedJointIdx) + { + const std::string& jointName = expected.getJointName(expectedJointIdx); + const JointIndex actualJointIdx = actual.getJointIndex(jointName); + + ASSERT_IS_TRUE(actualJointIdx != JOINT_INVALID_INDEX); + ASSERT_EQUAL_STRING(jointName, actual.getJointName(actualJointIdx)); + + const IJointConstPtr expectedJoint = expected.getJoint(expectedJointIdx); + const IJointConstPtr actualJoint = actual.getJoint(actualJointIdx); + + ASSERT_EQUAL_STRING(expected.getLinkName(expectedJoint->getFirstAttachedLink()), + actual.getLinkName(actualJoint->getFirstAttachedLink())); + ASSERT_EQUAL_STRING(expected.getLinkName(expectedJoint->getSecondAttachedLink()), + actual.getLinkName(actualJoint->getSecondAttachedLink())); + ASSERT_EQUAL_DOUBLE(expectedJoint->getNrOfDOFs(), actualJoint->getNrOfDOFs()); + ASSERT_EQUAL_DOUBLE(expectedJoint->getNrOfPosCoords(), actualJoint->getNrOfPosCoords()); + + const LinkIndex expectedLink1 = expectedJoint->getFirstAttachedLink(); + const LinkIndex expectedLink2 = expectedJoint->getSecondAttachedLink(); + const LinkIndex actualLink1 = actualJoint->getFirstAttachedLink(); + const LinkIndex actualLink2 = actualJoint->getSecondAttachedLink(); + + assertJSONRoundTripTransformsAreEqual(expectedJoint->getRestTransform(expectedLink1, + expectedLink2), + actualJoint->getRestTransform(actualLink1, + actualLink2), + tol); + + ASSERT_EQUAL_DOUBLE(expectedJoint->hasPosLimits(), actualJoint->hasPosLimits()); + if (expectedJoint->hasPosLimits()) + { + for (size_t i = 0; i < expectedJoint->getNrOfPosCoords(); ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedJoint->getMinPosLimit(i), + actualJoint->getMinPosLimit(i), + tol); + ASSERT_EQUAL_DOUBLE_TOL(expectedJoint->getMaxPosLimit(i), + actualJoint->getMaxPosLimit(i), + tol); + } + } + + ASSERT_EQUAL_DOUBLE(expectedJoint->hasEffortLimits(), actualJoint->hasEffortLimits()); + if (expectedJoint->hasEffortLimits()) + { + for (size_t i = 0; i < expectedJoint->getNrOfDOFs(); ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedJoint->getEffortLimit(i), + actualJoint->getEffortLimit(i), + tol); + } + } + + ASSERT_EQUAL_DOUBLE(expectedJoint->hasVelocityLimits(), actualJoint->hasVelocityLimits()); + if (expectedJoint->hasVelocityLimits()) + { + for (size_t i = 0; i < expectedJoint->getNrOfDOFs(); ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedJoint->getVelocityLimit(i), + actualJoint->getVelocityLimit(i), + tol); + } + } + + ASSERT_EQUAL_DOUBLE(expectedJoint->getJointDynamicsType(), + actualJoint->getJointDynamicsType()); + for (size_t i = 0; i < expectedJoint->getNrOfDOFs(); ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedJoint->getDamping(i), actualJoint->getDamping(i), tol); + ASSERT_EQUAL_DOUBLE_TOL(expectedJoint->getStaticFriction(i), + actualJoint->getStaticFriction(i), + tol); + } + + const auto* expectedFixed = dynamic_cast(expectedJoint); + const auto* expectedRevolute = dynamic_cast(expectedJoint); + const auto* expectedPrismatic = dynamic_cast(expectedJoint); + const auto* expectedRevoluteSO2 = dynamic_cast(expectedJoint); + const auto* expectedSpherical = dynamic_cast(expectedJoint); + + if (expectedFixed) + { + ASSERT_IS_TRUE(dynamic_cast(actualJoint) != nullptr); + } else if (expectedRevolute) + { + const auto* actualRevolute = dynamic_cast(actualJoint); + ASSERT_IS_TRUE(actualRevolute != nullptr); + + const Axis expectedAxis = expectedRevolute->getAxis(expectedLink2, expectedLink1); + const Axis actualAxis = actualRevolute->getAxis(actualLink2, actualLink1); + + for (int i = 0; i < 3; ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedAxis.getDirection()(i), + actualAxis.getDirection()(i), + tol); + ASSERT_EQUAL_DOUBLE_TOL(expectedAxis.getOrigin()(i), + actualAxis.getOrigin()(i), + tol); + } + } else if (expectedPrismatic) + { + const auto* actualPrismatic = dynamic_cast(actualJoint); + ASSERT_IS_TRUE(actualPrismatic != nullptr); + + const Axis expectedAxis = expectedPrismatic->getAxis(expectedLink2, expectedLink1); + const Axis actualAxis = actualPrismatic->getAxis(actualLink2, actualLink1); + + for (int i = 0; i < 3; ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedAxis.getDirection()(i), + actualAxis.getDirection()(i), + tol); + ASSERT_EQUAL_DOUBLE_TOL(expectedAxis.getOrigin()(i), + actualAxis.getOrigin()(i), + tol); + } + } else if (expectedRevoluteSO2) + { + const auto* actualRevoluteSO2 = dynamic_cast(actualJoint); + ASSERT_IS_TRUE(actualRevoluteSO2 != nullptr); + + const Axis expectedAxis = expectedRevoluteSO2->getAxis(expectedLink2, expectedLink1); + const Axis actualAxis = actualRevoluteSO2->getAxis(actualLink2, actualLink1); + + for (int i = 0; i < 3; ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedAxis.getDirection()(i), + actualAxis.getDirection()(i), + tol); + ASSERT_EQUAL_DOUBLE_TOL(expectedAxis.getOrigin()(i), + actualAxis.getOrigin()(i), + tol); + } + } else if (expectedSpherical) + { + const auto* actualSpherical = dynamic_cast(actualJoint); + ASSERT_IS_TRUE(actualSpherical != nullptr); + + for (int i = 0; i < 3; ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedSpherical->getJointCenter(expectedLink1)(i), + actualSpherical->getJointCenter(actualLink1)(i), + tol); + } + } else + { + ASSERT_IS_TRUE(false); + } + } + + for (FrameIndex expectedFrameIdx = expected.getNrOfLinks(); + expectedFrameIdx < expected.getNrOfFrames(); + ++expectedFrameIdx) + { + const std::string& frameName = expected.getFrameName(expectedFrameIdx); + const FrameIndex actualFrameIdx = actual.getFrameIndex(frameName); + + ASSERT_IS_TRUE(actualFrameIdx != FRAME_INVALID_INDEX); + ASSERT_EQUAL_STRING(frameName, actual.getFrameName(actualFrameIdx)); + ASSERT_EQUAL_STRING(expected.getLinkName(expected.getFrameLink(expectedFrameIdx)), + actual.getLinkName(actual.getFrameLink(actualFrameIdx))); + assertJSONRoundTripTransformsAreEqual(expected.getFrameTransform(expectedFrameIdx), + actual.getFrameTransform(actualFrameIdx), + tol); + } +} + +inline void +assertJSONRoundTripVector3AreEqual(const Vector3& lhs, const Vector3& rhs, double tol = 1e-10) +{ + for (int i = 0; i < 3; ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(lhs(i), rhs(i), tol); + } +} + +inline void +assertJSONRoundTripVector4AreEqual(const Vector4& lhs, const Vector4& rhs, double tol = 1e-10) +{ + for (int i = 0; i < 4; ++i) + { + ASSERT_EQUAL_DOUBLE_TOL(lhs(i), rhs(i), tol); + } +} + +inline void assertJSONRoundTripMaterialsAreEqual(const Material& expected, + const Material& actual, + double tol = 1e-10) +{ + ASSERT_EQUAL_STRING(expected.name(), actual.name()); + ASSERT_IS_TRUE(expected.hasColor() == actual.hasColor()); + if (expected.hasColor()) + { + assertJSONRoundTripVector4AreEqual(expected.color(), actual.color(), tol); + } + + ASSERT_IS_TRUE(expected.hasTexture() == actual.hasTexture()); + if (expected.hasTexture()) + { + ASSERT_EQUAL_STRING(expected.texture(), actual.texture()); + } +} + +inline void assertJSONRoundTripSolidShapesAreEqual(const SolidShape& expected, + const SolidShape& actual, + double tol = 1e-10) +{ + ASSERT_IS_TRUE(expected.isSphere() == actual.isSphere()); + ASSERT_IS_TRUE(expected.isBox() == actual.isBox()); + ASSERT_IS_TRUE(expected.isCylinder() == actual.isCylinder()); + ASSERT_IS_TRUE(expected.isExternalMesh() == actual.isExternalMesh()); + ASSERT_IS_TRUE(expected.isNameValid() == actual.isNameValid()); + if (expected.isNameValid()) + { + ASSERT_EQUAL_STRING(expected.getName(), actual.getName()); + } + + assertJSONRoundTripTransformsAreEqual(expected.getLink_H_geometry(), + actual.getLink_H_geometry(), + tol); + + ASSERT_IS_TRUE(expected.isMaterialSet() == actual.isMaterialSet()); + if (expected.isMaterialSet()) + { + assertJSONRoundTripMaterialsAreEqual(expected.getMaterial(), actual.getMaterial(), tol); + } + + if (expected.isSphere()) + { + ASSERT_EQUAL_DOUBLE_TOL(expected.asSphere()->getRadius(), + actual.asSphere()->getRadius(), + tol); + } else if (expected.isBox()) + { + ASSERT_EQUAL_DOUBLE_TOL(expected.asBox()->getX(), actual.asBox()->getX(), tol); + ASSERT_EQUAL_DOUBLE_TOL(expected.asBox()->getY(), actual.asBox()->getY(), tol); + ASSERT_EQUAL_DOUBLE_TOL(expected.asBox()->getZ(), actual.asBox()->getZ(), tol); + } else if (expected.isCylinder()) + { + ASSERT_EQUAL_DOUBLE_TOL(expected.asCylinder()->getLength(), + actual.asCylinder()->getLength(), + tol); + ASSERT_EQUAL_DOUBLE_TOL(expected.asCylinder()->getRadius(), + actual.asCylinder()->getRadius(), + tol); + } else if (expected.isExternalMesh()) + { + ASSERT_EQUAL_STRING(expected.asExternalMesh()->getFilename(), + actual.asExternalMesh()->getFilename()); + assertJSONRoundTripVector3AreEqual(expected.asExternalMesh()->getScale(), + actual.asExternalMesh()->getScale(), + tol); + } +} + +inline void assertJSONRoundTripModelSolidShapesAreEqual(const Model& expectedModel, + const Model& actualModel, + const ModelSolidShapes& expectedShapes, + const ModelSolidShapes& actualShapes, + double tol = 1e-10) +{ + const auto& expectedLinkShapes = expectedShapes.getLinkSolidShapes(); + const auto& actualLinkShapes = actualShapes.getLinkSolidShapes(); + + ASSERT_EQUAL_DOUBLE(expectedLinkShapes.size(), actualLinkShapes.size()); + + for (LinkIndex expectedLinkIdx = 0; expectedLinkIdx < expectedModel.getNrOfLinks(); + ++expectedLinkIdx) + { + const std::string& linkName = expectedModel.getLinkName(expectedLinkIdx); + const LinkIndex actualLinkIdx = actualModel.getLinkIndex(linkName); + + ASSERT_IS_TRUE(actualLinkIdx != LINK_INVALID_INDEX); + ASSERT_EQUAL_DOUBLE(expectedLinkShapes[expectedLinkIdx].size(), + actualLinkShapes[actualLinkIdx].size()); + + for (size_t shapeIndex = 0; shapeIndex < expectedLinkShapes[expectedLinkIdx].size(); + ++shapeIndex) + { + ASSERT_IS_TRUE(expectedLinkShapes[expectedLinkIdx][shapeIndex] != nullptr); + ASSERT_IS_TRUE(actualLinkShapes[actualLinkIdx][shapeIndex] != nullptr); + assertJSONRoundTripSolidShapesAreEqual(*expectedLinkShapes[expectedLinkIdx][shapeIndex], + *actualLinkShapes[actualLinkIdx][shapeIndex], + tol); + } + } +} + +inline void assertJSONRoundTripSensorsAreEqual(const Model& expectedModel, + const Model& actualModel, + double tol = 1e-10) +{ + const SensorsList& expectedSensors = expectedModel.sensors(); + const SensorsList& actualSensors = actualModel.sensors(); + + for (int sensorTypeIndex = 0; sensorTypeIndex < NR_OF_SENSOR_TYPES; ++sensorTypeIndex) + { + const SensorType sensorType = static_cast(sensorTypeIndex); + ASSERT_EQUAL_DOUBLE(expectedSensors.getNrOfSensors(sensorType), + actualSensors.getNrOfSensors(sensorType)); + + for (std::ptrdiff_t sensorIndex = 0; + sensorIndex < static_cast(expectedSensors.getNrOfSensors(sensorType)); + ++sensorIndex) + { + const Sensor* expectedSensor = expectedSensors.getSensor(sensorType, sensorIndex); + const Sensor* actualSensor = actualSensors.getSensor(sensorType, sensorIndex); + + ASSERT_IS_TRUE(expectedSensor != nullptr); + ASSERT_IS_TRUE(actualSensor != nullptr); + ASSERT_EQUAL_STRING(expectedSensor->getName(), actualSensor->getName()); + ASSERT_EQUAL_DOUBLE(expectedSensor->getSensorType(), actualSensor->getSensorType()); + + if (sensorType == SIX_AXIS_FORCE_TORQUE) + { + const auto* expectedFT + = dynamic_cast(expectedSensor); + const auto* actualFT = dynamic_cast(actualSensor); + ASSERT_IS_TRUE(expectedFT != nullptr); + ASSERT_IS_TRUE(actualFT != nullptr); + + ASSERT_EQUAL_STRING(expectedFT->getParentJoint(), actualFT->getParentJoint()); + ASSERT_EQUAL_STRING(expectedFT->getFirstLinkName(), actualFT->getFirstLinkName()); + ASSERT_EQUAL_STRING(expectedFT->getSecondLinkName(), actualFT->getSecondLinkName()); + ASSERT_EQUAL_STRING(expectedModel.getLinkName(expectedFT->getAppliedWrenchLink()), + actualModel.getLinkName(actualFT->getAppliedWrenchLink())); + + Transform expectedFirstLink_H_sensor; + Transform actualFirstLink_H_sensor; + ASSERT_IS_TRUE(expectedFT->getLinkSensorTransform(expectedFT->getFirstLinkIndex(), + expectedFirstLink_H_sensor)); + ASSERT_IS_TRUE(actualFT->getLinkSensorTransform(actualFT->getFirstLinkIndex(), + actualFirstLink_H_sensor)); + assertJSONRoundTripTransformsAreEqual(expectedFirstLink_H_sensor, + actualFirstLink_H_sensor, + tol); + + Transform expectedSecondLink_H_sensor; + Transform actualSecondLink_H_sensor; + ASSERT_IS_TRUE(expectedFT->getLinkSensorTransform(expectedFT->getSecondLinkIndex(), + expectedSecondLink_H_sensor)); + ASSERT_IS_TRUE(actualFT->getLinkSensorTransform(actualFT->getSecondLinkIndex(), + actualSecondLink_H_sensor)); + assertJSONRoundTripTransformsAreEqual(expectedSecondLink_H_sensor, + actualSecondLink_H_sensor, + tol); + } else if (sensorType == ACCELEROMETER) + { + const auto* expectedAccelerometer + = dynamic_cast(expectedSensor); + const auto* actualAccelerometer + = dynamic_cast(actualSensor); + ASSERT_IS_TRUE(expectedAccelerometer != nullptr); + ASSERT_IS_TRUE(actualAccelerometer != nullptr); + ASSERT_EQUAL_STRING(expectedAccelerometer->getParentLink(), + actualAccelerometer->getParentLink()); + assertJSONRoundTripTransformsAreEqual(expectedAccelerometer + ->getLinkSensorTransform(), + actualAccelerometer->getLinkSensorTransform(), + tol); + } else if (sensorType == GYROSCOPE) + { + const auto* expectedGyroscope + = dynamic_cast(expectedSensor); + const auto* actualGyroscope = dynamic_cast(actualSensor); + ASSERT_IS_TRUE(expectedGyroscope != nullptr); + ASSERT_IS_TRUE(actualGyroscope != nullptr); + ASSERT_EQUAL_STRING(expectedGyroscope->getParentLink(), + actualGyroscope->getParentLink()); + assertJSONRoundTripTransformsAreEqual(expectedGyroscope->getLinkSensorTransform(), + actualGyroscope->getLinkSensorTransform(), + tol); + } else if (sensorType == THREE_AXIS_ANGULAR_ACCELEROMETER) + { + const auto* expectedAngularAccelerometer + = dynamic_cast(expectedSensor); + const auto* actualAngularAccelerometer + = dynamic_cast(actualSensor); + ASSERT_IS_TRUE(expectedAngularAccelerometer != nullptr); + ASSERT_IS_TRUE(actualAngularAccelerometer != nullptr); + ASSERT_EQUAL_STRING(expectedAngularAccelerometer->getParentLink(), + actualAngularAccelerometer->getParentLink()); + assertJSONRoundTripTransformsAreEqual(expectedAngularAccelerometer + ->getLinkSensorTransform(), + actualAngularAccelerometer + ->getLinkSensorTransform(), + tol); + } else if (sensorType == THREE_AXIS_FORCE_TORQUE_CONTACT) + { + const auto* expectedContact + = dynamic_cast(expectedSensor); + const auto* actualContact + = dynamic_cast(actualSensor); + ASSERT_IS_TRUE(expectedContact != nullptr); + ASSERT_IS_TRUE(actualContact != nullptr); + ASSERT_EQUAL_STRING(expectedContact->getParentLink(), + actualContact->getParentLink()); + assertJSONRoundTripTransformsAreEqual(expectedContact->getLinkSensorTransform(), + actualContact->getLinkSensorTransform(), + tol); + + const std::vector expectedLoadCellLocations + = expectedContact->getLoadCellLocations(); + const std::vector actualLoadCellLocations + = actualContact->getLoadCellLocations(); + ASSERT_EQUAL_DOUBLE(expectedLoadCellLocations.size(), + actualLoadCellLocations.size()); + for (size_t loadCellIndex = 0; loadCellIndex < expectedLoadCellLocations.size(); + ++loadCellIndex) + { + for (int coordinateIndex = 0; coordinateIndex < 3; ++coordinateIndex) + { + ASSERT_EQUAL_DOUBLE_TOL(expectedLoadCellLocations[loadCellIndex]( + coordinateIndex), + actualLoadCellLocations[loadCellIndex]( + coordinateIndex), + tol); + } + } + } + } + } +} + +inline void assertModelsAreEqualIncludingShapesAndSensors(const Model& expected, + const Model& actual, + double tol = 1e-10) +{ + assertJSONRoundTripBaseModelAreEqual(expected, actual, tol); + assertJSONRoundTripModelSolidShapesAreEqual(expected, + actual, + expected.visualSolidShapes(), + actual.visualSolidShapes(), + tol); + assertJSONRoundTripModelSolidShapesAreEqual(expected, + actual, + expected.collisionSolidShapes(), + actual.collisionSolidShapes(), + tol); + assertJSONRoundTripSensorsAreEqual(expected, actual, tol); +} + +} // namespace iDynTree + +#endif diff --git a/src/tools/CMakeLists.txt b/src/tools/CMakeLists.txt index d04a16ed8c..ddff4e172b 100644 --- a/src/tools/CMakeLists.txt +++ b/src/tools/CMakeLists.txt @@ -18,6 +18,7 @@ macro(IDYNTREE_ADD_TOOL tool_name tool_code) endmacro(IDYNTREE_ADD_TOOL tool_name tool_code) idyntree_add_tool(idyntree-model-info idyntree-model-info.cpp) +idyntree_add_tool(idyntree-model-convert idyntree-model-convert.cpp) if(IDYNTREE_USES_IRRLICHT) idyntree_add_tool(idyntree-model-view idyntree-model-view.cpp) diff --git a/src/tools/idyntree-model-convert.cpp b/src/tools/idyntree-model-convert.cpp new file mode 100644 index 0000000000..0da6d2a0c5 --- /dev/null +++ b/src/tools/idyntree-model-convert.cpp @@ -0,0 +1,113 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#include +#include +#include + +#include "ModelIOFormatUtils.h" + +#include "cmdline.h" + +#include +#include +#include + +void addOptions(cmdline::parser& cmd) +{ + cmd.add("input", 'i', "Input model path.", true); + cmd.add("output", 'o', "Output model path.", true); + + cmd.add("input-format", + '\0', + "Explicit input model format (`urdf`, `sdf`, " + "`idyntree-model-json`). If omitted, infer from extension.", + false); + + cmd.add("output-format", + '\0', + "Explicit output model format (`urdf`, `idyntree-model-json`). " + "If omitted, infer from extension.", + false); +} + +int main(int argc, char** argv) +{ + cmdline::parser cmd; + addOptions(cmd); + cmd.parse_check(argc, argv); + + std::string inputPath = cmd.get("input"); + std::string outputPath = cmd.get("output"); + + inputPath = iDynTree::resolveURI(inputPath); + + std::string inputFormat; + if (cmd.exist("input-format")) + { + inputFormat = iDynTree::normalizeModelFormatName(cmd.get("input-format")); + } else + { + inputFormat = iDynTree::inferModelFormatFromFilename(inputPath); + } + + std::string outputFormat; + if (cmd.exist("output-format")) + { + outputFormat = iDynTree::normalizeModelFormatName(cmd.get("output-format")); + } else + { + outputFormat = iDynTree::inferModelFormatFromFilename(outputPath); + } + + if (inputFormat.empty()) + { + std::cerr << "Unable to infer input format from '" << inputPath + << "'. Please pass --input-format." << std::endl; + return EXIT_FAILURE; + } + + if (!iDynTree::isSupportedImportModelFormat(inputFormat)) + { + std::cerr << "Input format '" << inputFormat + << "' is not supported by ModelLoader. Supported import formats are: " + << "urdf, sdf, idyntree-model-json." << std::endl; + return EXIT_FAILURE; + } + + if (outputFormat.empty()) + { + std::cerr << "Unable to infer output format from '" << outputPath + << "'. Please pass --output-format." << std::endl; + return EXIT_FAILURE; + } + + if (!iDynTree::isSupportedExportModelFormat(outputFormat)) + { + std::cerr << "Output format '" << outputFormat + << "' is not supported by ModelExporter. Supported output formats are: " + << "urdf, idyntree-model-json." << std::endl; + return EXIT_FAILURE; + } + + iDynTree::ModelLoader loader; + if (!loader.loadModelFromFile(inputPath, inputFormat)) + { + std::cerr << "Impossible to read model at file '" << inputPath << "'" + << " with input format '" << inputFormat << "'." << std::endl; + return EXIT_FAILURE; + } + + iDynTree::ModelExporter exporter; + if (!exporter.init(loader.model()) || !exporter.exportModelToFile(outputPath, outputFormat)) + { + std::cerr << "Impossible to export model to file '" << outputPath << "'" + << " with output format '" << outputFormat << "'." << std::endl; + return EXIT_FAILURE; + } + + std::cout << "Converted model from '" << inputPath << "' (" << inputFormat << ")" + << " to '" << outputPath << "' (" << outputFormat << ")." << std::endl; + + return EXIT_SUCCESS; +}