From b5a4c8fc81884c940ce5ddf05e0daa0f4f7b3ba6 Mon Sep 17 00:00:00 2001 From: chenjian Date: Wed, 21 Jan 2026 16:28:15 +0800 Subject: [PATCH 1/7] update --- .../urdf_collision_decomposition/__init__.py | 18 ++ .../urdf_collision_decomposition.py | 169 ++++++++++++++++++ 2 files changed, 187 insertions(+) create mode 100644 embodichain/toolkits/urdf_collision_decomposition/__init__.py create mode 100644 embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py diff --git a/embodichain/toolkits/urdf_collision_decomposition/__init__.py b/embodichain/toolkits/urdf_collision_decomposition/__init__.py new file mode 100644 index 00000000..8e273b4d --- /dev/null +++ b/embodichain/toolkits/urdf_collision_decomposition/__init__.py @@ -0,0 +1,18 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2025 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +from .urdf_collision_decomposition import generate_urdf_convex_decomposition_collision + +__all__ = ["generate_urdf_convex_decomposition_collision"] diff --git a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py new file mode 100644 index 00000000..77f0593a --- /dev/null +++ b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py @@ -0,0 +1,169 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2025 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +import os +import xml.etree.ElementTree as ET +import open3d as o3d +from dexsim.kit.meshproc.convex_decomposition import convex_decomposition_coacd +from dexsim.kit.meshproc.utility import mesh_list_to_file +from embodichain.utils import logger + + +def generate_urdf_convex_decomposition_collision( + urdf_path: str, output_urdf_name: str, max_convex_hull_num: int = 16 +): + decomposer = URDFCollisionDecomposer( + urdf_path, max_convex_hull_num=max_convex_hull_num + ) + decomposer.decompose_collisions(output_urdf_name) + + +class URDFCollisionDecomposer: + def __init__(self, urdf_path: str, **kwargs): + self.urdf_path = urdf_path + self.urdf_dir = os.path.dirname(urdf_path) + self.urdf = ET.parse(urdf_path) + self.root = self.urdf.getroot() + + self.max_convex_hull_num = kwargs.get("max_convex_hull_num", 16) + + def _get_visual_collision_pairs(self): + visual_collision_pairs = dict() + for link in self.root.findall("link"): + link_name = link.get("name") + visual = link.find("visual") + collision = link.find("collision") + visual_collision_pairs[link_name] = (visual, collision, link) + + return visual_collision_pairs + + def generate_link_collision( + self, link_name: str, visual: ET.Element | None, collision: ET.Element | None, link: ET.Element + ): + if collision is None: + geom = visual.find("geometry") + # use visual geometry mesh + elif visual is not None: + geom = collision.find("geometry") + # use collision geometry mesh + pass + else: + logger.log_warning( + f"Link {link_name} has no visual and collision geometry." + ) + return + + pass + + def decompose_collisions(self, output_urdf_name: str): + visual_collision_pairs = self._get_visual_collision_pairs() + for link_name, (visual, collision, link) in visual_collision_pairs.items(): + if collision is None: + geom = visual.find("geometry") + # use visual geometry mesh + elif visual is not None: + geom = collision.find("geometry") + # use collision geometry mesh + pass + else: + logger.log_warning( + f"Link {link_name} has no visual and collision geometry." + ) + continue + + if geom is None: + logger.log_warning(f"Link {link_name} has no geometry.") + continue + mesh_elem = geom.find("mesh") + if mesh_elem is None: + logger.log_warning(f"Link {link_name} geometry is not a mesh.") + continue + mesh_filename = mesh_elem.get("filename") + mesh_path = os.path.join(self.urdf_dir, mesh_filename) + mesh_base_name = os.path.basename(mesh_filename).split(".")[0] + if not os.path.isfile(mesh_path): + logger.log_warning(f"Mesh file {mesh_path} does not exist.") + continue + + mesh = o3d.t.io.read_triangle_mesh(mesh_path) + _, convex_meshes = convex_decomposition_coacd( + mesh, max_convex_hull_num=self.max_convex_hull_num + ) + convex_mesh_file = f"{mesh_base_name}_auto_convex.obj" + # create collision mesh dir + collision_dir = os.path.join(self.urdf_dir, "Collision") + if not os.path.exists(collision_dir): + os.makedirs(collision_dir) + collision_relative_path = os.path.join("Collision", convex_mesh_file) + + mesh_list_to_file( + save_path=os.path.join(self.urdf_dir, collision_relative_path), + mesh_list=convex_meshes, + ) + + if collision is None: + # create collision element and save to urdf xml tree + collision = ET.SubElement(link, "collision") + collision_origin = ET.SubElement(collision, "origin") + visual_origin = visual.find("origin") + if visual_origin is not None: + collision_origin.set( + "xyz", visual_origin.get("xyz", "0 0 0") + ) + collision_origin.set( + "rpy", visual_origin.get("rpy", "0 0 0") + ) + else: + collision_origin.set("xyz", "0 0 0") + collision_origin.set("rpy", "0 0 0") + geom = ET.SubElement(collision, "geometry") + mesh = ET.SubElement(geom, "mesh") + mesh.set("filename", collision_relative_path) + else: + # update collision mesh file path + geom = collision.find("geometry") + mesh = geom.find("mesh") + mesh.set("filename", collision_relative_path) + # save to new urdf file + output_path = os.path.join(self.urdf_dir, output_urdf_name) + self.urdf.write(output_path) + + +if __name__ == "__main__": + import argparse + parser = argparse.ArgumentParser( + description="Create and simulate a camera with gizmo in SimulationManager" + ) + parser.add_argument( + "--urdf_path", + type=str, + help="Input urdf file path", + ) + parser.add_argument( + "--output_urdf_name", + type=str, + help="Output urdf file name, " + ) + parser.add_argument( + "--max_convex_hull_num", + type=int, + default=8, + help="Maximum number of convex hulls for decomposition", + ) + + args = parser.parse_args() + generate_urdf_convex_decomposition_collision( + args.urdf_path, args.output_urdf_name, args.max_convex_hull_num + ) \ No newline at end of file From bc34680dd71d83581cde521b765a3c10d79ecb5d Mon Sep 17 00:00:00 2001 From: chenjian Date: Wed, 21 Jan 2026 16:31:23 +0800 Subject: [PATCH 2/7] style --- .../urdf_collision_decomposition.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py index 77f0593a..b3492797 100644 --- a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py +++ b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py @@ -166,4 +166,4 @@ def decompose_collisions(self, output_urdf_name: str): args = parser.parse_args() generate_urdf_convex_decomposition_collision( args.urdf_path, args.output_urdf_name, args.max_convex_hull_num - ) \ No newline at end of file + ) From 83b388abe0ea5eeab62569449f8340bf3a46efcb Mon Sep 17 00:00:00 2001 From: chenjian Date: Wed, 21 Jan 2026 16:32:37 +0800 Subject: [PATCH 3/7] style --- .../urdf_collision_decomposition.py | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py index b3492797..1ee4efe4 100644 --- a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py +++ b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py @@ -50,8 +50,12 @@ def _get_visual_collision_pairs(self): return visual_collision_pairs def generate_link_collision( - self, link_name: str, visual: ET.Element | None, collision: ET.Element | None, link: ET.Element - ): + self, + link_name: str, + visual: ET.Element | None, + collision: ET.Element | None, + link: ET.Element, + ): if collision is None: geom = visual.find("geometry") # use visual geometry mesh @@ -119,12 +123,8 @@ def decompose_collisions(self, output_urdf_name: str): collision_origin = ET.SubElement(collision, "origin") visual_origin = visual.find("origin") if visual_origin is not None: - collision_origin.set( - "xyz", visual_origin.get("xyz", "0 0 0") - ) - collision_origin.set( - "rpy", visual_origin.get("rpy", "0 0 0") - ) + collision_origin.set("xyz", visual_origin.get("xyz", "0 0 0")) + collision_origin.set("rpy", visual_origin.get("rpy", "0 0 0")) else: collision_origin.set("xyz", "0 0 0") collision_origin.set("rpy", "0 0 0") @@ -143,6 +143,7 @@ def decompose_collisions(self, output_urdf_name: str): if __name__ == "__main__": import argparse + parser = argparse.ArgumentParser( description="Create and simulate a camera with gizmo in SimulationManager" ) @@ -151,11 +152,7 @@ def decompose_collisions(self, output_urdf_name: str): type=str, help="Input urdf file path", ) - parser.add_argument( - "--output_urdf_name", - type=str, - help="Output urdf file name, " - ) + parser.add_argument("--output_urdf_name", type=str, help="Output urdf file name, ") parser.add_argument( "--max_convex_hull_num", type=int, @@ -165,5 +162,5 @@ def decompose_collisions(self, output_urdf_name: str): args = parser.parse_args() generate_urdf_convex_decomposition_collision( - args.urdf_path, args.output_urdf_name, args.max_convex_hull_num + args.urdf_path, args.output_urdf_name, args.max_convex_hull_num ) From 423d9ebb70936c6950486127a4d03e6b748e3ca1 Mon Sep 17 00:00:00 2001 From: chenjian Date: Wed, 21 Jan 2026 17:00:45 +0800 Subject: [PATCH 4/7] update --- .../urdf_collision_decomposition.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py index 1ee4efe4..6ef39ee8 100644 --- a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py +++ b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py @@ -74,18 +74,17 @@ def generate_link_collision( def decompose_collisions(self, output_urdf_name: str): visual_collision_pairs = self._get_visual_collision_pairs() for link_name, (visual, collision, link) in visual_collision_pairs.items(): - if collision is None: - geom = visual.find("geometry") - # use visual geometry mesh - elif visual is not None: - geom = collision.find("geometry") - # use collision geometry mesh - pass - else: + if visual is None and collision is None: logger.log_warning( f"Link {link_name} has no visual and collision geometry." ) continue + if collision is None: + geom = visual.find("geometry") + # use visual geometry mesh + else: + # use collision geometry mesh + geom = collision.find("geometry") if geom is None: logger.log_warning(f"Link {link_name} has no geometry.") From 3bbc6157b0e304b4760022fd387893c8ff636d8b Mon Sep 17 00:00:00 2001 From: chenjian Date: Thu, 22 Jan 2026 19:46:33 +0800 Subject: [PATCH 5/7] update --- .../urdf_collision_decomposition.py | 165 -------- .../__init__.py | 4 +- .../toolkits/urdf_modifider/urdf_modifider.py | 370 ++++++++++++++++++ 3 files changed, 372 insertions(+), 167 deletions(-) delete mode 100644 embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py rename embodichain/toolkits/{urdf_collision_decomposition => urdf_modifider}/__init__.py (83%) create mode 100644 embodichain/toolkits/urdf_modifider/urdf_modifider.py diff --git a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py b/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py deleted file mode 100644 index 6ef39ee8..00000000 --- a/embodichain/toolkits/urdf_collision_decomposition/urdf_collision_decomposition.py +++ /dev/null @@ -1,165 +0,0 @@ -# ---------------------------------------------------------------------------- -# Copyright (c) 2021-2025 DexForce Technology Co., Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# ---------------------------------------------------------------------------- -import os -import xml.etree.ElementTree as ET -import open3d as o3d -from dexsim.kit.meshproc.convex_decomposition import convex_decomposition_coacd -from dexsim.kit.meshproc.utility import mesh_list_to_file -from embodichain.utils import logger - - -def generate_urdf_convex_decomposition_collision( - urdf_path: str, output_urdf_name: str, max_convex_hull_num: int = 16 -): - decomposer = URDFCollisionDecomposer( - urdf_path, max_convex_hull_num=max_convex_hull_num - ) - decomposer.decompose_collisions(output_urdf_name) - - -class URDFCollisionDecomposer: - def __init__(self, urdf_path: str, **kwargs): - self.urdf_path = urdf_path - self.urdf_dir = os.path.dirname(urdf_path) - self.urdf = ET.parse(urdf_path) - self.root = self.urdf.getroot() - - self.max_convex_hull_num = kwargs.get("max_convex_hull_num", 16) - - def _get_visual_collision_pairs(self): - visual_collision_pairs = dict() - for link in self.root.findall("link"): - link_name = link.get("name") - visual = link.find("visual") - collision = link.find("collision") - visual_collision_pairs[link_name] = (visual, collision, link) - - return visual_collision_pairs - - def generate_link_collision( - self, - link_name: str, - visual: ET.Element | None, - collision: ET.Element | None, - link: ET.Element, - ): - if collision is None: - geom = visual.find("geometry") - # use visual geometry mesh - elif visual is not None: - geom = collision.find("geometry") - # use collision geometry mesh - pass - else: - logger.log_warning( - f"Link {link_name} has no visual and collision geometry." - ) - return - - pass - - def decompose_collisions(self, output_urdf_name: str): - visual_collision_pairs = self._get_visual_collision_pairs() - for link_name, (visual, collision, link) in visual_collision_pairs.items(): - if visual is None and collision is None: - logger.log_warning( - f"Link {link_name} has no visual and collision geometry." - ) - continue - if collision is None: - geom = visual.find("geometry") - # use visual geometry mesh - else: - # use collision geometry mesh - geom = collision.find("geometry") - - if geom is None: - logger.log_warning(f"Link {link_name} has no geometry.") - continue - mesh_elem = geom.find("mesh") - if mesh_elem is None: - logger.log_warning(f"Link {link_name} geometry is not a mesh.") - continue - mesh_filename = mesh_elem.get("filename") - mesh_path = os.path.join(self.urdf_dir, mesh_filename) - mesh_base_name = os.path.basename(mesh_filename).split(".")[0] - if not os.path.isfile(mesh_path): - logger.log_warning(f"Mesh file {mesh_path} does not exist.") - continue - - mesh = o3d.t.io.read_triangle_mesh(mesh_path) - _, convex_meshes = convex_decomposition_coacd( - mesh, max_convex_hull_num=self.max_convex_hull_num - ) - convex_mesh_file = f"{mesh_base_name}_auto_convex.obj" - # create collision mesh dir - collision_dir = os.path.join(self.urdf_dir, "Collision") - if not os.path.exists(collision_dir): - os.makedirs(collision_dir) - collision_relative_path = os.path.join("Collision", convex_mesh_file) - - mesh_list_to_file( - save_path=os.path.join(self.urdf_dir, collision_relative_path), - mesh_list=convex_meshes, - ) - - if collision is None: - # create collision element and save to urdf xml tree - collision = ET.SubElement(link, "collision") - collision_origin = ET.SubElement(collision, "origin") - visual_origin = visual.find("origin") - if visual_origin is not None: - collision_origin.set("xyz", visual_origin.get("xyz", "0 0 0")) - collision_origin.set("rpy", visual_origin.get("rpy", "0 0 0")) - else: - collision_origin.set("xyz", "0 0 0") - collision_origin.set("rpy", "0 0 0") - geom = ET.SubElement(collision, "geometry") - mesh = ET.SubElement(geom, "mesh") - mesh.set("filename", collision_relative_path) - else: - # update collision mesh file path - geom = collision.find("geometry") - mesh = geom.find("mesh") - mesh.set("filename", collision_relative_path) - # save to new urdf file - output_path = os.path.join(self.urdf_dir, output_urdf_name) - self.urdf.write(output_path) - - -if __name__ == "__main__": - import argparse - - parser = argparse.ArgumentParser( - description="Create and simulate a camera with gizmo in SimulationManager" - ) - parser.add_argument( - "--urdf_path", - type=str, - help="Input urdf file path", - ) - parser.add_argument("--output_urdf_name", type=str, help="Output urdf file name, ") - parser.add_argument( - "--max_convex_hull_num", - type=int, - default=8, - help="Maximum number of convex hulls for decomposition", - ) - - args = parser.parse_args() - generate_urdf_convex_decomposition_collision( - args.urdf_path, args.output_urdf_name, args.max_convex_hull_num - ) diff --git a/embodichain/toolkits/urdf_collision_decomposition/__init__.py b/embodichain/toolkits/urdf_modifider/__init__.py similarity index 83% rename from embodichain/toolkits/urdf_collision_decomposition/__init__.py rename to embodichain/toolkits/urdf_modifider/__init__.py index 8e273b4d..c05cbbd9 100644 --- a/embodichain/toolkits/urdf_collision_decomposition/__init__.py +++ b/embodichain/toolkits/urdf_modifider/__init__.py @@ -13,6 +13,6 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- -from .urdf_collision_decomposition import generate_urdf_convex_decomposition_collision +from .urdf_modifider import generate_urdf_collision_convexes -__all__ = ["generate_urdf_convex_decomposition_collision"] +__all__ = ["generate_urdf_collision_convexes"] diff --git a/embodichain/toolkits/urdf_modifider/urdf_modifider.py b/embodichain/toolkits/urdf_modifider/urdf_modifider.py new file mode 100644 index 00000000..035f6a87 --- /dev/null +++ b/embodichain/toolkits/urdf_modifider/urdf_modifider.py @@ -0,0 +1,370 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2025 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +import os +import xml.etree.ElementTree as ET +import open3d as o3d +from dexsim.kit.meshproc.convex_decomposition import convex_decomposition_coacd +from dexsim.kit.meshproc.utility import mesh_list_to_file +from embodichain.utils import logger +import trimesh +import numpy as np + + +def generate_urdf_collision_convexes( + urdf_path: str, + output_urdf_name: str, + max_convex_hull_num: int = 16, + recompute_inertia: bool = False, + scale: np.ndarray = None, +): + decomposer = URDFModifider( + urdf_path, + max_convex_hull_num=max_convex_hull_num, + recompute_inertia=recompute_inertia, + scale=scale, + ) + decomposer.decompose_collisions() + decomposer.save_urdf(output_urdf_name) + + +class URDFModifider: + def __init__(self, urdf_path: str, **kwargs): + self.urdf_path = urdf_path + self.urdf_dir = os.path.dirname(urdf_path) + self.urdf = ET.parse(urdf_path) + self.root = self.urdf.getroot() + + self.max_convex_hull_num = kwargs.get("max_convex_hull_num", 16) + self.recompute_inertia = kwargs.get("recompute_inertia", False) + self.scale = kwargs.get("scale", None) + + def decompose_collisions(self): + link_dict = dict() + if self.scale is not None: + self.scale_urdf(self.scale) + + for link in self.root.findall("link"): + link_name = link.get("name") + link_dict[link_name] = link + + for link_name, link in link_dict.items(): + self.decomposite_link_collision(link_name, link) + if self.recompute_inertia: + self.recompute_link_inertia(link) + + def decomposite_link_collision( + self, + link_name: str, + link: ET.Element, + ): + visual = link.find("visual") + collision = link.find("collision") + if visual is None and collision is None: + logger.log_warning( + f"Link {link_name} has no visual and collision geometry." + ) + return + if collision is None: + geom = visual.find("geometry") + # use visual geometry mesh + else: + # use collision geometry mesh + geom = collision.find("geometry") + + if geom is None: + logger.log_warning(f"Link {link_name} has no geometry.") + return + mesh_elem = geom.find("mesh") + if mesh_elem is None: + logger.log_warning(f"Link {link_name} geometry is not a mesh.") + return + mesh_filename = mesh_elem.get("filename") + mesh_path = os.path.join(self.urdf_dir, mesh_filename) + mesh_base_name = os.path.basename(mesh_filename).split(".")[0] + if not os.path.isfile(mesh_path): + logger.log_warning(f"Mesh file {mesh_path} does not exist.") + return + + mesh = o3d.t.io.read_triangle_mesh(mesh_path) + _, convex_meshes = convex_decomposition_coacd( + mesh, max_convex_hull_num=self.max_convex_hull_num + ) + convex_mesh_file = f"{mesh_base_name}_auto_convex.obj" + # create collision mesh dir + collision_dir = os.path.join(self.urdf_dir, "Collision") + if not os.path.exists(collision_dir): + os.makedirs(collision_dir) + collision_relative_path = os.path.join("Collision", convex_mesh_file) + + mesh_list_to_file( + save_path=os.path.join(self.urdf_dir, collision_relative_path), + mesh_list=convex_meshes, + ) + + if collision is None: + # create collision element and save to urdf xml tree + collision = ET.SubElement(link, "collision") + collision_origin = ET.SubElement(collision, "origin") + visual_origin = visual.find("origin") + if visual_origin is not None: + collision_origin.set("xyz", visual_origin.get("xyz", "0 0 0")) + collision_origin.set("rpy", visual_origin.get("rpy", "0 0 0")) + else: + collision_origin.set("xyz", "0 0 0") + collision_origin.set("rpy", "0 0 0") + geom = ET.SubElement(collision, "geometry") + mesh = ET.SubElement(geom, "mesh") + mesh.set("filename", collision_relative_path) + else: + # update collision mesh file path + geom = collision.find("geometry") + mesh = geom.find("mesh") + mesh.set("filename", collision_relative_path) + + def recompute_link_inertia(self, link: ET.Element): + collision = link.find("collision") + if collision is None: + return + has_mesh = self.has_mesh(collision) + if not has_mesh: + return + geom = collision.find("geometry") + mesh_elem = geom.find("mesh") + mesh_filename = mesh_elem.get("filename") + mesh_path = os.path.join(self.urdf_dir, mesh_filename) + mass, com, inertia = self.compute_inertia_from_mesh(mesh_path) + if mass is None or com is None or inertia is None: + return + inertial = link.find("inertial") + if inertial is None: + inertial = ET.SubElement(link, "inertial") + mass_elem = inertial.find("mass") + if mass_elem is None: + mass_elem = ET.SubElement(inertial, "mass") + mass_elem.set("value", str(mass)) + origin_elem = inertial.find("origin") + if origin_elem is None: + origin_elem = ET.SubElement(inertial, "origin") + origin_elem.set("xyz", f"{com[0]} {com[1]} {com[2]}") + origin_elem.set("rpy", "0 0 0") + inertia_elem = inertial.find("inertia") + if inertia_elem is None: + inertia_elem = ET.SubElement(inertial, "inertia") + inertia_elem.set("ixx", str(inertia[0])) + inertia_elem.set("iyy", str(inertia[1])) + inertia_elem.set("izz", str(inertia[2])) + inertia_elem.set("ixy", str(inertia[3])) + inertia_elem.set("ixz", str(inertia[4])) + inertia_elem.set("iyz", str(inertia[5])) + + @staticmethod + def compute_inertia_from_mesh(mesh_path: str, density: float = 1.0): + mesh = trimesh.load(mesh_path, force="mesh") + if isinstance(mesh, trimesh.Scene): + if len(mesh.geometry) == 0: + logger.log_warning(f"Mesh scene {mesh_path} has no geometry.") + return None, None, None + mesh = trimesh.util.concatenate(tuple(mesh.geometry.values())) + + if not mesh.is_watertight: + logger.log_warning( + f"Mesh {mesh_path} is not watertight. Inertia computation may be inaccurate." + ) + mesh.density = density + + mass = mesh.mass + center_of_mass = mesh.center_mass + inertia = mesh.moment_inertia + inertia = [ + inertia[0, 0], + inertia[1, 1], + inertia[2, 2], + inertia[0, 1], + inertia[0, 2], + inertia[1, 2], + ] + + return mass, center_of_mass, inertia + + def has_mesh(self, colli_visual: ET.Element | None) -> bool: + if colli_visual is None: + return False + geom = colli_visual.find("geometry") + if geom is None: + return False + mesh = geom.find("mesh") + if mesh is None: + return False + filename = mesh.get("filename", "") + if not os.path.join(self.urdf_dir, filename): + return False + return True + + def scale_urdf(self, scale: list): + """_summary_ + + Args: + scale (np.ndarray): [scale_x, scale_y, scale_z] + """ + for link in self.root.findall("link"): + inertial_elem = link.find("inertial") + if inertial_elem: + origin_elem = inertial_elem.find("origin") + if origin_elem is not None: + xyz = origin_elem.get("xyz", "0 0 0").split() + xyz = [str(float(x) * s) for x, s in zip(xyz, scale)] + origin_elem.set("xyz", " ".join(xyz)) + mass_elem = inertial_elem.find("mass") + if mass_elem is not None: + mass = float(mass_elem.get("value", "1.0")) + mass_elem.set("value", str(mass * scale[0] * scale[1] * scale[2])) + + scale_dir = os.path.join(self.urdf_dir, "Scale") + if not os.path.exists(scale_dir): + os.makedirs(scale_dir) + + visual_elem = link.find("visual") + if visual_elem: + origin_elem = visual_elem.find("origin") + if origin_elem is not None: + xyz = origin_elem.get("xyz", "0 0 0").split() + xyz = [str(float(x) * s) for x, s in zip(xyz, scale)] + origin_elem.set("xyz", " ".join(xyz)) + geometry_elem = visual_elem.find("geometry") + if geometry_elem is not None: + mesh_elem = geometry_elem.find("mesh") + mesh_filename = mesh_elem.get("filename", "") + mesh_base_name = os.path.basename(mesh_filename).split(".")[0] + mesh_path = os.path.join(self.urdf_dir, mesh_filename) + if os.path.isfile(mesh_path): + mesh = trimesh.load(mesh_path, process=False) + mesh.apply_scale(scale) + # TODO: Triemsh output .obj file to the same directory will cause material overwrite issue + visual_scale_dir = os.path.join( + scale_dir, f"visual_{mesh_base_name}" + ) + if not os.path.exists(visual_scale_dir): + os.makedirs(visual_scale_dir) + scale_relative_path = os.path.join( + "Scale", f"visual_{mesh_base_name}", "scaled.obj" + ) + scaled_mesh_path = os.path.join( + self.urdf_dir, scale_relative_path + ) + mesh.export(scaled_mesh_path) + mesh_elem.set("filename", scale_relative_path) + + collision_elem = link.find("collision") + if collision_elem: + origin_elem = collision_elem.find("origin") + if origin_elem is not None: + xyz = origin_elem.get("xyz", "0 0 0").split() + xyz = [str(float(x) * s) for x, s in zip(xyz, scale)] + origin_elem.set("xyz", " ".join(xyz)) + geometry_elem = collision_elem.find("geometry") + if geometry_elem is not None: + mesh_elem = geometry_elem.find("mesh") + mesh_filename = mesh_elem.get("filename", "") + mesh_base_name = os.path.basename(mesh_filename).split(".")[0] + mesh_path = os.path.join(self.urdf_dir, mesh_filename) + if os.path.isfile(mesh_path): + mesh = trimesh.load(mesh_path, process=False) + mesh.apply_scale(scale) + # TODO: Triemsh output .obj file to the same directory will cause material overwrite issue + collision_scale_dir = os.path.join( + scale_dir, f"colli_{mesh_base_name}" + ) + if not os.path.exists(collision_scale_dir): + os.makedirs(collision_scale_dir) + scale_relative_path = os.path.join( + "Scale", + f"colli_{mesh_base_name}", + f"scaled_{mesh_base_name}.obj", + ) + scaled_mesh_path = os.path.join( + self.urdf_dir, scale_relative_path + ) + mesh.export(scaled_mesh_path) + mesh_elem.set("filename", scale_relative_path) + + for joint in self.root.findall("joint"): + origin_elem = joint.find("origin") + if origin_elem is not None: + xyz = origin_elem.get("xyz", "0 0 0").split() + xyz = [str(float(x) * s) for x, s in zip(xyz, scale)] + origin_elem.set("xyz", " ".join(xyz)) + if joint.get("type") == "prismatic": + limit_elem = joint.find("limit") + if limit_elem is not None: + lower = float(limit_elem.get("lower", "0.0")) + upper = float(limit_elem.get("upper", "0.0")) + # TODO: can be wrong scale + limit_elem.set("lower", str(lower * scale[0])) + limit_elem.set("upper", str(upper * scale[0])) + + def save_urdf(self, output_urdf_name: str): + # save to new urdf file + output_path = os.path.join(self.urdf_dir, output_urdf_name) + self.urdf.write(output_path) + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser( + description="Create and simulate a camera with gizmo in SimulationManager" + ) + parser.add_argument( + "--urdf_path", + type=str, + default="/home/chenjian/Downloads/StorageFurniture@45661@18/object_fromJson2urdf_decomposed.urdf", + help="Input urdf file path", + ) + parser.add_argument( + "--output_urdf_name", + type=str, + default="object_acd.urdf", + help="Output urdf file name, ", + ) + parser.add_argument( + "--max_convex_hull_num", + type=int, + default=8, + help="Maximum number of convex hulls for decomposition", + ) + parser.add_argument( + "--recompute_inertia", + default=False, + action="store_true", + help="Whether to recompute inertia after convex decomposition", + ) + + parser.add_argument( + "--scale", + type=float, + nargs=3, + default=None, + help="Scale the urdf by [scale_x, scale_y, scale_z]", + ) + + args = parser.parse_args() + generate_urdf_collision_convexes( + args.urdf_path, + args.output_urdf_name, + max_convex_hull_num=args.max_convex_hull_num, + recompute_inertia=args.recompute_inertia, + scale=args.scale, + ) From be54c6a31cbeb963938b19a5f8106e7d2c084dc9 Mon Sep 17 00:00:00 2001 From: chenjian Date: Thu, 22 Jan 2026 20:04:44 +0800 Subject: [PATCH 6/7] update --- embodichain/toolkits/{urdf_modifider => acd}/__init__.py | 0 embodichain/toolkits/{urdf_modifider => acd}/urdf_modifider.py | 3 +-- 2 files changed, 1 insertion(+), 2 deletions(-) rename embodichain/toolkits/{urdf_modifider => acd}/__init__.py (100%) rename embodichain/toolkits/{urdf_modifider => acd}/urdf_modifider.py (99%) diff --git a/embodichain/toolkits/urdf_modifider/__init__.py b/embodichain/toolkits/acd/__init__.py similarity index 100% rename from embodichain/toolkits/urdf_modifider/__init__.py rename to embodichain/toolkits/acd/__init__.py diff --git a/embodichain/toolkits/urdf_modifider/urdf_modifider.py b/embodichain/toolkits/acd/urdf_modifider.py similarity index 99% rename from embodichain/toolkits/urdf_modifider/urdf_modifider.py rename to embodichain/toolkits/acd/urdf_modifider.py index 035f6a87..df80a629 100644 --- a/embodichain/toolkits/urdf_modifider/urdf_modifider.py +++ b/embodichain/toolkits/acd/urdf_modifider.py @@ -330,13 +330,12 @@ def save_urdf(self, output_urdf_name: str): parser.add_argument( "--urdf_path", type=str, - default="/home/chenjian/Downloads/StorageFurniture@45661@18/object_fromJson2urdf_decomposed.urdf", help="Input urdf file path", ) parser.add_argument( "--output_urdf_name", type=str, - default="object_acd.urdf", + default="articulation_acd.urdf", help="Output urdf file name, ", ) parser.add_argument( From 70e95ecdf861f46ffb7a756faca22058dfae6b39 Mon Sep 17 00:00:00 2001 From: chenjian Date: Fri, 23 Jan 2026 10:03:50 +0800 Subject: [PATCH 7/7] update --- docs/source/index.rst | 1 + .../toolkits/convex_decomposition.md | 74 +++++++++++++++++++ docs/source/resources/toolkits/index.rst | 9 +++ 3 files changed, 84 insertions(+) create mode 100644 docs/source/resources/toolkits/convex_decomposition.md create mode 100644 docs/source/resources/toolkits/index.rst diff --git a/docs/source/index.rst b/docs/source/index.rst index c0ac442e..9e0bf7f0 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -45,6 +45,7 @@ Table of Contents resources/robot/index* resources/task/index* + resources/toolkits/index* resources/roadmap.md .. toctree:: diff --git a/docs/source/resources/toolkits/convex_decomposition.md b/docs/source/resources/toolkits/convex_decomposition.md new file mode 100644 index 00000000..fea02800 --- /dev/null +++ b/docs/source/resources/toolkits/convex_decomposition.md @@ -0,0 +1,74 @@ +# URDF Convex Decomposition Tool + +The URDF Convex Decomposition Tool is a utility within EmbodiChain designed to automatically process URDF models for simulation. It handles the decomposition of complex visual meshes into convex collision geometries and provides capabilities for model scaling and inertia recomputation. + +## Key Features + +- **Automated Convex Decomposition**: Uses the CoACD algorithm to decompose concave meshes into multiple convex hulls, essential for stable physics simulation. +- **URDF Modification**: Automatically generates a new URDF file linking to the newly created convex collision meshes. +- **Inertia Handling**: Supports recomputing inertial properties (mass, center of mass, inertia tensor) based on the geometry. +- **Model Scaling**: Allows for scaling the entire robot model (geometry, joints, origins) by specified factors. + +## Method 1: Python API Usage + +The tool provides a high-level function `generate_urdf_collision_convexes` for programmatic access. This is recommended for integrating the decomposition process into larger pipelines or scripts. + +**Parameters:** + +- `urdf_path`: Path to the input URDF file. +- `output_urdf_name`: Filename for the output URDF. +- `max_convex_hull_num`: Maximum number of convex hulls to generate per mesh (default: 16). +- `recompute_inertia`: Whether to recalculate inertial properties (default: False). +- `scale`: Optional numpy array `[x, y, z]` to scale the model. + +```python +from embodichain.toolkits.acd.urdf_modifider import generate_urdf_collision_convexes +import numpy as np + +# Example: Decompose and Scale +generate_urdf_collision_convexes( + urdf_path="./assets/robot.urdf", + output_urdf_name="robot_processed.urdf", + max_convex_hull_num=16, + recompute_inertia=True, + scale=np.array([1.0, 1.0, 1.0]) +) +print("Convex decomposition and inertia update completed.") +``` + +## Method 2: Command Line Interface (CLI) + +The tool can also be run directly from the terminal, which is useful for quick batch processing or standalone usage. + +**Command Structure:** + +```bash +python -m embodichain.toolkits.acd.urdf_modifider [OPTIONS] +``` + +### Argument Descriptions + +| Argument | Type | Default | Description | +|----------|------|---------|-------------| +| `--urdf_path` | str | Required | Path to the source URDF file. | +| `--output_urdf_name` | str | `articulation_acd.urdf` | Name of the generated URDF file. | +| `--max_convex_hull_num` | int | 8 | Maximum number of convex hulls for decomposition. | +| `--recompute_inertia` | flag | False | If present, recomputes inertia based on mesh geometry. | +| `--scale` | float | None | Scale factors (x y z). Example: `--scale 1.5 1.5 1.5`. | + +**Example Usage:** + +```bash +# Basic decomposition +python -m embodichain.toolkits.acd.urdf_modifider \ + --urdf_path ./assets/my_robot.urdf \ + --output_urdf_name my_robot_convex.urdf \ + --max_convex_hull_num 16 + +# Decomposition with scaling and inertia recomputation +python -m embodichain.toolkits.acd.urdf_modifider \ + --urdf_path ./assets/my_robot.urdf \ + --output_urdf_name my_robot_scaled.urdf \ + --recompute_inertia \ + --scale 0.5 0.5 0.5 +``` diff --git a/docs/source/resources/toolkits/index.rst b/docs/source/resources/toolkits/index.rst new file mode 100644 index 00000000..84fba264 --- /dev/null +++ b/docs/source/resources/toolkits/index.rst @@ -0,0 +1,9 @@ +ToolKits +====================== + + +.. toctree:: + :maxdepth: 1 + + convex_decomposition + \ No newline at end of file