Skip to content

Commit

Permalink
Add recursion in the assembly navigation
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nicogene
Nicogene committed May 23, 2024
1 parent f8520f5 commit 4433f7e
Show file tree
Hide file tree
Showing 3 changed files with 116 additions and 98 deletions.
8 changes: 6 additions & 2 deletions src/creo2urdf/include/creo2urdf/Creo2Urdf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
/**
Expand Down Expand Up @@ -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.
Expand All @@ -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 {
Expand Down
12 changes: 6 additions & 6 deletions src/creo2urdf/include/creo2urdf/Sensorizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> 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<std::string> buildSensorsXMLBlobs();
Expand Down
194 changes: 104 additions & 90 deletions src/creo2urdf/src/Creo2Urdf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,24 +16,116 @@

#include <Eigen/Core>


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;
}
}


// 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

Expand All @@ -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))
{
Expand All @@ -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);

Expand All @@ -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;
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit 4433f7e

Please sign in to comment.