simready-forge 0.1.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,14 @@
1
+ """simready-forge: a prompt or a photo in, an NVIDIA SimReady asset out (OpenUSD with materials, physics and joints)."""
2
+
3
+ from importlib.metadata import PackageNotFoundError, version
4
+
5
+ from .kit import Asset
6
+ from .scan import load_scan
7
+ from .validate import validate
8
+
9
+ try:
10
+ __version__ = version("simready-forge")
11
+ except PackageNotFoundError: # a source tree that was never installed
12
+ __version__ = "0.0.0"
13
+
14
+ __all__ = ["Asset", "__version__", "load_scan", "validate"]
simready_forge/cli.py ADDED
@@ -0,0 +1,221 @@
1
+ """The `simready-forge` command: what the plugin's skills and the builder agents run.
2
+
3
+ simready-forge validate <asset.usda>
4
+ simready-forge review <asset.usda> [--out DIR] [--isaac PYTHON]
5
+ simready-forge trellis txt2img|image <prompt|image> <out_dir> [--effort low|medium|high] [--server URL]
6
+ simready-forge views <mesh.glb> --height N [--units mm] --out views.png
7
+ simready-forge qcode "<object class>" | Q<id> [Q<id> ...]
8
+ simready-forge prefs show|set|reset [--data DIR] [--session ID] [--project DIR] [--scope S] [key=value ...]
9
+ simready-forge cost --session ID --project DIR [--since ISO]
10
+ simready-forge example --show | --build DIR
11
+ simready-forge version
12
+ """
13
+
14
+ import argparse
15
+ import json
16
+ import re
17
+ import sys
18
+ import time
19
+ import urllib.parse
20
+ import urllib.request
21
+ from pathlib import Path
22
+
23
+
24
+ def _wikidata(params, need):
25
+ url = "https://www.wikidata.org/w/api.php?" + urllib.parse.urlencode(params | {"language": "en", "format": "json"})
26
+ request = urllib.request.Request(
27
+ url, headers={"User-Agent": "simready-forge (github.com/serionist/simready-forge)"}
28
+ )
29
+ answer = {}
30
+ for attempt in range(3): # Wikidata now and then answers a good query without its results: a retry gets them
31
+ with urllib.request.urlopen(request, timeout=20) as response: # noqa: S310 (a fixed https URL)
32
+ answer = json.loads(response.read())
33
+ if need in answer:
34
+ return answer
35
+ time.sleep(2 * (attempt + 1))
36
+ sys.exit(f"Wikidata answered without {need!r}: {str(answer.get('error', answer))[:300]}")
37
+
38
+
39
+ def _qcode(words):
40
+ """Search for a class, or, given Q-codes, print what each one is: a remembered code is often someone else."""
41
+ if all(re.fullmatch(r"Q\d+", w) for w in words):
42
+ entities = _wikidata(
43
+ {"action": "wbgetentities", "ids": "|".join(words), "props": "labels|descriptions", "languages": "en"},
44
+ "entities",
45
+ )["entities"]
46
+ for qid in words:
47
+ item = entities.get(qid, {})
48
+ label = item.get("labels", {}).get("en", {}).get("value", "(no such item)")
49
+ print(f"{qid:>12} {label}: {item.get('descriptions', {}).get('en', {}).get('value', '')}")
50
+ return 0
51
+ for hit in _wikidata({"action": "wbsearchentities", "search": " ".join(words), "limit": 7}, "search")["search"]:
52
+ print(f"{hit['id']:>12} {hit.get('label', '')}: {hit.get('description', '')}")
53
+ return 0
54
+
55
+
56
+ def _prefs(args):
57
+ from . import prefs
58
+
59
+ where = {"data_dir": args.data, "session": args.session, "project": args.project}
60
+ if args.action == "set":
61
+ prefs.set_values(prefs.parse(args.pairs), args.scope, **where)
62
+ elif args.action == "reset":
63
+ prefs.reset(args.scope, **where)
64
+ values, source = prefs.resolve(**where)
65
+ print(json.dumps({"values": values, "source": source, "remembered": prefs.remembered(**where)}, indent=1))
66
+ return 0
67
+
68
+
69
+ def _example(args):
70
+ from .examples import cabinet
71
+
72
+ if args.show:
73
+ print(Path(cabinet.__file__).read_text(encoding="utf-8"))
74
+ return 0
75
+ return 0 if cabinet.build(args.build)["passed"] else 1
76
+
77
+
78
+ def main(argv=None):
79
+ for stream in (sys.stdout, sys.stderr): # Wikidata labels hold emoji; a Windows console's cp1252 cannot print them
80
+ reconfigure = getattr(stream, "reconfigure", None)
81
+ if reconfigure:
82
+ reconfigure(encoding="utf-8", errors="replace")
83
+ parser = argparse.ArgumentParser(prog="simready-forge", description=(__doc__ or "").split("\n")[0])
84
+ sub = parser.add_subparsers(dest="command", required=True)
85
+ p = sub.add_parser("validate", help="NVIDIA's SimReady validator, Robotics-Prop 4.0.0")
86
+ p.add_argument("asset")
87
+ p.add_argument("--out")
88
+ p = sub.add_parser("inspect", help="the asset's bodies, joints, materials and colliders, read from the USD")
89
+ p.add_argument("asset")
90
+ p.add_argument("--out")
91
+ p = sub.add_parser("poses", help="the review's joint poses without Isaac: overlaps and the lowest point")
92
+ p.add_argument("asset")
93
+ p.add_argument("--out")
94
+ p.add_argument(
95
+ "--empty",
96
+ nargs=6,
97
+ type=float,
98
+ action="append",
99
+ default=[],
100
+ metavar=("X0", "Y0", "Z0", "X1", "Y1", "Z1"),
101
+ help="a box in mm that should stay empty (an opening, a cavity); repeatable",
102
+ )
103
+ p.add_argument(
104
+ "--set",
105
+ action="append",
106
+ default=[],
107
+ metavar="JOINT=VALUE",
108
+ help="put joints at chosen targets too (degrees, mm for a slide), the rest at rest; repeatable",
109
+ )
110
+ p = sub.add_parser("probe", help="Isaac Sim physics probe: settle, tilt, push, lift, load")
111
+ p.add_argument("asset")
112
+ p.add_argument("--out")
113
+ p.add_argument("--isaac", help="Isaac Sim's Python")
114
+ p.add_argument(
115
+ "--tests",
116
+ nargs="+",
117
+ choices=("settle", "tilt", "push", "lift", "load"),
118
+ default=(),
119
+ help="run only these, keeping the others' earlier results",
120
+ )
121
+ p = sub.add_parser("review", help="Isaac Sim renders and PhysX checks (a software preview without Isaac Sim)")
122
+ p.add_argument("asset")
123
+ p.add_argument("--out")
124
+ p.add_argument("--isaac", help="Isaac Sim's Python")
125
+ p = sub.add_parser("trellis", help="local text-to-image and image-to-3D through ComfyUI")
126
+ p.add_argument("rest", nargs=argparse.REMAINDER)
127
+ p = sub.add_parser("views", help="front, side and top views of a generated mesh with a grid, to split it")
128
+ p.add_argument("mesh")
129
+ p.add_argument("--out", required=True)
130
+ p.add_argument("--units", default="mm")
131
+ p.add_argument("--height", type=float)
132
+ p.add_argument("--longest", type=float)
133
+ p.add_argument("--turn", type=float, default=0.0, help="degrees about Z, as load_scan(turn=)")
134
+ p = sub.add_parser("qcode", help="search Wikidata for a class, or check what Q-codes are")
135
+ p.add_argument("query", nargs="+")
136
+ p = sub.add_parser("prefs", help="the remembered choices")
137
+ p.add_argument("action", choices=["show", "set", "reset"])
138
+ p.add_argument("pairs", nargs="*")
139
+ p.add_argument("--scope", default="session")
140
+ p.add_argument("--data")
141
+ p.add_argument("--session")
142
+ p.add_argument("--project")
143
+ p = sub.add_parser("cost", help="tokens and dollars per role from the session transcripts")
144
+ p.add_argument("--session", required=True)
145
+ p.add_argument("--project", required=True)
146
+ p.add_argument("--since")
147
+ p.add_argument("--out")
148
+ p = sub.add_parser("example", help="the worked example: a cabinet with a hinged door and a drawer")
149
+ group = p.add_mutually_exclusive_group(required=True)
150
+ group.add_argument("--show", action="store_true")
151
+ group.add_argument("--build")
152
+ sub.add_parser("version")
153
+ args = parser.parse_args(argv)
154
+
155
+ if args.command == "validate":
156
+ from .validate import validate
157
+
158
+ return 0 if validate(args.asset, args.out)["passed"] else 1
159
+ if args.command == "inspect":
160
+ from .readback import inspect
161
+
162
+ text = json.dumps(inspect(args.asset), indent=1)
163
+ if args.out:
164
+ Path(args.out).parent.mkdir(parents=True, exist_ok=True)
165
+ Path(args.out).write_text(text, encoding="utf-8")
166
+ print(text)
167
+ return 0
168
+ if args.command == "poses":
169
+ from .poses import main as poses
170
+
171
+ chosen = {k: float(v) for k, v in (item.split("=", 1) for item in args.set)}
172
+ return poses(args.asset, args.out, args.empty, chosen)
173
+ if args.command == "probe":
174
+ from .review import probe
175
+
176
+ out = args.out or Path(args.asset).resolve().parent.parent.parent / f"{Path(args.asset).stem}_reports" / "probe"
177
+ report = probe(args.asset, out, args.isaac, tests=args.tests)
178
+ print(json.dumps({k: report.get(k) for k in ("passed", "all_passed", "error")}, indent=1))
179
+ return 0 if report.get("all_passed") else 1
180
+ if args.command == "review":
181
+ from .review import review
182
+
183
+ out = (
184
+ args.out or Path(args.asset).resolve().parent.parent.parent / f"{Path(args.asset).stem}_reports" / "review"
185
+ )
186
+ report = review(args.asset, out, args.isaac)
187
+ print(json.dumps(report, indent=1))
188
+ return 0 if report.get("passed", report.get("renderer") == "software") else 1
189
+ if args.command == "trellis":
190
+ from . import trellis
191
+
192
+ return trellis.main(args.rest)
193
+ if args.command == "views":
194
+ from .scan import load_scan
195
+
196
+ load_scan(args.mesh, args.units, height=args.height, longest=args.longest, turn=args.turn).views(args.out)
197
+ print(args.out)
198
+ return 0
199
+ if args.command == "qcode":
200
+ return _qcode(args.query)
201
+ if args.command == "prefs":
202
+ return _prefs(args)
203
+ if args.command == "cost":
204
+ from .cost import report
205
+
206
+ result = report(args.session, args.project, args.since)
207
+ text = json.dumps(result, indent=1)
208
+ if args.out:
209
+ Path(args.out).write_text(text, encoding="utf-8")
210
+ print(text)
211
+ return 0
212
+ if args.command == "example":
213
+ return _example(args)
214
+ from . import __version__
215
+
216
+ print(__version__)
217
+ return 0
218
+
219
+
220
+ if __name__ == "__main__":
221
+ sys.exit(main())
simready_forge/cost.py ADDED
@@ -0,0 +1,120 @@
1
+ """What a run cost: tokens and dollars per role, read from Claude Code's session transcripts at list prices.
2
+
3
+ Claude Code keeps a session at ~/.claude/projects/<project>/<session>.jsonl and its agents under
4
+ <session>/subagents/. The entry format is internal to Claude Code, so this is best effort and says so. An agent's role
5
+ comes from the marker its brief carries: `[simready-forge role: builder]` and so on.
6
+ """
7
+
8
+ import json
9
+ import re
10
+ from datetime import datetime
11
+ from pathlib import Path
12
+
13
+ # $ per million tokens: input, output, cache read, 5-minute cache write, 1-hour cache write (Anthropic list prices)
14
+ PRICES = {
15
+ "claude-fable-5-1": (10.0, 50.0, 0.25, 12.5, 20.0),
16
+ "claude-opus-5-5": (4.0, 20.0, 0.20, 5.0, 8.0),
17
+ "claude-sonnet-5-5": (2.0, 10.0, 0.20, 2.5, 4.0),
18
+ "claude-haiku-5-5": (0.10, 0.50, 0.01, 0.125, 0.20),
19
+ }
20
+ ROLE = re.compile(r"\[simready-forge role: ([a-z-]+)\]")
21
+
22
+
23
+ def project_folder(project_dir):
24
+ """Claude Code's folder name for a working directory: every non-alphanumeric character becomes '-'."""
25
+ return re.sub(r"[^A-Za-z0-9]", "-", str(project_dir))
26
+
27
+
28
+ def _stamp(entry):
29
+ value = entry.get("timestamp")
30
+ return datetime.fromisoformat(value.replace("Z", "+00:00")) if value else None
31
+
32
+
33
+ def _usage(path, since):
34
+ calls, model, role = {}, None, None
35
+ for line in Path(path).read_text(encoding="utf-8", errors="replace").splitlines():
36
+ try:
37
+ entry = json.loads(line)
38
+ except ValueError:
39
+ continue
40
+ when = _stamp(entry)
41
+ message = entry.get("message") or {}
42
+ if role is None and entry.get("type") == "user":
43
+ content = message.get("content")
44
+ text = (
45
+ content
46
+ if isinstance(content, str)
47
+ else "".join(b.get("text", "") for b in content or [] if isinstance(b, dict))
48
+ )
49
+ found = ROLE.search(text)
50
+ role = found.group(1) if found else "agent"
51
+ if since and when and when < since:
52
+ continue
53
+ if entry.get("type") == "assistant" and message.get("usage"):
54
+ calls[message.get("id") or entry.get("uuid")] = message["usage"]
55
+ model = message.get("model") or model
56
+ return calls, model, role
57
+
58
+
59
+ def _price(calls, model):
60
+ usage: dict[str, float | None] = {
61
+ "calls": len(calls),
62
+ "input": 0,
63
+ "output": 0,
64
+ "cache_read": 0,
65
+ "cache_write_5m": 0,
66
+ "cache_write_1h": 0,
67
+ }
68
+ tokens: dict[str, int] = dict.fromkeys(("input", "output", "cache_read", "cache_write_5m", "cache_write_1h"), 0)
69
+ for u in calls.values():
70
+ tokens["input"] += u.get("input_tokens", 0)
71
+ tokens["output"] += u.get("output_tokens", 0)
72
+ tokens["cache_read"] += u.get("cache_read_input_tokens", 0)
73
+ split = u.get("cache_creation") or {}
74
+ tokens["cache_write_1h"] += split.get("ephemeral_1h_input_tokens", 0)
75
+ tokens["cache_write_5m"] += (
76
+ split.get("ephemeral_5m_input_tokens", 0) if split else u.get("cache_creation_input_tokens", 0)
77
+ )
78
+ for key, value in tokens.items():
79
+ usage[key] = value
80
+ rates = PRICES.get(model or "")
81
+ usage["usd"] = round(sum(tokens[k] * r for k, r in zip(tokens, rates, strict=True)) / 1e6, 4) if rates else None
82
+ return usage
83
+
84
+
85
+ def report(session, project_dir, since=None, claude_home=None):
86
+ """Usage per role since a time (ISO), for the session and its agents."""
87
+ home = Path(claude_home) if claude_home else Path.home() / ".claude"
88
+ base = home / "projects" / project_folder(project_dir)
89
+ start = datetime.fromisoformat(since.replace("Z", "+00:00")) if since else None
90
+ rows = []
91
+ main = base / f"{session}.jsonl"
92
+ if main.is_file():
93
+ calls, model, _ = _usage(main, start)
94
+ rows.append({"role": "coordinator", "model": model} | _price(calls, model))
95
+ for path in sorted((base / str(session) / "subagents").glob("agent-*.jsonl")):
96
+ calls, model, role = _usage(path, start)
97
+ if calls:
98
+ rows.append({"role": role, "model": model, "agent": path.stem} | _price(calls, model))
99
+ totals = {
100
+ "usd": round(sum(r["usd"] or 0 for r in rows), 4),
101
+ "unpriced_models": sorted({r["model"] for r in rows if r["usd"] is None and r["model"]}),
102
+ "tokens": sum(
103
+ r["input"] + r["output"] + r["cache_read"] + r["cache_write_5m"] + r["cache_write_1h"] for r in rows
104
+ ),
105
+ }
106
+ by_role = {}
107
+ for r in rows:
108
+ entry = by_role.setdefault(r["role"], {"usd": 0.0, "calls": 0, "agents": 0})
109
+ entry["usd"] = round(entry["usd"] + (r["usd"] or 0), 4)
110
+ entry["calls"] += r["calls"]
111
+ entry["agents"] += 1
112
+ return {
113
+ "session": session,
114
+ "since": since,
115
+ "rows": rows,
116
+ "by_role": by_role,
117
+ "totals": totals,
118
+ "basis": "Anthropic list prices per million tokens; read from Claude Code transcripts (internal format, "
119
+ "best effort)",
120
+ }
File without changes
@@ -0,0 +1,124 @@
1
+ """Worked example: a 400 mm oak bedside cabinet with a hinged door and a drawer on runners.
2
+
3
+ simready-forge example --show print this file
4
+ simready-forge example --build <dir> build it, export it and validate it
5
+
6
+ The pattern every build script follows: real dimensions, build123d shapes in the assembled pose, one body per moving
7
+ part, materials with their physics values, joints with limits, then save() and validate().
8
+ """
9
+
10
+ import numpy as np
11
+ from build123d import Box, Cylinder, Pos, Rot
12
+
13
+ from simready_forge import Asset, validate
14
+
15
+ W, D, H, T = 400, 350, 500, 18 # mm: width (X), depth (Y, front at -Y), height (Z), board thickness
16
+
17
+
18
+ def board(x0, x1, y0, y1, z0, z1):
19
+ return Pos((x0 + x1) / 2, (y0 + y1) / 2, (z0 + z1) / 2) * Box(x1 - x0, y1 - y0, z1 - z0)
20
+
21
+
22
+ def knob(x, z):
23
+ """A round knob on the front face, sticking out toward -Y."""
24
+ return Pos(x, -D / 2 - T, z) * Rot(90, 0, 0) * (Pos(0, 0, 6) * Cylinder(5, 12) + Pos(0, 0, 16) * Cylinder(12, 8))
25
+
26
+
27
+ def wood_texture(size=512, seed=3):
28
+ """Oak grain along the image's width, tileable both ways: integer frequencies only, so it repeats without seams."""
29
+ rng = np.random.default_rng(seed)
30
+ u, v = np.meshgrid(np.arange(size) / size, np.arange(size) / size)
31
+
32
+ def wave(amplitude, fu, fv): # integer frequencies, so every term repeats across the tile
33
+ return amplitude * np.sin(2 * np.pi * (fu * u + fv * v) + rng.uniform(0, 2 * np.pi))
34
+
35
+ # rings: their spacing varies down the board (a third to twice the mean) and they wander gently along it
36
+ spacing = sum(wave(a, 0, k) / (2 * np.pi * k) for a, k in ((5.0, 1), (3.0, 2), (2.0, 3)))
37
+ wander = sum(wave(rng.uniform(0.04, 0.1), fu, fv) for fu, fv in ((1, 0), (1, 1), (2, -1), (1, 2), (3, 1)))
38
+ phase = (12 * v + spacing + wander) % 1.0
39
+ strength = 0.6 + 0.4 * (0.5 + 0.5 * (wave(1, 0, 5) + wave(1, 0, 7)) / 2) # some late-wood lines darker
40
+ late = strength * np.exp(-((np.minimum(phase, 1 - phase) / 0.06) ** 2))
41
+ # long streaks: noise blurred along the grain by a Gaussian that wraps round, so the edges still meet
42
+ distance = np.minimum(np.arange(size), size - np.arange(size))
43
+ kernel = np.fft.fft(np.exp(-0.5 * (distance / 24.0) ** 2))
44
+ streaks = np.real(np.fft.ifft(np.fft.fft(rng.normal(0, 1, (size, size)), axis=1) * kernel, axis=1))
45
+ streaks /= streaks.std()
46
+ g = np.clip(0.72 - 0.38 * late + 0.07 * streaks + 0.03 * rng.normal(0, 1, (size, size)), 0, 1)[..., None]
47
+ return (np.array([120, 78, 44]) * (1 - g) + np.array([196, 150, 102]) * g).astype(np.uint8)
48
+
49
+
50
+ def build(out_dir="simready_assets", formats=("usd", "urdf", "glb")):
51
+ asset = Asset(
52
+ "bedside_cabinet",
53
+ out_dir,
54
+ label="nightstand",
55
+ qcode="Q667999",
56
+ units="mm",
57
+ category="furniture",
58
+ description="400 mm oak bedside cabinet: a drawer on runners above a hinged door, steel knobs",
59
+ )
60
+ oak = asset.material(
61
+ "oak",
62
+ texture=wood_texture(),
63
+ uv_scale=3.0,
64
+ lengthwise=True,
65
+ roughness=0.55,
66
+ friction=(0.5, 0.4),
67
+ restitution=0.1,
68
+ density=700,
69
+ label="wood",
70
+ nonvisual=("wood", "clearcoat"),
71
+ )
72
+ steel = asset.material(
73
+ "brushed_steel",
74
+ color=(0.75, 0.75, 0.77),
75
+ metallic=1.0,
76
+ roughness=0.35,
77
+ friction=(0.4, 0.3),
78
+ restitution=0.2,
79
+ density=7850,
80
+ label="steel",
81
+ nonvisual=("steel", "none"),
82
+ )
83
+
84
+ # everything that does not move is one body
85
+ carcass = asset.body("carcass", label="cabinet body")
86
+ panels = {
87
+ "left": (-W / 2, -W / 2 + T, -D / 2, D / 2, 0, H),
88
+ "right": (W / 2 - T, W / 2, -D / 2, D / 2, 0, H),
89
+ "top": (-W / 2, W / 2, -D / 2, D / 2, H - T, H),
90
+ "bottom": (-W / 2 + T, W / 2 - T, -D / 2, D / 2, 40, 40 + T),
91
+ "shelf": (-W / 2 + T, W / 2 - T, -D / 2, D / 2, 330, 330 + T),
92
+ "back": (-W / 2 + T, W / 2 - T, D / 2 - 8, D / 2, 40, H - T),
93
+ }
94
+ for name, dims in panels.items():
95
+ asset.add(carcass, name, board(*dims), oak, collider="box")
96
+
97
+ # the door under the shelf, hinged on its left edge: it opens outward with negative angles about +Z
98
+ door = asset.body("door")
99
+ asset.add(
100
+ door,
101
+ "panel",
102
+ board(-W / 2 + T + 1, W / 2 - T - 1, -D / 2 - T, -D / 2, 40 + T + 1, 330 - 1),
103
+ oak,
104
+ collider="box",
105
+ )
106
+ asset.add(door, "knob", knob(W / 2 - T - 40, 200), steel)
107
+ asset.revolute("door_hinge", carcass, door, anchor=(-W / 2 + T, -D / 2 - T / 2, 200), axis="Z", limits=(-110, 0))
108
+
109
+ # the drawer above the shelf, pulling out toward -Y
110
+ drawer = asset.body("drawer")
111
+ z0, z1, x0, x1 = 330 + T + 2, H - T - 2, -W / 2 + T + 2, W / 2 - T - 2
112
+ sides = {
113
+ "front": (x0, x1, -D / 2 - T, -D / 2, z0, z1),
114
+ "floor": (x0, x1, -D / 2, D / 2 - 30, z0, z0 + 8),
115
+ "side_l": (x0, x0 + 12, -D / 2, D / 2 - 30, z0 + 8, z1 - 20),
116
+ "side_r": (x1 - 12, x1, -D / 2, D / 2 - 30, z0 + 8, z1 - 20),
117
+ "rear": (x0, x1, D / 2 - 42, D / 2 - 30, z0 + 8, z1 - 20),
118
+ }
119
+ for name, dims in sides.items():
120
+ asset.add(drawer, name, board(*dims), oak, collider="box")
121
+ asset.add(drawer, "knob", knob(0, (z0 + z1) / 2), steel)
122
+ asset.prismatic("drawer_slide", carcass, drawer, anchor=(0, 0, (z0 + z1) / 2), axis="Y", limits=(-250, 0))
123
+
124
+ return validate(asset.save(formats=formats))
@@ -0,0 +1,170 @@
1
+ """Exports of a saved asset: URDF (links, inertia, joints, OBJ visuals, STL hull colliders) and GLB (PBR)."""
2
+
3
+ import math
4
+ import shutil
5
+ from pathlib import Path
6
+ from typing import cast
7
+ from xml.sax.saxutils import quoteattr
8
+
9
+ import numpy as np
10
+
11
+ INERTIA = (("ixx", 0, 0), ("ixy", 0, 1), ("ixz", 0, 2), ("iyy", 1, 1), ("iyz", 1, 2), ("izz", 2, 2))
12
+ AXIS_VECTORS = {"X": "1 0 0", "Y": "0 1 0", "Z": "0 0 1"}
13
+
14
+
15
+ def _fmt(values):
16
+ return " ".join(f"{float(v):.9g}" for v in values)
17
+
18
+
19
+ def _damping(joint):
20
+ """URDF damping is per radian for a hinge (N*m*s/rad); the kit's, like UsdPhysics', is per degree."""
21
+ damping = joint["drive"]["damping"]
22
+ return damping * math.degrees(1.0) if joint["kind"] == "revolute" else damping
23
+
24
+
25
+ def trees(model):
26
+ """The joint trees: [(root body, [(joint, parent, child), ...])]. A body with no joints is a tree of its own."""
27
+ children = {j["body1"].name for j in model["joints"]}
28
+ out = []
29
+ for root in [b["name"] for b in model["bodies"] if b["name"] not in children]:
30
+ edges, frontier = [], [root]
31
+ while frontier:
32
+ parent = frontier.pop(0)
33
+ for j in model["joints"]:
34
+ if j["body0"].name == parent:
35
+ edges.append((j, parent, j["body1"].name))
36
+ frontier.append(j["body1"].name)
37
+ out.append((root, edges))
38
+ return out
39
+
40
+
41
+ def inertia(body):
42
+ """The body's centre of mass and inertia tensor about it, from its collider hulls scaled to its mass."""
43
+ import trimesh
44
+
45
+ hulls = [trimesh.Trimesh(v, f) for part in body["parts"] for v, f in part["hulls"]]
46
+ if not hulls:
47
+ hulls = [trimesh.Trimesh(part["points"], part["tris"]).convex_hull for part in body["parts"]]
48
+ merged = cast(trimesh.Trimesh, trimesh.util.concatenate(hulls))
49
+ if merged.volume <= 0:
50
+ merged = merged.convex_hull
51
+ merged.density = body["mass"] / merged.volume
52
+ return np.asarray(merged.center_mass), np.asarray(merged.moment_inertia)
53
+
54
+
55
+ def urdf(model, folder, warnings):
56
+ """One URDF per joint tree; a link's frame sits at the anchor of the joint that carries it, as URDF expects."""
57
+ import trimesh
58
+
59
+ folder = Path(folder)
60
+ if folder.exists():
61
+ shutil.rmtree(folder)
62
+ (folder / "meshes").mkdir(parents=True)
63
+ bodies = {b["name"]: b for b in model["bodies"]}
64
+ forest = trees(model)
65
+ if len(forest) > 1:
66
+ warnings.append(f"URDF: {len(forest)} separate trees (loose bodies) written as {len(forest)} files")
67
+ files = []
68
+ for index, (root, edges) in enumerate(forest):
69
+ name = model["name"] if len(forest) == 1 else f"{model['name']}_{index}"
70
+ frame = {root: bodies[root]["origin"]}
71
+ for j, _, child in edges:
72
+ frame[child] = j["anchor"]
73
+ lines = [f"<robot name={quoteattr(name)}>"]
74
+ for link in [root] + [child for _, _, child in edges]:
75
+ body = bodies[link]
76
+ shift = body["origin"] - frame[link] # body-local points -> link frame
77
+ com, tensor = inertia(body)
78
+ lines += [
79
+ f" <link name={quoteattr(link)}>",
80
+ " <inertial>",
81
+ f' <origin xyz="{_fmt(com + shift)}" rpy="0 0 0"/>',
82
+ f' <mass value="{body["mass"]:.6g}"/>',
83
+ " <inertia " + " ".join(f'{k}="{tensor[i, j]:.9g}"' for k, i, j in INERTIA) + "/>",
84
+ " </inertial>",
85
+ ]
86
+ for part in body["parts"]:
87
+ m = part["material"]
88
+ if part.get("visible", True): # an invisible part is a collider only
89
+ obj = folder / "meshes" / f"{link}_{part['name']}.obj"
90
+ trimesh.Trimesh(part["points"] + shift, part["tris"], process=False).export(obj)
91
+ rgba = (*[float(c) for c in m.color[:3]], float(m.opacity))
92
+ lines += [
93
+ " <visual>",
94
+ f' <geometry><mesh filename="meshes/{obj.name}"/></geometry>',
95
+ f' <material name={quoteattr(m.name)}><color rgba="{_fmt(rgba)}"/></material>',
96
+ " </visual>",
97
+ ]
98
+ for k, (v, f) in enumerate(part["hulls"]):
99
+ stl = folder / "meshes" / f"{link}_{part['name']}_collider{k}.stl"
100
+ trimesh.Trimesh(v + shift, f, process=False).export(stl)
101
+ lines.append(
102
+ f' <collision><geometry><mesh filename="meshes/{stl.name}"/></geometry></collision>'
103
+ )
104
+ lines.append(" </link>")
105
+ for j, parent, child in edges:
106
+ lo, hi = j["limits"]
107
+ if j["kind"] == "revolute":
108
+ lo, hi = math.radians(lo), math.radians(hi)
109
+ free = not (math.isfinite(lo) and math.isfinite(hi)) # a free spin is URDF's continuous joint
110
+ limit = (
111
+ ' <limit effort="100" velocity="2"/>'
112
+ if free
113
+ else (f' <limit lower="{lo:.9g}" upper="{hi:.9g}" effort="100" velocity="2"/>')
114
+ )
115
+ lines += [
116
+ f' <joint name={quoteattr(j["name"])} type="{"continuous" if free else j["kind"]}">',
117
+ f" <parent link={quoteattr(parent)}/>",
118
+ f" <child link={quoteattr(child)}/>",
119
+ f' <origin xyz="{_fmt(j["anchor"] - frame[parent])}" rpy="0 0 0"/>',
120
+ f' <axis xyz="{AXIS_VECTORS[j["axis"]]}"/>',
121
+ limit,
122
+ f' <dynamics damping="{_damping(j):.6g}" friction="0"/>',
123
+ " </joint>",
124
+ ]
125
+ lines.append("</robot>")
126
+ path = folder / f"{name}.urdf"
127
+ path.write_text("\n".join(lines) + "\n", encoding="utf-8")
128
+ files.append(path)
129
+ return files
130
+
131
+
132
+ def glb(model, path):
133
+ """A GLB of the rest pose with PBR materials: base colour textures where the material has one."""
134
+ import trimesh
135
+ from PIL import Image
136
+ from trimesh.visual import TextureVisuals
137
+ from trimesh.visual.material import PBRMaterial
138
+
139
+ path = Path(path)
140
+ path.parent.mkdir(parents=True, exist_ok=True)
141
+ scene = trimesh.Scene()
142
+ materials = {}
143
+ for body in model["bodies"]:
144
+ for part in [p for p in body["parts"] if p.get("visible", True)]:
145
+ m = part["material"]
146
+ if m.name not in materials:
147
+ image = Image.open(m.files["base_color"]).convert("RGB") if "base_color" in m.files else None
148
+ factor = (
149
+ None if image is not None else [*(np.array(m.color[:3]) * 255).astype(int), int(255 * m.opacity)]
150
+ )
151
+ materials[m.name] = PBRMaterial(
152
+ name=m.name,
153
+ baseColorFactor=factor,
154
+ baseColorTexture=image,
155
+ metallicFactor=float(m.metallic),
156
+ roughnessFactor=float(m.roughness),
157
+ alphaMode="BLEND" if m.opacity < 1 else "OPAQUE",
158
+ )
159
+ # one vertex per triangle corner, so the face-varying UVs survive as glTF vertex UVs
160
+ corners = (part["points"] + body["origin"])[part["tris"]].reshape(-1, 3)
161
+ faces = np.arange(len(corners)).reshape(-1, 3)
162
+ visual = TextureVisuals(uv=np.asarray(part["uv"]), material=materials[m.name])
163
+ mesh = trimesh.Trimesh(corners, faces, visual=visual, process=False)
164
+ scene.add_geometry(
165
+ mesh, node_name=f"{body['name']}/{part['name']}", geom_name=f"{body['name']}_{part['name']}"
166
+ )
167
+ # glTF is Y-up: turn the Z-up asset so viewers show it standing
168
+ scene.apply_transform(trimesh.transformations.rotation_matrix(-math.pi / 2, [1, 0, 0]))
169
+ scene.export(path)
170
+ return path