From d5e78e031b074a1274795a503391558f4f4d61ed Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 12 Jun 2024 17:43:05 +0200 Subject: [PATCH 1/2] Fix bug preventing export of iDynTree::Model with non-zero axis offset to URDF --- src/core/include/iDynTree/Axis.h | 5 + src/core/src/Axis.cpp | 19 ++++ src/model_io/codecs/src/URDFModelExport.cpp | 11 ++- .../codecs/tests/ModelExporterUnitTest.cpp | 95 ++++++++++++++++++- 4 files changed, 125 insertions(+), 5 deletions(-) diff --git a/src/core/include/iDynTree/Axis.h b/src/core/include/iDynTree/Axis.h index bcfee4318b..aba4513a73 100644 --- a/src/core/include/iDynTree/Axis.h +++ b/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. diff --git a/src/core/src/Axis.cpp b/src/core/src/Axis.cpp index 0b32544dee..1c2d7b14dc 100644 --- a/src/core/src/Axis.cpp +++ b/src/core/src/Axis.cpp @@ -2,6 +2,7 @@ // SPDX-License-Identifier: BSD-3-Clause #include +#include #include #include #include @@ -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; diff --git a/src/model_io/codecs/src/URDFModelExport.cpp b/src/model_io/codecs/src/URDFModelExport.cpp index d65fb29767..f2d3fb5f5a 100644 --- a/src/model_io/codecs/src/URDFModelExport.cpp +++ b/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; } diff --git a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp b/src/model_io/codecs/tests/ModelExporterUnitTest.cpp index e461f7b5a3..57914b09d2 100644 --- a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp +++ b/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(); + + // 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(); + + // 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; From 1a88dd11dbe3c3a607785fda258e2b2077b49277 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 12 Jun 2024 17:53:09 +0200 Subject: [PATCH 2/2] Bump version to 12.3.1 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e1634a7af9..bc92263850 100644 --- a/CMakeLists.txt +++ b/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