From 363e01b8b48e69cb2f5bd56617bd48705aa33327 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 12 Jul 2024 16:56:33 +0200 Subject: [PATCH 1/5] Something is not working --- src/creo2urdf/src/Creo2Urdf.cpp | 37 ++++++++++++----------- src/creo2urdf/src/Utils.cpp | 53 ++++++++++++++++++++------------- 2 files changed, 51 insertions(+), 39 deletions(-) diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index 011aa01..d8354a4 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -250,24 +250,21 @@ void Creo2Urdf::OnCommand() { continue; } - - 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 " + 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 = asm_owner_H_parent_link.inverse() * asm_owner_H_child_link; + iDynTree::Transform parentLink_H_childLink = iDynTree::Transform::Identity(); + parentLink_H_childLink = 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; - iDynTree::Transform oldChild_H_newChild; - std::tie(ret, axis, oldChild_H_newChild) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale); + iDynTree::Transform parentLink_H_newParentLink; + std::tie(ret, axis, parentLink_H_newParentLink) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale); if (!ret && warningsAreFatal) { @@ -280,31 +277,35 @@ void Creo2Urdf::OnCommand() { axis = axis.reverse(); } - auto urdf_child_link_name = getRenameElementFromConfig(child_link_name); + auto urdf_parent_link_name = getRenameElementFromConfig(parent_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 + // It does something but the transform is somehow wrong, in case of proper definition of the csys parentLink_H_newParentLink 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(); + parentLink_H_childLink = parentLink_H_newParentLink.inverse() * parentLink_H_childLink;// newParentLink_H_childLink + auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->getLink_H_geometry(); + auto link_H_visual_solid_shape = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->getLink_H_geometry(); + + + //printToMessageWindow("link_H_collision_solid_shape " + link_H_collision_solid_shape.toString()); + //printToMessageWindow("parentLink_H_newParentLink " + parentLink_H_newParentLink.toString()); - 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); + idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->setLink_H_geometry(parentLink_H_newParentLink.inverse() * link_H_collision_solid_shape); + idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->setLink_H_geometry(parentLink_H_newParentLink.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(); - dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition())); + dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parentLink_H_childLink.getPosition())); } else if (joint_info.second.type == JointType::Linear) { joint_sh_ptr = std::make_shared(); - dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition())); + dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parentLink_H_childLink.getPosition())); } - joint_sh_ptr->setRestTransform(parent_H_child); + joint_sh_ptr->setRestTransform(parentLink_H_childLink); double conversion_factor = 1.0; if (joint_info.second.type == JointType::Revolute) { conversion_factor = deg2rad; @@ -336,7 +337,7 @@ void Creo2Urdf::OnCommand() { } } else if (joint_info.second.type == JointType::Fixed) { - iDynTree::FixedJoint joint(parent_H_child); + iDynTree::FixedJoint joint(parentLink_H_childLink); if (idyn_model.addJoint(getRenameElementFromConfig(parent_link_name), getRenameElementFromConfig(child_link_name), joint_name, &joint) == iDynTree::JOINT_INVALID_INDEX) { printToMessageWindow("FAILED TO ADD JOINT " + joint_name, c2uLogLevel::WARN); diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index 96316cc..02b3bf9 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -175,18 +175,18 @@ std::pair getTransformFromPart(pfcModel_ptr modelhdl, 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(); + iDynTree::Transform csys_H_newLinkFrame = 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, oldChild_H_newChild }; + return { false, axis_unit_vector, csys_H_newLinkFrame }; } if (axis_name.empty()) - return { false, axis_unit_vector, oldChild_H_newChild }; + return { false, axis_unit_vector, csys_H_newLinkFrame }; pfcAxis* axis = nullptr; @@ -199,22 +199,17 @@ std::tuple getAxisFromPart(pfcMo } } + if (!axis) { + printToMessageWindow("Unable to find the axis " + axis_name + " in " + string(modelhdl->GetFullName()), c2uLogLevel::WARN); + return { false, axis_unit_vector, csys_H_newLinkFrame }; + } + auto axis_data = wfcWAxis::cast(axis)->GetAxisData(); 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 }); - - } + auto& csys_H_linkFrame = getTransformFromPart(modelhdl, link_frame_name, scale).second; - // There are just two points in the array auto unit = computeUnitVectorFromAxis(axis_line); @@ -222,15 +217,31 @@ std::tuple getAxisFromPart(pfcMo 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; + // There are just two points in the array - 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, oldChild_H_newChild }; + 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]; + csys_H_newLinkFrame.setPosition({ x, y, z }); + + auto linkFrame_H_newLinkFrame = csys_H_linkFrame.inverse() * csys_H_newLinkFrame; + + axis_unit_vector = csys_H_newLinkFrame.inverse() * axis_unit_vector; // We might benefit from performing this operation directly in Creo + axis_unit_vector.Normalize(); + return { true, axis_unit_vector, linkFrame_H_newLinkFrame }; + + } + else { + axis_unit_vector = csys_H_linkFrame.inverse() * axis_unit_vector; // We might benefit from performing this operation directly in Creo + axis_unit_vector.Normalize(); + return { true, axis_unit_vector, iDynTree::Transform::Identity() }; + } + return { false, axis_unit_vector, iDynTree::Transform::Identity() }; } std::string extractFolderPath(const std::string& filePath) { From a4b61ed6dec6eeafd3d4016d2f71678246e332ee Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 17 Jul 2024 14:54:51 +0200 Subject: [PATCH 2/5] let's try to move the axis --- src/creo2urdf/include/creo2urdf/Utils.h | 4 +-- src/creo2urdf/src/Creo2Urdf.cpp | 32 ++++++++++------------- src/creo2urdf/src/Utils.cpp | 34 +++++++++---------------- 3 files changed, 27 insertions(+), 43 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index 64157c6..f4c1c62 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/src/creo2urdf/include/creo2urdf/Utils.h @@ -437,9 +437,9 @@ 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::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. + * @return std::tuple> Tuple containing a success/failure flag, the axis direction, and the position of the middle point of the axis in the link csys 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); +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 d8354a4..8ea59e8 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -263,8 +263,8 @@ void Creo2Urdf::OnCommand() { if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) { iDynTree::Direction axis; - iDynTree::Transform parentLink_H_newParentLink; - std::tie(ret, axis, parentLink_H_newParentLink) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale); + iDynTree::Position axis_mid_point_pos_in_parent; + std::tie(ret, axis, axis_mid_point_pos_in_parent) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale); if (!ret && warningsAreFatal) { @@ -278,31 +278,25 @@ void Creo2Urdf::OnCommand() { } auto urdf_parent_link_name = getRenameElementFromConfig(parent_link_name); + + auto axis_origin = iDynTree::Position::Zero(); - - // FIXME - // It does something but the transform is somehow wrong, in case of proper definition of the csys parentLink_H_newParentLink is the identity - // Once we found the proper transform we should updatea also the spatial inertia - - parentLink_H_childLink = parentLink_H_newParentLink.inverse() * parentLink_H_childLink;// newParentLink_H_childLink - auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->getLink_H_geometry(); - auto link_H_visual_solid_shape = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->getLink_H_geometry(); - - - //printToMessageWindow("link_H_collision_solid_shape " + link_H_collision_solid_shape.toString()); - //printToMessageWindow("parentLink_H_newParentLink " + parentLink_H_newParentLink.toString()); - - idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->setLink_H_geometry(parentLink_H_newParentLink.inverse() * link_H_collision_solid_shape); - idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_parent_link_name)][0]->setLink_H_geometry(parentLink_H_newParentLink.inverse() * link_H_visual_solid_shape); + if (parent_link_frame == "CSYS") { + axis_origin = axis_mid_point_pos_in_parent; + } + else { + axis_origin = parentLink_H_childLink.getPosition(); + } + std::shared_ptr joint_sh_ptr; if (joint_info.second.type == JointType::Revolute) { joint_sh_ptr = std::make_shared(); - dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parentLink_H_childLink.getPosition())); + dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, axis_origin)); } else if (joint_info.second.type == JointType::Linear) { joint_sh_ptr = std::make_shared(); - dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parentLink_H_childLink.getPosition())); + dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, axis_origin)); } joint_sh_ptr->setRestTransform(parentLink_H_childLink); diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index 02b3bf9..84805c1 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -172,21 +172,21 @@ std::pair getTransformFromPart(pfcModel_ptr modelhdl, return { false, H_child }; } -std::tuple 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 csys_H_newLinkFrame = iDynTree::Transform::Identity(); + iDynTree::Position axis_mid_point_pos = iDynTree::Position::Zero(); 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, csys_H_newLinkFrame }; + return { false, axis_unit_vector, axis_mid_point_pos }; } if (axis_name.empty()) - return { false, axis_unit_vector, csys_H_newLinkFrame }; + return { false, axis_unit_vector, axis_mid_point_pos }; pfcAxis* axis = nullptr; @@ -201,7 +201,7 @@ std::tuple getAxisFromPart(pfcMo if (!axis) { printToMessageWindow("Unable to find the axis " + axis_name + " in " + string(modelhdl->GetFullName()), c2uLogLevel::WARN); - return { false, axis_unit_vector, csys_H_newLinkFrame }; + return { false, axis_unit_vector, axis_mid_point_pos }; } auto axis_data = wfcWAxis::cast(axis)->GetAxisData(); @@ -224,24 +224,14 @@ std::tuple getAxisFromPart(pfcMo // 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]; - csys_H_newLinkFrame.setPosition({ x, y, z }); - - auto linkFrame_H_newLinkFrame = csys_H_linkFrame.inverse() * csys_H_newLinkFrame; - - axis_unit_vector = csys_H_newLinkFrame.inverse() * axis_unit_vector; // We might benefit from performing this operation directly in Creo - axis_unit_vector.Normalize(); - return { true, axis_unit_vector, linkFrame_H_newLinkFrame }; - + axis_mid_point_pos[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0]; + axis_mid_point_pos[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1]; + axis_mid_point_pos[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2]; } - else { - axis_unit_vector = csys_H_linkFrame.inverse() * axis_unit_vector; // We might benefit from performing this operation directly in Creo - axis_unit_vector.Normalize(); - return { true, axis_unit_vector, iDynTree::Transform::Identity() }; - } - return { false, axis_unit_vector, iDynTree::Transform::Identity() }; + + axis_unit_vector = csys_H_linkFrame.inverse() * axis_unit_vector; // We might benefit from performing this operation directly in Creo + axis_unit_vector.Normalize(); + return { true, axis_unit_vector, axis_mid_point_pos }; } std::string extractFolderPath(const std::string& filePath) { From 44685fdc4f364eec8066a2d29c7f0ad9a20bca15 Mon Sep 17 00:00:00 2001 From: Nicolo Genesio Date: Mon, 22 Jul 2024 15:48:34 +0200 Subject: [PATCH 3/5] Move the frames in order that the axis pass through their origin --- CMakeLists.txt | 2 +- src/creo2urdf/CMakeLists.txt | 1 + src/creo2urdf/src/Creo2Urdf.cpp | 8 +++++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 003a534..40fbb04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,7 @@ include(GNUInstallDirs) include(FeatureSummary) find_package(Eigen3 REQUIRED) -find_package(iDynTree 12.3.1 REQUIRED) +find_package(iDynTree 12.4.0 REQUIRED) find_package(yaml-cpp REQUIRED) find_package(LibXml2 REQUIRED) diff --git a/src/creo2urdf/CMakeLists.txt b/src/creo2urdf/CMakeLists.txt index b5e91c1..b525a6f 100644 --- a/src/creo2urdf/CMakeLists.txt +++ b/src/creo2urdf/CMakeLists.txt @@ -69,6 +69,7 @@ add_compile_definitions(creo2urdf PUBLIC PRO_MACHINE=36 # Link dependencies target_link_libraries(creo2urdf PRIVATE iDynTree::idyntree-modelio iDynTree::idyntree-high-level + iDynTree::idyntree-model yaml-cpp::yaml-cpp LibXml2::LibXml2 Eigen3::Eigen diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index 8ea59e8..692fef3 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -13,6 +13,7 @@ #include #include +#include #include @@ -440,7 +441,12 @@ void Creo2Urdf::OnCommand() { bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOptions options) { iDynTree::ModelExporter mdl_exporter; - mdl_exporter.init(mdl); + // Convert modelToExport in a URDF-compatible model (using the default base link) + iDynTree::Model modelToExportURDFCompatible; + + bool ok = iDynTree::moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(mdl, modelToExportURDFCompatible); + + mdl_exporter.init(modelToExportURDFCompatible); mdl_exporter.setExportingOptions(options); if (!mdl_exporter.isValid()) From eb10745ec4faca003725bd3996c62558a2cc4322 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Tue, 23 Jul 2024 16:01:22 +0200 Subject: [PATCH 4/5] Better handle the moveLinkFramesToBeCompatibleWithURDF --- src/creo2urdf/include/creo2urdf/Creo2Urdf.h | 1 + src/creo2urdf/src/Creo2Urdf.cpp | 35 ++++++++++++--------- src/creo2urdf/src/Utils.cpp | 20 ++++++------ 3 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h index cd5c460..18b5e85 100644 --- a/src/creo2urdf/include/creo2urdf/Creo2Urdf.h +++ b/src/creo2urdf/include/creo2urdf/Creo2Urdf.h @@ -151,6 +151,7 @@ class Creo2Urdf : public pfcUICommandActionListener { 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. */ pfcSession_ptr m_session_ptr{ nullptr }; /**< Handle to the Creo session. */ + bool m_need_to_move_link_frames_to_be_compatible_with_URDF{ false }; /**< Flag indicating whether to move link frames to be compatible with URDF. */ }; class Creo2UrdfAccess : public pfcUICommandAccessListener { diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index 692fef3..15640b6 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -263,9 +263,9 @@ void Creo2Urdf::OnCommand() { if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) { - iDynTree::Direction axis; + iDynTree::Direction direction; iDynTree::Position axis_mid_point_pos_in_parent; - std::tie(ret, axis, axis_mid_point_pos_in_parent) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale); + std::tie(ret, direction, axis_mid_point_pos_in_parent) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale); if (!ret && warningsAreFatal) { @@ -275,29 +275,27 @@ void Creo2Urdf::OnCommand() { if (config["reverseRotationAxis"].IsDefined() && config["reverseRotationAxis"].Scalar().find(joint_name) != std::string::npos) { - axis = axis.reverse(); + direction = direction.reverse(); } auto urdf_parent_link_name = getRenameElementFromConfig(parent_link_name); - - auto axis_origin = iDynTree::Position::Zero(); - if (parent_link_frame == "CSYS") { - axis_origin = axis_mid_point_pos_in_parent; - } - else { - axis_origin = parentLink_H_childLink.getPosition(); + iDynTree::Axis idyn_axis{ direction, parentLink_H_childLink.getPosition() }; + + // Check if the axis is aligned with the link frame + if (parent_link_frame == "CSYS" && idyn_axis.getDistanceBetweenAxisAndPoint(axis_mid_point_pos_in_parent) > 1e-7 ) { + idyn_axis.setOrigin(axis_mid_point_pos_in_parent); + m_need_to_move_link_frames_to_be_compatible_with_URDF = true; } - std::shared_ptr joint_sh_ptr; if (joint_info.second.type == JointType::Revolute) { joint_sh_ptr = std::make_shared(); - dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, axis_origin)); + dynamic_cast(joint_sh_ptr.get())->setAxis(idyn_axis); } else if (joint_info.second.type == JointType::Linear) { joint_sh_ptr = std::make_shared(); - dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, axis_origin)); + dynamic_cast(joint_sh_ptr.get())->setAxis(idyn_axis); } joint_sh_ptr->setRestTransform(parentLink_H_childLink); @@ -444,7 +442,16 @@ bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOp // Convert modelToExport in a URDF-compatible model (using the default base link) iDynTree::Model modelToExportURDFCompatible; - bool ok = iDynTree::moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(mdl, modelToExportURDFCompatible); + if (m_need_to_move_link_frames_to_be_compatible_with_URDF) { + bool ok = iDynTree::moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(mdl, modelToExportURDFCompatible); + if (!ok) { + printToMessageWindow("Failed to move link frames to be URDF compatible", c2uLogLevel::WARN); + return false; + } + } + else { + modelToExportURDFCompatible = mdl; + } mdl_exporter.init(modelToExportURDFCompatible); mdl_exporter.setExportingOptions(options); diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index 84805c1..9055b8c 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -200,9 +200,9 @@ std::tuple getAxisFromPart(pfcMod } if (!axis) { - printToMessageWindow("Unable to find the axis " + axis_name + " in " + string(modelhdl->GetFullName()), c2uLogLevel::WARN); - return { false, axis_unit_vector, axis_mid_point_pos }; - } + printToMessageWindow("Unable to find the axis " + axis_name + " in " + string(modelhdl->GetFullName()), c2uLogLevel::WARN); + return { false, axis_unit_vector, axis_mid_point_pos }; + } auto axis_data = wfcWAxis::cast(axis)->GetAxisData(); @@ -220,14 +220,12 @@ std::tuple getAxisFromPart(pfcMod // There are just two points in the array - 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(); - axis_mid_point_pos[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0]; - axis_mid_point_pos[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1]; - axis_mid_point_pos[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2]; - } + // We use the medium point of the axis as offset + pfcPoint3D_ptr pstart = axis_line->GetEnd1(); + pfcPoint3D_ptr pend = axis_line->GetEnd2(); + axis_mid_point_pos[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0]; + axis_mid_point_pos[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1]; + axis_mid_point_pos[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2]; axis_unit_vector = csys_H_linkFrame.inverse() * axis_unit_vector; // We might benefit from performing this operation directly in Creo axis_unit_vector.Normalize(); From d9adbdf4e5caaad5c2cb657801415a467b8fde91 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 2 Aug 2024 11:59:34 +0200 Subject: [PATCH 5/5] Update readme --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 2049882..449d533 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,6 @@ Generate URDF model from CREO Parametric mechanisms. - Subassemblies are **NOT** yet supported. - The joints are in position 0. - Right now it can handle only **REVOLUTE**, **PRISMATIC** and **FIXED** joints. -- In each joint, the frame of the child link must lie on the joint's axis (https://github.com/icub-tech-iit/creo2urdf/issues/99). ## Installation from binaries