Fix frame export for multi-root models.

Do not export frames if corresponding link is not in the traversal.
This can happen if the model has links (and frames attached to those links) that
are not present in the exported traversal.

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;
 }