Fix STL simplification for our needs

onshape_urdf_exporter/__main__.py

@@ -1,11 +1,13 @@
 import hashlib
 import os
+from pathlib import Path
 from sys import exit
 
 import commentjson as json
 import numpy as np
-from colorama import Back, Fore, Style
+from colorama import Fore, Style
 
+from .stl_utils import simplify_stl
 from .robot_description import RobotURDF
 
 partNames = {}
@@ -90,11 +92,15 @@ def addPart(occurrence, matrix):
             + Style.RESET_ALL
         )
 
+        stl_file: Path | None
         if partIsIgnore(justPart):
-            stlFile = None
+            stl_file = None
         else:
-            stlFile = sanitize_filename(prefix + ".stl")
-            # shorten the configuration to a maximum number of chars to prevent errors. Necessary for standard parts like screws
+            stl_file = Path(config["outputDirectory"]) / sanitize_filename(
+                prefix + ".stl"
+            )
+            # shorten the configuration to a maximum number of chars to prevent errors.
+            # Necessary for standard parts like screws
             if len(part["configuration"]) > 40:
                 shortend_configuration = hashlib.md5(
                     part["configuration"].encode("utf-8")
@@ -108,17 +114,16 @@ def addPart(occurrence, matrix):
                 part["partId"],
                 shortend_configuration,
             )
-            with open(config["outputDirectory"] + "/" + stlFile, "wb") as stream:
-                stream.write(stl)
+            stl_file.write_bytes(stl)
+            if config["simplifySTLs"]:
+                simplify_stl(stl_file)
 
             stlMetadata = sanitize_filename(prefix + ".part")
             with open(
                 config["outputDirectory"] + "/" + stlMetadata, "w", encoding="utf-8"
             ) as stream:
                 json.dump(part, stream, indent=4, sort_keys=True)
 
-            stlFile = config["outputDirectory"] + "/" + stlFile
-
         # Obtain metadatas about part to retrieve color
         if config["color"] is not None:
             color = config["color"]
@@ -190,7 +195,7 @@ def addPart(occurrence, matrix):
         if robot.relative:
             pose = np.linalg.inv(matrix) * pose
 
-        robot.add_part(pose, stlFile, mass, com, inertia, color, prefix)
+        robot.add_part(pose, stl_file, mass, com, inertia, color, prefix)
 
     partNames = {}
 

onshape_urdf_exporter/config.py

@@ -91,9 +91,7 @@ def configGet(name, default=None, hasDefault=False, valuesList=None):
     "mergeSTLs", "no", valuesList=["no", "visual", "collision", "all"]
 )
 config["maxSTLSize"] = configGet("maxSTLSize", 3)
-config["simplifySTLs"] = configGet(
-    "simplifySTLs", "no", valuesList=["no", "visual", "collision", "all"]
-)
+config["simplifySTLs"] = configGet("simplifySTLs", False)
 
 # Post-import commands to execute
 config["postImportCommands"] = configGet("postImportCommands", [])

onshape_urdf_exporter/robot_description.py

@@ -1,13 +1,11 @@
 import math
-import os
+from pathlib import Path
 from typing import Any, TextIO
 from xml.etree import ElementTree as ET
 
 import numpy as np
 import numpy.typing as npt
 
-from . import stl_combine
-
 
 def rotation_matrix_to_euler_angles(
     r: npt.NDArray[np.float64],
@@ -29,7 +27,6 @@ def rotation_matrix_to_euler_angles(
 
 
 def add_origin_element(parent: ET.Element, matrix: npt.NDArray[np.float64]) -> None:
-    # TODO: Make this not use '%' for formatting
     x = matrix[0, 3]
     y = matrix[1, 3]
     z = matrix[2, 3]
@@ -58,7 +55,6 @@ def __init__(self, name: str):
         self.robot_name = name
         self.mesh_dir = None
 
-        self._mesh = {"visual": None, "collision": None}
         self._color = np.array([0.0, 0.0, 0.0])
         self._color_mass = 0.0
         self._link_childs = 0
@@ -90,7 +86,6 @@ def joint_max_velocity_for(self, joint_name: str) -> float:
             return self.joint_max_velocity
 
     def reset_link(self) -> None:
-        self._mesh = {"visual": None, "collision": None}
         self._color = np.array([0.0, 0.0, 0.0])
         self._color_mass = 0
         self._link_childs = 0
@@ -114,26 +109,6 @@ def add_link_dynamics(
 
         self._dynamics.append({"mass": mass, "com": com, "inertia": inertia})
 
-    def merge_stl(
-        self,
-        stl: str,
-        matrix: npt.NDArray[np.float64],
-        color: npt.NDArray[np.float64],
-        mass: float,
-        node: str = "visual",
-    ) -> None:
-        if node == "visual":
-            self._color += np.array(color) * mass
-            self._color_mass += mass
-
-        m = stl_combine.load_mesh(stl)
-        stl_combine.apply_matrix(m, matrix)
-
-        if self._mesh[node] is None:
-            self._mesh[node] = m
-        else:
-            self._mesh[node] = stl_combine.combine_meshes(self._mesh[node], m)
-
     def link_dynamics(
         self,
     ) -> tuple[float, npt.NDArray[np.float64], npt.NDArray[np.float64]]:
@@ -189,7 +164,8 @@ def add_dummy_link(
         if visual_stl is not None:
             if visual_matrix is None or visual_color is None:
                 raise RuntimeError(
-                    "visual_matrix, visual_stl, and visual_color must all be set if any one are set"
+                    "visual_matrix, visual_stl, and visual_color must all be set if "
+                    "any one are set"
                 )
 
             self.add_stl(
@@ -242,29 +218,6 @@ def end_link(self) -> None:
 
         mass, com, inertia = self.link_dynamics()
 
-        for node in ["visual", "collision"]:
-            if self._mesh[node] is not None:
-                if self.mesh_dir is None:
-                    raise RuntimeError("mesh_dir must be set before saving meshes")
-
-                if node == "visual" and self._color_mass > 0:
-                    color = self._color / self._color_mass
-                else:
-                    color = np.array([0.5, 0.5, 0.5])
-
-                filename = self._link_name + "_" + node + ".stl"
-                stl_combine.save_mesh(self._mesh[node], self.mesh_dir + "/" + filename)
-                if self.should_simplify_stls(node):
-                    stl_combine.simplify_stl(self.mesh_dir + "/" + filename)
-                self.add_stl(
-                    self._active_link,
-                    np.identity(4),
-                    filename,
-                    color,
-                    self._link_name,
-                    node,
-                )
-
         inertial = ET.SubElement(self._active_link, "inertial")
         ET.SubElement(
             inertial, "origin", xyz="%.20g %.20g %.20g" % (com[0], com[1], com[2])
@@ -339,7 +292,7 @@ def add_stl(
     def add_part(
         self,
         matrix: npt.NDArray[np.float64],
-        stl: str | None,
+        stl: Path | None,
         mass: float,
         com: npt.NDArray[np.float64],
         inertia: npt.NDArray[np.float64],
@@ -353,15 +306,13 @@ def add_part(
             if not self.draw_collisions:
                 if self.use_fixed_links:
                     self._visuals.append(
-                        [matrix, self.package_name + os.path.basename(stl), color]
+                        [matrix, self.package_name + stl.name, color]
                     )
-                elif self.should_merge_stls("visual"):
-                    self.merge_stl(stl, matrix, color, mass)
                 else:
                     self.add_stl(
                         self._active_link,
                         matrix,
-                        os.path.basename(stl),
+                        stl.name,
                         color,
                         name,
                         "visual",
@@ -371,18 +322,14 @@ def add_part(
             if self.draw_collisions:
                 entries.append("visual")
             for entry in entries:
-                # We don't have pure shape, we use the mesh
-                if self.should_merge_stls(entry):
-                    self.merge_stl(stl, matrix, color, mass, entry)
-                else:
-                    self.add_stl(
-                        self._active_link,
-                        matrix,
-                        os.path.basename(stl),
-                        color,
-                        name,
-                        entry,
-                    )
+                self.add_stl(
+                    self._active_link,
+                    matrix,
+                    stl.name,
+                    color,
+                    name,
+                    entry,
+                )
 
         self.add_link_dynamics(matrix, mass, com, inertia)
 

onshape_urdf_exporter/stl_utils.py

@@ -0,0 +1,30 @@
+from pathlib import Path
+
+import open3d as o3d
+
+
+def simplify_stl(path: Path) -> None:
+    """Optimize a single STL file in-place, if it isn't already optimized."""
+
+    print(f"Optimizing {str(path)}")
+    simple_mesh: o3d.geometry.TriangleMesh = o3d.io.read_triangle_mesh(str(path))
+    simple_mesh = simple_mesh.compute_triangle_normals()
+    simple_mesh = simple_mesh.simplify_vertex_clustering(
+        voxel_size=0.0025, contraction=o3d.geometry.SimplificationContraction.Quadric
+    )
+    simple_mesh = simple_mesh.merge_close_vertices(eps=0.001)
+    simple_mesh = simple_mesh.remove_duplicated_vertices()
+    simple_mesh = simple_mesh.remove_duplicated_triangles()
+    simple_mesh = simple_mesh.remove_degenerate_triangles()
+    simple_mesh = simple_mesh.remove_non_manifold_edges()
+    simple_mesh = simple_mesh.compute_triangle_normals()
+    simple_mesh = simple_mesh.compute_vertex_normals()
+
+    assert o3d.io.write_triangle_mesh(
+        str(path),
+        simple_mesh,
+        write_vertex_normals=False,
+        write_triangle_uvs=False,
+        compressed=False,
+        write_vertex_colors=False,
+    )