From fd5fa3063cf2e92584e53142b6914edb173f0d70 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 17 Apr 2024 01:00:37 +0200 Subject: [PATCH 1/2] Extend iDynTree::ModelLoader::loadReducedModelFromFullModel method to optionally take a given value for lumped models --- .../include/iDynTree/ModelTransformers.h | 21 ++++- src/model/src/ModelTransformers.cpp | 51 +++++++++++- src/model/tests/ModelUnitTest.cpp | 27 +++++++ .../codecs/include/iDynTree/ModelLoader.h | 80 +++++++++++++++++++ src/model_io/codecs/src/ModelLoader.cpp | 36 ++++++++- 5 files changed, 206 insertions(+), 9 deletions(-) diff --git a/src/model/include/iDynTree/ModelTransformers.h b/src/model/include/iDynTree/ModelTransformers.h index 6e54b9e75f..fae1d932ef 100644 --- a/src/model/include/iDynTree/ModelTransformers.h +++ b/src/model/include/iDynTree/ModelTransformers.h @@ -14,6 +14,7 @@ #ifndef IDYNTREE_MODEL_TRANSFORMERS_H #define IDYNTREE_MODEL_TRANSFORMERS_H +#include #include #include @@ -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& 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& jointsInReducedModel, + Model& reducedModel, + const std::unordered_map& 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. diff --git a/src/model/src/ModelTransformers.cpp b/src/model/src/ModelTransformers.cpp index 6a314864dc..81f5a4ebad 100644 --- a/src/model/src/ModelTransformers.cpp +++ b/src/model/src/ModelTransformers.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -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& 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 emptyRemovedJointPositions; + return createReducedModel(fullModel, jointsInReducedModel, reducedModel, emptyRemovedJointPositions); +} + bool createModelWithNormalizedJointNumbering(const Model& model, const std::string& baseForNormalizedJointNumbering, Model& normalizedModel) diff --git a/src/model/tests/ModelUnitTest.cpp b/src/model/tests/ModelUnitTest.cpp index d5462e6f6d..fb9b975d42 100644 --- a/src/model/tests/ModelUnitTest.cpp +++ b/src/model/tests/ModelUnitTest.cpp @@ -92,6 +92,29 @@ void getRandomSubsetOfJoints(const Model & model, } } +void getRandomJointPositonsForJointsNotInReducedModels(const Model & fullModel, + const std::vector& subsetOfJointsInReducedModel, + std::unordered_map& removedJointPositions, + FreeFloatingPos& fullModelPos) +{ + for(auto jntName: subsetOfJointsInReducedModel) + { + std::cerr << " " << jntName; + } + std::cerr << std::endl; + for(JointIndex jntIndex = 0; jntIndex < fullModel.getNrOfJoints(); jntIndex++) + { + // Check if joint is in reduced model + std::string jointName = fullModel.getJointName(jntIndex); + if (!isStringInVector(jointName, subsetOfJointsInReducedModel)) + { + double jointConf = iDynTree::getRandomDouble(); + removedJointPositions[jointName] = jointConf; + fullModelPos.jointPos()(fullModel.getJoint(jntIndex)->getPosCoordsOffset()) = jointConf; + } + } +} + class RNEAHelperClass { private: @@ -197,6 +220,10 @@ void checkReducedModel(const Model & model) std::vector jointInReducedModel; getRandomSubsetOfJoints(model,jnts,jointInReducedModel); + // Get random positions for reduced models + std::unordered_map removedJointPositions; + getRandomJointPositonsForJointsNotInReducedModels(model, jointInReducedModel, removedJointPositions, fullPos); + Model reducedModel; bool ok = createReducedModel(model,jointInReducedModel,reducedModel); diff --git a/src/model_io/codecs/include/iDynTree/ModelLoader.h b/src/model_io/codecs/include/iDynTree/ModelLoader.h index 9a0c1c8cb2..33c56a7f65 100644 --- a/src/model_io/codecs/include/iDynTree/ModelLoader.h +++ b/src/model_io/codecs/include/iDynTree/ModelLoader.h @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -200,6 +201,85 @@ class ModelLoader const std::vector & consideredJoints, const std::string filetype="", const std::vector& 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 & consideredJoints, + const std::unordered_map& 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 & consideredJoints, + const std::unordered_map& removedJointPositions, + const std::string filetype="", + const std::vector& 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 & consideredJoints, + const std::unordered_map& removedJointPositions, + const std::string filetype="", + const std::vector& packageDirs = {}); + /** * Get the loaded model. diff --git a/src/model_io/codecs/src/ModelLoader.cpp b/src/model_io/codecs/src/ModelLoader.cpp index ff6ba59c7c..ba9f20c619 100644 --- a/src/model_io/codecs/src/ModelLoader.cpp +++ b/src/model_io/codecs/src/ModelLoader.cpp @@ -8,7 +8,6 @@ #include #include - #include #include @@ -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 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& packageDirs /*= {}*/) + { + std::unordered_map 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& packageDirs /*= {}*/) + { + std::unordered_map 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& 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& removedJointPositions, const std::string filetype, const std::vector& 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& removedJointPositions, const std::string filetype, const std::vector& 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) { From ac54ad23b261d80eeb63bad0a58d676d88d935aa Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 17 Apr 2024 23:34:27 +0200 Subject: [PATCH 2/2] Add test for new createReducedModel argument --- src/model/tests/ModelUnitTest.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/model/tests/ModelUnitTest.cpp b/src/model/tests/ModelUnitTest.cpp index fb9b975d42..3b56279a4a 100644 --- a/src/model/tests/ModelUnitTest.cpp +++ b/src/model/tests/ModelUnitTest.cpp @@ -97,20 +97,20 @@ void getRandomJointPositonsForJointsNotInReducedModels(const Model & fullModel, std::unordered_map& removedJointPositions, FreeFloatingPos& fullModelPos) { - for(auto jntName: subsetOfJointsInReducedModel) - { - std::cerr << " " << jntName; - } - std::cerr << std::endl; 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)) { - double jointConf = iDynTree::getRandomDouble(); - removedJointPositions[jointName] = jointConf; - fullModelPos.jointPos()(fullModel.getJoint(jntIndex)->getPosCoordsOffset()) = jointConf; + if (fullModel.getJoint(jntIndex)->getNrOfDOFs() == 1) + { + double jointConf = iDynTree::getRandomDouble(); + removedJointPositions[jointName] = jointConf; + fullModelPos.jointPos()(fullModel.getJoint(jntIndex)->getPosCoordsOffset()) = jointConf; + } } } } @@ -217,6 +217,8 @@ 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 jointInReducedModel; getRandomSubsetOfJoints(model,jnts,jointInReducedModel); @@ -225,7 +227,7 @@ void checkReducedModel(const Model & model) 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); @@ -253,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);