Merge pull request #576 from robotology/addModelCalibrationHelper

Add ModelCalibrationHelper class

doc/releases/v0_12.md

@@ -64,6 +64,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 ### `modelio`
 * Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
 * Added support in the URDF parser to correctly parse the optional name parameter of visual and collision elements.
+* Added `iDynTree::ModelCalibrationHelper` to simplify loading a model from file, update its inertial parameters and exporting again to file (https://github.com/robotology/idyntree/pull/576).
 
 ### `yarprobotstatepublisher`
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.

src/model_io/urdf/CMakeLists.txt

@@ -15,6 +15,7 @@ endif()
 set(IDYNTREE_MODELIO_URDF_HEADERS include/iDynTree/ModelIO/URDFDofsImport.h
                                   include/iDynTree/ModelIO/ModelLoader.h
                                   include/iDynTree/ModelIO/ModelExporter.h
+                                  include/iDynTree/ModelIO/ModelCalibrationHelper.h
                                   include/deprecated/iDynTree/ModelIO/URDFModelImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFGenericSensorsImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFSolidShapesImport.h)
@@ -48,6 +49,7 @@ set(IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES src/URDFDocument.cpp
 set(IDYNTREE_MODELIO_URDF_SOURCES src/URDFDofsImport.cpp
                                   src/ModelLoader.cpp
                                   src/ModelExporter.cpp
+                                  src/ModelCalibrationHelper.cpp
                                   src/deprecated/URDFModelImport.cpp
                                   src/deprecated/URDFGenericSensorsImport.cpp
                                   src/deprecated/URDFSolidShapesImport.cpp

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

@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2016 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_CALIBRATION_HELPER_H
+#define IDYNTREE_MODEL_CALIBRATION_HELPER_H
+
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/Sensors/Sensors.h>
+#include <iDynTree/ModelIO/ModelExporter.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace iDynTree
+{
+
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Helper class to load a model, modify its parameters based on calibration,
+ * and save it again to file.
+ *
+ */
+class ModelCalibrationHelper
+{
+private:
+    class ModelCalibrationHelperPimpl;
+    std::unique_ptr<ModelCalibrationHelperPimpl> m_pimpl;
+
+public:
+
+    /**
+     * @name Constructor/Destructor
+     */
+    //@{
+
+    /**
+     * Constructor
+     *
+     */
+    ModelCalibrationHelper();
+
+    ~ModelCalibrationHelper();
+
+    //@}
+
+    /**
+     * 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.
+     *
+     */
+    bool loadModelFromString(const std::string & modelString, const std::string & filetype="urdf");
+
+    /**
+     * 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.
+     *
+     */
+    bool loadModelFromFile(const std::string & filename, const std::string & filetype="urdf");
+
+    /**
+     * Update the inertial parameters of the loaded model.
+     *
+     * @note the inertial params vector follow the structure of the Model::getInertialParameters method.
+     */
+    bool updateModelInertialParametersToString(std::string & modelString, 
+                                               const iDynTree::VectorDynSize& inertialParams,
+                                               const std::string filetype="urdf",
+                                               const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions());
+
+    /**
+     * Update the inertial parameters of the loaded model.
+     *
+     * @note the inertial params vector follows the structure of the Model::getInertialParameters method.
+     */
+    bool updateModelInertialParametersToFile(const std::string & filename, 
+                                             const iDynTree::VectorDynSize& inertialParams,
+                                             const std::string filetype="urdf",
+                                             const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions());
+
+    /**
+     * Get the loaded model.
+     *
+     * @note This always return the model loaded via loadModel methods, and is not affected by the updateModel methods.
+     */
+    const Model & model();
+
+    /**
+     * Get the loaded sensors.
+     * 
+     * @note This always return the model loaded via loadModel method, and is not affected by the updateModel methods.
+     */
+    const SensorsList & sensors();
+
+    /**
+     * Return true if the model have been correctly true.
+     *
+     * @note This always return the validity of the model loaded via loadModel method, and is not affected by the updateModel methods.
+     * @return True if the model was loaded correctly.
+     */
+    bool isValid();
+    //@}
+};
+
+}
+
+#endif

src/model_io/urdf/src/ModelCalibrationHelper.cpp

@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2016 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.
+ */
+
+#include <iDynTree/ModelIO/ModelCalibrationHelper.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+#include <iDynTree/ModelIO/ModelExporter.h>
+
+#include <string>
+#include <vector>
+
+namespace iDynTree
+{
+    class ModelCalibrationHelper::ModelCalibrationHelperPimpl {
+    public:
+        ModelLoader modelLoader;
+        ModelExporter modelExporter;
+
+        ModelCalibrationHelperPimpl() {}
+    };
+
+    ModelCalibrationHelper::ModelCalibrationHelper()
+    : m_pimpl(new ModelCalibrationHelperPimpl())
+    {
+    }
+
+    ModelCalibrationHelper::~ModelCalibrationHelper() {}
+
+    const Model& ModelCalibrationHelper::model()
+    {
+        return m_pimpl->modelLoader.model();
+    }
+
+    const SensorsList& ModelCalibrationHelper::sensors()
+    {
+        return m_pimpl->modelLoader.sensors();
+    }
+
+    bool ModelCalibrationHelper::isValid()
+    {
+        return m_pimpl->modelLoader.isValid();
+    }
+
+    bool ModelCalibrationHelper::loadModelFromString(const std::string& xmlString,
+                                                     const std::string& filetype)
+    {
+        return m_pimpl->modelLoader.loadModelFromString(xmlString, filetype);
+    }
+
+    bool ModelCalibrationHelper::loadModelFromFile(const std::string& filename,
+                                        const std::string& filetype)
+    {
+        return m_pimpl->modelLoader.loadModelFromFile(filename, filetype);
+    }
+
+    bool ModelCalibrationHelper::updateModelInertialParametersToString(std::string & model_string, 
+                                                                       const iDynTree::VectorDynSize& inertialParams,
+                                                                       const std::string filetype,
+                                                                       const ModelExporterOptions options)
+    {
+        Model exportedModel = this->model();
+        SensorsList exportedSensors = this->sensors();
+
+        bool ok = exportedModel.updateInertialParameters(inertialParams);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToString", "Error in iDynTree::Model::updateInertialParameters method.");
+            return false;
+        }
+
+        ok = m_pimpl->modelExporter.init(exportedModel, exportedSensors, options);
+        ok = ok && m_pimpl->modelExporter.exportModelToString(model_string, filetype);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToString", "Error in ModelExporter::exportModelToString method.");
+            return false;
+        }
+
+        return true;
+    }
+
+
+    bool ModelCalibrationHelper::updateModelInertialParametersToFile(const std::string & filename, 
+                                                                     const iDynTree::VectorDynSize& inertialParams,
+                                                                     const std::string filetype,
+                                                                     const ModelExporterOptions options)
+    {
+        Model exportedModel = this->model();
+        SensorsList exportedSensors = this->sensors();
+
+        bool ok = exportedModel.updateInertialParameters(inertialParams);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToFile", "Error in iDynTree::Model::updateInertialParameters method.");
+            return false;
+        }
+
+        ok = m_pimpl->modelExporter.init(exportedModel, exportedSensors, options);
+        ok = ok && m_pimpl->modelExporter.exportModelToFile(filename, filetype);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToFile", "Error in ModelExporter::exportModelToFile method.");
+            return false;
+        }
+
+        return true;
+    }
+
+}

src/model_io/urdf/tests/CMakeLists.txt

@@ -24,6 +24,7 @@ macro(add_modelio_urdf_unit_test classname)
 endmacro()
 
 add_modelio_urdf_unit_test(URDFModelImport)
+add_modelio_urdf_unit_test(ModelCalibrationHelper)
 add_modelio_urdf_unit_test(ModelExporter)
 add_modelio_urdf_unit_test(URDFGenericSensorImport)
 add_modelio_urdf_unit_test(PredictSensorsMeasurement)

src/model_io/urdf/tests/ModelCalibrationHelperUnitTest.cpp

@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2018 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.
+ */
+
+#include "testModels.h"
+
+#include <iDynTree/Core/TestUtils.h>
+
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+#include <iDynTree/ModelIO/ModelCalibrationHelper.h>
+
+#include <cassert>
+#include <cstdio>
+#include <cstdlib>
+#include <algorithm>
+
+using namespace iDynTree;
+
+double getTotalMass(const Model& model)
+{
+    double totalMass = 0.0;
+    for(size_t l=0; l < model.getNrOfLinks(); l++)
+    {
+        totalMass += model.getLink(l)->getInertia().getMass();
+    }
+
+    return totalMass;
+}
+
+int main()
+{
+    // Open file with a single link with mass 1
+    std::string urdfFileName = getAbsModelPath("oneLink.urdf");
+
+    ModelCalibrationHelper mdlCalibHelper;
+    bool ok = mdlCalibHelper.loadModelFromFile(urdfFileName);
+    ASSERT_IS_TRUE(ok);
+
+    // Check that the mass is one
+    double expectedMass = 1.0;
+    ASSERT_EQUAL_DOUBLE(getTotalMass(mdlCalibHelper.model()), expectedMass);
+
+    // Modify the mass as an inertial paramters
+    VectorDynSize inertialParams(10*mdlCalibHelper.model().getNrOfLinks());
+
+    mdlCalibHelper.model().getInertialParameters(inertialParams);
+
+    ASSERT_EQUAL_DOUBLE(inertialParams(0), expectedMass);
+    double newMass = 2.0;
+    inertialParams(0) = newMass;
+
+    std::string newModel;
+    ok = mdlCalibHelper.updateModelInertialParametersToString(newModel, inertialParams);
+    ASSERT_IS_TRUE(ok);
+
+    // Write to file for debug
+    ok = mdlCalibHelper.updateModelInertialParametersToFile("ModelCalibrationHelperUnitTestModel.urdf", inertialParams);
+    ASSERT_IS_TRUE(ok);
+
+    std::cerr << newModel << std::endl;
+
+    // Verify mass
+    ModelLoader mdlLoader;
+
+    ok = mdlLoader.loadModelFromString(newModel);
+    ASSERT_IS_TRUE(ok);
+    ASSERT_EQUAL_DOUBLE(getTotalMass(mdlLoader.model()), newMass);
+
+
+
+
+
+    return EXIT_SUCCESS;
+}