Feature viser collision viz

foam/utility/__init__.py

@@ -103,6 +103,13 @@ def load_urdf(urdf_path: Path) -> URDFDict:
         return xml
 
 
+def load_srdf(srdf_path: Path) -> URDFDict:
+    with open(srdf_path, 'r') as f:
+        xml = xmltodict.parse(f.read())
+        xml['robot']['@path'] = srdf_path
+        return xml
+
+
 def get_urdf_primitives(urdf: URDFDict, shrinkage: float = 1.) -> list[URDFPrimitive]:
     primitives = []
     for link in urdf['robot']['link']:
@@ -256,6 +263,45 @@ def set_urdf_spheres(urdf: URDFDict, spheres):
     print(f"spheres: {total_spheres}")
 
 
+def set_srdf_link_sphere_approximations(srdf: URDFDict, spheres):
+    spheres_by_link = {}
+    total_spheres = 0
+    for key, spherization in spheres.items():
+        link_name = key.split('::')[0]
+        if link_name not in spheres_by_link:
+            spheres_by_link[link_name] = []
+
+        spheres_by_link[link_name].extend(spherization.spheres)
+        total_spheres += len(spherization.spheres)
+
+    approximations = []
+    for link_name in sorted(spheres_by_link.keys()):
+        link_spheres = [
+            {
+                '@center': ' '.join(map(str, sphere.origin.tolist())),
+                '@radius': sphere.radius,
+            }
+            for sphere in spheres_by_link[link_name]
+        ]
+
+        approximations.append(
+            {
+                '@link': link_name,
+                'sphere': link_spheres,
+            }
+        )
+
+    if approximations:
+        srdf['robot']['link_sphere_approximation'] = approximations
+    elif 'link_sphere_approximation' in srdf['robot']:
+        del srdf['robot']['link_sphere_approximation']
+
+
 def save_urdf(urdf: URDFDict, filename: Path):
     with open(filename, 'w') as f:
         f.write(xmltodict.unparse(urdf, pretty = True))
+
+
+def save_srdf(srdf: URDFDict, filename: Path):
+    with open(filename, 'w') as f:
+        f.write(xmltodict.unparse(srdf, pretty = True))

pyproject.toml

@@ -1,58 +1,66 @@
-[build-system]
-requires = ["setuptools>=61.0"]
-build-backend = "setuptools.build_meta"
-
-[project]
-name = "foam"
-version = "0.1.1"
-authors = [
-  { name="Zachary Kingston", email="zkingston@purude.edu" },
-]
-description  = "Interface for creating spherical approximation of meshes"
-readme = "README.md"
-requires-python = ">=3.10"
-classifiers = [
-    "Programming Language :: Python :: 3",
-    "License :: OSI Approved :: MIT License",
-    "Operating System :: OS Independent",
-]
-
-dependencies = [
-    "trimesh",
-    "scipy",
-    "vhacdx"
-]
-
-[project.urls]
-"Homepage" = "https://github.com/KavrakiLab/foam"
-"Bug Tracker" = "https://github.com/KavrakiLab/foam/issues"
-
-[tool.setuptools.packages.find]
-
-[tool.ruff]
-select = ["E", "F", "W", "D", "UP", "B", "A", "C4", "PIE", "RET", "SIM", "ARG", "PTH", "PLE", "PLR", "PLW", "NPY" ]
-ignore = []
-
-fixable = ["E", "F", "W", "D", "UP", "B", "A", "C4", "PIE", "RET", "SIM", "ARG", "PTH", "PLE", "PLR", "PLW", "NPY" ]
-unfixable = []
-exclude = [
-    ".eggs",
-    ".git",
-    ".mypy_cache",
-    ".ruff_cache",
-]
-line-length = 110
-# Allow unused variables when underscore-prefixed.
-dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$"
-target-version = "py310"
-
-[tool.pyright]
-include = ["."]
-exclude = [
-    "**/__pycache__",
-    "env",
-    ".eggs",
-    ".git",
-]
-pythonVersion = "3.10"
-pythonPlatform = "Linux"
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "foam"
+version = "0.1.1"
+authors = [
+  { name="Zachary Kingston", email="zkingston@purude.edu" },
+]
+description  = "Interface for creating spherical approximation of meshes"
+readme = "README.md"
+requires-python = ">=3.10"
+classifiers = [
+    "Programming Language :: Python :: 3",
+    "License :: OSI Approved :: MIT License",
+    "Operating System :: OS Independent",
+]
+
+dependencies = [
+    "trimesh",
+    "scipy",
+    "vhacdx",
+]
+
+[project.optional-dependencies]
+viz = [
+    "viser",
+    "yourdfpy",
+    "matplotlib",
+    "pyglet",
+]
+
+[project.urls]
+"Homepage" = "https://github.com/KavrakiLab/foam"
+"Bug Tracker" = "https://github.com/KavrakiLab/foam/issues"
+
+[tool.setuptools.packages.find]
+
+[tool.ruff]
+select = ["E", "F", "W", "D", "UP", "B", "A", "C4", "PIE", "RET", "SIM", "ARG", "PTH", "PLE", "PLR", "PLW", "NPY" ]
+ignore = []
+
+fixable = ["E", "F", "W", "D", "UP", "B", "A", "C4", "PIE", "RET", "SIM", "ARG", "PTH", "PLE", "PLR", "PLW", "NPY" ]
+unfixable = []
+exclude = [
+    ".eggs",
+    ".git",
+    ".mypy_cache",
+    ".ruff_cache",
+]
+line-length = 110
+# Allow unused variables when underscore-prefixed.
+dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$"
+target-version = "py310"
+
+[tool.pyright]
+include = ["."]
+exclude = [
+    "**/__pycache__",
+    "env",
+    ".eggs",
+    ".git",
+]
+pythonVersion = "3.10"
+pythonPlatform = "Linux"

requirements.txt

@@ -8,11 +8,13 @@ numpy==2.2.2
 packaging==24.2
 pillow==11.1.0
 pyglet==1.5.31
-pybullet==3.1.7
 pyparsing==3.2.1
 python-dateutil==2.9.0.post0
 scipy==1.15.1
 six==1.17.0
 termcolor==2.5.0
 trimesh==4.5.3
+vhacdx==0.0.10
+viser==1.0.21
 xmltodict==0.14.2
+yourdfpy==0.0.60

scripts/generate_sphere_urdf.py

@@ -135,8 +135,16 @@ def main(
         for primitive in primitives
         }
 
-    set_urdf_spheres(urdf, mesh_spheres | primitive_spheres)
-    save_urdf(urdf, Path(output))
+    all_spheres = mesh_spheres | primitive_spheres
+
+    output_path = Path(output)
+    if output_path.suffix.lower() == ".srdf":
+        srdf = load_srdf(output_path)
+        set_srdf_link_sphere_approximations(srdf, all_spheres)
+        save_srdf(srdf, output_path)
+    else:
+        set_urdf_spheres(urdf, all_spheres)
+        save_urdf(urdf, output_path)
 
 
 if __name__ == "__main__":