From 1650c8f31dcb094b276a15fd136c810405b39f98 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 13 Jun 2024 11:43:04 +0200 Subject: [PATCH] Commit to be squashed and fixed --- src/creo2urdf/include/creo2urdf/Utils.h | 2 +- src/creo2urdf/src/Creo2Urdf.cpp | 30 ++++++++++++------------- src/creo2urdf/src/Utils.cpp | 20 ++++++++--------- 3 files changed, 25 insertions(+), 27 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index a78b750..f0de1fb 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/src/creo2urdf/include/creo2urdf/Utils.h @@ -439,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::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 646c4f7..094a537 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -267,8 +267,11 @@ void Creo2Urdf::OnCommand() { if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) { iDynTree::Direction axis; - iDynTree::Position axis_offset; - std::tie(ret, axis, axis_offset) = 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); + + + parent_H_child = parent_H_child * oldChild_H_newChild; if (!ret && warningsAreFatal) { @@ -283,32 +286,29 @@ void Creo2Urdf::OnCommand() { auto urdf_child_link_name = getRenameElementFromConfig(child_link_name); + printToMessageWindow("joint " + urdf_child_link_name); + // THIS IS NOT HAVING EFFECT - /*auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry(); + 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(); - link_H_collision_solid_shape.setPosition(link_H_collision_solid_shape.getPosition() - axis_offset); - link_H_visual_solid_shape.setPosition(link_H_visual_solid_shape.getPosition() - axis_offset); + printToMessageWindow("BEFORE link_H_collision_solid_shape " + link_H_visual_solid_shape.toString()); - idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(link_H_collision_solid_shape); - idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(link_H_visual_solid_shape); + 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); + link_H_visual_solid_shape = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry(); + printToMessageWindow("AFTER link_H_collision_solid_shape " + link_H_visual_solid_shape.toString()); - parent_H_child.setPosition(parent_H_child.getPosition() + axis_offset); - */ - - - // I need to rotate the axis offset by the parent_H_child rotation ???? - //axis_offset = parent_H_child.getRotation() * axis_offset; 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() + axis_offset)); + dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.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() + axis_offset)); + dynamic_cast(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition())); } joint_sh_ptr->setRestTransform(parent_H_child); diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index e08ebbe..96316cc 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -172,22 +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::Position axis_offset; - axis_offset.zero(); + 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, axis_offset }; + return { false, axis_unit_vector, oldChild_H_newChild }; } if (axis_name.empty()) - return { false, axis_unit_vector, axis_offset }; + return { false, axis_unit_vector, oldChild_H_newChild }; pfcAxis* axis = nullptr; @@ -208,11 +207,11 @@ std::tuple getAxisFromPart(pfcMod // We use the medium point of the axis as offset pfcPoint3D_ptr pstart = axis_line->GetEnd1(); pfcPoint3D_ptr pend = axis_line->GetEnd2(); - axis_offset[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0]; - axis_offset[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1]; - axis_offset[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2]; + 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 }); - printToMessageWindow("Axis offset: " + axis_offset.toString()); } // There are just two points in the array @@ -227,12 +226,11 @@ std::tuple getAxisFromPart(pfcMod auto& csys_H_child = getTransformFromPart(modelhdl, link_frame_name, scale).second; - printToMessageWindow("csys_H_child: " + csys_H_child.toString()); 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, axis_offset }; + return { true, axis_unit_vector, oldChild_H_newChild }; } std::string extractFolderPath(const std::string& filePath) {