Fix SphericalJoint definition

src/creo2urdf/include/creo2urdf/Utils.h

@@ -420,6 +420,21 @@ std::pair<bool, std::string> getFirstCoordinateSystemName(pfcModel_ptr modelhdl)
  */
 std::pair<bool, iDynTree::Transform> getTransformFromOwnerToLinkFrame(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale);
 
+
+/**
+ * @brief Retrieves the position of a specified datum in the given part.
+ *
+ * @param modelhdl The part model.
+ * @param datum_name The name of the datum for which the position is requested.
+ * @param scale scaling factor for expressing the position of the datum.
+ *
+ * @return A std::pair<bool, iDynTree::Position> containing the result of the operation:
+ *         - The first element is a boolean indicating success (true) or failure (false).
+ *         - The second element is an iDynTree::Position representing the position of the specified datum.
+ *           If the operation fails, this position will be a zero vector.
+*/
+std::pair<bool, iDynTree::Position> getPointCoordFromPart(pfcModel_ptr modelhdl, const std::string& datum_name, const array<double, 3>& scale);
+
 /**
  * @brief Retrieves the transformation matrix representing the coordinate system of a specified link frame in the given part.
  *

src/creo2urdf/src/Creo2Urdf.cpp

@@ -382,9 +382,9 @@ void Creo2Urdf::OnCommand() {
                 idyn_model.getLinkIndex(getRenameElementFromConfig(child_link_name))
             );
             joint.setRestTransform(parentLink_H_childLink);
-            iDynTree::Transform parent_link_H_joint_center = iDynTree::Transform::Identity();
-            std::tie(ret, parent_link_H_joint_center) = getTransformFromPart(parent_model, datum_name, scale);
-            joint.setJointCenter(idyn_model.getLinkIndex(getRenameElementFromConfig(parent_link_name)), parent_link_H_joint_center.getPosition());
+            iDynTree::Position center_in_parent_link = iDynTree::Position::Zero();
+            std::tie(ret, center_in_parent_link) = getPointCoordFromPart(parent_model, datum_name, scale);
+            joint.setJointCenter(idyn_model.getLinkIndex(getRenameElementFromConfig(parent_link_name)), center_in_parent_link);
             if (idyn_model.addJoint(joint_name, &joint) == iDynTree::JOINT_INVALID_INDEX) {
                 printToMessageWindow("FAILED TO ADD JOINT " + joint_name, c2uLogLevel::WARN);
                 if (warningsAreFatal) {

src/creo2urdf/src/ElementTreeManager.cpp

@@ -57,13 +57,20 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s
             pfcComponentConstraintType::pfcASM_CONSTRAINT_ALIGN,
             pfcModelItemType::pfcITEM_AXIS);
     }
-    else if (joint.type == JointType::Fixed || joint.type == JointType::Spherical)
+    else if (joint.type == JointType::Fixed)
     {
         joint.datum_name = getConstraintDatum(feat,
             pfcComponentConstraintType::pfcASM_CONSTRAINT_CSYS,
             pfcModelItemType::pfcITEM_COORD_SYS);
     }
-    else {
+    else if (joint.type == JointType::Spherical)
+    {
+        joint.datum_name = getConstraintDatum(feat,
+            pfcComponentConstraintType::pfcASM_CONSTRAINT_ALIGN,
+            pfcModelItemType::pfcITEM_POINT);
+    }
+    else
+    {
         printToMessageWindow("Joint type not supported!", c2uLogLevel::WARN);
         return false;
     }

src/creo2urdf/src/Utils.cpp

@@ -146,6 +146,40 @@ std::pair<bool, std::string> getFirstCoordinateSystemName(pfcModel_ptr modelhdl)
     return { true,std::string(csys) };
 }
 
+std::pair<bool, iDynTree::Position> getPointCoordFromPart(pfcModel_ptr modelhdl, const std::string& datum_name, const array<double, 3>& scale) {
+    iDynTree::Position point_coord;
+    auto datum_list = modelhdl->ListItems(pfcModelItemType::pfcITEM_POINT);
+
+    if (datum_list->getarraysize() == 0) {
+        printToMessageWindow("There are no Points in the part " + string(modelhdl->GetFullName()), c2uLogLevel::WARN);
+        return { false, point_coord };
+    }
+
+    for (size_t i = 0; i < datum_list->getarraysize(); i++)
+    {
+        auto datum_elem = datum_list->get(xint(i));
+
+        auto datum = pfcPoint::cast(datum_elem);
+
+        if (string(datum->GetName()) != datum_name)
+        {
+            continue;
+        }
+
+        auto coord = datum->GetPoint();
+
+        point_coord[0] = coord->get(0) * scale[0];
+        point_coord[1] = coord->get(1) * scale[1];
+        point_coord[2] = coord->get(2) * scale[2];
+
+        return { true, point_coord };
+    }
+
+    printToMessageWindow("Unable to find the Point " + datum_name + " in " + string(modelhdl->GetFullName()), c2uLogLevel::WARN);
+    return { false, point_coord };
+
+}
+
 std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale) {
 
     iDynTree::Transform H_child;