Merge pull request #1174 from robotology/fix495

Extend iDynTree::ModelLoader::loadReducedModelFromFullModel method to optionally take a given value for lumped models

src/model/include/iDynTree/ModelTransformers.h

@@ -14,6 +14,7 @@
 #ifndef IDYNTREE_MODEL_TRANSFORMERS_H
 #define IDYNTREE_MODEL_TRANSFORMERS_H
 
+#include <unordered_map>
 #include <string>
 #include <vector>
 
@@ -41,7 +42,7 @@ class Traversal;
  * and with the same transform is added to the model.
  *
  * @warning This function does not handle SensorsList contained inside the input model.
- * 
+ *
  * \note The definition of "fake link" used in this function excludes
  *       the case in which two fake links are attached to one another.
  *
@@ -65,6 +66,22 @@ bool createReducedModel(const Model& fullModel,
                         const std::vector<std::string>& jointsInReducedModel,
                         Model& reducedModel);
 
+/**
+ * This function takes in input a iDynTree::Model and
+ * an ordered list of joints and returns a model with
+ * just the joint specified in the list, with that exact order.
+ *
+ * All other joints are be removed by lumping (i.e. fusing together)
+ * the inertia of the links that are connected by that joint, assuming the joint
+ * to be in "rest" position (i.e. zero for revolute or prismatic joints), or the position
+ * specified in the removedJointPositions if a given joint is specified in removedJointPositions
+ *
+ */
+bool createReducedModel(const Model& fullModel,
+                        const std::vector<std::string>& jointsInReducedModel,
+                        Model& reducedModel,
+                        const std::unordered_map<std::string, double>& removedJointPositions);
+
 /**
  * @brief Given a specified base, return a model with a "normalized" joint numbering for that base.
  *
@@ -79,7 +96,7 @@ bool createReducedModel(const Model& fullModel,
  *       this numbering is not required by any algorithm in iDynTree, but it may be useful for
  *       example to ensure that for a chain model the joint numbering is the one induced by the
  *       kinematic structure.
- * 
+ *
  * @warning This function does not handle SensorsList contained inside the input model.
  *
  * @return true if all went well, false if there was an error in conversion.

src/model/src/ModelTransformers.cpp

@@ -14,6 +14,7 @@
 #include <iDynTree/SixAxisForceTorqueSensor.h>
 
 #include <cassert>
+#include <unordered_map>
 #include <set>
 #include <vector>
 
@@ -329,7 +330,8 @@ void reducedModelAddSolidShapes(const Model& fullModel,
 
 bool createReducedModel(const Model& fullModel,
                         const std::vector< std::string >& jointsInReducedModel,
-                        Model& reducedModel)
+                        Model& reducedModel,
+                        const std::unordered_map<std::string, double>& removedJointPositions)
 {
     // We use the default traversal for deciding the base links of the reduced model
     Traversal fullModelTraversal;
@@ -361,10 +363,45 @@ bool createReducedModel(const Model& fullModel,
 
     // The position for the joint removed from the model is supposed to be 0
     FreeFloatingPos jointPos(fullModel);
+
     // \todo used an appropriate method here
-    for(size_t posCoord=0; posCoord < fullModel.getNrOfPosCoords(); posCoord++)
+    for(JointIndex jntIdx=0; jntIdx < fullModel.getNrOfJoints(); jntIdx++)
     {
-        jointPos.jointPos()(posCoord) = 0.0;
+        // Get nr of DOFs for joint
+        size_t nrOfDofs = fullModel.getJoint(jntIdx)->getNrOfDOFs();
+
+        // Nothing to do if the joint is fixed
+        if (nrOfDofs == 0)
+        {
+            continue;
+        }
+
+        // If the joint has 1 DOF, either use the value specified in removedJointPositions
+        // or if no value is found in removedJointPositions, use 0
+        if (nrOfDofs == 1)
+        {
+            auto it = removedJointPositions.find(fullModel.getJointName(jntIdx));
+
+            if (it != removedJointPositions.end())
+            {
+                jointPos.jointPos()(fullModel.getJoint(jntIdx)->getPosCoordsOffset()) = it->second;
+            }
+            else
+            {
+                jointPos.jointPos()(fullModel.getJoint(jntIdx)->getPosCoordsOffset()) = 0.0;
+            }
+
+            continue;
+        }
+
+        // If the joint has nrOfDofs > 1, we do not support specifying it in removedJointPositions
+        if (nrOfDofs > 1)
+        {
+            for (size_t j=0; j < nrOfDofs; j++)
+            {
+                jointPos.jointPos()(fullModel.getJoint(jntIdx)->getPosCoordsOffset() + j) = 0.0;
+            }
+        }
     }
 
     for(size_t linkInReducedModel = 0;
@@ -694,6 +731,14 @@ bool createReducedModel(const Model& fullModel,
     return ok;
 }
 
+bool createReducedModel(const Model& fullModel,
+                        const std::vector< std::string >& jointsInReducedModel,
+                        Model& reducedModel)
+{
+    std::unordered_map<std::string, double> emptyRemovedJointPositions;
+    return createReducedModel(fullModel, jointsInReducedModel, reducedModel, emptyRemovedJointPositions);
+}
+
 bool createModelWithNormalizedJointNumbering(const Model& model,
                                              const std::string& baseForNormalizedJointNumbering,
                                              Model& normalizedModel)

src/model/tests/ModelUnitTest.cpp

@@ -92,6 +92,29 @@ void getRandomSubsetOfJoints(const Model & model,
     }
 }
 
+void getRandomJointPositonsForJointsNotInReducedModels(const Model & fullModel,
+                                                       const std::vector<std::string>& subsetOfJointsInReducedModel,
+                                                       std::unordered_map<std::string, double>& removedJointPositions,
+                                                       FreeFloatingPos& fullModelPos)
+{
+    for(JointIndex jntIndex = 0; jntIndex < fullModel.getNrOfJoints(); jntIndex++)
+    {
+        // Check if joint is in reduced model
+        std::string jointName = fullModel.getJointName(jntIndex);
+
+        // Only set non-zero position if the DOF size is exactly 1
+        if (!isStringInVector(jointName, subsetOfJointsInReducedModel))
+        {
+            if (fullModel.getJoint(jntIndex)->getNrOfDOFs() == 1)
+            {
+                double jointConf = iDynTree::getRandomDouble();
+                removedJointPositions[jointName] = jointConf;
+                fullModelPos.jointPos()(fullModel.getJoint(jntIndex)->getPosCoordsOffset()) = jointConf;
+            }
+        }
+    }
+}
+
 class RNEAHelperClass
 {
 private:
@@ -194,11 +217,17 @@ void checkReducedModel(const Model & model)
     // is giving the same results
     for(size_t jnts=0; jnts < model.getNrOfJoints(); jnts += 5)
     {
+        FreeFloatingPos fullPos(model);
+
         std::vector<std::string> jointInReducedModel;
         getRandomSubsetOfJoints(model,jnts,jointInReducedModel);
 
+        // Get random positions for reduced models
+        std::unordered_map<std::string, double> removedJointPositions;
+        getRandomJointPositonsForJointsNotInReducedModels(model, jointInReducedModel, removedJointPositions, fullPos);
+
         Model reducedModel;
-        bool ok = createReducedModel(model,jointInReducedModel,reducedModel);
+        bool ok = createReducedModel(model, jointInReducedModel, reducedModel, removedJointPositions);
 
         ASSERT_EQUAL_DOUBLE(ok,1.0);
 
@@ -226,7 +255,6 @@ void checkReducedModel(const Model & model)
         reducedAcc.baseAcc() = getRandomTwist();
         getRandomVector(reducedAcc.jointAcc());
 
-        FreeFloatingPos fullPos(model);
         FreeFloatingVel fullVel(model);
         FreeFloatingAcc fullAcc(model);
         FreeFloatingGeneralizedTorques fullTrqs(model);

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

@@ -7,6 +7,7 @@
 #include <iDynTree/Model.h>
 #include <iDynTree/Sensors.h>
 
+#include <unordered_map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -200,6 +201,85 @@ class ModelLoader
                                   const std::vector<std::string> & consideredJoints,
                                   const std::string filetype="",
                                   const std::vector<std::string>& packageDirs = {});
+    /**
+     * Load reduced model from another model, specifyng only the desired joints in the model.
+     *
+     * All other joints will be considered to be fixed to their default position,
+     * and their child links will be lumped together.
+     *
+     * @note the order of the degreese of freedom of the newly loaded model
+     * will be the one specified by the input joints serialization, i.e. consideredJoints
+     *
+     * @param[in] filename path to the file to load.
+     * @param[in] consideredJoints list of joints to consider in the model.
+     * @param[in] removedJointPositions map between the dof name of the dof no in consideredJoints and their (fixed) position in the reduced model
+     * @param[in] filetype (optional) explicit definition of the type of the loaded file. Only "urdf" is supported at the moment.
+     * @return true if all went well (files were correctly loaded and consistent), false otherwise.
+     *
+     * \note Until https://github.com/robotology/idyntree/issues/132 is fixed, this method does not
+     *       accounts for sensors.
+     *
+     */
+    bool loadReducedModelFromFullModel(const Model& fullModel,
+                                       const std::vector<std::string> & consideredJoints,
+                                       const std::unordered_map<std::string, double>& removedJointPositions,
+                                       const std::string filetype="");
+
+    /**
+     * Load reduced model from string, specifyng only the desired joints in the model.
+     *
+     * All other joints will be considered to be fixed to their default position,
+     * and their child links will be lumped together.
+     *
+     * @param[in] modelString string containg the model of the robot.
+     * @param[in] consideredJoints list of joints to consider in the model.
+     * @param[in] removedJointPositions map between the dof name of the dof no in consideredJoints and their (fixed) position in the reduced model
+     * @param[in] filetype (optional) explicit definiton of the filetype to load.
+     *                     Only "urdf" is supported at the moment.
+     * @param[in] packageDirs (optional) a vector containing the different directories where to
+     *                        search for model meshes
+     * @return true if all went well (files were correctly loaded and consistent), false otherwise.
+     *
+     * @note In case no package is specified ModelLoader will look for the meshes in `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`.
+     *
+     */
+    bool loadReducedModelFromString(const std::string modelString,
+                                    const std::vector<std::string> & consideredJoints,
+                                    const std::unordered_map<std::string, double>& removedJointPositions,
+                                    const std::string filetype="",
+                                    const std::vector<std::string>& packageDirs = {});
+
+    /**
+     * Load reduced model from file, specifyng only the desired joints in the model.
+     *
+     * All other joints will be considered to be fixed to their default position,
+     * and their child links will be lumped together.
+     *
+     * @param[in] filename path to the file to load.
+     * @param[in] consideredJoints list of joints to consider in the model.
+     * @param[in] removedJointPositions map between the dof name of the dof no in consideredJoints and their (fixed) position in the reduced model
+     * @param[in] filetype (optional) explicit definiton of the filetype to load.
+     *                     Only "urdf" is supported at the moment.
+     * @param[in] packageDirs (optional) a vector containing the different directories where to
+     *                        search for model meshes
+     * @return true if all went well (files were correctly loaded and consistent), false otherwise.
+     *
+     * @note In case no package is specified ModelLoader will look for the meshes in `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`.
+     */
+    bool loadReducedModelFromFile(const std::string filename,
+                                  const std::vector<std::string> & consideredJoints,
+                                  const std::unordered_map<std::string, double>& removedJointPositions,
+                                  const std::string filetype="",
+                                  const std::vector<std::string>& packageDirs = {});
+
 
     /**
      * Get the loaded model.

src/model_io/codecs/src/ModelLoader.cpp

@@ -8,7 +8,6 @@
 #include <iDynTree/XMLParser.h>
 #include <iDynTree/ModelTransformers.h>
 
-
 #include <string>
 #include <vector>
 
@@ -128,11 +127,38 @@ namespace iDynTree
 
     bool ModelLoader::loadReducedModelFromFullModel(const Model& fullModel,
                                                     const std::vector< std::string >& consideredJoints,
+                                                    const std::string filetype)
+    {
+        std::unordered_map<std::string, double> emptyRemovedJointPositions;
+        return this->loadReducedModelFromFullModel(fullModel, consideredJoints, emptyRemovedJointPositions, filetype);
+    }
+
+    bool ModelLoader::loadReducedModelFromString(const std::string modelString,
+                                                 const std::vector< std::string >& consideredJoints,
+                                                 const std::string filetype,
+                                                 const std::vector<std::string>& packageDirs /*= {}*/)
+    {
+        std::unordered_map<std::string, double> emptyRemovedJointPositions;
+        return this->loadReducedModelFromString(modelString, consideredJoints, emptyRemovedJointPositions, filetype, packageDirs);
+    }
+
+    bool ModelLoader::loadReducedModelFromFile(const std::string filename,
+                                               const std::vector< std::string >& consideredJoints,
+                                               const std::string filetype,
+                                               const std::vector<std::string>& packageDirs /*= {}*/)
+    {
+        std::unordered_map<std::string, double> emptyRemovedJointPositions;
+        return this->loadReducedModelFromFile(filename, consideredJoints, emptyRemovedJointPositions, filetype, packageDirs);
+    }
+
+    bool ModelLoader::loadReducedModelFromFullModel(const Model& fullModel,
+                                                    const std::vector< std::string >& consideredJoints,
+                                                    const std::unordered_map<std::string, double>& removedJointPositions,
                                                     const std::string /*filetype*/)
     {
         Model _modelReduced;
         _modelReduced.setPackageDirs(fullModel.getPackageDirs());
-        bool ok = createReducedModel(fullModel,consideredJoints,_modelReduced);
+        bool ok = createReducedModel(fullModel,consideredJoints,_modelReduced, removedJointPositions);
 
         if( !ok )
         {
@@ -144,6 +170,7 @@ namespace iDynTree
 
     bool ModelLoader::loadReducedModelFromString(const std::string modelString,
                                                  const std::vector< std::string >& consideredJoints,
+                                                 const std::unordered_map<std::string, double>& removedJointPositions,
                                                  const std::string filetype,
                                                  const std::vector<std::string>& packageDirs /*= {}*/)
     {
@@ -153,7 +180,7 @@ namespace iDynTree
         _modelReduced.setPackageDirs(packageDirs);
 
         parsingCorrect = createReducedModel(_modelFull, consideredJoints,
-                                            _modelReduced);
+                                            _modelReduced, removedJointPositions);
 
         if (!parsingCorrect)
         {
@@ -165,6 +192,7 @@ namespace iDynTree
 
     bool ModelLoader::loadReducedModelFromFile(const std::string filename,
                                                const std::vector< std::string >& consideredJoints,
+                                               const std::unordered_map<std::string, double>& removedJointPositions,
                                                const std::string filetype,
                                                const std::vector<std::string>& packageDirs /*= {}*/)
     {
@@ -173,7 +201,7 @@ namespace iDynTree
         Model _modelFull = m_pimpl->m_model, _modelReduced;
         _modelReduced.setPackageDirs(packageDirs);
 
-        parsingCorrect = createReducedModel(_modelFull,consideredJoints,_modelReduced);
+        parsingCorrect = createReducedModel(_modelFull,consideredJoints,_modelReduced,removedJointPositions);
 
         if (!parsingCorrect)
         {