From 4237f2c4aa4f39482251c79fe47ca1445ccba35c Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 23 May 2024 16:38:44 +0200 Subject: [PATCH 1/7] Add recursion in the assembly navigation --- src/creo2urdf/include/creo2urdf/Creo2Urdf.h | 8 +- src/creo2urdf/include/creo2urdf/Sensorizer.h | 12 +- src/creo2urdf/src/Creo2Urdf.cpp | 194 ++++++++++--------- 3 files changed, 116 insertions(+), 98 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index 13d918f..d8117ee 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h @@ -66,7 +66,7 @@ class Creo2Urdf : public pfcUICommandActionListener { Creo2Urdf(const std::string& yaml_path, const std::string& csv_path, const std::string& output_path, pfcModel_ptr asm_model_ptr) : m_yaml_path(yaml_path), m_csv_path(csv_path), m_output_path(output_path), - m_asm_model_ptr(asm_model_ptr) { } + m_root_asm_model_ptr(asm_model_ptr) { } private: /** @@ -124,6 +124,8 @@ class Creo2Urdf : public pfcUICommandActionListener { */ bool loadYamlConfig(const std::string& filename); + bool processAsmItems(pfcModelItems_ptr asmListItems); + /** * @brief Get the renamed element from the configuration. * @param elem_name The original element name. @@ -147,7 +149,9 @@ class Creo2Urdf : public pfcUICommandActionListener { std::string m_yaml_path{ "" }; /**< Path to the YAML configuration file. */ std::string m_csv_path{ "" }; /**< Path to the CSV file containing joint information. */ std::string m_output_path{ "" }; /**< Output path for the exported URDF file. */ - pfcModel_ptr m_asm_model_ptr{ nullptr }; /**< Handle to the Creo model. */ + pfcModel_ptr m_root_asm_model_ptr{ nullptr }; /**< Handle to the Creo model. */ + ElementTreeManager m_element_tree; /**< Manager for the element tree. */ + pfcSession_ptr m_session_ptr{ nullptr }; /**< Handle to the Creo session. */ }; class Creo2UrdfAccess : public pfcUICommandAccessListener { diff --git a/src/creo2urdf/include/creo2urdf/Sensorizer.h b/src/creo2urdf/include/creo2urdf/Sensorizer.h index 3f19a52..47cbb9d 100644 --- a/src/creo2urdf/include/creo2urdf/Sensorizer.h +++ b/src/creo2urdf/include/creo2urdf/Sensorizer.h @@ -65,18 +65,18 @@ struct Sensorizer { /** * @brief Builds a vector of XML trees as strings for force/torque sensors, - * starting from the information retrieved in the YAML. The XML trees are returned in this - * way so that they can be added as blobs using iDynTree. - * + * starting from the information retrieved in the YAML. The XML trees are returned in this + * way so that they can be added as blobs using iDynTree. + * * @return A vector of strings representing the XML blobs. */ std::vector buildFTXMLBlobs(); /** * @brief Builds a vector of XML trees as strings for general sensors, - * starting from the information retrieved in the YAML. The XML trees are returned in this - * way so that they can be added as blobs using iDynTree. - * + * starting from the information retrieved in the YAML. The XML trees are returned in this + * way so that they can be added as blobs using iDynTree. + * * @return A vector of strings representing the XML blobs. */ std::vector buildSensorsXMLBlobs(); diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index c169259..c5f665d 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -16,16 +16,108 @@ #include + +bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems) { + + for (int i = 0; i < asmListItems->getarraysize(); i++) + { + bool ret{ false }; + auto asmItemAsFeat = pfcFeature::cast(asmListItems->get(i)); + if (asmItemAsFeat->GetFeatType() != pfcFeatureType::pfcFEATTYPE_COMPONENT) + { + continue; + } + + auto component_handle = m_session_ptr->RetrieveModel(pfcComponentFeat::cast(asmItemAsFeat)->GetModelDescr()); + + if (!component_handle) { + return false; + } + + auto type = component_handle->GetType(); + if (type == pfcMDL_ASSEMBLY) { + auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE); + processAsmItems(sub_asm_component_list); + } + + xintsequence_ptr seq = xintsequence::create(); + seq->append(asmItemAsFeat->GetId()); + + m_element_tree.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map); + + pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(m_root_asm_model_ptr), seq); + + auto link_name = string(component_handle->GetFullName()); + + iDynTree::Transform root_H_link = iDynTree::Transform::Identity(); + iDynTree::Transform csysPart_H_link = iDynTree::Transform::Identity(); + + std::string urdf_link_name = getRenameElementFromConfig(link_name); + + std::string link_frame_name = ""; + + for (const auto& lf : config["linkFrames"]) { + if (lf["linkName"].Scalar() != urdf_link_name) + { + continue; + } + link_frame_name = lf["frameName"].Scalar(); + } + + 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, root_H_link) = getTransformFromRootToChild(comp_path, component_handle, link_frame_name, scale); + + if (!ret && warningsAreFatal) + { + return false; + } + + auto mass_prop = pfcSolid::cast(component_handle)->GetMassProperty(); + + std::tie(ret, csysPart_H_link) = getTransformFromPart(component_handle, link_frame_name, scale); + if (!ret && warningsAreFatal) + { + return false; + } + + iDynTree::Link link; + link.setInertia(computeSpatialInertiafromCreo(mass_prop, csysPart_H_link, urdf_link_name)); + + if (!link.getInertia().isPhysicallyConsistent()) + { + printToMessageWindow(link_name + " is NOT physically consistent!", c2uLogLevel::WARN); + if (warningsAreFatal) { + return false; + } + } + + LinkInfo l_info{ urdf_link_name, component_handle, root_H_link, link_frame_name }; + link_info_map.insert(std::make_pair(link_name, l_info)); + populateExportedFrameInfoMap(component_handle); + + idyn_model.addLink(urdf_link_name, link); + if (!addMeshAndExport(component_handle, link_frame_name)) { + printToMessageWindow("Failed to export mesh for " + link_name, c2uLogLevel::WARN); + if (warningsAreFatal) { + return false; + } + } + } +} + void Creo2Urdf::OnCommand() { - pfcSession_ptr session_ptr = pfcGetProESession(); - if (!session_ptr) { + m_session_ptr = pfcGetProESession(); + if (!m_session_ptr) { printToMessageWindow("Failed to get the session", c2uLogLevel::WARN); return; } - if (!m_asm_model_ptr) { - m_asm_model_ptr = session_ptr->GetCurrentModel(); - if (!m_asm_model_ptr) { + if (!m_root_asm_model_ptr) { + m_root_asm_model_ptr = m_session_ptr->GetCurrentModel(); + if (!m_root_asm_model_ptr) { printToMessageWindow("Failed to get the current model", c2uLogLevel::WARN); return; } @@ -33,7 +125,7 @@ void Creo2Urdf::OnCommand() { // TODO Principal units probably to be changed from MM to M before getting the model properties - // pfcSolid_ptr solid_ptr = pfcSolid::cast(session_ptr->GetCurrentModel()); + // pfcSolid_ptr solid_ptr = pfcSolid::cast(m_session_ptr->GetCurrentModel()); // auto length_unit = solid_ptr->GetPrincipalUnits()->GetUnit(pfcUnitType::pfcUNIT_LENGTH); // length_unit->Modify(pfcUnitConversionFactor::Create(0.001), length_unit->GetReferenceUnit()); // IT DOES NOT WORK @@ -43,7 +135,7 @@ void Creo2Urdf::OnCommand() { // YAML file path if (m_yaml_path.empty()) { - m_yaml_path = string(session_ptr->UIOpenFile(yaml_file_open_option)); + m_yaml_path = string(m_session_ptr->UIOpenFile(yaml_file_open_option)); } if (!loadYamlConfig(m_yaml_path)) { @@ -54,14 +146,14 @@ void Creo2Urdf::OnCommand() { if (m_csv_path.empty()) { auto csv_file_open_option = pfcFileOpenOptions::Create("*.csv"); csv_file_open_option->SetDialogLabel("Select the csv"); - m_csv_path = string(session_ptr->UIOpenFile(csv_file_open_option)); + m_csv_path = string(m_session_ptr->UIOpenFile(csv_file_open_option)); } rapidcsv::Document joints_csv_table(m_csv_path, rapidcsv::LabelParams(0, 0)); // Output folder path if (m_output_path.empty()) { auto output_folder_open_option = pfcDirectorySelectionOptions::Create(); output_folder_open_option->SetDialogLabel("Select the output dir"); - m_output_path = string(session_ptr->UISelectDirectory(output_folder_open_option)); + m_output_path = string(m_session_ptr->UISelectDirectory(output_folder_open_option)); } printToMessageWindow("Output path is: " + m_output_path); @@ -71,7 +163,7 @@ void Creo2Urdf::OnCommand() { bool ret; - auto asm_component_list = m_asm_model_ptr->ListItems(pfcModelItemType::pfcITEM_FEATURE); + auto asm_component_list = m_root_asm_model_ptr->ListItems(pfcModelItemType::pfcITEM_FEATURE); if (asm_component_list->getarraysize() == 0) { printToMessageWindow("There are no FEATURES in the asm", c2uLogLevel::WARN); return; @@ -115,86 +207,8 @@ void Creo2Urdf::OnCommand() { sensorizer.readFTSensorsFromConfig(config); sensorizer.readSensorsFromConfig(config); - ElementTreeManager elem_tree; - // Let's traverse the model tree and get all links and axis properties - for (int i = 0; i < asm_component_list->getarraysize(); i++) - { - auto feat = pfcFeature::cast(asm_component_list->get(i)); - - if (feat->GetFeatType() != pfcFeatureType::pfcFEATTYPE_COMPONENT) - { - continue; - } - - xintsequence_ptr seq = xintsequence::create(); - seq->append(feat->GetId()); - - elem_tree.populateJointInfoFromElementTree(feat, joint_info_map); - - pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(m_asm_model_ptr), seq); - - auto component_handle = session_ptr->RetrieveModel(pfcComponentFeat::cast(feat)->GetModelDescr()); - - auto link_name = string(component_handle->GetFullName()); - - iDynTree::Transform root_H_link = iDynTree::Transform::Identity(); - iDynTree::Transform csysPart_H_link = iDynTree::Transform::Identity(); - - std::string urdf_link_name = getRenameElementFromConfig(link_name); - - std::string link_frame_name = ""; - - for (const auto& lf : config["linkFrames"]) { - if (lf["linkName"].Scalar() != urdf_link_name) - { - continue; - } - link_frame_name = lf["frameName"].Scalar(); - } - - 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, root_H_link) = getTransformFromRootToChild(comp_path, component_handle, link_frame_name, scale); - - if (!ret && warningsAreFatal) - { - return; - } - - auto mass_prop = pfcSolid::cast(component_handle)->GetMassProperty(); - - std::tie(ret, csysPart_H_link) = getTransformFromPart(component_handle, link_frame_name, scale); - if (!ret && warningsAreFatal) - { - return; - } - - iDynTree::Link link; - link.setInertia(computeSpatialInertiafromCreo(mass_prop, csysPart_H_link, urdf_link_name)); - - if (!link.getInertia().isPhysicallyConsistent()) - { - printToMessageWindow(link_name + " is NOT physically consistent!", c2uLogLevel::WARN); - if (warningsAreFatal) { - return; - } - } - - LinkInfo l_info{urdf_link_name, component_handle, root_H_link, link_frame_name }; - link_info_map.insert(std::make_pair(link_name, l_info)); - populateExportedFrameInfoMap(component_handle); - - idyn_model.addLink(urdf_link_name, link); - if (!addMeshAndExport(component_handle, link_frame_name)) { - printToMessageWindow("Failed to export mesh for " + link_name, c2uLogLevel::WARN); - if (warningsAreFatal) { - return; - } - } - } + bool ok = processAsmItems(asm_component_list); // Now we have to add joints to the iDynTree model @@ -378,7 +392,7 @@ void Creo2Urdf::OnCommand() { m_csv_path.clear(); m_output_path.clear(); config = YAML::Node(); - m_asm_model_ptr = nullptr; + m_root_asm_model_ptr = nullptr; return; } From 999929a1b276b7a12807f0d7a49371c77aa22d9d Mon Sep 17 00:00:00 2001 From: Nicogene Date: Tue, 28 May 2024 15:38:43 +0200 Subject: [PATCH 2/7] Still not working but several improvements --- src/creo2urdf/include/creo2urdf/Creo2Urdf.h | 2 +- src/creo2urdf/src/Creo2Urdf.cpp | 23 +++++++++++++++++---- src/creo2urdf/src/Sensorizer.cpp | 7 ++++++- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index d8117ee..487b355 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h @@ -124,7 +124,7 @@ class Creo2Urdf : public pfcUICommandActionListener { */ bool loadYamlConfig(const std::string& filename); - bool processAsmItems(pfcModelItems_ptr asmListItems); + bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner); /** * @brief Get the renamed element from the configuration. diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index c5f665d..bb9c765 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -17,7 +17,7 @@ #include -bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems) { +bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner) { for (int i = 0; i < asmListItems->getarraysize(); i++) { @@ -34,18 +34,27 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems) { return false; } + printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName())); + auto type = component_handle->GetType(); if (type == pfcMDL_ASSEMBLY) { auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE); - processAsmItems(sub_asm_component_list); + auto ok = processAsmItems(sub_asm_component_list, component_handle); + if (!ok) { + return false; + } + else { + continue; + } } xintsequence_ptr seq = xintsequence::create(); seq->append(asmItemAsFeat->GetId()); + m_element_tree.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map); - pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(m_root_asm_model_ptr), seq); + pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(model_owner), seq); auto link_name = string(component_handle->GetFullName()); @@ -70,6 +79,8 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems) { } std::tie(ret, root_H_link) = getTransformFromRootToChild(comp_path, component_handle, link_frame_name, scale); + printToMessageWindow(root_H_link.toString()); + if (!ret && warningsAreFatal) { return false; @@ -208,7 +219,11 @@ void Creo2Urdf::OnCommand() { sensorizer.readSensorsFromConfig(config); // Let's traverse the model tree and get all links and axis properties - bool ok = processAsmItems(asm_component_list); + bool ok = processAsmItems(asm_component_list, m_root_asm_model_ptr); + if (!ok) { + printToMessageWindow("Failed to process the assembly", c2uLogLevel::WARN); + return; + } // Now we have to add joints to the iDynTree model diff --git a/src/creo2urdf/src/Sensorizer.cpp b/src/creo2urdf/src/Sensorizer.cpp index 1797d66..0bb9945 100644 --- a/src/creo2urdf/src/Sensorizer.cpp +++ b/src/creo2urdf/src/Sensorizer.cpp @@ -40,6 +40,11 @@ void Sensorizer::readSensorsFromConfig(const YAML::Node & config) if (s["exportedFrameName"].IsDefined()) { exported_frame_name = s["exportedFrameName"].Scalar(); } + std::vector sensor_blobs; + if (s["sensorBlobs"].IsDefined()) + { + sensor_blobs = s["sensorBlobs"].as>(); + } try { @@ -51,7 +56,7 @@ void Sensorizer::readSensorsFromConfig(const YAML::Node & config) export_frame, stringToEnum(sensor_type_map, s["sensorType"].Scalar()), update_rate, - s["sensorBlobs"].as>() }); + sensor_blobs }); } catch (YAML::Exception& e) { From 0fae2686dfe37a1fef791056c8cec46a410f079e Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 29 May 2024 17:12:40 +0200 Subject: [PATCH 3/7] Try to concatenate transforms from subassembly to items --- src/creo2urdf/include/creo2urdf/Creo2Urdf.h | 2 +- src/creo2urdf/src/Creo2Urdf.cpp | 34 ++++++++++++++++----- src/creo2urdf/src/Sensorizer.cpp | 5 +++ 3 files changed, 32 insertions(+), 9 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index 487b355..b9c3202 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h @@ -124,7 +124,7 @@ class Creo2Urdf : public pfcUICommandActionListener { */ bool loadYamlConfig(const std::string& filename); - bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner); + bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parent_H_csysItem = iDynTree::Transform::Identity()); /** * @brief Get the renamed element from the configuration. diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index bb9c765..d655706 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -17,7 +17,7 @@ #include -bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner) { +bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parent_H_csysItem) { for (int i = 0; i < asmListItems->getarraysize(); i++) { @@ -39,7 +39,18 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod auto type = component_handle->GetType(); if (type == pfcMDL_ASSEMBLY) { auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE); - auto ok = processAsmItems(sub_asm_component_list, component_handle); + iDynTree::Transform parentAsmCsys_H_asmCsys = iDynTree::Transform::Identity(); + bool ret{ false }; + std::tie(ret, parentAsmCsys_H_asmCsys) = getTransformFromPart(component_handle, "ASM_CSYS", scale); + if(!ret) { + printToMessageWindow("Failed to get the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::WARN); + //return false; + } + else { + printToMessageWindow("Got the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::INFO); + printToMessageWindow(parentAsmCsys_H_asmCsys.toString()); + } + auto ok = processAsmItems(sub_asm_component_list, component_handle, parentAsmCsys_H_asmCsys); if (!ok) { return false; } @@ -58,8 +69,9 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod auto link_name = string(component_handle->GetFullName()); - iDynTree::Transform root_H_link = iDynTree::Transform::Identity(); + iDynTree::Transform csysAsm_H_link = iDynTree::Transform::Identity(); iDynTree::Transform csysPart_H_link = iDynTree::Transform::Identity(); + iDynTree::Transform parent_H_link = iDynTree::Transform::Identity(); std::string urdf_link_name = getRenameElementFromConfig(link_name); @@ -77,9 +89,11 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod printToMessageWindow(link_name + " misses the frame in the linkFrames section, CSYS will be used instead", c2uLogLevel::WARN); link_frame_name = "CSYS"; } - std::tie(ret, root_H_link) = getTransformFromRootToChild(comp_path, component_handle, link_frame_name, scale); + std::tie(ret, csysAsm_H_link) = getTransformFromRootToChild(comp_path, component_handle, link_frame_name, scale); + + parent_H_link = parent_H_csysItem * csysAsm_H_link; - printToMessageWindow(root_H_link.toString()); + //printToMessageWindow(csysAsm_H_link.toString()); if (!ret && warningsAreFatal) { @@ -105,7 +119,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod } } - LinkInfo l_info{ urdf_link_name, component_handle, root_H_link, link_frame_name }; + LinkInfo l_info{ urdf_link_name, component_handle, parent_H_link, link_frame_name }; link_info_map.insert(std::make_pair(link_name, l_info)); populateExportedFrameInfoMap(component_handle); @@ -231,12 +245,16 @@ void Creo2Urdf::OnCommand() { auto parent_link_name = joint_info.second.parent_link_name; auto child_link_name = joint_info.second.child_link_name; auto axis_name = joint_info.second.datum_name; + auto joint_name = getRenameElementFromConfig(parent_link_name + "--" + child_link_name); + // This handles the case of a "cut" assembly, where we have an axis but we miss the child link. - if (child_link_name.empty()) { + if (child_link_name.empty() || link_info_map.find(parent_link_name) == link_info_map.end() || link_info_map.find(child_link_name) == link_info_map.end()) { + printToMessageWindow("Skipping joint " + joint_name + " child link name " + child_link_name + " parent link name " + parent_link_name , c2uLogLevel::WARN); continue; } - auto joint_name = getRenameElementFromConfig(parent_link_name + "--" + child_link_name); + + printToMessageWindow("Adding joint " + joint_name); auto root_H_parent_link = link_info_map.at(parent_link_name).root_H_link; auto root_H_child_link = link_info_map.at(child_link_name).root_H_link; auto child_model = link_info_map.at(child_link_name).modelhdl; diff --git a/src/creo2urdf/src/Sensorizer.cpp b/src/creo2urdf/src/Sensorizer.cpp index 0bb9945..113ff78 100644 --- a/src/creo2urdf/src/Sensorizer.cpp +++ b/src/creo2urdf/src/Sensorizer.cpp @@ -119,6 +119,11 @@ void Sensorizer::assignTransformToFTSensor(const std::map Date: Thu, 6 Jun 2024 15:46:18 +0200 Subject: [PATCH 4/7] The light at the end of the tunnel --- src/creo2urdf/include/creo2urdf/Creo2Urdf.h | 1 - .../include/creo2urdf/ElementTreeManager.h | 10 +++--- src/creo2urdf/include/creo2urdf/Utils.h | 2 +- src/creo2urdf/src/Creo2Urdf.cpp | 18 ++++++---- src/creo2urdf/src/ElementTreeManager.cpp | 34 ++++++++++++------- src/creo2urdf/src/Sensorizer.cpp | 17 ++++++++-- 6 files changed, 55 insertions(+), 27 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index b9c3202..f87e96c 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h @@ -150,7 +150,6 @@ class Creo2Urdf : public pfcUICommandActionListener { std::string m_csv_path{ "" }; /**< Path to the CSV file containing joint information. */ 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. */ - ElementTreeManager m_element_tree; /**< Manager for the element tree. */ pfcSession_ptr m_session_ptr{ nullptr }; /**< Handle to the Creo session. */ }; diff --git a/src/creo2urdf/include/creo2urdf/ElementTreeManager.h b/src/creo2urdf/include/creo2urdf/ElementTreeManager.h index a7acafe..947427e 100644 --- a/src/creo2urdf/include/creo2urdf/ElementTreeManager.h +++ b/src/creo2urdf/include/creo2urdf/ElementTreeManager.h @@ -64,7 +64,7 @@ class ElementTreeManager { * @param[out] joint_info_map A map containing joint information. * @return True if successful, false otherwise. */ - bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& joint_info_map); + bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& joint_info_map, bool is_asm=false); /** * @brief Gets the constraint type between two assembled parts. @@ -85,10 +85,10 @@ class ElementTreeManager { std::string getChildName(); private: - wfcElementTree_ptr tree = nullptr; ///< Pointer to the ElementTree of the part as feature. - wfcWFeature_ptr wfeat = nullptr; ///< Pointer to the part as feature. - pfcSolid_ptr parent_solid; ///< Pointer to the parent solid. - pfcSolid_ptr child_solid; ///< Pointer to the child solid. + wfcElementTree_ptr tree{ nullptr }; ///< Pointer to the ElementTree of the part as feature. + wfcWFeature_ptr wfeat{ nullptr }; ///< Pointer to the part as feature. + pfcSolid_ptr parent_solid{ nullptr }; ///< Pointer to the parent solid. + pfcSolid_ptr child_solid{ nullptr }; ///< Pointer to the child solid. /* * @brief Retrieves the name of a common datum for the given model item type. diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index ff6372a..2f2f4d6 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/src/creo2urdf/include/creo2urdf/Utils.h @@ -186,7 +186,7 @@ struct FTSensorInfo { std::string frame{"sensor"}; ///< Frame associated with the FT sensor. std::string sensorName{""}; ///< Name of the FT sensor. std::string frameName{""}; ///< Name of the frame. - std::string linkName{" "}; ///< Name of the associated link. + std::string linkName{""}; ///< Name of the associated link. std::string exportedFrameName{""}; ///< Name of the exported frame. iDynTree::Transform parent_link_H_sensor{iDynTree::Transform::Identity()}; ///< 3D transform from parent link to sensor. iDynTree::Transform child_link_H_sensor{iDynTree::Transform::Identity()}; ///< 3D transform from child link to sensor. diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index d655706..c2fddb9 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -33,12 +33,18 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod if (!component_handle) { return false; } + ElementTreeManager element_tree_manager; - printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName())); + //printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName())); auto type = component_handle->GetType(); if (type == pfcMDL_ASSEMBLY) { auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE); + auto ok = element_tree_manager.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map); + if (!ok) { + // WE NEED TO FIX the failure, the paths are not cleared + //continue; + } iDynTree::Transform parentAsmCsys_H_asmCsys = iDynTree::Transform::Identity(); bool ret{ false }; std::tie(ret, parentAsmCsys_H_asmCsys) = getTransformFromPart(component_handle, "ASM_CSYS", scale); @@ -47,10 +53,10 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod //return false; } else { - printToMessageWindow("Got the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::INFO); - printToMessageWindow(parentAsmCsys_H_asmCsys.toString()); + //printToMessageWindow("Got the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::INFO); + //printToMessageWindow(parentAsmCsys_H_asmCsys.toString()); } - auto ok = processAsmItems(sub_asm_component_list, component_handle, parentAsmCsys_H_asmCsys); + ok = processAsmItems(sub_asm_component_list, component_handle, parentAsmCsys_H_asmCsys); if (!ok) { return false; } @@ -63,7 +69,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod seq->append(asmItemAsFeat->GetId()); - m_element_tree.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map); + element_tree_manager.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map); pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(model_owner), seq); @@ -245,7 +251,7 @@ void Creo2Urdf::OnCommand() { auto parent_link_name = joint_info.second.parent_link_name; auto child_link_name = joint_info.second.child_link_name; auto axis_name = joint_info.second.datum_name; - auto joint_name = getRenameElementFromConfig(parent_link_name + "--" + child_link_name); + auto joint_name = getRenameElementFromConfig(joint_info.first); // This handles the case of a "cut" assembly, where we have an axis but we miss the child link. if (child_link_name.empty() || link_info_map.find(parent_link_name) == link_info_map.end() || link_info_map.find(child_link_name) == link_info_map.end()) { diff --git a/src/creo2urdf/src/ElementTreeManager.cpp b/src/creo2urdf/src/ElementTreeManager.cpp index 8fe1399..afe48f9 100644 --- a/src/creo2urdf/src/ElementTreeManager.cpp +++ b/src/creo2urdf/src/ElementTreeManager.cpp @@ -24,13 +24,16 @@ ElementTreeManager::ElementTreeManager(pfcFeature_ptr feat, std::map& joint_info_map) +bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& joint_info_map, bool is_asm) { wfeat = wfcWFeature::cast(feat); try { tree = wfeat->GetElementTree(nullptr, wfcFEAT_EXTRACT_NO_OPTS); + std::string joint_name = std::to_string(feat->GetId()) + ".xml"; + //printToMessageWindow("Element tree extracted for feature " + std::to_string(feat->GetId()), c2uLogLevel::INFO); + tree->WriteElementTreeToFile(wfcELEMTREE_XML, joint_name.c_str()); } xcatchbegin xcatchcip(pfcXToolkitInvalidType) @@ -41,11 +44,16 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s JointInfo joint; - if (!retrieveSolidReferences()) + + if (!retrieveSolidReferences()) { + printToMessageWindow("Could not retrieve solid references!", c2uLogLevel::WARN); return false; + } joint.child_link_name = getChildName(); joint.parent_link_name = getParentName(); + std::string joint_name = joint.parent_link_name + "--" + joint.child_link_name; + printToMessageWindow("JOINT !! " + joint_name, c2uLogLevel::INFO); joint.type = proAsmCompSetType_to_JointType.at(static_cast(getConstraintType())); if (joint.type == JointType::Revolute) @@ -72,7 +80,7 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s return false; } - joint_info_map.insert({ joint.datum_name, joint }); + joint_info_map.insert({ joint_name, joint }); return true; } @@ -126,8 +134,9 @@ string ElementTreeManager::getConstraintDatum(pfcFeature_ptr feat, pfcComponentC std::string ElementTreeManager::getParentName() { - if (tree == nullptr) + if (!tree || !parent_solid) { + printToMessageWindow("Tree or parent solid is null!", c2uLogLevel::WARN); return ""; } try{ @@ -143,13 +152,14 @@ std::string ElementTreeManager::getParentName() std::string ElementTreeManager::getChildName() { - if (tree == nullptr) + if (!tree || !child_solid) { + printToMessageWindow("Tree or child solid is null!", c2uLogLevel::WARN); return ""; } try { - return std::string(child_solid->GetFullName()); + return std::string(child_solid->GetFullName()); } xcatchbegin xcatchcip(defaultEx) @@ -185,18 +195,18 @@ bool ElementTreeManager::retrieveSolidReferences() for (int m = 0; m < extrefs->getarraysize(); m++) { auto extref = extrefs->get(m)->GetAsmcomponents()->GetPathToRef()->GetLeaf(); - - if (std::string(extref->GetFullName()) != retrievePartName()) - { + // While defining a constraint the first part is the parent link and the second part is the child link + if (extref && !parent_solid) { parent_solid = extref; } - if (std::string(extref->GetFullName()) == retrievePartName()) - { + else if (extref && !child_solid && parent_solid) { child_solid = extref; } + else { + break; + } } } - return true; } diff --git a/src/creo2urdf/src/Sensorizer.cpp b/src/creo2urdf/src/Sensorizer.cpp index 113ff78..5c769f5 100644 --- a/src/creo2urdf/src/Sensorizer.cpp +++ b/src/creo2urdf/src/Sensorizer.cpp @@ -119,12 +119,18 @@ void Sensorizer::assignTransformToFTSensor(const std::map& pair) { + return pair.second.datum_name == f.second.frameName; + }); + + if (joint_it == joint_info_map.end()) { continue; } - JointInfo j_info = joint_info_map.at(f.second.frameName); + JointInfo j_info = joint_it->second; LinkInfo parent_l_info = link_info_map.at(j_info.parent_link_name); LinkInfo child_l_info = link_info_map.at(j_info.child_link_name); @@ -261,6 +267,13 @@ void Sensorizer::assignTransformToSensors(const std::map Date: Tue, 11 Jun 2024 14:26:19 +0200 Subject: [PATCH 5/7] Transforms between links correct, axes to be fixed --- CMakeLists.txt | 2 +- src/creo2urdf/include/creo2urdf/Creo2Urdf.h | 2 +- .../include/creo2urdf/ElementTreeManager.h | 6 +- src/creo2urdf/include/creo2urdf/Utils.h | 9 +- src/creo2urdf/src/Creo2Urdf.cpp | 140 +++++++++--------- src/creo2urdf/src/ElementTreeManager.cpp | 34 ++++- src/creo2urdf/src/Utils.cpp | 27 +++- 7 files changed, 135 insertions(+), 85 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 57d6fc7..6a16011 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,7 @@ include(GNUInstallDirs) include(FeatureSummary) find_package(Eigen3 REQUIRED) -find_package(iDynTree REQUIRED) +find_package(iDynTree 12.3.1 REQUIRED) find_package(yaml-cpp REQUIRED) find_package(LibXml2 REQUIRED) diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index f87e96c..cd5c460 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h @@ -124,7 +124,7 @@ class Creo2Urdf : public pfcUICommandActionListener { */ bool loadYamlConfig(const std::string& filename); - bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parent_H_csysItem = iDynTree::Transform::Identity()); + bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parentAsm_H_csysAsm = iDynTree::Transform::Identity()); /** * @brief Get the renamed element from the configuration. diff --git a/src/creo2urdf/include/creo2urdf/ElementTreeManager.h b/src/creo2urdf/include/creo2urdf/ElementTreeManager.h index 947427e..9507e60 100644 --- a/src/creo2urdf/include/creo2urdf/ElementTreeManager.h +++ b/src/creo2urdf/include/creo2urdf/ElementTreeManager.h @@ -64,7 +64,7 @@ class ElementTreeManager { * @param[out] joint_info_map A map containing joint information. * @return True if successful, false otherwise. */ - bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& joint_info_map, bool is_asm=false); + bool populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& joint_info_map); /** * @brief Gets the constraint type between two assembled parts. @@ -126,4 +126,8 @@ class ElementTreeManager { * @return A pair representing the limits (min, max). */ std::pair retrieveLimits(pfcFeature_ptr feat); + + // SEEMS to work but we need to investigate further, using this may allow to refactor the code deeply + + pfcTransform3D_ptr retrieveTransform(pfcFeature_ptr feat); }; diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index 2f2f4d6..f0de1fb 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/src/creo2urdf/include/creo2urdf/Utils.h @@ -258,7 +258,8 @@ struct JointInfo { struct LinkInfo { std::string name{""}; ///< Name of the link. pfcModel_ptr modelhdl{nullptr}; ///< Pointer to the Creo model associated with the link. - iDynTree::Transform root_H_link{iDynTree::Transform::Identity()}; ///< 3D Transform from the root to the link's reference frame. + iDynTree::Transform rootAsm_H_linkFrame{iDynTree::Transform::Identity()}; ///< 3D Transform from the root to the link's reference frame. + iDynTree::Transform csysAsm_H_linkFrame{iDynTree::Transform::Identity()}; ///< 3D Transform from the assembly to the link's reference frame. std::string link_frame_name{""}; ///< Name of the link frame. }; @@ -400,7 +401,7 @@ void printRotationMatrix(pfcMatrix3D_ptr m); void sanitizeSTL(std::string stl); /** - * @brief Retrieves the transformation from the root to a specified link frame in the context of a component path. + * @brief Retrieves the transformation from the owner assembly to a specified link frame in the context of a component path. * * @param comp_path component path that represents the assembly hierarchy. * @param modelhdl The part model in which the link frame is defined. @@ -412,7 +413,7 @@ void sanitizeSTL(std::string stl); * - The second element is an iDynTree::Transform representing the transformation from the root to the specified link frame. * If the operation fails, this transformation will be an identity transformation. */ -std::pair getTransformFromRootToChild(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array& scale); +std::pair getTransformFromOwnerToLinkFrame(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array& scale); /** * @brief Retrieves the transformation matrix representing the coordinate system of a specified link frame in the given part. @@ -438,7 +439,7 @@ std::pair getTransformFromPart(pfcModel_ptr modelhdl, * @param scale The scaling factor for the origin of the child frame * @return std::pair Pair containing a success/failure flag, and the axis direction */ -std::pair getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array& scale); +std::tuple getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array& scale); /** * @brief Extracts the folder path from a file path. diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index c2fddb9..6765f2b 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -17,7 +17,7 @@ #include -bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parent_H_csysItem) { +bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parentAsm_H_csysAsm) { for (int i = 0; i < asmListItems->getarraysize(); i++) { @@ -33,38 +33,13 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod if (!component_handle) { return false; } + + auto type = component_handle->GetType(); + ElementTreeManager element_tree_manager; //printToMessageWindow("Processing " + string(component_handle->GetFullName()) + " Owner " + string(model_owner->GetFullName())); - auto type = component_handle->GetType(); - if (type == pfcMDL_ASSEMBLY) { - auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE); - auto ok = element_tree_manager.populateJointInfoFromElementTree(asmItemAsFeat, joint_info_map); - if (!ok) { - // WE NEED TO FIX the failure, the paths are not cleared - //continue; - } - iDynTree::Transform parentAsmCsys_H_asmCsys = iDynTree::Transform::Identity(); - bool ret{ false }; - std::tie(ret, parentAsmCsys_H_asmCsys) = getTransformFromPart(component_handle, "ASM_CSYS", scale); - if(!ret) { - printToMessageWindow("Failed to get the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::WARN); - //return false; - } - else { - //printToMessageWindow("Got the transform from " + string(model_owner->GetFullName()) + " (the parent assembly) to " + string(component_handle->GetFullName()), c2uLogLevel::INFO); - //printToMessageWindow(parentAsmCsys_H_asmCsys.toString()); - } - ok = processAsmItems(sub_asm_component_list, component_handle, parentAsmCsys_H_asmCsys); - if (!ok) { - return false; - } - else { - continue; - } - } - xintsequence_ptr seq = xintsequence::create(); seq->append(asmItemAsFeat->GetId()); @@ -73,33 +48,50 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod pfcComponentPath_ptr comp_path = pfcCreateComponentPath(pfcAssembly::cast(model_owner), seq); + iDynTree::Transform csysAsm_H_linkFrame = iDynTree::Transform::Identity(); + iDynTree::Transform csysPart_H_link_frame = iDynTree::Transform::Identity(); + iDynTree::Transform parentAsm_H_linkFrame = iDynTree::Transform::Identity(); + + std::string link_frame_name{ "" }; auto link_name = string(component_handle->GetFullName()); + std::string urdf_link_name { "" }; + + if (type == pfcMDL_ASSEMBLY) { + link_frame_name = "ASM_CSYS"; + } + else { + link_frame_name = "CSYS"; + urdf_link_name = getRenameElementFromConfig(link_name); + for (const auto& lf : config["linkFrames"]) { + if (lf["linkName"].Scalar() != urdf_link_name) + { + continue; + } + link_frame_name = lf["frameName"].Scalar(); + } - iDynTree::Transform csysAsm_H_link = iDynTree::Transform::Identity(); - iDynTree::Transform csysPart_H_link = iDynTree::Transform::Identity(); - iDynTree::Transform parent_H_link = iDynTree::Transform::Identity(); + 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, csysAsm_H_linkFrame) = getTransformFromOwnerToLinkFrame(comp_path, component_handle, link_frame_name , scale); - std::string urdf_link_name = getRenameElementFromConfig(link_name); + parentAsm_H_linkFrame = parentAsm_H_csysAsm * csysAsm_H_linkFrame; - std::string link_frame_name = ""; + if (type == pfcMDL_ASSEMBLY) { + auto sub_asm_component_list = component_handle->ListItems(pfcModelItemType::pfcITEM_FEATURE); - for (const auto& lf : config["linkFrames"]) { - if (lf["linkName"].Scalar() != urdf_link_name) - { + bool ok = processAsmItems(sub_asm_component_list, component_handle, parentAsm_H_linkFrame); + if (!ok) { + return false; + } + else { continue; } - link_frame_name = lf["frameName"].Scalar(); } - 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, csysAsm_H_link) = getTransformFromRootToChild(comp_path, component_handle, link_frame_name, scale); - - parent_H_link = parent_H_csysItem * csysAsm_H_link; - - //printToMessageWindow(csysAsm_H_link.toString()); + //printToMessageWindow(csysAsm_H_linkFrame.toString()); if (!ret && warningsAreFatal) { @@ -108,14 +100,14 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod auto mass_prop = pfcSolid::cast(component_handle)->GetMassProperty(); - std::tie(ret, csysPart_H_link) = getTransformFromPart(component_handle, link_frame_name, scale); + std::tie(ret, csysPart_H_link_frame) = getTransformFromPart(component_handle, link_frame_name, scale); if (!ret && warningsAreFatal) { return false; } iDynTree::Link link; - link.setInertia(computeSpatialInertiafromCreo(mass_prop, csysPart_H_link, urdf_link_name)); + link.setInertia(computeSpatialInertiafromCreo(mass_prop, csysPart_H_link_frame, urdf_link_name)); if (!link.getInertia().isPhysicallyConsistent()) { @@ -125,7 +117,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod } } - LinkInfo l_info{ urdf_link_name, component_handle, parent_H_link, link_frame_name }; + LinkInfo l_info{ urdf_link_name, component_handle, parentAsm_H_linkFrame, csysAsm_H_linkFrame, link_frame_name }; link_info_map.insert(std::make_pair(link_name, l_info)); populateExportedFrameInfoMap(component_handle); @@ -141,6 +133,14 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod void Creo2Urdf::OnCommand() { + // Let's clear the map in case of multiple click + if (joint_info_map.size() > 0) { + joint_info_map.clear(); + link_info_map.clear(); + exported_frame_info_map.clear(); + assigned_inertias_map.clear(); + assigned_collision_geometry_map.clear(); + } m_session_ptr = pfcGetProESession(); if (!m_session_ptr) { printToMessageWindow("Failed to get the session", c2uLogLevel::WARN); @@ -200,15 +200,6 @@ void Creo2Urdf::OnCommand() { return; } - // Let's clear the map in case of multiple click - if (joint_info_map.size() > 0) { - joint_info_map.clear(); - link_info_map.clear(); - exported_frame_info_map.clear(); - assigned_inertias_map.clear(); - assigned_collision_geometry_map.clear(); - } - if (config["warningsAreFatal"].IsDefined()) { warningsAreFatal = config["warningsAreFatal"].as(); } @@ -260,22 +251,23 @@ void Creo2Urdf::OnCommand() { } - printToMessageWindow("Adding joint " + joint_name); - auto root_H_parent_link = link_info_map.at(parent_link_name).root_H_link; - auto root_H_child_link = link_info_map.at(child_link_name).root_H_link; + + auto asm_owner_H_parent_link = link_info_map.at(parent_link_name).rootAsm_H_linkFrame; + auto asm_owner_H_child_link = link_info_map.at(child_link_name).rootAsm_H_linkFrame; auto child_model = link_info_map.at(child_link_name).modelhdl; auto parent_model = link_info_map.at(parent_link_name).modelhdl; auto parent_link_frame = link_info_map.at(parent_link_name).link_frame_name; - //printToMessageWindow("Parent link H " + root_H_parent_link.toString()); - //printToMessageWindow("Child link H " + root_H_child_link.toString()); + //printToMessageWindow("Parent link H " + asm_owner_H_parent_link.toString()); + //printToMessageWindow("Child link H " + asm_owner_H_child_link.toString()); iDynTree::Transform parent_H_child = iDynTree::Transform::Identity(); - parent_H_child = root_H_parent_link.inverse() * root_H_child_link; + parent_H_child = asm_owner_H_parent_link.inverse() * asm_owner_H_child_link; if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) { iDynTree::Direction axis; - std::tie(ret, axis) = 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); if (!ret && warningsAreFatal) { @@ -288,6 +280,20 @@ void Creo2Urdf::OnCommand() { axis = axis.reverse(); } + auto urdf_child_link_name = getRenameElementFromConfig(child_link_name); + + + // FIXME + // It does something but the transform is somehow wrong, in case of proper definition of the csys oldChild_H_newChild is the identity + // Once we found the proper transform we should updatea also the spatial inertia + + parent_H_child = parent_H_child * oldChild_H_newChild; + 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(); + + 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); + std::shared_ptr joint_sh_ptr; if (joint_info.second.type == JointType::Revolute) { joint_sh_ptr = std::make_shared(); @@ -480,7 +486,7 @@ iDynTree::SpatialInertia Creo2Urdf::computeSpatialInertiafromCreo(pfcMassPropert iDynTree::Position com_child({ com->get(0) * scale[0] , com->get(1) * scale[1], com->get(2) * scale[2] }); - // Account for csysPart_H_link transformation + // Account for csysPart_H_link_frame transformation // See https://github.com/icub-tech-iit/ergocub-software/issues/224#issuecomment-1985692598 for full contents // The COM returned by Creo's GetGravityCenter seems to be expressed in the root frame, so we need diff --git a/src/creo2urdf/src/ElementTreeManager.cpp b/src/creo2urdf/src/ElementTreeManager.cpp index afe48f9..fa1d52f 100644 --- a/src/creo2urdf/src/ElementTreeManager.cpp +++ b/src/creo2urdf/src/ElementTreeManager.cpp @@ -24,16 +24,13 @@ ElementTreeManager::ElementTreeManager(pfcFeature_ptr feat, std::map& joint_info_map, bool is_asm) +bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, std::map& joint_info_map) { wfeat = wfcWFeature::cast(feat); try { tree = wfeat->GetElementTree(nullptr, wfcFEAT_EXTRACT_NO_OPTS); - std::string joint_name = std::to_string(feat->GetId()) + ".xml"; - //printToMessageWindow("Element tree extracted for feature " + std::to_string(feat->GetId()), c2uLogLevel::INFO); - tree->WriteElementTreeToFile(wfcELEMTREE_XML, joint_name.c_str()); } xcatchbegin xcatchcip(pfcXToolkitInvalidType) @@ -49,11 +46,9 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s printToMessageWindow("Could not retrieve solid references!", c2uLogLevel::WARN); return false; } - joint.child_link_name = getChildName(); joint.parent_link_name = getParentName(); std::string joint_name = joint.parent_link_name + "--" + joint.child_link_name; - printToMessageWindow("JOINT !! " + joint_name, c2uLogLevel::INFO); joint.type = proAsmCompSetType_to_JointType.at(static_cast(getConstraintType())); if (joint.type == JointType::Revolute) @@ -235,6 +230,33 @@ std::string ElementTreeManager::retrievePartName() xcatchend } + +pfcTransform3D_ptr ElementTreeManager::retrieveTransform(pfcFeature_ptr feat) { + pfcTransform3D_ptr parentCsys_H_childCsys = nullptr; + + wfcElemPathItems_ptr elemItems = wfcElemPathItems::create(); + wfcElemPathItem_ptr Item; + wfcElement_ptr element; + + Item = wfcElemPathItem::Create(wfcELEM_PATH_ITEM_TYPE_ID, wfcPRO_E_COMPONENT_INIT_POS); + elemItems->append(Item); + + wfcElementPath_ptr transform_path = wfcElementPath::Create(elemItems); + element = tree->GetElement(transform_path); + + auto value_ptr = element->GetValue(); + if(!value_ptr) + printToMessageWindow("wfcPRO_E_COMPONENT_INIT_POS value is null"); + else { + parentCsys_H_childCsys = value_ptr->GetTransformValue(); + // Because the transform is from child to parent, we need to invert it + parentCsys_H_childCsys->Invert(); + } + + return parentCsys_H_childCsys; +} + + std::pair ElementTreeManager::retrieveLimits(pfcFeature_ptr feat) { wfcElemPathItems_ptr elemItems = wfcElemPathItems::create(); diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index 05983f2..96316cc 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -101,7 +101,7 @@ void sanitizeSTL(std::string stl) output.close(); } -std::pair getTransformFromRootToChild(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array& scale) { +std::pair getTransformFromOwnerToLinkFrame(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array& scale) { iDynTree::Transform csysAsm_H_link = iDynTree::Transform::Identity(); @@ -172,20 +172,21 @@ std::pair getTransformFromPart(pfcModel_ptr modelhdl, return { false, H_child }; } -std::pair getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array& scale) { +std::tuple getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array& scale) { iDynTree::Direction axis_unit_vector; + 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 }; + return { false, axis_unit_vector, oldChild_H_newChild }; } if (axis_name.empty()) - return { false, axis_unit_vector }; + return { false, axis_unit_vector, oldChild_H_newChild }; pfcAxis* axis = nullptr; @@ -202,18 +203,34 @@ std::pair getAxisFromPart(pfcModel_ptr modelhdl, cons auto axis_line = pfcLineDescriptor::cast(axis_data); // cursed cast from hell + 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(); + 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 }); + + } + + // There are just two points in the array + auto unit = computeUnitVectorFromAxis(axis_line); axis_unit_vector.setVal(0, unit[0]); axis_unit_vector.setVal(1, unit[1]); axis_unit_vector.setVal(2, unit[2]); + // auto csysAsm_H_csysPart = fromCreo(comp_path->GetTransform(xtrue), scale); + + auto& csys_H_child = getTransformFromPart(modelhdl, link_frame_name, scale).second; 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 }; + return { true, axis_unit_vector, oldChild_H_newChild }; } std::string extractFolderPath(const std::string& filePath) { From 1620a5910429f219d2daa8ffd69f3e9029e744b8 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 13 Jun 2024 15:06:10 +0200 Subject: [PATCH 6/7] Cleanup tabs --- src/creo2urdf/src/Creo2Urdf.cpp | 4 ++-- src/creo2urdf/src/Sensorizer.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index 6765f2b..011aa01 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -60,7 +60,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod link_frame_name = "ASM_CSYS"; } else { - link_frame_name = "CSYS"; + link_frame_name = "CSYS"; urdf_link_name = getRenameElementFromConfig(link_name); for (const auto& lf : config["linkFrames"]) { if (lf["linkName"].Scalar() != urdf_link_name) @@ -74,7 +74,7 @@ bool Creo2Urdf::processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr mod printToMessageWindow(link_name + " misses the frame in the linkFrames section, CSYS will be used instead", c2uLogLevel::WARN); link_frame_name = "CSYS"; } - } + } std::tie(ret, csysAsm_H_linkFrame) = getTransformFromOwnerToLinkFrame(comp_path, component_handle, link_frame_name , scale); parentAsm_H_linkFrame = parentAsm_H_csysAsm * csysAsm_H_linkFrame; diff --git a/src/creo2urdf/src/Sensorizer.cpp b/src/creo2urdf/src/Sensorizer.cpp index 5c769f5..d599a23 100644 --- a/src/creo2urdf/src/Sensorizer.cpp +++ b/src/creo2urdf/src/Sensorizer.cpp @@ -269,10 +269,10 @@ void Sensorizer::assignTransformToSensors(const std::map Date: Thu, 13 Jun 2024 15:08:16 +0200 Subject: [PATCH 7/7] Remove misleading warning --- src/creo2urdf/include/creo2urdf/Utils.h | 2 +- src/creo2urdf/src/ElementTreeManager.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index f0de1fb..64157c6 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/src/creo2urdf/include/creo2urdf/Utils.h @@ -437,7 +437,7 @@ std::pair getTransformFromPart(pfcModel_ptr modelhdl, * @param axis_name The name of the desired axis of which to retrieve the direction * @param link_frame_name The name of the frame belonging to modelhdl * @param scale The scaling factor for the origin of the child frame - * @return std::pair Pair containing a success/failure flag, and the axis direction + * @return std::tuple> Tuple containing a success/failure flag, the axis direction, and the transform oldLink_H_newLink in order that the frame lies on the axis. */ std::tuple getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array& scale); diff --git a/src/creo2urdf/src/ElementTreeManager.cpp b/src/creo2urdf/src/ElementTreeManager.cpp index fa1d52f..d993640 100644 --- a/src/creo2urdf/src/ElementTreeManager.cpp +++ b/src/creo2urdf/src/ElementTreeManager.cpp @@ -43,7 +43,6 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s if (!retrieveSolidReferences()) { - printToMessageWindow("Could not retrieve solid references!", c2uLogLevel::WARN); return false; } joint.child_link_name = getChildName();