Add support for exporting additional frames in ModelExporter

src/model/include/iDynTree/Model/Model.h

@@ -417,6 +417,18 @@ namespace iDynTree
          */
         LinkIndex getFrameLink(const FrameIndex frameIndex) const;
 
+        /**
+         * Get the additional frames of a specified link.
+         *
+         * @note The vector of returned frame index is ordered according to the frame index.
+         * @warning This method searches linearly over all the frames.
+         *
+         * @param[in] link a LinkIndex of the specified link,
+         * @param[out] frames a vector of FrameIndex of the frame indeces attached to the specified link index,
+         * @return true if the specified link is a valid link, false otherwise.
+         */
+        bool getLinkAdditionalFrames(const LinkIndex lnkIndex, std::vector<FrameIndex>& frameIndeces) const;
+
         /**
          * Get the nr of neighbors of a given link.
          */

src/model/src/Model.cpp

@@ -703,6 +703,29 @@ LinkIndex Model::getFrameLink(const FrameIndex frameIndex) const
     return LINK_INVALID_INDEX;
 }
 
+bool Model::getLinkAdditionalFrames(const LinkIndex lnkIndex, std::vector<FrameIndex>& frameIndices) const
+{
+    if (!isValidLinkIndex(lnkIndex)) {
+        std::stringstream ss;
+        ss << "LinkIndex " << lnkIndex << " is not valid, should be between 0 and " << this->getNrOfLinks()-1;
+        reportError("Model", "getLinkAdditionalFrames", ss.str().c_str());
+        return false;
+    }
+
+    frameIndices.resize(0);
+    // FrameIndex from 0 to this->getNrOfLinks()-1 are reserved for implicit frame of Links
+    // with the corresponding LinkIndex, while the frameIndex from getNrOfLinks() to getNrOfFrames()-1
+    // are the one of actual additional frames. See iDynTree::Model docs for more details
+    for (FrameIndex frameIndex=this->getNrOfLinks(); frameIndex < this->getNrOfFrames(); frameIndex++) {
+        if (this->getFrameLink(frameIndex) == lnkIndex) {
+            frameIndices.push_back(frameIndex);
+        }
+    }
+
+    return true;
+}
+
+
 
 unsigned int Model::getNrOfNeighbors(const LinkIndex link) const
 {

src/model/tests/ModelUnitTest.cpp

@@ -289,12 +289,24 @@ void checkSimpleModel()
 
         model.addJoint("fixedJoint",&fixJoint);
 
+        model.addAdditionalFrameToLink("link0", "frame0_0", iDynTree::Transform::Identity());
+        model.addAdditionalFrameToLink("link1", "frame1_0", iDynTree::Transform::Identity());
+        model.addAdditionalFrameToLink("link1", "frame1_1", iDynTree::Transform::Identity());
+
+
         ASSERT_EQUAL_DOUBLE(model.getNrOfLinks(),2);
         ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(),1);
         ASSERT_EQUAL_DOUBLE(model.getNrOfNeighbors(0),1);
         ASSERT_EQUAL_DOUBLE(model.getNrOfNeighbors(1),1);
         ASSERT_EQUAL_DOUBLE(model.getNeighbor(0,0).neighborLink,1);
         ASSERT_EQUAL_DOUBLE(model.getNeighbor(1,0).neighborLink,0);
+        std::vector<FrameIndex> link0_frames;
+        model.getLinkAdditionalFrames(0, link0_frames);
+        ASSERT_EQUAL_DOUBLE(link0_frames.size(), 1);
+        std::vector<FrameIndex> link1_frames;
+        model.getLinkAdditionalFrames(1, link1_frames);
+        ASSERT_EQUAL_DOUBLE(link1_frames.size(), 2);
+        ASSERT_IS_TRUE(link1_frames[0] < link1_frames[1]);
 
         createCopyAndDestroy(model);
         checkComputeTraversal(model);

src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h

@@ -63,11 +63,30 @@ class ModelExporterOptions
  *
  * Furthermore, currently the model exporter only exports a subset of the features
  * supported in iDynTree::Model. In particular, the following features are not exported:
- * * Additional frames
+ *
  * * Visual and collision geometries
  * * Sensors
  * * Joint limits, damping and static friction.
  *
+ * # Format documentation
+ *
+ * The following format are  supported  by the exporter.
+ *
+ * | Format | Extendend Name | Website |  String for filetype argument  |
+ * |:-----------------------------:|:-------:|:-------:|:--------:|
+ * | URDF | Unified Robot Description Format  | http://wiki.ros.org/urdf | `urdf` |
+ *
+ * ## URDF
+ *
+ * As the URDF format does not distinguish between frames and links (see https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56
+ * for an extensive discussion on this) the URDF model exporter converts iDynTree's *additional frames* to mass-less and shape-less
+ * *fake* URDF links that are connected as child links via `fixed` joints to the corresponding **real** URDF links.
+ *
+ * Furthermore, it is widespread use in URDF models to never use a real link (with mass) as the root link of a model, mainly
+ * due to workaround a bug in official %KDL parser used in ROS (see https://github.com/ros/kdl_parser/issues/27 for more info). For this reason,
+ * if the selected base_link has at least one additional frame, the first additional frame of the base link is added as a **parent** fake URDF link,
+ * instead as a **child** fake URDF link as done with the rest of %iDynTree's additional frames. If no additional frame is available for the base link,
+ * the base link of the URDF will have a mass, and will generate a warning then used with the ROS's [`kdl_parser`](https://github.com/ros/kdl_parser) .
  *
  */
 class ModelExporter

src/model_io/urdf/src/URDFModelExport.cpp

@@ -29,6 +29,15 @@
 namespace iDynTree
 {
 
+/*
+ * This file uses extensivly the libxml2's tree API, see
+ * http://www.xmlsoft.org/examples/index.html#Tree for more info.
+ * Usage of the C-based libxml2 API in C++ means that is necessary to
+ * use the BAD_CAST macro for handling const-correctness.
+ * See https://stackoverflow.com/questions/45058878/why-libxml2-uses-bad-cast-everywhere-in-a-c-c-code
+ * and http://www.xmlsoft.org/html/libxml-xmlstring.html#BAD_CAST for details on this.
+ */
+
 /*
  * Add a <origin> URDF element to the specified parent element with the specified transform.
  *
@@ -133,6 +142,7 @@ bool exportLink(const Link &link, const std::string linkName, xmlNodePtr parent_
     return ok;
 }
 
+
 /**
  * Add a <joint> URDF element to the specified parent element with the specified joint info.
  *
@@ -241,6 +251,68 @@ bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr chi
     return ok;
 }
 
+/*
+ * Add a <link> and <joint> URDF elements to the specified parent element for the specified additional frame.
+ *
+ * URDF does not have the concept of frame (see https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56),
+ * so for each additional frame we need to add a URDF "fake" mass-less link.
+ *
+ * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function modifies it to be something like:
+ *
+ * <parent_element>
+ *   <link name="<frame_name>"/>
+ *   <joint name="<frame_name>_fixed_joint" type="fixed">
+ *      <origin xyz="0.1 0.2 0.3" rpy="0 0 3.1416"/>
+ *      <parent link="<link_name>"/>
+ *      <child link="<frame_name>"/>
+ *   </joint>
+ * <parent_element/>
+ *
+ * where the actual values of the added element and attributes depend on the input frame_name, link_H_frame, parent_link_name and direction_options arguments.
+ */
+enum exportAdditionalFrameDirectionOption {
+    FAKE_LINK_IS_CHILD,
+    FAKE_LINK_IS_PARENT
+};
+bool exportAdditionalFrame(const std::string frame_name, Transform link_H_frame, const std::string link_name, exportAdditionalFrameDirectionOption direction_option, xmlNodePtr parent_element)
+{
+    bool ok=true;
+
+    // Export fake link
+    xmlNodePtr link_xml = xmlNewChild(parent_element, NULL, BAD_CAST "link", NULL);
+    xmlNewProp(link_xml, BAD_CAST "name", BAD_CAST frame_name.c_str());
+
+    // Export fake joint
+    xmlNodePtr joint_xml = xmlNewChild(parent_element, NULL, BAD_CAST "joint", NULL);
+    std::string fake_joint_name = frame_name + "_fixed_joint";
+    xmlNewProp(joint_xml, BAD_CAST "name", BAD_CAST fake_joint_name.c_str());
+    xmlNewProp(joint_xml, BAD_CAST "type", BAD_CAST "fixed");
+
+    // origin
+    exportTransform(link_H_frame, joint_xml);
+
+    std::string parent_of_fake_joint, child_of_fake_joint;
+
+    if (direction_option == FAKE_LINK_IS_CHILD) {
+        parent_of_fake_joint = link_name;
+        child_of_fake_joint = frame_name;
+    } else {
+        // direction_option == FAKE_LINK_IS_PARENT
+        parent_of_fake_joint = frame_name;
+        child_of_fake_joint = link_name;
+    }
+
+    // parent
+    xmlNodePtr parent_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "parent", NULL);
+    xmlNewProp(parent_xml, BAD_CAST "link", BAD_CAST parent_of_fake_joint.c_str());
+
+    // child
+    xmlNodePtr child_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "child", NULL);
+    xmlNewProp(child_xml, BAD_CAST "link", BAD_CAST child_of_fake_joint.c_str());
+
+    return ok;
+}
+
 bool URDFStringFromModel(const iDynTree::Model & model,
                          std::string & urdf_string,
                          const ModelExporterOptions options)
@@ -272,6 +344,21 @@ bool URDFStringFromModel(const iDynTree::Model & model,
         return false;
     }
 
+    // If the base link has at least an additional frame, add it as parent URDF link
+    // as a workaround for https://github.com/ros/kdl_parser/issues/27
+    FrameIndex baseFakeLinkFrameIndex = FRAME_INVALID_INDEX;
+    std::vector<FrameIndex> frameIndices;
+    ok = model.getLinkAdditionalFrames(baseLinkIndex, frameIndices);
+    if (ok && frameIndices.size() >= 1) {
+        baseFakeLinkFrameIndex = frameIndices[0];
+        ok = ok && exportAdditionalFrame(model.getFrameName(baseFakeLinkFrameIndex),
+                                         model.getFrameTransform(baseFakeLinkFrameIndex),
+                                         model.getLinkName(model.getFrameLink(baseFakeLinkFrameIndex)),
+                                         FAKE_LINK_IS_PARENT,
+                                         robot);
+    }
+
+
     // Create a Traversal
     Traversal exportTraversal;
     if (!model.computeFullTreeTraversal(exportTraversal, baseLinkIndex))
@@ -300,7 +387,18 @@ bool URDFStringFromModel(const iDynTree::Model & model,
         ok = ok && exportLink(*visitedLink, visitedLinkName, robot);
     }
 
-    // TODO(traversaro) : export additional frames
+    // Export all the additional frames that are not parent of the base link
+    for (FrameIndex frameIndex=model.getNrOfLinks(); frameIndex < static_cast<FrameIndex>(model.getNrOfFrames()); frameIndex++)
+    {
+        if (frameIndex != baseFakeLinkFrameIndex) {
+            ok = ok && exportAdditionalFrame(model.getFrameName(frameIndex),
+                                             model.getFrameTransform(frameIndex),
+                                             model.getLinkName(model.getFrameLink(frameIndex)),
+                                             FAKE_LINK_IS_CHILD,
+                                             robot);
+        }
+    }
+
 
     xmlChar *xmlbuff=0;
     int buffersize=0;

src/model_io/urdf/tests/ModelExporterUnitTest.cpp

@@ -57,9 +57,7 @@ void checkImportExportURDF(std::string fileName)
     ASSERT_EQUAL_DOUBLE(model.getNrOfLinks(), modelReloaded.getNrOfLinks());
     ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(), modelReloaded.getNrOfJoints());
     ASSERT_EQUAL_DOUBLE(model.getNrOfDOFs(), modelReloaded.getNrOfDOFs());
-
-    // TODO(traversaro) : uncomment the following line when frames are correctly handled by the exporter
-    // ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(), modelReloaded.getNrOfLinks());
+    ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(), modelReloaded.getNrOfFrames());
 
     // Verify that the link correspond (note that the serialization could have changed)
     for(int lnkIndex=0; lnkIndex < model.getNrOfLinks(); lnkIndex++) {
@@ -72,6 +70,16 @@ void checkImportExportURDF(std::string fileName)
         ASSERT_EQUAL_MATRIX(inertia.asMatrix(), inertiaReloaded.asMatrix());
     }
 
+    // Verify that the frame correspond (note that the serialization could have changed)
+    for(FrameIndex frameIndex=model.getNrOfLinks(); frameIndex < model.getNrOfFrames(); frameIndex++) {
+        FrameIndex frameIndexInReloaded = modelReloaded.getFrameIndex(model.getFrameName(frameIndex));
+        ASSERT_IS_TRUE(frameIndexInReloaded != FRAME_INVALID_INDEX);
+        ASSERT_IS_TRUE(model.getFrameName(frameIndex) == modelReloaded.getFrameName(frameIndexInReloaded));
+        Transform link_H_frame = model.getFrameTransform(frameIndex);
+        Transform link_H_frame_reloaded = modelReloaded.getFrameTransform(frameIndexInReloaded);
+        ASSERT_EQUAL_MATRIX(link_H_frame.asHomogeneousTransform(), link_H_frame_reloaded.asHomogeneousTransform());
+    }
+
 }