78 lines
2.9 KiB
Python
78 lines
2.9 KiB
Python
"""One-time, ROS-free export of this repository's known description macros.
|
|
|
|
This is deliberately not a general Xacro interpreter. Runtime uses the emitted URDFs.
|
|
"""
|
|
from copy import deepcopy
|
|
from pathlib import Path
|
|
import re
|
|
import shutil
|
|
import xml.etree.ElementTree as ET
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
SOURCE = ROOT / "iiwa_description"
|
|
DEST = ROOT / "iiwa_genesis_description"
|
|
NS = "{http://www.ros.org/wiki/xacro}"
|
|
|
|
|
|
def export():
|
|
props = {}
|
|
for name in ("params", "meshes"):
|
|
for node in ET.parse(SOURCE / f"urdf/{name}.xacro").getroot():
|
|
if node.tag == NS + "property":
|
|
props[node.get("name")] = node.get("value")
|
|
|
|
def substitute(value, variables):
|
|
for _ in range(8):
|
|
updated = re.sub(r"\$\{([^}]+)\}", lambda m: variables[m[1]], value)
|
|
if updated == value:
|
|
return value
|
|
value = updated
|
|
raise ValueError("Unresolved variable")
|
|
|
|
macros = {n.get("name"): n for n in ET.parse(SOURCE / "urdf/macros.xacro").getroot()}
|
|
|
|
def expand(node):
|
|
if not node.tag.startswith(NS):
|
|
return [deepcopy(node)]
|
|
name = node.tag[len(NS):]
|
|
if name == "include":
|
|
return []
|
|
result = []
|
|
for template in macros[name]:
|
|
item = deepcopy(template)
|
|
for child in item.iter():
|
|
child.attrib = {k: substitute(v, props | node.attrib) for k, v in child.attrib.items()}
|
|
result.append(item)
|
|
return result
|
|
|
|
def write(robot, path):
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
for mesh in robot.iter("mesh"):
|
|
source = mesh.get("filename")
|
|
prefix = "package://iiwa_description/resource/meshes/"
|
|
if not source.startswith(prefix):
|
|
raise ValueError(source)
|
|
relative = source[len(prefix):]
|
|
target = DEST / "urdf/meshes" / relative
|
|
target.parent.mkdir(parents=True, exist_ok=True)
|
|
shutil.copy2(SOURCE / "resource/meshes" / relative, target)
|
|
mesh.set("filename", ("meshes/" if path.parent.name == "urdf" else "../urdf/meshes/") + relative)
|
|
ET.indent(robot)
|
|
ET.ElementTree(robot).write(path, encoding="utf-8", xml_declaration=True)
|
|
|
|
base = ET.Element("robot", name="iiwa7")
|
|
for name in ("links", "joints"):
|
|
for node in ET.parse(SOURCE / f"urdf/{name}.xacro").getroot():
|
|
base.extend(expand(node))
|
|
write(deepcopy(base), DEST / "urdf/iiwa_without_gripper.urdf")
|
|
for node in ET.parse(SOURCE / "urdf/tools/patron.xacro").getroot():
|
|
if node.tag in ("link", "joint"):
|
|
base.append(deepcopy(node))
|
|
write(base, DEST / "urdf/iiwa_with_gripper.urdf")
|
|
write(ET.parse(SOURCE / "urdf/table/table.urdf").getroot(), DEST / "table/table.urdf")
|
|
shutil.copy2(SOURCE / "LICENSE", DEST / "LICENSE")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
export()
|