Add recursion in the assembly navigation

CMakeLists.txt

@@ -12,7 +12,7 @@ include(GNUInstallDirs)
 include(FeatureSummary)
 
 find_package(Eigen3 REQUIRED)
-find_package(iDynTree REQUIRED)
+find_package(iDynTree 12.3.1 REQUIRED)
 find_package(yaml-cpp REQUIRED)
 find_package(LibXml2 REQUIRED)
 

src/creo2urdf/include/creo2urdf/Creo2Urdf.h

@@ -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:
     /**
@@ -124,6 +124,8 @@ class Creo2Urdf : public pfcUICommandActionListener {
      */
     bool loadYamlConfig(const std::string& filename);
 
+    bool processAsmItems(pfcModelItems_ptr asmListItems, pfcModel_ptr model_owner, iDynTree::Transform parentAsm_H_csysAsm = iDynTree::Transform::Identity());
+
     /**
      * @brief Get the renamed element from the configuration.
      * @param elem_name The original element name.
@@ -147,7 +149,8 @@ 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. */
+    pfcSession_ptr m_session_ptr{ nullptr }; /**< Handle to the Creo session. */
 };
 
 class Creo2UrdfAccess : public pfcUICommandAccessListener {

src/creo2urdf/include/creo2urdf/ElementTreeManager.h

@@ -85,10 +85,10 @@ class ElementTreeManager {
     std::string getChildName();
 
 private:
-    wfcElementTree_ptr tree = nullptr; ///< Pointer to the ElementTree of the part as feature.
-    wfcWFeature_ptr wfeat = nullptr;   ///< Pointer to the part as feature.
-    pfcSolid_ptr parent_solid;         ///< Pointer to the parent solid.
-    pfcSolid_ptr child_solid;          ///< Pointer to the child solid.
+    wfcElementTree_ptr tree{ nullptr }; ///< Pointer to the ElementTree of the part as feature.
+    wfcWFeature_ptr wfeat{ nullptr };   ///< Pointer to the part as feature.
+    pfcSolid_ptr parent_solid{ nullptr };         ///< Pointer to the parent solid.
+    pfcSolid_ptr child_solid{ nullptr };          ///< Pointer to the child solid.
 
     /*
      * @brief Retrieves the name of a common datum for the given model item type.
@@ -126,4 +126,8 @@ class ElementTreeManager {
      * @return A pair representing the limits (min, max).
      */
     std::pair<double, double> retrieveLimits(pfcFeature_ptr feat);
+
+    // SEEMS to work but we need to investigate further, using this may allow to refactor the code deeply
+
+    pfcTransform3D_ptr retrieveTransform(pfcFeature_ptr feat);
 };

src/creo2urdf/include/creo2urdf/Sensorizer.h

@@ -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();

src/creo2urdf/include/creo2urdf/Utils.h

@@ -186,7 +186,7 @@ struct FTSensorInfo {
     std::string frame{"sensor"}; ///< Frame associated with the FT sensor.
     std::string sensorName{""}; ///< Name of the FT sensor.
     std::string frameName{""}; ///< Name of the frame.
-    std::string linkName{" "}; ///< Name of the associated link.
+    std::string linkName{""}; ///< Name of the associated link.
     std::string exportedFrameName{""}; ///< Name of the exported frame.
     iDynTree::Transform parent_link_H_sensor{iDynTree::Transform::Identity()}; ///< 3D transform from parent link to sensor.
     iDynTree::Transform child_link_H_sensor{iDynTree::Transform::Identity()}; ///< 3D transform from child link to sensor.
@@ -258,7 +258,8 @@ struct JointInfo {
 struct LinkInfo {
     std::string name{""}; ///< Name of the link.
     pfcModel_ptr modelhdl{nullptr}; ///< Pointer to the Creo model associated with the link.
-    iDynTree::Transform root_H_link{iDynTree::Transform::Identity()}; ///< 3D Transform from the root to the link's reference frame.
+    iDynTree::Transform rootAsm_H_linkFrame{iDynTree::Transform::Identity()}; ///< 3D Transform from the root to the link's reference frame.
+    iDynTree::Transform csysAsm_H_linkFrame{iDynTree::Transform::Identity()}; ///< 3D Transform from the assembly to the link's reference frame.
     std::string link_frame_name{""}; ///< Name of the link frame.
 };
 
@@ -400,7 +401,7 @@ void printRotationMatrix(pfcMatrix3D_ptr m);
 void sanitizeSTL(std::string stl);
 
 /**
- * @brief Retrieves the transformation from the root to a specified link frame in the context of a component path.
+ * @brief Retrieves the transformation from the owner assembly to a specified link frame in the context of a component path.
  *
  * @param comp_path component path that represents the assembly hierarchy.
  * @param modelhdl The part model in which the link frame is defined.
@@ -412,7 +413,7 @@ void sanitizeSTL(std::string stl);
  *         - The second element is an iDynTree::Transform representing the transformation from the root to the specified link frame.
  *           If the operation fails, this transformation will be an identity transformation.
  */
-std::pair<bool, iDynTree::Transform> getTransformFromRootToChild(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale);
+std::pair<bool, iDynTree::Transform> getTransformFromOwnerToLinkFrame(pfcComponentPath_ptr comp_path, pfcModel_ptr modelhdl, const std::string& link_frame_name, const array<double, 3>& scale);
 
 /**
  * @brief Retrieves the transformation matrix representing the coordinate system of a specified link frame in the given part.
@@ -436,9 +437,9 @@ std::pair<bool, iDynTree::Transform> 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::pair<bool, iDynTree::Direction>  Pair containing a success/failure flag, and the axis direction
+ * @return std::tuple<bool, iDynTree::Direction, iDynTree::Transform>>  Tuple containing a success/failure flag, the axis direction, and the transform oldLink_H_newLink in order that the frame lies on the axis.
  */
-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);
+std::tuple<bool, iDynTree::Direction, iDynTree::Transform> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);
 
 /**
  * @brief Extracts the folder path from a file path.