added --insert-mesh --mesh-transform option to reduce file size of pr…

…oto files. It also decreases loading times in webots significantly.
astumpf · Sep 26, 2020 · 4a9da68 · 4a9da68
1 parent f51fec9
commit 4a9da68
Show file tree

Hide file tree

Showing 4 changed files with 62 additions and 18 deletions.
diff --git a/demo.py b/demo.py
@@ -14,6 +14,11 @@
                          ' The filename minus the .proto extension will be the robot name.')
     optParser.add_option('--normal', dest='normal', action='store_true', default=False,
                          help='If set, the normals are exported if present in the URDF definition.')
+    optParser.add_option('--insert-mesh', dest='insertMesh', action='store_true', default=False,
+                         help='If set, the mesh is exported in the proto file. Otherwise the filepath to the meshes are exported.')
+    optParser.add_option('--mesh-transform', dest='meshTransform', default='',
+                         help='Set the transformation composed by a translation and euler axis representation required to visualize meshed loaded from files correctly.'
+                         'Usage: --mesh-transform="x, y, z, r, p, y, angle')
     optParser.add_option('--box-collision', dest='boxCollision', action='store_true', default=False,
                          help='If set, the bounding objects are approximated using boxes.')
     optParser.add_option('--disable-mesh-optimization', dest='disableMeshOptimization', action='store_true', default=False,
@@ -32,5 +37,5 @@
                          'the first 3 joints of your robot to the specified values, and leave the rest with their default value.')
     options, args = optParser.parse_args()
 
-    convert2urdf(options.inFile, options.outFile, options.normal, options.boxCollision, options.disableMeshOptimization,
-                 options.enableMultiFile, options.staticBase, options.toolSlot, options.initRotation, options.initPos)
+    convert2urdf(options.inFile, options.outFile, options.normal, options.insertMesh, options.meshTransform, options.boxCollision,
+                 options.disableMeshOptimization, options.enableMultiFile, options.staticBase, options.toolSlot, options.initRotation, options.initPos)
diff --git a/urdf2webots/importer.py b/urdf2webots/importer.py
@@ -46,7 +46,7 @@ def mkdirSafe(directory):
             print('Directory "' + directory + '" already exists!')
 
 
-def convert2urdf(inFile, outFile=None, normal=False, boxCollision=False,
+def convert2urdf(inFile, outFile=None, normal=False, insertMesh=False, meshTransform='', boxCollision=False,
                  disableMeshOptimization=False, enableMultiFile=False, staticBase=False, toolSlot=None,
                  initRotation='0 1 0 0', initPos=None):
     if not os.path.exists(inFile):
@@ -180,7 +180,8 @@ def convert2urdf(inFile, outFile=None, normal=False, boxCollision=False,
 
                 urdf2webots.writeProto.declaration(protoFile, robotName, initRotation)
                 urdf2webots.writeProto.URDFLink(protoFile, rootLink, 1, parentList, childList, linkList, jointList,
-                                                sensorList, boxCollision=boxCollision, normal=normal, robot=True)
+                                                sensorList, boxCollision=boxCollision, normal=normal,
+                                                insertMesh=insertMesh, meshTransform=meshTransform, robot=True)
                 protoFile.write('}\n')
                 protoFile.close()
                 return
@@ -194,6 +195,11 @@ def convert2urdf(inFile, outFile=None, normal=False, boxCollision=False,
                          'of the resulting PROTO file. The filename minus the .proto extension will be the robot name.')
     optParser.add_option('--normal', dest='normal', action='store_true', default=False,
                          help='If set, the normals are exported if present in the URDF definition.')
+    optParser.add_option('--insert-mesh', dest='insertMesh', action='store_true', default=False,
+                         help='If set, the mesh is exported in the proto file. Otherwise the filepath to the meshes are exported.')
+    optParser.add_option('--mesh-transform', dest='meshTransform', default='',
+                         help='Set the transformation composed by a translation and euler axis representation required to visualize meshed loaded from files correctly.'
+                         'Usage: --mesh-transform="x, y, z, r, p, y, angle')
     optParser.add_option('--box-collision', dest='boxCollision', action='store_true', default=False,
                          help='If set, the bounding objects are approximated using boxes.')
     optParser.add_option('--disable-mesh-optimization', dest='disableMeshOptimization', action='store_true', default=False,
@@ -213,5 +219,5 @@ def convert2urdf(inFile, outFile=None, normal=False, boxCollision=False,
                          'default value')
     options, args = optParser.parse_args()
 
-    convert2urdf(options.inFile, options.outFile, options.normal, options.boxCollision, options.disableMeshOptimization,
-                 options.enableMultiFile, options.staticBase, options.toolSlot, options.initRotation, options.initPos)
+    convert2urdf(options.inFile, options.outFile, options.normal, options.insertMesh, options.meshTransform, options.boxCollision,
+                 options.disableMeshOptimization, options.enableMultiFile, options.staticBase, options.toolSlot, options.initRotation, options.initPos)
diff --git a/urdf2webots/parserURDF.py b/urdf2webots/parserURDF.py
@@ -97,6 +97,7 @@ def __init__(self):
         self.cylinder = Cylinder()
         self.sphere = Sphere()
         self.trimesh = Trimesh()
+        self.trimeshFile = ""
         self.scale = [1.0, 1.0, 1.0]
         self.name = None
         self.defName = None
@@ -387,6 +388,7 @@ def getSTLMesh(filename, node):
     b = struct.unpack("<3f", stlFile.read(12))
     c = struct.unpack("<3f", stlFile.read(12))
     struct.unpack("H", stlFile.read(2))
+    node.geometry.trimeshFile = filename
     trimesh = node.geometry.trimesh
     trimesh.coord.append(a)
     trimesh.coord.append(b)
@@ -454,6 +456,7 @@ def getOBJMesh(filename, node, link):
                 collision.geometry.scale = node.geometry.scale
                 extSring = '_%d' % (counter) if counter != 0 else ''
                 collision.geometry.name = '%s%s' % (os.path.splitext(os.path.basename(filename))[0], extSring)
+                collision.geometry.trimeshFile = filename
                 counter += 1
             elif header == 'f':  # face
                 vertices = body.split()
@@ -512,6 +515,7 @@ def getColladaMesh(filename, node, link):
                 index += 1
                 visual.position = node.position
                 visual.rotation = node.rotation
+                visual.geometry.trimeshFile = filename
                 visual.material.diffuse.red = node.material.diffuse.red
                 visual.material.diffuse.green = node.material.diffuse.green
                 visual.material.diffuse.blue = node.material.diffuse.blue
@@ -584,6 +588,7 @@ def getColladaMesh(filename, node, link):
                 collision = Collision()
                 collision.position = node.position
                 collision.rotation = node.rotation
+                collision.geometry.trimeshFile = filename
                 collision.geometry.scale = node.geometry.scale
                 for value in data.vertex:
                     collision.geometry.trimesh.coord.append(numpy.array(value))

diff --git a/urdf2webots/writeProto.py b/urdf2webots/writeProto.py
@@ -1,5 +1,6 @@
 """Import modules."""
 
+import os
 import math
 import numpy as np
 
@@ -79,7 +80,7 @@ def declaration(proto, robotName, initRotation):
 
 def URDFLink(proto, link, level, parentList, childList, linkList, jointList, sensorList,
              jointPosition=[0.0, 0.0, 0.0], jointRotation=[1.0, 0.0, 0.0, 0.0],
-             boxCollision=False, normal=False, dummy=False, robot=False, endpoint=False):
+             boxCollision=False, normal=False, insertMesh=False, meshTransform="", dummy=False, robot=False, endpoint=False):
     """Write a link iteratively."""
     indent = '  '
     haveChild = False
@@ -108,7 +109,7 @@ def URDFLink(proto, link, level, parentList, childList, linkList, jointList, sen
             if not haveChild:
                 haveChild = True
                 proto.write((level + 1) * indent + 'children [\n')
-            URDFShape(proto, link, level + 2, normal)
+            URDFShape(proto, link, level + 2, normal, insertMesh, meshTransform)
         # 2: export Sensors
         for sensor in sensorList:
             if sensor.parentLink == link.name:
@@ -123,7 +124,7 @@ def URDFLink(proto, link, level, parentList, childList, linkList, jointList, sen
                     haveChild = True
                     proto.write((level + 1) * indent + 'children [\n')
                 URDFJoint(proto, joint, level + 2, parentList, childList,
-                          linkList, jointList, sensorList, boxCollision, normal)
+                          linkList, jointList, sensorList, boxCollision, normal, insertMesh, meshTransform)
         # 4: export ToolSlot if specified
         if link.name == toolSlot:
             if not haveChild:
@@ -331,11 +332,21 @@ def computeDefName(name):
     return name.replace(' ', '_').replace('.', '_')
 
 
-def URDFVisual(proto, visualNode, level, normal=False):
+def URDFVisual(proto, visualNode, level, normal=False, insertMesh=False, meshTransform=""):
     """Write a Visual."""
     indent = '  '
     shapeLevel = level
 
+    useMeshfile = not insertMesh and visualNode.geometry.trimeshFile
+
+    if useMeshfile and meshTransform:
+        transform = meshTransform.split()
+        proto.write(shapeLevel * indent + 'Transform {\n')
+        proto.write((shapeLevel + 1) * indent + 'translation %s %s %s\n' % (transform[0], transform[1], transform[2]))
+        proto.write((shapeLevel + 1) * indent + 'rotation %s %s %s %s\n' % (transform[3], transform[4], transform[5], transform[6]))
+        proto.write((shapeLevel + 1) * indent + 'children [\n')
+        shapeLevel = shapeLevel + 2
+
     proto.write(shapeLevel * indent + 'Shape {\n')
     if visualNode.material.defName is not None:
         proto.write((shapeLevel + 1) * indent + 'appearance USE %s\n' % visualNode.material.defName)
@@ -388,6 +399,13 @@ def URDFVisual(proto, visualNode, level, normal=False):
         proto.write((shapeLevel + 2) * indent + 'radius ' + str(visualNode.geometry.sphere.radius) + '\n')
         proto.write((shapeLevel + 1) * indent + '}\n')
 
+    elif useMeshfile:
+        proto.write((shapeLevel + 1) * indent + 'geometry Mesh {\n')
+        proto.write((shapeLevel + 2) * indent + 'url [\n')
+        proto.write((shapeLevel + 3) * indent + '\"meshes/' + os.path.basename(visualNode.geometry.trimeshFile) + '\"\n')
+        proto.write((shapeLevel + 2) * indent + ']\n')
+        proto.write((shapeLevel + 1) * indent + '}\n')
+
     elif visualNode.geometry.trimesh.coord:
         meshType = 'IndexedLineSet' if visualNode.geometry.lineset else 'IndexedFaceSet'
         if visualNode.geometry.defName is not None:
@@ -475,10 +493,16 @@ def URDFVisual(proto, visualNode, level, normal=False):
             if not visualNode.geometry.lineset:
                 proto.write((shapeLevel + 2) * indent + 'creaseAngle 1\n')
             proto.write((shapeLevel + 1) * indent + '}\n')
+
     proto.write(shapeLevel * indent + '}\n')
 
+    if useMeshfile and meshTransform:
+        shapeLevel = shapeLevel - 2
+        proto.write((shapeLevel + 1) * indent + ']\n')
+        proto.write(shapeLevel * indent + '}\n')
+
 
-def URDFShape(proto, link, level, normal=False):
+def URDFShape(proto, link, level, normal=False, insertMesh=False, meshTransform=""):
     """Write a Shape."""
     indent = '  '
     shapeLevel = level
@@ -497,7 +521,7 @@ def URDFShape(proto, link, level, normal=False):
             proto.write((shapeLevel + 1) * indent + 'children [\n')
             shapeLevel += 2
             transform = True
-        if enableMultiFile and visualNode.geometry.trimesh.coord:
+        if enableMultiFile and (visualNode.geometry.trimesh.coord or visualNode.geometry.trimeshFile):
             name = visualNode.geometry.defName
             if name is None:
                 if visualNode.geometry.name is not None:
@@ -510,20 +534,24 @@ def URDFShape(proto, link, level, normal=False):
                 header(meshProtoFile, tags=['hidden'])
                 meshProtoFile.write('PROTO %sMesh [\n]\n{\n' % name)
                 visualNode.material.defName = None
-                URDFVisual(meshProtoFile, visualNode, 1, normal)
+                URDFVisual(meshProtoFile, visualNode, 1, normal, insertMesh, meshTransform)
                 meshProtoFile.write('}\n')
                 meshProtoFile.close()
             proto.write(shapeLevel * indent + '%sMesh {\n' % name + shapeLevel * indent + '}\n')
         else:
-            URDFVisual(proto, visualNode, shapeLevel, normal)
+            URDFVisual(proto, visualNode, shapeLevel, normal, insertMesh, meshTransform)
         if transform:
             proto.write((shapeLevel - 1) * indent + ']\n')
             proto.write((shapeLevel - 2) * indent + '}\n')
             shapeLevel -= 2
 
+        # workaround: skip adding multiple proto files which would refer to the same mesh file
+        if not insertMesh and visualNode.geometry.trimeshFile:
+            break
+
 
 def URDFJoint(proto, joint, level, parentList, childList, linkList, jointList,
-              sensorList, boxCollision, normal):
+              sensorList, boxCollision, normal, insertMesh, meshTransform):
     """Write a Joint iteratively."""
     indent = '  '
     if not joint.axis:
@@ -584,7 +612,7 @@ def URDFJoint(proto, joint, level, parentList, childList, linkList, jointList,
             if childLink.name == joint.child:
                 URDFLink(proto, childLink, level, parentList, childList,
                          linkList, jointList, sensorList, joint.position, joint.rotation,
-                         boxCollision, normal)
+                         boxCollision, normal, insertMesh, meshTransform)
         return
 
     elif joint.type == 'floating' or joint.type == 'planar':
@@ -615,13 +643,13 @@ def URDFJoint(proto, joint, level, parentList, childList, linkList, jointList,
         if childLink.name == joint.child:
             URDFLink(proto, childLink, level + 1, parentList, childList,
                      linkList, jointList, sensorList, endpointPosition, endpointRotation,
-                     boxCollision, normal, endpoint=True)
+                     boxCollision, normal, insertMesh, meshTransform, endpoint=True)
             assert(not found_link)
             found_link = True
     # case that non-existing link cited, set dummy flag
     if not found_link and joint.child:
         URDFLink(proto, joint.child, level + 1, parentList, childList,
                  linkList, jointList, sensorList, endpointPosition, endpointRotation,
-                 boxCollision, normal, dummy=True)
+                 boxCollision, normal, insertMesh, meshTransform, dummy=True)
         print('warning: link ' + joint.child + ' is dummy!')
     proto.write(level * indent + '}\n')