diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index 18b5e85..71a28c1 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/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. diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index f4c1c62..ddae40f 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/src/creo2urdf/include/creo2urdf/Utils.h @@ -400,6 +400,8 @@ void printRotationMatrix(pfcMatrix3D_ptr m); */ void sanitizeSTL(std::string stl); +std::pair getFirstCoordinateSystemName(pfcModel_ptr modelhdl); + /** * @brief Retrieves the transformation from the owner assembly to a specified link frame in the context of a component path. * diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index f62670b..e9ba097 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -17,7 +17,6 @@ #include - 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("lower_limit", joint_name) * conversion_factor; - double max = joints_csv_table.GetCell("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("damping", joint_name); - max = joints_csv_table.GetCell("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("lower_limit", joint_name) * conversion_factor; + double max = csv.GetCell("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("damping", joint_name)); + joint.setStaticFriction(0, csv.GetCell("friction", joint_name)); + + return true; +} + bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOptions options) { iDynTree::ModelExporter mdl_exporter; diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index 9055b8c..de451f4 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -105,7 +105,17 @@ std::pair 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 getTransformFromOwnerToLinkFrame(pfcCompone } +std::pair 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 getTransformFromPart(pfcModel_ptr modelhdl, const std::string& link_frame_name, const array& scale) { iDynTree::Transform H_child; diff --git a/src/creo2urdf/assets/logo.png b/src/creo2urdf/text/resource/logo.png similarity index 100% rename from src/creo2urdf/assets/logo.png rename to src/creo2urdf/text/resource/logo.png