Skip to content

3D modeling in Python, built for LLMs.

17.1 KB
Raw Blame
"""Export as USD: the URDF's tree as text, in UsdGeom and UsdPhysics. The stage is
read back by USD's own library (`usd_read.py`, under a Python with `usd-core`, never
bpy's) and held to the URDF of the same program — its names, pivots, axes and ranges —
then posed by its own joints, as `test_export_joints` poses the URDF."""
import json
import math
import os
import re
import subprocess
import tempfile
import unittest
from pathlib import Path

import bpy
from mathutils import Matrix

import sand3d as s3d
from sand3d import _core
from test_export_joints import axis_angle, machine, mat, mul, obj_points, urdf_fk

HERE = Path(__file__).resolve().parent
#: A Python with usd-core: $SAND3D_USD_PYTHON, which must then work, or .cache/venv-usd's.
USD_PYTHON = os.environ.get("SAND3D_USD_PYTHON") or str(HERE.parent / ".cache/venv-usd/bin/python")


def read_back(path):
    if not os.environ.get("SAND3D_USD_PYTHON") and not Path(USD_PYTHON).exists():
        raise unittest.SkipTest("no Python with usd-core: set SAND3D_USD_PYTHON")
    done = subprocess.run([USD_PYTHON, str(HERE / "usd_read.py"), str(path)], capture_output=True, text=True,
                          timeout=300)
    if done.returncode:
        raise AssertionError(f"usd_read.py failed:\n{done.stderr[-3000:]}")
    return json.loads(done.stdout)


def quat_matrix(q):
    w, x, y, z = q
    return [[1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
            [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
            [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)]]


def local(pose):
    return mat(quat_matrix(pose["orientation"]), pose["position"])


def inverse(m):
    r = [[m[c][rr] for c in range(3)] for rr in range(3)]
    return mat(r, [-sum(r[i][k] * m[k][3] for k in range(3)) for i in range(3)])


def unit(axis):
    return [1.0 if a == axis else 0.0 for a in "XYZ"]


def usd_fk(stage, values):
    """{body path: world matrix} for joint `values` (USD units: degrees, metres; by joint
    name), mimic joints followed."""
    world = {p: b["world"] for p, b in stage["bodies"].items()}
    left = dict(stage["joints"])
    while left:
        ready = [p for p, j in left.items() if j["body0"] not in [k["body1"] for k in left.values()]]
        for p in ready:
            j = left.pop(p)
            name = p.rsplit("/", 1)[1]
            q = values.get(name, 0.0)
            if j["mimic"]:
                q = j["mimic"]["coef0"] + j["mimic"]["coef1"] * values.get(j["mimic"]["joint"].rsplit("/", 1)[1], 0.0)
            a = unit(j["axis"])
            if j["type"] == "revolute":
                motion = mat(axis_angle(a, math.radians(q)), [0, 0, 0])
            else:
                motion = mat([[1, 0, 0], [0, 1, 0], [0, 0, 1]], [q * v for v in a])
            world[j["body1"]] = mul(mul(mul(world[j["body0"]], local(j["local0"])), motion), inverse(local(j["local1"])))
    return world


def close(a, b, tol=1e-6):
    return max(abs(x - y) for ra, rb in zip(a, b) for x, y in zip(ra, rb)) < tol


class Usd(unittest.TestCase):
    """Read back by USD's own library."""

    @classmethod
    def setUpClass(cls):
        s3d.empty_scene()
        cls.root, cls.parts = machine()
        cls.tmp = Path(tempfile.mkdtemp())
        s3d.pose(cls.parts["door"], 70)                    # both files are the rest, whatever the pose
        s3d.export(cls.root, cls.tmp / "washer.usda")
        s3d.export(cls.root, cls.tmp / "washer.urdf")
        s3d.pose(cls.parts["door"], 0)
        cls.stage = read_back(cls.tmp / "washer.usda")
        cls.robot, cls.links = urdf_fk(cls.tmp / "washer.urdf", {})

    def name(self, path):
        return path.rsplit("/", 1)[1]

    def test_the_stage_is_valid_and_simready_shaped(self):
        st = self.stage
        self.assertEqual(st["errors"], [])
        self.assertEqual((st["defaultPrim"], st["upAxis"], st["metersPerUnit"]), ("washer", "Z", 1.0))
        self.assertNotIn("other", st)                       # no fixed joint, no collider
        (art,) = st["articulations"]
        self.assertEqual(art["path"], "/washer")
        self.assertEqual(art["joints"], sorted(st["joints"]))
        self.assertTrue(all(j["articulated"] for j in st["joints"].values()))
        self.assertTrue(all(p.startswith("/washer/Joints/") for p in st["joints"]))
        self.assertTrue(all(p.startswith("/washer/Geometry/") for p in st["bodies"]))

    def test_the_names_are_the_urdfs(self):
        st = self.stage
        links = {l.get("name") for l in self.robot.findall("link")}
        self.assertEqual({self.name(p) for p in st["bodies"]}, links)
        joints = {j.get("name"): j for j in self.robot.findall("joint")}
        self.assertEqual({self.name(p) for p in st["joints"]}, set(joints) - {"cord"})   # free: no joint
        for p, j in st["joints"].items():
            urdf = joints[self.name(p)]
            self.assertEqual((self.name(j["body0"]), self.name(j["body1"])),
                             (urdf.find("parent").get("link"), urdf.find("child").get("link")))
        visuals = {v.get("name"): l.get("name") for l in self.robot.findall("link") for v in l.findall("visual")}
        self.assertEqual({self.name(p): p.split("/")[-2] for p in st["meshes"]}, visuals)

    def test_bodies_pivots_and_axes_are_the_urdfs(self):
        st = self.stage
        for p, b in st["bodies"].items():
            self.assertTrue(close(b["world"], self.links[self.name(p)]), p)
        joints = {j.get("name"): j for j in self.robot.findall("joint")}
        for p, j in st["joints"].items():
            with self.subTest(joint=p):
                frame0 = mul(st["bodies"][j["body0"]]["world"], local(j["local0"]))
                frame1 = mul(st["bodies"][j["body1"]]["world"], local(j["local1"]))
                self.assertTrue(close(frame0, frame1), "the joint's two frames part at rest")
                child = self.links[self.name(j["body1"])]
                self.assertTrue(close([[r[3]] for r in frame0[:3]], [[r[3]] for r in child[:3]]))
                urdf_axis = [float(v) for v in joints[self.name(p)].find("axis").get("xyz").split()]
                want = [sum(child[r][c] * urdf_axis[c] for c in range(3)) for r in range(3)]
                got = [sum(frame0[r][c] * unit(j["axis"])[c] for c in range(3)) for r in range(3)]
                self.assertTrue(close([want], [got]), (want, got))

    def test_ranges_and_mimic_are_the_urdfs(self):
        joints = {j.get("name"): j for j in self.robot.findall("joint")}
        per_si = {"revolute": 180 / math.pi, "prismatic": 1.0}           # a USD unit per SI unit
        for p, j in self.stage["joints"].items():
            with self.subTest(joint=p):
                urdf = joints[self.name(p)]
                limit = urdf.find("limit")
                if urdf.get("type") == "continuous":
                    self.assertIsNone(limit)
                    self.assertIsNone(j["limit"])
                else:
                    want = [float(limit.get(k)) * per_si[j["type"]] for k in ("lower", "upper")]
                    self.assertTrue(close([want], [j["limit"]], 1e-5), (want, j["limit"]))
                mimic = urdf.find("mimic")
                if mimic is None:
                    self.assertIsNone(j["mimic"])
                    continue
                self.assertEqual(self.name(j["mimic"]["joint"]), mimic.get("joint"))
                lead = self.stage["joints"][j["mimic"]["joint"]]["type"]
                self.assertEqual(j["mimic"]["coef0"], 0)
                self.assertAlmostEqual(j["mimic"]["coef1"] * per_si[lead] / per_si[j["type"]],
                                       float(mimic.get("multiplier")), places=9)
        self.assertEqual(self.stage["joints"]["/washer/Joints/door_hinge"]["limit"], [0, 143])
        self.assertIsNone(self.stage["joints"]["/washer/Joints/knob_turn"]["limit"])     # continuous: ±inf

    def test_the_meshes_are_the_objs(self):
        for p, m in self.stage["meshes"].items():
            with self.subTest(mesh=p):
                obj = Path(self.tmp / "washer_meshes" / f"{self.name(p)}.obj").read_text().splitlines()
                pts = obj_points(self.tmp / "washer_meshes" / f"{self.name(p)}.obj")
                self.assertTrue(close(m["points"], pts, 1e-7))
                faces = [int(v) - 1 for line in obj if line.startswith("f ") for v in line.split()[1:]]
                self.assertEqual(m["indices"], faces)
                self.assertEqual(set(m["counts"]), {3})
                self.assertEqual((len(m["normals"]), m["interpolation"], m["subdivision"]),
                                 (len(faces), "faceVarying", "none"))
                for t in range(0, len(faces), 3):                # each corner's normal on its triangle's side
                    a, b, c = (pts[i] for i in faces[t:t + 3])
                    u, v = [q - p for p, q in zip(a, b)], [q - p for p, q in zip(a, c)]
                    n = [u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2], u[0] * v[1] - u[1] * v[0]]
                    for corner in m["normals"][t:t + 3]:
                        self.assertGreater(sum(x * y for x, y in zip(n, corner)), 0)
                        self.assertAlmostEqual(math.hypot(*corner), 1, places=6)

    def test_the_materials(self):
        st = self.stage
        meshes = {self.name(p): m["material"] for p, m in st["meshes"].items()}
        self.assertIsNone(meshes["cab"])
        glass, plastic = st["materials"][meshes["door"]], st["materials"][meshes["drawer"]]
        self.assertEqual(meshes["door_2"], meshes["door"])
        self.assertEqual((glass["id"], glass["diffuseColor"], glass["opacity"]), ("UsdPreviewSurface", [1, 1, 1], 0))
        linear = [(c / 255 / 12.92 if c / 255 <= 0.04045 else ((c / 255 + 0.055) / 1.055) ** 2.4)
                  for c in (0x33, 0x66, 0x99)]
        self.assertTrue(close([plastic["diffuseColor"]], [linear], 1e-7))
        self.assertEqual((plastic["opacity"], plastic["roughness"]), (0.5, 0.5))

    def test_the_usd_posed_by_its_own_joints_is_where_pose_puts_the_parts(self):
        p = self.parts
        by_mesh = {"door_2": p["twin"]}
        for values in ({}, {"door_hinge": 90, "knob_turn": 30}, {"door_hinge": 143, "drawer_slide": 150},
                       {"cap_turn": 450}, {"door_hinge": 60, "knob_turn": -200, "cap_turn": 33}):
            with self.subTest(values=values):
                usd = {}
                for name, v in values.items():
                    if name == "door_hinge":
                        s3d.pose(p["door"], v)
                        s3d.pose(p["twin"], v)
                        usd["door_hinge_2"] = v
                    elif name == "cap_turn":
                        s3d.pose(p["cap"], v, joint=name)
                    else:
                        s3d.pose(p[name.split("_")[0]], v)
                    usd[name] = v / 1000 if name == "drawer_slide" else v
                world = usd_fk(self.stage, usd)
                for path, m in self.stage["meshes"].items():
                    name = self.name(path)
                    ob = by_mesh.get(name) or p[name]
                    body = world[path.rsplit("/", 1)[0]]
                    pts = [[sum(body[r][i] * v[i] for i in range(3)) + body[r][3] for r in range(3)] for v in m["points"]]
                    w = _core.world(ob)
                    want = [w @ v.co / 1000 for v in ob.data.vertices]
                    err = max(math.dist(a, b) for a, b in zip(pts, want))
                    self.assertLess(err, 2e-6, name)
                for name in ("door", "twin", "knob", "drawer"):
                    s3d.pose(p[name], 0)
                s3d.pose(p["cap"], 0, joint="cap_turn")


class Text(unittest.TestCase):
    """What the text says, without USD's library."""

    def setUp(self):
        s3d.empty_scene()
        self.tmp = Path(tempfile.mkdtemp())

    def test_the_usda_is_the_rest_whatever_the_pose(self):
        root, parts = machine()
        s3d.export(root, self.tmp / "a.usda")
        s3d.pose(parts["door"], 90)
        s3d.pose(parts["drawer"], 100)
        s3d.export(root, self.tmp / "b.usda")
        self.assertEqual((self.tmp / "a.usda").read_bytes(), (self.tmp / "b.usda").read_bytes())

    def test_the_scene_is_left_as_it_was(self):
        root, _ = machine()
        before = sorted((o.name, tuple(map(tuple, o.matrix_basis))) for o in bpy.data.objects)
        meshes, mats = sorted(m.name for m in bpy.data.meshes), sorted(m.name for m in bpy.data.materials)
        s3d.export(root, self.tmp / "washer.usda")
        self.assertEqual(sorted((o.name, tuple(map(tuple, o.matrix_basis))) for o in bpy.data.objects), before)
        self.assertEqual(sorted(m.name for m in bpy.data.meshes), meshes)
        self.assertEqual(sorted(m.name for m in bpy.data.materials), mats)

    def test_binary_usd_is_refused(self):
        root, _ = machine()
        for suffix, tool in ((".usdc", "usdcat x.usda -o x.usdc"), (".usd", "usdcat x.usda -o x.usd"),
                             (".USDZ", "usdzip")):
            with self.assertRaisesRegex(ValueError, rf"^export: {suffix.lower()} is written by USD's own library, "
                                                    rf"which sand3d does not take: write \.usda, USD as text, and "
                                                    rf"convert it with USD's tools \({re.escape(tool)}\)$"):
                s3d.export(root, self.tmp / f"washer{suffix}")
        self.assertEqual(list(self.tmp.iterdir()), [])

    def test_an_unknown_extension_is_refused(self):
        """It used to write a GLB under any other name: washer.stl was a GLB."""
        root, _ = machine()
        for name, said in (("washer.ply", ".ply"), ("washer.gltf", ".gltf"), ("washer.FBX", ".fbx"),
                           ("washer", "a path with no extension")):
            with self.assertRaisesRegex(ValueError, rf"^export: {re.escape(said)} is not a format sand3d writes: "
                                                    rf"name the file \.glb, \.obj, \.stl, \.urdf or \.usda$"):
                s3d.export(root, self.tmp / name)
        self.assertEqual(list(self.tmp.iterdir()), [])
        s3d.export(root, self.tmp / "washer.GLB")
        self.assertTrue((self.tmp / "washer.GLB").is_file())

    def test_names_become_identifiers(self):
        base = s3d.box("2 base", 100, 100, 10)
        lids = [s3d.at(s3d.box(n, 50, 50, 5), z=10) for n in ("lid-a", "lid_a")]
        for lid in lids:
            s3d.joint("hinge #1", lid, s3d.bbox(base, "+X+Z"), axis="+Y", range=(0, 90))
        s3d.export(s3d.group("box café", base, *lids), self.tmp / "b.usda")
        text = (self.tmp / "b.usda").read_text()
        self.assertIn('defaultPrim = "box_caf_"', text)
        for prim in ('Xform "box_caf_"', 'Mesh "_2_base"', 'Xform "lid_a"', 'Mesh "lid_a_2"',
                     'RevoluteJoint "hinge__1"', 'RevoluteJoint "hinge__1_2"', '</box_caf_/Geometry/lid_a_2>'):
            self.assertIn(prim, text)
        self.assertTrue(text.isascii())

    def test_an_object_with_no_joint_is_one_rigid_body(self):
        root = s3d.group("thing", s3d.box("b", 10, 10, 10), s3d.at(s3d.cylinder("c", 4, 20), x=20))
        s3d.export(root, self.tmp / "thing.usda")
        text = (self.tmp / "thing.usda").read_text()
        self.assertNotIn("Articulation", text)
        self.assertNotIn("Joints", text)
        self.assertNotIn("Looks", text)
        self.assertEqual(text.count("PhysicsRigidBodyAPI"), 1)
        self.assertEqual(re.findall(r'def Mesh "(\w+)"', text), ["b", "c"])

    def test_the_program_unit_goes_to_metres(self):
        s3d.units("cm")
        cab = s3d.box("cab", 60, 60, 85)
        door = s3d.at(s3d.box("door", 2, 56, 80), x=31, z=2.5)
        s3d.joint("slide", door, s3d.bbox(cab, "+X"), kind="prismatic", axis="+X", range=(0, 20))
        s3d.export(s3d.group("w", cab, door), self.tmp / "cm.usda")
        text = (self.tmp / "cm.usda").read_text()
        self.assertIn("point3f physics:localPos0 = (0.3, 0, 0.425)", text)
        self.assertIn("float physics:upperLimit = 0.2", text)
        self.assertIn("float3[] extent = [(-0.3, -0.3, 0), (0.3, 0.3, 0.85)]", text)

    def test_a_chain_of_followers_follows_its_head(self):
        base = s3d.box("base", 300, 100, 10)
        gears = [s3d.at(s3d.cylinder(f"g{i}", 20, 10), x=100 * i - 100, z=10) for i in range(3)]
        s3d.joint("a", gears[0], base.face("top").move(x=-100), range=(0, 360))
        s3d.joint("b", gears[1], base.face("top"), follows="a", ratio=-2)
        s3d.joint("c", gears[2], base.face("top").move(x=100), follows="b", ratio=-0.25)
        s3d.export(s3d.group("train", base, *gears), self.tmp / "t.usda")
        text = (self.tmp / "t.usda").read_text()
        self.assertEqual(re.findall(r"newton:mimicJoint = <([^>]+)>\n.*\n.*newton:mimicCoef1 = (\S+)", text),
                         [("/train/Joints/a", "-2"), ("/train/Joints/a", "0.5")])

    def test_a_joint_hanging_outside_what_is_exported(self):
        _, p = machine()
        front = s3d.group("front", p["door"], p["knob"])
        with self.assertRaisesRegex(ValueError, r"^export: 'door' hangs on 'cab', which is not in 'front': "
                                                r"export a group that holds both$"):
            s3d.export(front, self.tmp / "front.usda")
        self.assertEqual(list(self.tmp.iterdir()), [])


if __name__ == "__main__":
    unittest.main()