Merge pull request #914 from francesco-romano/fix/export_frames

Fix frame export for multi-root models.

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

src/model_io/urdf/src/URDFModelExport.cpp

@@ -10,11 +10,12 @@
 #include <iDynTree/Core/SpatialInertia.h>
 #include <iDynTree/Core/Transform.h>
 #include <iDynTree/Core/VectorFixSize.h>
-#include <iDynTree/Model/Model.h>
-#include <iDynTree/Model/Traversal.h>
 #include <iDynTree/Model/FixedJoint.h>
+#include <iDynTree/Model/Indices.h>
+#include <iDynTree/Model/Model.h>
 #include <iDynTree/Model/PrismaticJoint.h>
 #include <iDynTree/Model/RevoluteJoint.h>
+#include <iDynTree/Model/Traversal.h>
 
 #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<FrameIndex> 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<FrameIndex>(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)),

src/model_io/urdf/tests/ModelExporterUnitTest.cpp

@@ -11,6 +11,7 @@
 
 
 #include <iDynTree/Model/Model.h>
+#include <iDynTree/Model/RevoluteJoint.h>
 #include <iDynTree/ModelIO/ModelLoader.h>
 #include <iDynTree/ModelIO/ModelExporter.h>
 
@@ -19,6 +20,7 @@
 #include <cstdio>
 #include <cstdlib>
 #include <algorithm>
+#include <memory>
 #include <fstream>
 
 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<RevoluteJoint>();
+    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;
 }