diff --git a/CHANGELOG.md b/CHANGELOG.md index 563c368be46..b39b5adc1c8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,13 +6,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] - ### Added - Added support for exporting joint position limits to URDF for 1-DoF joints (prismatic and revolute). - Added pybind11 python bindings for adding and reading joint limits. ### Fixed +- In the URDF exporter, export only frames attached to the exported traversal [#914](https://github.com/robotology/idyntree/pull/914). - Fixed handling of the AMENT_PREFIX_PATH environment variable (https://github.com/robotology/idyntree/pull/915). ## [4.2.0] - 2021-07-23 diff --git a/src/model_io/urdf/src/URDFModelExport.cpp b/src/model_io/urdf/src/URDFModelExport.cpp index 5718caee337..d0dbe413cc1 100644 --- a/src/model_io/urdf/src/URDFModelExport.cpp +++ b/src/model_io/urdf/src/URDFModelExport.cpp @@ -10,11 +10,12 @@ #include #include #include -#include -#include #include +#include +#include #include #include +#include #include "URDFParsingUtils.h" @@ -503,7 +504,7 @@ bool URDFStringFromModel(const iDynTree::Model & model, // options.exportFirstBaseLinkAdditionalFrameAsFakeURDFBase is set to false // If options.exportFirstBaseLinkAdditionalFrameAsFakeURDFBase is set to false, // baseFakeLinkFrameIndex remains set to FRAME_INVALID_INDEX, a - // and all the additional frames of the base link get exported as child fake links + // and all the additional frames of the base link get exported as child fake links // in the loop and the end of this function FrameIndex baseFakeLinkFrameIndex = FRAME_INVALID_INDEX; std::vector frameIndices; @@ -546,10 +547,14 @@ bool URDFStringFromModel(const iDynTree::Model & model, ok = ok && exportLink(*visitedLink, visitedLinkName, model, robot); } - // Export all the additional frames that are not parent of the base link + // Export all the additional frames. for (FrameIndex frameIndex=model.getNrOfLinks(); frameIndex < static_cast(model.getNrOfFrames()); frameIndex++) { - if (frameIndex != baseFakeLinkFrameIndex) { + // Check that the frame is not parent of the base link and that the + // frame link is present in the traversal. + if (frameIndex != baseFakeLinkFrameIndex + && exportTraversal.getTraversalIndexFromLinkIndex(model.getFrameLink(frameIndex)) != TRAVERSAL_INVALID_INDEX) { + ok = ok && exportAdditionalFrame(model.getFrameName(frameIndex), model.getFrameTransform(frameIndex), model.getLinkName(model.getFrameLink(frameIndex)), diff --git a/src/model_io/urdf/tests/ModelExporterUnitTest.cpp b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp index 05ea11db89e..014b119620b 100644 --- a/src/model_io/urdf/tests/ModelExporterUnitTest.cpp +++ b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp @@ -11,6 +11,7 @@ #include +#include #include #include @@ -19,6 +20,7 @@ #include #include #include +#include #include using namespace iDynTree; @@ -163,6 +165,59 @@ void checkImportExportURDF(std::string fileName) } +void testFramesNotInTraversal() { + // Create a model with two different roots and export only one. + + Model model; + SpatialInertia zeroInertia; + zeroInertia.zero(); + + Link link; + link.setInertia(zeroInertia); + + // Create a 2 links - one joint model. + model.addLink("link_1_1", link); + model.addLink("link_1_2", link); + + auto joint = std::make_unique(); + model.addJoint("link_1_1", "link_1_2", "j1", joint.get()); + + // Add a frame to the body. + model.addAdditionalFrameToLink("link_1_2", "f1", Transform::Identity()); + + // Add also another link not connected to the other body. + model.addLink("link_2_1", link); + + // Add a frame to this body too. + model.addAdditionalFrameToLink("link_2_1", "f2", Transform::Identity()); + + // Export the model. We want to export only the first body. + ModelExporter exporter; + // Set the root link to link_1_1. + ModelExporterOptions options; + options.baseLink = "link_1_1"; + exporter.setExportingOptions(options); + + bool ok = exporter.init(model, SensorsList()); + ASSERT_IS_TRUE(ok); + std::string urdfString; + ok = exporter.exportModelToString(urdfString); + ASSERT_IS_TRUE(ok); + + // Reload the model. + ModelLoader loader; + + ok = loader.loadModelFromString(urdfString); + ASSERT_IS_TRUE(ok); + Model modelReloaded = loader.model(); + + // Check that the reloaded model contains only the 2 links - 1 joint model. + ASSERT_EQUAL_DOUBLE(modelReloaded.getNrOfLinks(), 2); + ASSERT_EQUAL_DOUBLE(modelReloaded.getNrOfJoints(), 1); + ASSERT_EQUAL_DOUBLE(modelReloaded.getNrOfDOFs(), 1); + ASSERT_EQUAL_DOUBLE(modelReloaded.getNrOfFrames(), 2 + 1); +} + int main() { @@ -179,6 +234,8 @@ int main() checkImportExportURDF(urdfFileName); } + testFramesNotInTraversal(); + return EXIT_SUCCESS; }