Commit to be squashed and fixed

Author: Nicogene

src/creo2urdf/include/creo2urdf/Utils.h

@@ -439,7 +439,7 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
  * @param scale The scaling factor for the origin of the child frame
  * @return std::pair<bool, iDynTree::Direction>  Pair containing a success/failure flag, and the axis direction
  */
-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);
+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);
 
 /**
  * @brief Extracts the folder path from a file path.

src/creo2urdf/src/Creo2Urdf.cpp

@@ -267,8 +267,11 @@ void Creo2Urdf::OnCommand() {
         if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) {
 
             iDynTree::Direction axis;
-            iDynTree::Position axis_offset;
-            std::tie(ret, axis, axis_offset) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
+            iDynTree::Transform oldChild_H_newChild;
+            std::tie(ret, axis, oldChild_H_newChild) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
+
+
+            parent_H_child = parent_H_child * oldChild_H_newChild;
 
             if (!ret && warningsAreFatal)
             {
@@ -283,32 +286,29 @@ void Creo2Urdf::OnCommand() {
 
             auto urdf_child_link_name = getRenameElementFromConfig(child_link_name);
 
+            printToMessageWindow("joint " + urdf_child_link_name);
+
             // THIS IS NOT HAVING EFFECT
-            /*auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
+            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();
 
-            link_H_collision_solid_shape.setPosition(link_H_collision_solid_shape.getPosition() - axis_offset);
-            link_H_visual_solid_shape.setPosition(link_H_visual_solid_shape.getPosition() - axis_offset);
+            printToMessageWindow("BEFORE link_H_collision_solid_shape " + link_H_visual_solid_shape.toString());
 
-            idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(link_H_collision_solid_shape);
-            idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(link_H_visual_solid_shape);
+            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);
 
+            link_H_visual_solid_shape = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
+            printToMessageWindow("AFTER link_H_collision_solid_shape " + link_H_visual_solid_shape.toString());
 
-            parent_H_child.setPosition(parent_H_child.getPosition() + axis_offset);
-            */
-
-
-            // I need to rotate the axis offset by the parent_H_child rotation ????
-            //axis_offset = parent_H_child.getRotation() * axis_offset;
 
             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() + axis_offset));
+                dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition()));
             }
             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() + axis_offset));
+                dynamic_cast<iDynTree::PrismaticJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition()));
             }
 
             joint_sh_ptr->setRestTransform(parent_H_child);

src/creo2urdf/src/Utils.cpp

@@ -172,22 +172,21 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
     return { false, H_child };
 }
 
-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) {
+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) {
 
     iDynTree::Direction axis_unit_vector;
-    iDynTree::Position axis_offset;
-    axis_offset.zero();
+    iDynTree::Transform oldChild_H_newChild = iDynTree::Transform::Identity();
     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, axis_offset };
+        return { false, axis_unit_vector, oldChild_H_newChild };
     }
 
     if (axis_name.empty())
-        return { false, axis_unit_vector, axis_offset };
+        return { false, axis_unit_vector, oldChild_H_newChild };
 
     pfcAxis* axis = nullptr;
 
@@ -208,11 +207,11 @@ std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcMod
         // We use the medium point of the axis as offset
         pfcPoint3D_ptr pstart = axis_line->GetEnd1();
         pfcPoint3D_ptr pend = axis_line->GetEnd2();
-        axis_offset[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0];
-        axis_offset[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1];
-        axis_offset[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2];
+        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 });
 
-        printToMessageWindow("Axis offset: " + axis_offset.toString());
     }
 
     // There are just two points in the array
@@ -227,12 +226,11 @@ std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcMod
 
 
     auto& csys_H_child = getTransformFromPart(modelhdl, link_frame_name, scale).second;
-    printToMessageWindow("csys_H_child: " + csys_H_child.toString());
 
     axis_unit_vector = csys_H_child.inverse() * axis_unit_vector;  // We might benefit from performing this operation directly in Creo
     axis_unit_vector.Normalize();
 
-    return { true, axis_unit_vector, axis_offset };
+    return { true, axis_unit_vector, oldChild_H_newChild };
 }
 
 std::string extractFolderPath(const std::string& filePath) {