From 3383f177fb063faf96044ea43381e9495dc05546 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 11 Jan 2024 11:55:11 +0100 Subject: [PATCH] Add the support for prismatic joints It fixes #67 --- src/creo2urdf/include/creo2urdf/Utils.h | 2 +- src/creo2urdf/src/Creo2Urdf.cpp | 33 +++++++++++++++++------- src/creo2urdf/src/ElementTreeManager.cpp | 10 +++++++ src/creo2urdf/src/Utils.cpp | 2 +- 4 files changed, 36 insertions(+), 11 deletions(-) diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h index cd39c6c..2cab67c 100644 --- a/src/creo2urdf/include/creo2urdf/Utils.h +++ b/src/creo2urdf/include/creo2urdf/Utils.h @@ -246,6 +246,6 @@ std::pair getTransformFromRootToChild(pfcComponentPat std::pair getTransformFromPart(pfcModel_ptr modelhdl, const std::string& link_frame_name, const array& scale); -std::pair getRotationAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array& scale); +std::pair getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array& scale); #endif // !UTILS_H diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp index f1097a3..129061a 100644 --- a/src/creo2urdf/src/Creo2Urdf.cpp +++ b/src/creo2urdf/src/Creo2Urdf.cpp @@ -9,6 +9,7 @@ #include #include #include +#include void Creo2Urdf::OnCommand() { @@ -192,9 +193,9 @@ void Creo2Urdf::OnCommand() { iDynTree::Transform parent_H_child = iDynTree::Transform::Identity(); parent_H_child = root_H_parent_link.inverse() * root_H_child_link; - if (joint_info.second.type == JointType::Revolute) { + if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) { iDynTree::Direction axis; - std::tie(ret, axis) = getRotationAxisFromPart(parent_model, axis_name, parent_link_frame, scale); + std::tie(ret, axis) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale); if (!ret && warningsAreFatal) { @@ -206,27 +207,41 @@ void Creo2Urdf::OnCommand() { axis = axis.reverse(); } - iDynTree::RevoluteJoint joint(parent_H_child, { axis, parent_H_child.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, 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())); + } + + joint_sh_ptr->setRestTransform(parent_H_child); + + //iDynTree::RevoluteJoint revolute_joint(parent_H_child, { axis, parent_H_child.getPosition() }); + //iDynTree::PrismaticJoint prismatic_joint; (parent_H_child, { axis, parent_H_child.getPosition() }); + if (joint_info.second.type == JointType::Revolute) // Read limits from CSV data, until it is possible to do so from Creo directly if (joints_csv_table.GetRowIdx(joint_name) >= 0) { double min = joints_csv_table.GetCell("lower_limit", joint_name) * deg2rad; double max = joints_csv_table.GetCell("upper_limit", joint_name) * deg2rad; - joint.enablePosLimits(true); - joint.setPosLimits(0, min, max); + joint_sh_ptr->enablePosLimits(true); + joint_sh_ptr->setPosLimits(0, min, max); // TODO we have to retrieve the rest transform from creo //joint.setRestTransform(); min = joints_csv_table.GetCell("damping", joint_name); max = joints_csv_table.GetCell("friction", joint_name); - joint.setJointDynamicsType(iDynTree::URDFJointDynamics); - joint.setDamping(0, min); - joint.setStaticFriction(0, max); + joint_sh_ptr->setJointDynamicsType(iDynTree::URDFJointDynamics); + joint_sh_ptr->setDamping(0, min); + joint_sh_ptr->setStaticFriction(0, max); } if (idyn_model.addJoint(getRenameElementFromConfig(parent_link_name), - getRenameElementFromConfig(child_link_name), joint_name, &joint) == iDynTree::JOINT_INVALID_INDEX) { + getRenameElementFromConfig(child_link_name), joint_name, joint_sh_ptr.get()) == iDynTree::JOINT_INVALID_INDEX) { printToMessageWindow("FAILED TO ADD JOINT " + joint_name, c2uLogLevel::WARN); if (warningsAreFatal) { return; diff --git a/src/creo2urdf/src/ElementTreeManager.cpp b/src/creo2urdf/src/ElementTreeManager.cpp index b94382d..a8bc8d8 100644 --- a/src/creo2urdf/src/ElementTreeManager.cpp +++ b/src/creo2urdf/src/ElementTreeManager.cpp @@ -58,6 +58,16 @@ bool ElementTreeManager::populateJointInfoFromElementTree(pfcFeature_ptr feat, s pfcComponentConstraintType::pfcASM_CONSTRAINT_CSYS, pfcModelItemType::pfcITEM_COORD_SYS); } + else if (joint.type == JointType::Linear) { + // We assume that one axis is used to defined the linear joint + joint.datum_name = getConstraintDatum(feat, + pfcComponentConstraintType::pfcASM_CONSTRAINT_ALIGN, + pfcModelItemType::pfcITEM_AXIS); + } + else { + printToMessageWindow("Joint type not supported!", c2uLogLevel::WARN); + return false; + } joint_info_map.insert({ joint.datum_name, joint }); diff --git a/src/creo2urdf/src/Utils.cpp b/src/creo2urdf/src/Utils.cpp index ab95f1d..b92d15f 100644 --- a/src/creo2urdf/src/Utils.cpp +++ b/src/creo2urdf/src/Utils.cpp @@ -171,7 +171,7 @@ std::pair getTransformFromPart(pfcModel_ptr modelhdl, return { false, H_child }; } -std::pair getRotationAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array& scale) { +std::pair getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array& scale) { iDynTree::Direction axis_unit_vector; axis_unit_vector.zero();