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)
  {