Add ModelExporter class, initial support for URDF export and tests

fixup

doc/releases/v0_12.md

@@ -55,6 +55,9 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 ### `Model`
 * Implement `getTotalMass()` method for Model class
 
+### `modelio`
+* Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
+
 ### `yarprobotstatepublisher`
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
 * `robot` option is deprecated, and replaced by `name-prefix`.

src/model_io/urdf/CMakeLists.txt

@@ -14,6 +14,7 @@ endif()
 
 set(IDYNTREE_MODELIO_URDF_HEADERS include/iDynTree/ModelIO/URDFDofsImport.h
                                   include/iDynTree/ModelIO/ModelLoader.h
+                                  include/iDynTree/ModelIO/ModelExporter.h
                                   include/deprecated/iDynTree/ModelIO/URDFModelImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFGenericSensorsImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFSolidShapesImport.h)
@@ -29,7 +30,8 @@ set(IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS include/private/URDFDocument.h
                                           include/private/MaterialElement.h
                                           include/private/VisualElement.h
                                           include/private/GeometryElement.h
-                                          include/private/URDFParsingUtils.h)
+                                          include/private/URDFParsingUtils.h
+                                          include/private/URDFModelExport.h)
 
 set(IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES src/URDFDocument.cpp
                                               src/InertialElement.cpp
@@ -41,14 +43,15 @@ set(IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES src/URDFDocument.cpp
                                               src/ForceTorqueSensorElement.cpp
                                               src/MaterialElement.cpp
                                               src/VisualElement.cpp
-                                              src/GeometryElement.cpp
-                                              )
+                                              src/GeometryElement.cpp)
 
 set(IDYNTREE_MODELIO_URDF_SOURCES src/URDFDofsImport.cpp
                                   src/ModelLoader.cpp
+                                  src/ModelExporter.cpp
                                   src/deprecated/URDFModelImport.cpp
                                   src/deprecated/URDFGenericSensorsImport.cpp
-                                  src/deprecated/URDFSolidShapesImport.cpp)
+                                  src/deprecated/URDFSolidShapesImport.cpp
+                                  src/URDFModelExport.cpp)
 
 SOURCE_GROUP("Source Files" FILES ${IDYNTREE_MODELIO_URDF_SOURCES})
 SOURCE_GROUP("Source Files\\XML Elements" FILES ${IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES})
@@ -60,7 +63,7 @@ list(APPEND IDYNTREE_MODELIO_URDF_SOURCES ${IDYNTREE_MODELIO_URDF_XMLELEMENTS_SO
 # share headers with all iDynTree targets
 set(libraryname idyntree-modelio-urdf)
 
-add_library(${libraryname} ${IDYNTREE_MODELIO_URDF_SOURCES} ${IDYNTREE_MODELIO_URDF_HEADERS} ${IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS})
+add_library(${libraryname} ${IDYNTREE_MODELIO_URDF_SOURCES} ${IDYNTREE_MODELIO_URDF_HEADERS} ${IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS} $<TARGET_OBJECTS:idyntree-private-fpconv>)
 
 target_compile_features(${libraryname} PRIVATE cxx_auto_type cxx_delegating_constructors cxx_final cxx_lambdas cxx_lambda_init_captures)
 
@@ -72,11 +75,14 @@ target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURR
                                                   ${LIBXML2_INCLUDE_DIR})
 
 target_link_libraries(${libraryname} LINK_PUBLIC idyntree-core idyntree-model idyntree-sensors idyntree-modelio-xml
-                                     LINK_PRIVATE ${TinyXML_LIBRARIES})
+                                     LINK_PRIVATE ${LIBXML2_LIBRARIES})
 
 target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS} ${LIBXML2_DEFINITIONS})
 
 target_include_directories(${libraryname} PRIVATE ${EIGEN3_INCLUDE_DIR})
+# 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>)
 
 # Ensure that build include directories are always included before system ones
 get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)

src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h

@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef IDYNTREE_MODEL_EXPORTER_H
+#define IDYNTREE_MODEL_EXPORTER_H
+
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/Sensors/Sensors.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace iDynTree
+{
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Options for the iDynTree exporter.
+ */
+class ModelExporterOptions
+{
+public:
+    /**
+     * Specify the base link of the exported model.
+     *
+     * Differently from the iDynTree::Model class, some file formats (such as the URDF) represent the multibody structure
+     * using a directed tree representation, for which it is necessary to explicitly specify a base link.
+     *
+     * If this string is empty (default value), the default base link contained in the model will be used.
+     *
+     * Default value: "".
+     * Supported formats: urdf.
+     */
+    std::string baseLink;
+
+    /**
+     * Constructor.
+     */
+    ModelExporterOptions();
+
+};
+
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * 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 .
+ *
+ * Only iDynTree::Model classes that represent multibody system with no loops
+ * can be exported.
+ *
+ * Furthermore, currently the model exporter only exports a subset of the features
+ * supported in iDynTree::Model. In particular, the following features are not exported:
+ * * Additional frames
+ * * Visual and collision geometries
+ * * Sensors
+ * * Joint limits, damping and static friction.
+ *
+ *
+ */
+class ModelExporter
+{
+private:
+
+    class Pimpl;
+    std::unique_ptr<Pimpl> m_pimpl;
+
+public:
+
+    /**
+     * @name Constructor/Destructor
+     */
+    //@{
+
+    /**
+     * Constructor
+     */
+    ModelExporter();
+
+    ~ModelExporter();
+
+    //@}
+
+    /**
+     * @name Model exporting and definition methods
+     * This methods are used to export the structure of your model.
+     */
+    //@{
+    const ModelExporterOptions& exportingOptions() const;
+
+    void setExportingOptions(const ModelExporterOptions& options);
+
+    /**
+     * Specifies the model of the robot to export.
+     *
+     * @param[in] model The used model.
+     * @param[in] sensors The used sensors.
+     * @param[in] options The used options.
+     * @return true if all went well, false otherwise.
+     */
+    bool init(const Model& model,
+              const SensorsList& sensors=SensorsList(),
+              const ModelExporterOptions options=ModelExporterOptions());
+
+    /**
+     * Get the loaded model that will be exported.
+     *
+     */
+    const Model & model();
+
+    /**
+     * Get the loaded sensors that will be exported.
+     */
+    const SensorsList & sensors();
+
+    /**
+     * Return true if the model have been correctly loaded, and can be exported.
+     *
+     * @return True if the model was loaded correctly.
+     */
+    bool isValid();
+
+    /**
+     * 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.
+     *
+     */
+    bool exportModelToString(std::string & modelString, const std::string filetype="urdf");
+
+    /**
+     * Export the model of the robot to an external file.
+     *
+     * @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.
+     *
+     */
+    bool exportModelToFile(const std::string & filename, const std::string filetype="urdf");
+    //@}
+};
+
+}
+
+#endif

src/model_io/urdf/include/private/URDFModelExport.h

@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Silvio Traversaro
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ */
+
+#ifndef IDYNTREE_URDF_MODEL_EXPORT_H
+#define IDYNTREE_URDF_MODEL_EXPORT_H
+
+#include <string>
+
+#include <iDynTree/ModelIO/ModelExporter.h>
+
+namespace iDynTree
+
+{
+
+class Model;
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Export a iDynTree::Model object to a URDF file.
+ *
+ * @see iDynTree::ModelExporterOptions for more details on supported and default options.
+ *
+ * @warning This function does not support exporting sensor or solid shapes at the moment.
+ *
+ * @param[in] urdf_filename Path to the URDF file that will be created.
+ *                          It can be either a relative filename with respect to the current working directory,
+ *                          or an absolute filename.
+ * @param[in] options the iDynTree::ModelExporterOptions struct of options passed to the parser.
+ * @return true if all went ok, false otherwise.
+ */
+bool URDFFromModel(const iDynTree::Model & model,
+                   const std::string & urdf_filename,
+                   const ModelExporterOptions options=ModelExporterOptions());
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Export a iDynTree::Model object to a URDF string.
+ *
+ * @see iDynTree::ModelExporterOptions for more details on supported and default options.
+ *
+ * @warning This function does not support exporting sensor or solid shapes at the moment.
+ *
+ * @param[in] options the iDynTree::ModelExporterOptions struct of options passed to the parser.
+ * @return true if all went ok, false otherwise.
+ */
+bool URDFStringFromModel(const iDynTree::Model & output,
+                         std::string & urdf_string,
+                         const ModelExporterOptions options=ModelExporterOptions());
+
+
+}
+
+#endif

src/model_io/urdf/include/private/URDFParsingUtils.h

@@ -11,6 +11,7 @@
 #ifndef IDYNTREE_URDF_PARSING_UTILS_H
 #define IDYNTREE_URDF_PARSING_UTILS_H
 
+#include <cmath>
 #include <cstdlib>
 #include <sstream>
 #include <string>
@@ -19,6 +20,8 @@
 #include <iDynTree/Core/Transform.h>
 #include <iDynTree/Core/VectorFixSize.h>
 
+#include <fpconv.h>
+
 namespace iDynTree
 {
 
@@ -47,6 +50,21 @@ bool inline stringToUnsignedIntWithClassicLocale(const std::string & inStr, unsi
     return !(ss.fail());
 }
 
+bool inline doubleToStringWithClassicLocale(const double & inDouble, std::string& outStr)
+{
+    if (std::isnan(inDouble) || std::isinf(inDouble)) {
+        return false;
+    }
+
+    // fpconv returns nul-terminated strings, that can be converted directly to C++ std::string
+    // see https://github.com/night-shift/fpconv/tree/4a087d1b2df765baa409536931916a2c082cdda4#example-usage
+    char buf[24 + 1];
+    int str_len = idyntree_private_fpconv_dtoa(inDouble, buf);
+    buf[str_len] = '\0';
+    outStr = buf;
+    return true;
+}
+
 std::string inline intToString(const int inInt)
 {
     std::stringstream ss;
@@ -116,6 +134,27 @@ bool inline vector3FromString(const std::string & vector_str, Vector3 & out)
     return true;
 }
 
+template<typename iDynTreeVectorType>
+bool inline vectorToString(const iDynTreeVectorType & in, std::string & out_str)
+{
+    std::stringstream ss;
+    bool ok = true;
+    for (unsigned int i = 0; i < in.size(); ++i)
+    {
+        std::string bufStr;
+        ok = ok && doubleToStringWithClassicLocale(in(i), bufStr);
+        if (i != 0)
+        {
+            ss << " ";
+        }
+        ss << bufStr;
+    }
+
+    out_str = ss.str();
+
+    return ok;
+}
+
 
 bool inline vector4FromString(const std::string & vector_str, Vector4 & out)
 {