Better handle the moveLinkFramesToBeCompatibleWithURDF

icub-tech-iit · Aug 2, 2024 · eb10745 · eb10745
1 parent 44685fd
commit eb10745
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 25 deletions.
diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/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 {

diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp
@@ -263,9 +263,9 @@ void Creo2Urdf::OnCommand() {
 
         if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) {
 
-            iDynTree::Direction axis;
+            iDynTree::Direction direction;
             iDynTree::Position axis_mid_point_pos_in_parent;
-            std::tie(ret, axis, axis_mid_point_pos_in_parent) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
+            std::tie(ret, direction, axis_mid_point_pos_in_parent) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
 
             if (!ret && warningsAreFatal)
             {
@@ -275,29 +275,27 @@ void Creo2Urdf::OnCommand() {
             if (config["reverseRotationAxis"].IsDefined() && 
                 config["reverseRotationAxis"].Scalar().find(joint_name) != std::string::npos)
             {
-                axis = axis.reverse();
+                direction = direction.reverse();
             }
 
             auto urdf_parent_link_name = getRenameElementFromConfig(parent_link_name);
-
-            auto axis_origin = iDynTree::Position::Zero();
 
-            if (parent_link_frame == "CSYS") {
-                axis_origin = axis_mid_point_pos_in_parent;
-            }
-            else {
-                axis_origin = parentLink_H_childLink.getPosition();
+            iDynTree::Axis idyn_axis{ direction, parentLink_H_childLink.getPosition() };
+
+            // 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, axis_origin));
+                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, axis_origin));
+                dynamic_cast<iDynTree::PrismaticJoint*>(joint_sh_ptr.get())->setAxis(idyn_axis);
             }
 
             joint_sh_ptr->setRestTransform(parentLink_H_childLink);
@@ -444,7 +442,16 @@ bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOp
     // Convert modelToExport in a URDF-compatible model (using the default base link)
     iDynTree::Model modelToExportURDFCompatible;
 
-    bool ok = iDynTree::moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(mdl, 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);

diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp
@@ -200,9 +200,9 @@ std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcMod
     }
 
     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 };
-	}
+        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();
 
@@ -220,14 +220,12 @@ std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcMod
 
     // There are just two points in the array
 
-    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();
-        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];
-    }
+    // 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();