Move the linkFrames in case of the axis of the joint does not pass through the csys

CMakeLists.txt

@@ -12,7 +12,7 @@ include(GNUInstallDirs)
 include(FeatureSummary)
 
 find_package(Eigen3 REQUIRED)
-find_package(iDynTree 12.3.1 REQUIRED)
+find_package(iDynTree 12.4.0 REQUIRED)
 find_package(yaml-cpp REQUIRED)
 find_package(LibXml2 REQUIRED)
 

README.md

@@ -12,7 +12,6 @@ Generate URDF model from CREO Parametric mechanisms.
 - Subassemblies are **NOT** yet supported.
 - The joints are in position 0.
 - Right now it can handle only **REVOLUTE**, **PRISMATIC** and **FIXED** joints.
-- In each joint, the frame of the child link must lie on the joint's axis (https://github.com/icub-tech-iit/creo2urdf/issues/99).
 
 ## Installation from binaries
 

src/creo2urdf/CMakeLists.txt

@@ -69,6 +69,7 @@ add_compile_definitions(creo2urdf PUBLIC PRO_MACHINE=36
 # Link dependencies
 target_link_libraries(creo2urdf PRIVATE iDynTree::idyntree-modelio
                                         iDynTree::idyntree-high-level
+                                        iDynTree::idyntree-model
                                         yaml-cpp::yaml-cpp
                                         LibXml2::LibXml2
                                         Eigen3::Eigen

src/creo2urdf/include/creo2urdf/Creo2Urdf.h

@@ -151,6 +151,7 @@ class Creo2Urdf : public pfcUICommandActionListener {
     std::string m_output_path{ "" }; /**< Output path for the exported URDF file. */
     pfcModel_ptr m_root_asm_model_ptr{ nullptr }; /**< Handle to the Creo model. */
     pfcSession_ptr m_session_ptr{ nullptr }; /**< Handle to the Creo session. */
+    bool m_need_to_move_link_frames_to_be_compatible_with_URDF{ false }; /**< Flag indicating whether to move link frames to be compatible with URDF. */
 };
 
 class Creo2UrdfAccess : public pfcUICommandAccessListener {

src/creo2urdf/include/creo2urdf/Utils.h

@@ -437,9 +437,9 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
  * @param axis_name The name of the desired axis of which to retrieve the direction
  * @param link_frame_name The name of the frame belonging to modelhdl
  * @param scale The scaling factor for the origin of the child frame
- * @return std::tuple<bool, iDynTree::Direction, iDynTree::Transform>>  Tuple containing a success/failure flag, the axis direction, and the transform oldLink_H_newLink in order that the frame lies on the axis.
+ * @return std::tuple<bool, iDynTree::Direction, iDynTree::Position>>  Tuple containing a success/failure flag, the axis direction, and the position of the middle point of the axis in the link csys in order that the frame lies on the axis.
  */
-std::tuple<bool, iDynTree::Direction, iDynTree::Transform> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);
+std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);
 
 /**
  * @brief Extracts the folder path from a file path.

src/creo2urdf/src/Creo2Urdf.cpp

@@ -13,6 +13,7 @@
 
 #include <iDynTree/PrismaticJoint.h>
 #include <iDynTree/EigenHelpers.h>
+#include <iDynTree/ModelTransformers.h>
 
 #include <Eigen/Core>
 
@@ -250,24 +251,21 @@ void Creo2Urdf::OnCommand() {
             continue;
         }
 
-
-
         auto asm_owner_H_parent_link = link_info_map.at(parent_link_name).rootAsm_H_linkFrame;
         auto asm_owner_H_child_link = link_info_map.at(child_link_name).rootAsm_H_linkFrame;
-        auto child_model = link_info_map.at(child_link_name).modelhdl;
         auto parent_model = link_info_map.at(parent_link_name).modelhdl;
         auto parent_link_frame = link_info_map.at(parent_link_name).link_frame_name;
 
         //printToMessageWindow("Parent link H " + asm_owner_H_parent_link.toString());
         //printToMessageWindow("Child  link H " + asm_owner_H_child_link.toString());
-        iDynTree::Transform parent_H_child = iDynTree::Transform::Identity();
-        parent_H_child = asm_owner_H_parent_link.inverse() * asm_owner_H_child_link;
+        iDynTree::Transform parentLink_H_childLink = iDynTree::Transform::Identity();
+        parentLink_H_childLink = asm_owner_H_parent_link.inverse() * asm_owner_H_child_link;
 
         if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) {
 
-            iDynTree::Direction axis;
-            iDynTree::Transform oldChild_H_newChild;
-            std::tie(ret, axis, oldChild_H_newChild) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
+            iDynTree::Direction direction;
+            iDynTree::Position axis_mid_point_pos_in_parent;
+            std::tie(ret, direction, axis_mid_point_pos_in_parent) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
 
             if (!ret && warningsAreFatal)
             {
@@ -277,34 +275,30 @@ void Creo2Urdf::OnCommand() {
             if (config["reverseRotationAxis"].IsDefined() && 
                 config["reverseRotationAxis"].Scalar().find(joint_name) != std::string::npos)
             {
-                axis = axis.reverse();
+                direction = direction.reverse();
             }
 
-            auto urdf_child_link_name = getRenameElementFromConfig(child_link_name);
-
+            auto urdf_parent_link_name = getRenameElementFromConfig(parent_link_name);
 
-            // FIXME
-            // It does something but the transform is somehow wrong, in case of proper definition of the csys oldChild_H_newChild is the identity
-            // Once we found the proper transform we should updatea also the spatial inertia
+            iDynTree::Axis idyn_axis{ direction, parentLink_H_childLink.getPosition() };
 
-            parent_H_child = parent_H_child * oldChild_H_newChild;
-            auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
-            auto link_H_visual_solid_shape    = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
-
-            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(oldChild_H_newChild.inverse() * link_H_collision_solid_shape);
-            idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(oldChild_H_newChild.inverse() *  link_H_visual_solid_shape);
+            // Check if the axis is aligned with the link frame
+            if (parent_link_frame == "CSYS" && idyn_axis.getDistanceBetweenAxisAndPoint(axis_mid_point_pos_in_parent) > 1e-7 ) {
+                idyn_axis.setOrigin(axis_mid_point_pos_in_parent);
+                m_need_to_move_link_frames_to_be_compatible_with_URDF = true;
+            }
 
             std::shared_ptr<iDynTree::IJoint> joint_sh_ptr;
             if (joint_info.second.type == JointType::Revolute) {
                 joint_sh_ptr = std::make_shared<iDynTree::RevoluteJoint>();
-                dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition()));
+                dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(idyn_axis);
             }
             else if (joint_info.second.type == JointType::Linear) {
                 joint_sh_ptr = std::make_shared<iDynTree::PrismaticJoint>();
-                dynamic_cast<iDynTree::PrismaticJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition()));
+                dynamic_cast<iDynTree::PrismaticJoint*>(joint_sh_ptr.get())->setAxis(idyn_axis);
             }
 
-            joint_sh_ptr->setRestTransform(parent_H_child);
+            joint_sh_ptr->setRestTransform(parentLink_H_childLink);
             double conversion_factor = 1.0;
             if (joint_info.second.type == JointType::Revolute) {
                 conversion_factor = deg2rad;
@@ -336,7 +330,7 @@ void Creo2Urdf::OnCommand() {
             }
         }
         else if (joint_info.second.type == JointType::Fixed) {
-            iDynTree::FixedJoint joint(parent_H_child);
+            iDynTree::FixedJoint joint(parentLink_H_childLink);
             if (idyn_model.addJoint(getRenameElementFromConfig(parent_link_name),
                 getRenameElementFromConfig(child_link_name), joint_name, &joint) == iDynTree::JOINT_INVALID_INDEX) {
                 printToMessageWindow("FAILED TO ADD JOINT " + joint_name, c2uLogLevel::WARN);
@@ -445,7 +439,21 @@ void Creo2Urdf::OnCommand() {
 bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOptions options) {
     iDynTree::ModelExporter mdl_exporter;
 
-    mdl_exporter.init(mdl);
+    // Convert modelToExport in a URDF-compatible model (using the default base link)
+    iDynTree::Model modelToExportURDFCompatible;
+
+    if (m_need_to_move_link_frames_to_be_compatible_with_URDF) {
+        bool ok = iDynTree::moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(mdl, modelToExportURDFCompatible);
+        if (!ok) {
+            printToMessageWindow("Failed to move link frames to be URDF compatible", c2uLogLevel::WARN);
+            return false;
+        }
+    }
+    else {
+        modelToExportURDFCompatible = mdl;
+    }
+
+    mdl_exporter.init(modelToExportURDFCompatible);
     mdl_exporter.setExportingOptions(options);
 
     if (!mdl_exporter.isValid())

src/creo2urdf/src/Utils.cpp

@@ -172,21 +172,21 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
     return { false, H_child };
 }
 
-std::tuple<bool, iDynTree::Direction, iDynTree::Transform> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array<double, 3>& scale) {
+std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array<double, 3>& scale) {
 
     iDynTree::Direction axis_unit_vector;
-    iDynTree::Transform oldChild_H_newChild = iDynTree::Transform::Identity();
+    iDynTree::Position axis_mid_point_pos = iDynTree::Position::Zero();
     axis_unit_vector.zero();
 
     auto axes_list = modelhdl->ListItems(pfcModelItemType::pfcITEM_AXIS);
     if (axes_list->getarraysize() == 0) {
         printToMessageWindow("There is no Axis in the part " + string(modelhdl->GetFullName()), c2uLogLevel::WARN);
 
-        return { false, axis_unit_vector, oldChild_H_newChild };
+        return { false, axis_unit_vector, axis_mid_point_pos };
     }
 
     if (axis_name.empty())
-        return { false, axis_unit_vector, oldChild_H_newChild };
+        return { false, axis_unit_vector, axis_mid_point_pos };
 
     pfcAxis* axis = nullptr;
 
@@ -199,38 +199,37 @@ std::tuple<bool, iDynTree::Direction, iDynTree::Transform> getAxisFromPart(pfcMo
         }
     }
 
+    if (!axis) {
+        printToMessageWindow("Unable to find the axis " + axis_name + " in " + string(modelhdl->GetFullName()), c2uLogLevel::WARN);
+        return { false, axis_unit_vector, axis_mid_point_pos };
+    }
+
     auto axis_data = wfcWAxis::cast(axis)->GetAxisData();
 
     auto axis_line = pfcLineDescriptor::cast(axis_data); // cursed cast from hell
 
-    if (link_frame_name == "CSYS") {
-        // We use the medium point of the axis as offset
-        pfcPoint3D_ptr pstart = axis_line->GetEnd1();
-        pfcPoint3D_ptr pend = axis_line->GetEnd2();
-        auto x = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0];
-        auto y = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1];
-        auto z = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2];
-        oldChild_H_newChild.setPosition({ x, y, z });
-
-    }
+    auto& csys_H_linkFrame = getTransformFromPart(modelhdl, link_frame_name, scale).second;
 
-    // There are just two points in the array
 
     auto unit = computeUnitVectorFromAxis(axis_line);
 
     axis_unit_vector.setVal(0, unit[0]);
     axis_unit_vector.setVal(1, unit[1]);
     axis_unit_vector.setVal(2, unit[2]);
 
-   // auto csysAsm_H_csysPart = fromCreo(comp_path->GetTransform(xtrue), scale);
-
 
-    auto& csys_H_child = getTransformFromPart(modelhdl, link_frame_name, scale).second;
+    // There are just two points in the array
 
-    axis_unit_vector = csys_H_child.inverse() * axis_unit_vector;  // We might benefit from performing this operation directly in Creo
+    // We use the medium point of the axis as offset
+    pfcPoint3D_ptr pstart = axis_line->GetEnd1();
+    pfcPoint3D_ptr pend = axis_line->GetEnd2();
+    axis_mid_point_pos[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0];
+    axis_mid_point_pos[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1];
+    axis_mid_point_pos[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2];
+
+    axis_unit_vector = csys_H_linkFrame.inverse() * axis_unit_vector;  // We might benefit from performing this operation directly in Creo
     axis_unit_vector.Normalize();
-
-    return { true, axis_unit_vector, oldChild_H_newChild };
+    return { true, axis_unit_vector, axis_mid_point_pos };
 }
 
 std::string extractFolderPath(const std::string& filePath) {