Check if model is skeleton and use first csys if CSYS does not exist + small refactor of joints csv configuration

src/creo2urdf/include/creo2urdf/Creo2Urdf.h

@@ -126,6 +126,9 @@ class Creo2Urdf : public pfcUICommandActionListener {
 
     bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parentAsm_H_csysAsm = iDynTree::Transform::Identity());
 
+    bool setJointParametersFromCsv(const rapidcsv::Document& csv, const std::string& joint_name, 
+        iDynTree::IJoint& joint, double conversion_factor);
+
     /**
      * @brief Get the renamed element from the configuration.
      * @param elem_name The original element name.

src/creo2urdf/include/creo2urdf/Utils.h

@@ -400,6 +400,8 @@ void printRotationMatrix(pfcMatrix3D_ptr m);
  */
 void sanitizeSTL(std::string stl);
 
+std::pair<bool, std::string> getFirstCoordinateSystemName(pfcModel_ptr modelhdl);
+
 /**
  * @brief Retrieves the transformation from the owner assembly to a specified link frame in the context of a component path.
  *

src/creo2urdf/src/Creo2Urdf.cpp

@@ -17,7 +17,6 @@
 
 #include <Eigen/Core>
 
-
 bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parentAsm_H_csysAsm) {
 
     for (int i = 0; i < asmListItems->getarraysize(); i++)
@@ -31,20 +30,22 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
 
         auto component_handle = m_session_ptr->RetrieveModel(pfcComponentFeat::cast(asmItemAsFeat)->GetModelDescr());
 
-        if (!component_handle) {
+        if (component_handle == nullptr) {
             return false;
         }
 
-        auto type = component_handle->GetType();
-
-        ElementTreeManager element_tree_manager;
+        if(pfcSolid::cast(component_handle)->GetIsSkeleton())
+        {   
+            printToMessageWindow(std::string(component_handle->GetFullName()) + " is a skeleton, skipping", c2uLogLevel::INFO);
+            continue;
+        }
 
         //printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName()));
 
         xintsequence_ptr seq = xintsequence::create();
         seq->append(asmItemAsFeat->GetId());
 
-
+        ElementTreeManager element_tree_manager;
         element_tree_manager.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map);
 
         pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(model_owner), seq);
@@ -56,12 +57,14 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
         std::string link_frame_name{ "" };
         auto link_name = string(component_handle->GetFullName());
         std::string urdf_link_name { "" };
-
+
+        auto type = component_handle->GetType();
+
         if (type == pfcMDL_ASSEMBLY) {
             link_frame_name = "ASM_CSYS";
         }
         else {
-            link_frame_name = "CSYS";
+            link_frame_name = "";
             urdf_link_name = getRenameElementFromConfig(link_name);
             for (const auto& lf : config["linkFrames"]) {
                 if (lf["linkName"].Scalar() != urdf_link_name)
@@ -72,11 +75,14 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
             }
 
             if (link_frame_name.empty()) {
-                printToMessageWindow(link_name + " misses the frame in the linkFrames section, CSYS will be used instead", c2uLogLevel::WARN);
-                link_frame_name = "CSYS";
+                std::tie(ret, link_frame_name) = getFirstCoordinateSystemName(component_handle);
+
+                if (!ret) return false;
+
+                printToMessageWindow(link_name + " misses the frame in the linkFrames section, " + link_frame_name + " will be used instead", c2uLogLevel::WARN);
             }
         }
-        std::tie(ret, csysAsm_H_linkFrame) = getTransformFromOwnerToLinkFrame(comp_path, component_handle, link_frame_name , scale);
+        std::tie(ret, csysAsm_H_linkFrame) = getTransformFromOwnerToLinkFrame(comp_path, component_handle, link_frame_name, scale);
 
         parentAsm_H_linkFrame = parentAsm_H_csysAsm * csysAsm_H_linkFrame;
 
@@ -130,6 +136,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod
             }
         }
     }
+    return true;
 }
 
 void Creo2Urdf::OnCommand() {
@@ -305,21 +312,7 @@ void Creo2Urdf::OnCommand() {
             }
 
             // Read limits from CSV data, until it is possible to do so from Creo directly
-            if (joints_csv_table.GetRowIdx(joint_name) >= 0) {
-                double min = joints_csv_table.GetCell<double>("lower_limit", joint_name) * conversion_factor;
-                double max = joints_csv_table.GetCell<double>("upper_limit", joint_name) * conversion_factor;
-
-                joint_sh_ptr->enablePosLimits(true);
-                joint_sh_ptr->setPosLimits(0, min, max);
-                // TODO we have to retrieve the rest transform from creo
-                //joint.setRestTransform();
-
-                min = joints_csv_table.GetCell<double>("damping", joint_name);
-                max = joints_csv_table.GetCell<double>("friction", joint_name);
-                joint_sh_ptr->setJointDynamicsType(iDynTree::URDFJointDynamics);
-                joint_sh_ptr->setDamping(0, min);
-                joint_sh_ptr->setStaticFriction(0, max);
-            }
+            setJointParametersFromCsv(joints_csv_table, joint_name, *joint_sh_ptr, conversion_factor);
 
             if (idyn_model.addJoint(getRenameElementFromConfig(parent_link_name),
                 getRenameElementFromConfig(child_link_name), joint_name, joint_sh_ptr.get()) == iDynTree::JOINT_INVALID_INDEX) {
@@ -436,6 +429,29 @@ void Creo2Urdf::OnCommand() {
     return;
 }
 
+bool Creo2Urdf::setJointParametersFromCsv(const rapidcsv::Document& csv, const std::string& joint_name, 
+    iDynTree::IJoint& joint, double conversion_factor = 1.0)
+{
+    if (csv.GetRowIdx(joint_name) < 0) return false;
+
+    double min = csv.GetCell<double>("lower_limit", joint_name) * conversion_factor;
+    double max = csv.GetCell<double>("upper_limit", joint_name) * conversion_factor;
+
+    if (!std::isinf(min) && !std::isinf(max))
+    {
+        joint.enablePosLimits(true);
+        joint.setPosLimits(0, min, max);
+    }
+    // TODO we have to retrieve the rest transform from creo
+    //joint.setRestTransform();
+
+    joint.setJointDynamicsType(iDynTree::URDFJointDynamics);
+    joint.setDamping(0, csv.GetCell<double>("damping", joint_name));
+    joint.setStaticFriction(0, csv.GetCell<double>("friction", joint_name));
+
+    return true;
+}
+
 bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOptions options) {
     iDynTree::ModelExporter mdl_exporter;
 

src/creo2urdf/src/Utils.cpp

@@ -105,7 +105,17 @@ std::pair<bool, iDynTree::Transform> getTransformFromOwnerToLinkFrame(pfcCompone
 
     iDynTree::Transform csysAsm_H_link = iDynTree::Transform::Identity();
 
-    auto csysAsm_H_csysPart = fromCreo(comp_path->GetTransform(xtrue), scale);
+    auto csysAsm_H_csysPart = iDynTree::Transform::Identity();
+    try {
+        csysAsm_H_csysPart = fromCreo(comp_path->GetTransform(xtrue), scale);
+    }
+    xcatchbegin
+    xcatchcip(defaultEx)
+    {
+        printToMessageWindow("Exception caught: Could not retrieve transform of " + link_frame_name, c2uLogLevel::WARN);
+    }
+    xcatchend
+
     iDynTree::Transform csysPart_H_link;
 
     bool ret = false;
@@ -123,6 +133,19 @@ std::pair<bool, iDynTree::Transform> getTransformFromOwnerToLinkFrame(pfcCompone
 
 }
 
+std::pair<bool, std::string> getFirstCoordinateSystemName(pfcModel_ptr modelhdl)
+{
+    auto csys_list = modelhdl->ListItems(pfcModelItemType::pfcITEM_COORD_SYS);
+
+    if (csys_list->getarraysize() == 0) {
+        printToMessageWindow("There are no Coordinate Systems in the part " + std::string(modelhdl->GetFullName()), c2uLogLevel::WARN);
+        return { false, "" };
+    }
+
+    auto csys = pfcCoordSystem::cast(csys_list->get(0))->GetName();
+    return { true,std::string(csys) };
+}
+
 std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale) {
 
     iDynTree::Transform H_child;