Add the support for prismatic joints

It fixes #67
icub-tech-iit · Jan 11, 2024 · 3383f17 · 3383f17
1 parent f7e54e6
commit 3383f17
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 11 deletions.
diff --git a/src/creo2urdf/include/creo2urdf/Utils.h b/src/creo2urdf/include/creo2urdf/Utils.h
@@ -246,6 +246,6 @@ std::pair<bool, iDynTree::Transform> getTransformFromRootToChild(pfcComponentPat
 
 std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale);
 
-std::pair<bool, iDynTree::Direction> getRotationAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);
+std::pair<bool, iDynTree::Direction> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);
 
 #endif // !UTILS_H
diff --git a/src/creo2urdf/src/Creo2Urdf.cpp b/src/creo2urdf/src/Creo2Urdf.cpp
@@ -9,6 +9,7 @@
 #include <creo2urdf/Creo2Urdf.h>
 #include <creo2urdf/Utils.h>
 #include <pfcExceptions.h>
+#include <iDynTree/PrismaticJoint.h>
 
 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<iDynTree::IJoint> joint_sh_ptr;
+            if (joint_info.second.type == JointType::Revolute) {
+                joint_sh_ptr = std::make_shared<iDynTree::RevoluteJoint>();
+                dynamic_cast<iDynTree::RevoluteJoint*>(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<iDynTree::PrismaticJoint>();
+                dynamic_cast<iDynTree::PrismaticJoint*>(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<double>("lower_limit", joint_name) * deg2rad;
                 double max = joints_csv_table.GetCell<double>("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<double>("damping", joint_name);
                 max = joints_csv_table.GetCell<double>("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
@@ -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
@@ -171,7 +171,7 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
     return { false, H_child };
 }
 
-std::pair<bool, iDynTree::Direction> getRotationAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array<double, 3>& scale) {
+std::pair<bool, iDynTree::Direction> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array<double, 3>& scale) {
 
     iDynTree::Direction axis_unit_vector;
     axis_unit_vector.zero();