Merge pull request #60 from robotology/fixSensorExportedFrames

Include the additionalTransformation when the flag exportFrameInURDF is set to true for the sensors

CHANGELOG.md

@@ -6,9 +6,14 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+## [0.4.2] - 2024-01-25
+
 ### Add
 - Add the possibility to export the frame coincident with the ft sensor frame (https://github.com/robotology/simmechanics-to-urdf/pull/53).
 
+### Fixed 
+- Include the additionalTransformation when the flag exportFrameInURDF is set to true for the sensors (https://github.com/robotology/simmechanics-to-urdf/pull/60).
+
 ## [0.4.1] - 2022-12-20
 
 ### Fixed

setup.py

@@ -5,7 +5,7 @@
 
 
 setup(name='simmechanics_to_urdf',
-      version='0.4.1',
+      version='0.4.2',
       description='Converts SimMechanics XML to URDF',
       author='Silvio Traversaro, David V. Lu',
       author_email='[email protected]',

simmechanics_to_urdf/firstgen.py

@@ -451,7 +451,11 @@ def parseYAMLConfig(self, configFile):
                     exported_frame["frameReferenceLink"] = ftSens["frameReferenceLink"];
                 else:
                     exported_frame["frameReferenceLink"] = ftSens["linkName"];
-
+                map_key = (exported_frame["frameReferenceLink"], exported_frame["frameName"]);
+                # If the frame has been exported with a transformation, add it to the exported frame on sensor side
+                if (map_key in self.exportedFramesMap.keys() and ("additionalTransformation" in self.exportedFramesMap[map_key].keys())):
+                    existing_exported_frame = self.exportedFramesMap[map_key];
+                    exported_frame["additionalTransformation"] = existing_exported_frame["additionalTransformation"];
                 self.exportedFramesMap[
                     (exported_frame["frameReferenceLink"], exported_frame["frameName"])] = exported_frame;
 
@@ -495,7 +499,11 @@ def parseYAMLConfig(self, configFile):
                     exported_frame["frameReferenceLink"] = sensor["frameReferenceLink"];
                 else:
                     exported_frame["frameReferenceLink"] = sensor["linkName"];
-
+                map_key = (exported_frame["frameReferenceLink"], exported_frame["frameName"]);
+                # If the frame has been exported with a transformation, add it to the exported frame on sensor side
+                if (map_key in self.exportedFramesMap.keys() and ("additionalTransformation" in self.exportedFramesMap[map_key].keys())):
+                    existing_exported_frame = self.exportedFramesMap[map_key];
+                    exported_frame["additionalTransformation"] = existing_exported_frame["additionalTransformation"];
                 self.exportedFramesMap[
                     (exported_frame["frameReferenceLink"], exported_frame["frameName"])] = exported_frame;