Merge pull request #1188 from robotology/fixurdfexportofstrangeframes

Fix bug preventing export of iDynTree::Model with non-zero axis offset to URDF and bump version to 12.3.1

CMakeLists.txt

@@ -3,7 +3,7 @@
 
 cmake_minimum_required(VERSION 3.16)
 
-project(iDynTree VERSION 12.3.0
+project(iDynTree VERSION 12.3.1
                  LANGUAGES C CXX)
 
 # Disable in source build, unless Eclipse is used

src/core/include/iDynTree/Axis.h

@@ -132,6 +132,11 @@ namespace iDynTree
          */
         Axis reverse() const;
 
+        /**
+         * Compute distance between the axis and a given point
+         */
+        double getDistanceBetweenAxisAndPoint(const iDynTree::Position& point) const;
+
         /**
          * @name Output helpers.
          *  Output helpers.

src/core/src/Axis.cpp

@@ -2,6 +2,7 @@
 // SPDX-License-Identifier: BSD-3-Clause
 
 #include <iDynTree/Axis.h>
+#include <iDynTree/EigenHelpers.h>
 #include <iDynTree/Transform.h>
 #include <iDynTree/TransformDerivative.h>
 #include <iDynTree/Twist.h>
@@ -258,6 +259,24 @@ namespace iDynTree
                     this->getOrigin());
     }
 
+    double Axis::getDistanceBetweenAxisAndPoint(const iDynTree::Position& point) const
+    {
+        Eigen::Vector3d direction = iDynTree::toEigen(this->getDirection()).normalized();
+
+        // Vector from the offset point of the to the given point
+        Eigen::Vector3d axisOrigin_p_point = iDynTree::toEigen(point) - iDynTree::toEigen(this->getOrigin());
+
+        // Project the axisOrigin_p_point onto the axis direction
+        Eigen::Vector3d projection = direction * axisOrigin_p_point.dot(direction);
+
+        // Calculate the closest point on the axis to the given point
+        Eigen::Vector3d closestPointOnAxis = iDynTree::toEigen(this->getOrigin()) + projection;
+
+        // Calculate the distance between the closest point on the axis and the given point
+        return (closestPointOnAxis - iDynTree::toEigen(point)).norm();
+    }
+
+
     std::string Axis::toString() const
     {
         std::stringstream ss;

src/model_io/codecs/src/URDFModelExport.cpp

@@ -357,13 +357,16 @@ bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr chi
             return false;
         }
 
-        // Check that the axis is URDF-compatible
-        if (toEigen(axis.getOrigin()).norm() > 1e-7)
+        // Check that the axis is URDF-compatible, i.e. that the distance between the
+        // axis and the child link origin is zero
+        double distanceBetweenAxisAndOrigin = axis.getDistanceBetweenAxisAndPoint(iDynTree::Position::Zero());
+        if (distanceBetweenAxisAndOrigin > 1e-7)
         {
             std::cerr << "[ERROR] URDFModelExport: Impossible to convert joint "
                       <<   model.getJointName(joint->getIndex()) << " to a URDF joint without moving the link frame of link "
-                      << model.getLinkName(childLink->getIndex()) << " , because its origin is "
-                      << axis.getOrigin().toString()  << std::endl;
+                      << model.getLinkName(childLink->getIndex()) << " , because the axis origin is "
+                      << axis.getOrigin().toString() << " the axis direction is " << axis.getDirection().toString() 
+                      << " and the distance between the axis and (0,0,0) is " << distanceBetweenAxisAndOrigin << std::endl;
             return false;
         }
 

src/model_io/codecs/tests/ModelExporterUnitTest.cpp

@@ -163,7 +163,6 @@ void checkImportExportURDF(std::string fileName)
 
 void testFramesNotInTraversal() {
     // Create a model with two different roots and export only one.
-
     Model model;
     SpatialInertia zeroInertia;
     zeroInertia.zero();
@@ -214,6 +213,99 @@ void testFramesNotInTraversal() {
     ASSERT_EQUAL_DOUBLE(modelReloaded.getNrOfFrames(), 2 + 1);
 }
 
+void testJointAxisWithNonZeroOriginButPassingThroughChildLinkFrameOrigin() {
+    // iDynTree has no constraint of where the joint axis passes w.r.t. to
+    // the link frames of the link it connects, while URDF constraints the
+    // joint axis to pass through the origin of the child link frame
+
+    // This test checks that a model with a non-zero offset origin but that
+    // passes through the child link origin is correctly exported
+    Model model;
+    SpatialInertia zeroInertia;
+    zeroInertia.zero();
+
+    Link link;
+    link.setInertia(zeroInertia);
+
+    // Create a 2 links - one joint model.
+    std::string link1Name = "link_1";
+    std::string link2Name = "link_2";
+    std::string jointName = "j1";
+
+    model.addLink(link1Name, link);
+    model.addLink(link2Name, link);
+
+    auto joint = std::make_unique<RevoluteJoint>();
+
+    // Add a offset in the x direction between link_1 and link_2
+    iDynTree::Transform link1_X_link2 = iDynTree::Transform::Identity();
+    link1_X_link2.setPosition(iDynTree::Position(1.0, 0.0, 0.0));
+    joint->setRestTransform(link1_X_link2);
+
+    // Mark the rotation of the joint along z, and add a offset of the axis along z
+    iDynTree::Axis axis;
+    axis.setDirection(iDynTree::Direction(0.0, 0.0, 1.0));
+    axis.setOrigin(iDynTree::Position(0.0, 0.0, 1.0));
+    joint->setAxis(axis, model.getLinkIndex(link2Name));
+
+    model.addJoint(link1Name, link2Name, jointName, joint.get());
+
+    ModelExporter exporter;
+    bool ok = exporter.init(model);
+    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);
+
+    // Reload model
+    Model modelReloaded = loader.model();
+
+    // Check that the reloaded have the same transform of the original model
+    iDynTree::VectorDynSize jointPos;
+    jointPos.resize(model.getNrOfPosCoords());
+    for (size_t i=0; i++; i<10)
+    {
+        // Arbitrary joint angles
+        jointPos(0) = 0.1*i-0.5;
+        iDynTree::Transform link1_H_link2_orig =
+            model.getJoint(model.getJointIndex(jointName))->getTransform(jointPos, model.getLinkIndex(link1Name), model.getLinkIndex(link2Name));
+        iDynTree::Transform link1_H_link2_reloaded =
+            modelReloaded.getJoint(modelReloaded.getJointIndex(jointName))->getTransform(jointPos, modelReloaded.getLinkIndex(link1Name), modelReloaded.getLinkIndex(link2Name));
+
+        ASSERT_EQUAL_TRANSFORM(link1_H_link2_orig, link1_H_link2_reloaded);
+    }
+
+    // Create a new model with an axis that does not pass through the origin of the child link and verify that it can't be loaded
+    Model modelThatCantBeExportedToUrdf;
+    modelThatCantBeExportedToUrdf.addLink(link1Name, link);
+    modelThatCantBeExportedToUrdf.addLink(link2Name, link);
+    auto jointNew = std::make_unique<RevoluteJoint>();
+
+    // Add a offset in the x direction between link_1 and link_2
+    iDynTree::Transform link1_X_link2New = iDynTree::Transform::Identity();
+    link1_X_link2New.setPosition(iDynTree::Position(1.0, 0.0, 0.0));
+    jointNew->setRestTransform(link1_X_link2);
+
+    // Mark the rotation of the joint along z, and add a offset of the axis along y
+    iDynTree::Axis axisNew;
+    axisNew.setDirection(iDynTree::Direction(0.0, 0.0, 1.0));
+    axisNew.setOrigin(iDynTree::Position(0.0, 1.0, 0.0));
+    jointNew->setAxis(axisNew, modelThatCantBeExportedToUrdf.getLinkIndex(link2Name));
+
+    modelThatCantBeExportedToUrdf.addJoint(link1Name, link2Name, jointName, jointNew.get());
+
+    ModelExporter exporterNew;
+    ok = exporterNew.init(modelThatCantBeExportedToUrdf);
+    ASSERT_IS_TRUE(ok);
+    ok = exporterNew.exportModelToString(urdfString);
+    ASSERT_IS_FALSE(ok);
+}
+
 
 int main()
 {
@@ -231,6 +323,7 @@ int main()
     }
 
     testFramesNotInTraversal();
+    testJointAxisWithNonZeroOriginButPassingThroughChildLinkFrameOrigin();
 
 
     return EXIT_SUCCESS;