From b780df19064e76c5c0c3937d8fab4e42e2b437bc Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 5 Dec 2024 17:39:42 +0100 Subject: [PATCH] Add iDynTree::removeAdditionalFramesFromModel function to create a new model by removing additional frames from a given model (#1219) --- CMakeLists.txt | 2 +- .../include/iDynTree/ModelTransformers.h | 21 +++++ src/model/src/ModelTransformers.cpp | 91 ++++++++++++++++--- src/model/tests/ModelTransformersUnitTest.cpp | 30 +++++- 4 files changed, 127 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 024dfba04c..4559be0ed8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.16) -project(iDynTree VERSION 13.1.1 +project(iDynTree VERSION 13.2.0 LANGUAGES C CXX) # Disable in source build, unless Eclipse is used diff --git a/src/model/include/iDynTree/ModelTransformers.h b/src/model/include/iDynTree/ModelTransformers.h index 698d0136b5..96c185aee3 100644 --- a/src/model/include/iDynTree/ModelTransformers.h +++ b/src/model/include/iDynTree/ModelTransformers.h @@ -166,6 +166,27 @@ bool addValidNamesToAllSolidShapes(const iDynTree::Model& inputModel, bool moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(const iDynTree::Model& inputModel, iDynTree::Model& outputModel); +/** + * \function Remove all additional frames from the model, except a specified allowlist. + * + * This function takes in input a model, and return a model with all the additional + * frame list removed, except for the additional frames whose name is specified in + * the specified allowlist. + * + * @note The main use of this function is for processing models that need to be + * passed to other libraries or physics engines, where the additional frames + * may create problems or create performance problem. As long as you are using + * iDynTree, the presence of additional frames does not impact the performance + * of kinematics or dynamics algorithms, so there is no need to call this function + * to remove the additional frames. + * + * @return true if all went well, false if there was an error. + * + */ +bool removeAdditionalFramesFromModel(const Model& modelWithAllAdditionalFrames, + Model& modelWithOnlyAllowedAdditionalFrames, + const std::vector allowedAdditionalFrames = std::vector()); + } diff --git a/src/model/src/ModelTransformers.cpp b/src/model/src/ModelTransformers.cpp index fdfa4fa937..d11bf5e8da 100644 --- a/src/model/src/ModelTransformers.cpp +++ b/src/model/src/ModelTransformers.cpp @@ -13,6 +13,7 @@ #include #include +#include #include #include #include @@ -274,14 +275,41 @@ void computeTransformToSubModelBaseWithAdditionalTransform(const Model& fullMode } } +void addAdditionalFrameIfAllowed(Model& reducedModel, + const std::string linkInReducedModel, + const std::string additionalFrameName, + const Transform& subModelBase_H_additionalFrame, + bool includeAllAdditionalFrames, + const std::vector& allowedAdditionalFrames) +{ + bool shouldWeAddTheAdditionalFrame = true; + + // Check if we need to add the additional frame or not + if (!includeAllAdditionalFrames) + { + // If allowedAdditionalFrames has a value, we only need to add additional frames specified ther + if (std::find(allowedAdditionalFrames.begin(), allowedAdditionalFrames.end(), additionalFrameName) == allowedAdditionalFrames.end()) + { + shouldWeAddTheAdditionalFrame = false; + } + } + + if (shouldWeAddTheAdditionalFrame) + { + reducedModel.addAdditionalFrameToLink(linkInReducedModel,additionalFrameName, + subModelBase_H_additionalFrame); + } +} + void reducedModelAddAdditionalFrames(const Model& fullModel, Model& reducedModel, const std::string linkInReducedModel, const Traversal& linkSubModel, const FreeFloatingPos& pos, LinkPositions& subModelBase_X_link, - const std::unordered_map& newLink_H_oldLink - ) + const std::unordered_map& newLink_H_oldLink, + bool includeAllAdditionalFrames, + const std::vector& allowedAdditionalFrames) { // First compute the transform between each link in the submodel and the submodel base computeTransformToTraversalBaseWithAdditionalTransform(fullModel,linkSubModel,pos.jointPos(),subModelBase_X_link,newLink_H_oldLink); @@ -307,12 +335,11 @@ void reducedModelAddAdditionalFrames(const Model& fullModel, if( parentLink != 0 ) { std::string additionalFrameName = fullModel.getFrameName(visitedLinkIndex); - Transform subModelBase_H_additionalFrame = subModelBase_X_link(visitedLinkIndex); - - reducedModel.addAdditionalFrameToLink(linkInReducedModel,additionalFrameName, - subModelBase_H_additionalFrame); + addAdditionalFrameIfAllowed(reducedModel, linkInReducedModel, + additionalFrameName, subModelBase_H_additionalFrame, + includeAllAdditionalFrames, allowedAdditionalFrames); } // For all the link of the submodel, transfer their additional frame @@ -327,8 +354,9 @@ void reducedModelAddAdditionalFrames(const Model& fullModel, Transform subModelBase_H_additionalFrame = subModelBase_H_visitedLink*visitedLink_H_additionalFrame; - reducedModel.addAdditionalFrameToLink(linkInReducedModel,additionalFrameName, - subModelBase_H_additionalFrame); + addAdditionalFrameIfAllowed(reducedModel, linkInReducedModel, + additionalFrameName, subModelBase_H_additionalFrame, + includeAllAdditionalFrames, allowedAdditionalFrames); } } } @@ -391,15 +419,22 @@ void reducedModelAddSolidShapes(const Model& fullModel, // of both: // * createReducedModel : function to create a reduced model given the specified joints // * moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink: function to make sure a model is URDF compatible -// The logic is similar to the createReducedModel, but as an additional option this function takes in input -// a std::vector newLink_H_oldLink vector (of size fullModel.getNrOfLinks() that can be used -// to specify an optional additional transform of the final link used in the "reduced model" +// * removeAdditionalFramesFromModel: function to remove additional frames from a URDF +// +// The logic is similar to the createReducedModel, but with additional options: +// * std::vector newLink_H_oldLink vector (of size fullModel.getNrOfLinks() that can be used +// to specify an optional additional transform of the final link used in the "reduced model" +// * includeAllAdditionalFrames, std::vector : If includeAllAdditionalFrames is true, +// all the additional frames are of the input model are copied in the reduced model, if includeAllAdditionalFrames is true +// is True only the additional frames with the name contained in allowedAdditionalFrames are copied to the reduce model bool createReducedModelAndChangeLinkFrames(const Model& fullModel, const std::vector< std::string >& jointsInReducedModel, Model& reducedModel, const std::unordered_map& removedJointPositions, const std::unordered_map& newLink_H_oldLink, - bool addOriginalLinkFrameWith_original_frame_suffix) + bool addOriginalLinkFrameWith_original_frame_suffix, + bool includeAllAdditionalFrames, + const std::vector& allowedAdditionalFrames) { // We use the default traversal for deciding the base links of the reduced model Traversal fullModelTraversal; @@ -518,7 +553,8 @@ bool createReducedModelAndChangeLinkFrames(const Model& fullModel, // As this quantity is influenced by newLink_H_oldLink, this is passed along reducedModelAddAdditionalFrames(fullModel,reducedModel, linkName,subModels.getTraversal(linkInReducedModel), - jointPos,subModelBase_X_link,newLink_H_oldLink); + jointPos,subModelBase_X_link,newLink_H_oldLink, + includeAllAdditionalFrames,allowedAdditionalFrames); // Lump the visual and collision shapes in the new model reducedModelAddSolidShapes(fullModel,reducedModel, @@ -824,7 +860,8 @@ bool createReducedModel(const Model& fullModel, // We do not want to move the link frames in createReducedModel std::unordered_map newLink_H_oldLink; bool addOriginalLinkFrameWith_original_frame_suffix = false; - return createReducedModelAndChangeLinkFrames(fullModel, jointsInReducedModel, reducedModel, removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix); + bool includeAllAdditionalFrames = true; + return createReducedModelAndChangeLinkFrames(fullModel, jointsInReducedModel, reducedModel, removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix, includeAllAdditionalFrames, {}); } bool createReducedModel(const Model& fullModel, @@ -1083,8 +1120,9 @@ bool moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(const iDynTree::Model } bool addOriginalLinkFrameWith_original_frame_suffix = true; + bool includeAllAdditionalFrames = true; bool okReduced = createReducedModelAndChangeLinkFrames(inputModel, consideredJoints, outputModel, - removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix); + removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix, includeAllAdditionalFrames, {}); if (okReduced) { @@ -1099,4 +1137,27 @@ bool moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(const iDynTree::Model } } +bool removeAdditionalFramesFromModel(const Model& modelWithAllAdditionalFrames, + Model& modelWithOnlyAllowedAdditionalFrames, + const std::vector allowedAdditionalFrames) +{ + // Get list of all joints to pass as considered joints + std::vector consideredJoints; + for(iDynTree::JointIndex jntIdx=0; jntIdx < modelWithAllAdditionalFrames.getNrOfJoints(); jntIdx++) + { + consideredJoints.push_back(modelWithAllAdditionalFrames.getJointName(jntIdx)); + } + + bool includeAllAdditionalFrames = false; + return createReducedModelAndChangeLinkFrames(modelWithAllAdditionalFrames, + consideredJoints, + modelWithOnlyAllowedAdditionalFrames, + std::unordered_map(), + std::unordered_map(), + false, + includeAllAdditionalFrames, + allowedAdditionalFrames); + +} + } diff --git a/src/model/tests/ModelTransformersUnitTest.cpp b/src/model/tests/ModelTransformersUnitTest.cpp index d56c33f0a9..87f3768e65 100644 --- a/src/model/tests/ModelTransformersUnitTest.cpp +++ b/src/model/tests/ModelTransformersUnitTest.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -43,10 +44,37 @@ void checkThatOneSphereGetsAName() ASSERT_IS_TRUE(oneSphereModelWithValidName.collisionSolidShapes().getLinkSolidShapes()[0][0]->getName() == "link0_collision"); } +void checkRemoveAdditionalFramesFromModel() +{ + // Create random model with 10 links and 10 additional frames + iDynTree::Model modelWithAllAdditionalFrames = getRandomModel(10, 10); + + // Create an allow list of three additional frames + std::vector allowedAdditionalFrames; + allowedAdditionalFrames.push_back(modelWithAllAdditionalFrames.getFrameName(modelWithAllAdditionalFrames.getNrOfLinks() + 7)); + allowedAdditionalFrames.push_back(modelWithAllAdditionalFrames.getFrameName(modelWithAllAdditionalFrames.getNrOfLinks() + 1)); + allowedAdditionalFrames.push_back(modelWithAllAdditionalFrames.getFrameName(modelWithAllAdditionalFrames.getNrOfLinks() + 3)); + + // Create a model with only the allowed additional frames + iDynTree::Model modelWithOnlyAllowedAdditionalFrames; + ASSERT_IS_TRUE(removeAdditionalFramesFromModel(modelWithAllAdditionalFrames, modelWithOnlyAllowedAdditionalFrames, allowedAdditionalFrames)); + + // Check that the model with only the allowed additional frames has the correct number of links and additional frames + ASSERT_IS_TRUE(modelWithOnlyAllowedAdditionalFrames.getNrOfLinks() == modelWithAllAdditionalFrames.getNrOfLinks()); + ASSERT_IS_TRUE(modelWithOnlyAllowedAdditionalFrames.getNrOfFrames() == modelWithOnlyAllowedAdditionalFrames.getNrOfLinks() + allowedAdditionalFrames.size()); + + // Check that the additional frames contained in the modelWithOnlyAllowedAdditionalFrames are the one specified in modelWithOnlyAllowedAdditionalFrames + for (size_t i = 0; i < allowedAdditionalFrames.size(); i++) + { + ASSERT_IS_TRUE(modelWithOnlyAllowedAdditionalFrames.isFrameNameUsed(allowedAdditionalFrames[i])); + } +} + int main() { checkThatOneSphereGetsAName(); + checkRemoveAdditionalFramesFromModel(); return EXIT_SUCCESS; -} \ No newline at end of file +}