109 lines
4.3 KiB
Python
109 lines
4.3 KiB
Python
 
|
"""Export the OpenSCAD robot as meshes for the Unity simulation.
|
||
|
|
|
||
|
|
python export_unity.py (from hardware/openscad, needs OpenSCAD)
|
||
|
|
|
||
|
|
Writes Hexapod_Simulation_Unity/Assets/HexapodSim/Resources/OpenScadModel/*.bytes,
|
||
|
|
loaded by Assets/HexapodSim/OpenScadModel.cs. Every file is one rig segment
|
||
|
|
in the robot frame (mm, +X forward, +Y left, +Z up), relative to the
|
||
|
|
segment's pivot, in the rest pose that matches the rig's zero rotations
|
||
|
|
(coxa 0, femur 0, tibia -90):
|
||
|
|
body Z = 0 on the joint plane (13.5 mm above the bottom plate)
|
||
|
|
leg_<side>_* left = as leg 4 (mount +90), right = its mirror (mount -90)
|
||
|
|
|
||
|
|
File format (little endian): b"HXM1", int32 groups, then per group
|
||
|
|
float32 r, g, b, a, int32 triangles, triangles * 9 float32 (x, y, z).
|
||
|
|
Triangles wind counter-clockwise seen from outside (right-handed).
|
||
|
|
"""
|
||
|
|
import os, re, struct, subprocess, sys, tempfile
|
||
|
|
|
||
|
|
HERE = os.path.dirname(os.path.abspath(__file__)).replace("\\", "/")
|
||
|
|
OUT = os.path.normpath(os.path.join(HERE, "..", "..", "Hexapod_Simulation_Unity", "Assets",
|
||
|
|
"HexapodSim", "Resources", "OpenScadModel"))
|
||
|
|
OPENSCAD = os.environ.get("OPENSCAD", r"C:/Program Files/OpenSCAD/openscad.exe")
|
||
|
|
HEADER = f"use <{HERE}/body.scad>\nuse <{HERE}/leg.scad>\n"
|
||
|
|
JOINT_PLANE_Z = 13.5
|
||
|
|
|
||
|
|
|
||
|
|
def hexcol(h, a=1.0):
|
||
|
|
h = h.lstrip("#")
|
||
|
|
return [int(h[i:i + 2], 16) / 255 for i in (0, 2, 4)] + [a]
|
||
|
|
|
||
|
|
|
||
|
|
# segment -> list of (colour, OpenSCAD geometry in the segment's frame)
|
||
|
|
BODY = [
|
||
|
|
("#3a3a3a", f"translate([0, 0, -{JOINT_PLANE_Z}]) fusion_to_robot() bottom_half();"),
|
||
|
|
("#555555", f"translate([0, 0, -{JOINT_PLANE_Z}]) fusion_to_robot() top_half();"),
|
||
|
|
("#2a2a2a", f"translate([0, 0, -{JOINT_PLANE_Z}]) fusion_to_robot() body_servos_uv();"),
|
||
|
|
("#2b6b3a", f"translate([0, 0, -{JOINT_PLANE_Z}]) pcb_in_body();"),
|
||
|
|
]
|
||
|
|
LEFT = "rotate([0, 0, 90]) " # leg frame -> robot frame for the left legs (mount +90)
|
||
|
|
LEG = {
|
||
|
|
"coxa": [
|
||
|
|
("#d9822b", LEFT + "{ coxa_bracket_upper(); coxa_bracket_lower(); }"),
|
||
|
|
("#bdbdbd", LEFT + "translate([55, 0, 0]) along_y(28.7) horn_adapter();"),
|
||
|
|
("#5a3a1e", LEFT + "coxa_cables();"),
|
||
|
|
],
|
||
|
|
"femur": [
|
||
|
|
("#4a7fbf", LEFT + "{ link_channel(); link_lid(); }"),
|
||
|
|
("#2a2a2a", LEFT + "link_servos();"),
|
||
|
|
("#5a3a1e", LEFT + "femur_cables();"),
|
||
|
|
],
|
||
|
|
"tibia": [
|
||
|
|
("#5fa35f", LEFT + "{ tibia_bearing_half(); tibia_horn_half(); }"),
|
||
|
|
("#bdbdbd", LEFT + "along_y(28.7) horn_adapter();"),
|
||
|
|
("#1c1c1c", LEFT + "foot_sock();"),
|
||
|
|
],
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
def render(geometry, tmp):
|
||
|
|
scad = os.path.join(tmp, "part.scad"); stl = os.path.join(tmp, "part.stl")
|
||
|
|
with open(scad, "w") as f:
|
||
|
|
f.write(HEADER + geometry + "\n")
|
||
|
|
r = subprocess.run([OPENSCAD, "-o", stl, scad], capture_output=True, text=True)
|
||
|
|
if r.returncode != 0 or not os.path.exists(stl):
|
||
|
|
sys.exit(f"OpenSCAD failed on: {geometry}\n{r.stderr}")
|
||
|
|
tris = []
|
||
|
|
num = r"([-+0-9.eE]+)"
|
||
|
|
for m in re.finditer(r"outer loop\s+vertex\s+%s\s+%s\s+%s\s+vertex\s+%s\s+%s\s+%s\s+vertex\s+%s\s+%s\s+%s" % ((num,) * 9),
|
||
|
|
open(stl).read()):
|
||
|
|
tris.append([float(v) for v in m.groups()])
|
||
|
|
os.remove(stl)
|
||
|
|
return tris
|
||
|
|
|
||
|
|
|
||
|
|
def mirror_y(tris):
|
||
|
|
# (x, y, z) -> (x, -y, z) and swap two vertices to keep outward winding
|
||
|
|
out = []
|
||
|
|
for t in tris:
|
||
|
|
a, b, c = t[0:3], t[3:6], t[6:9]
|
||
|
|
a, b, c = [a[0], -a[1], a[2]], [b[0], -b[1], b[2]], [c[0], -c[1], c[2]]
|
||
|
|
out.append(a + c + b)
|
||
|
|
return out
|
||
|
|
|
||
|
|
|
||
|
|
def write(name, groups):
|
||
|
|
path = os.path.join(OUT, name + ".bytes")
|
||
|
|
with open(path, "wb") as f:
|
||
|
|
f.write(b"HXM1" + struct.pack("<i", len(groups)))
|
||
|
|
for col, tris in groups:
|
||
|
|
f.write(struct.pack("<4f", *col) + struct.pack("<i", len(tris)))
|
||
|
|
for t in tris:
|
||
|
|
f.write(struct.pack("<9f", *t))
|
||
|
|
print(f" {name}: {sum(len(t) for _, t in groups)} triangles")
|
||
|
|
|
||
|
|
|
||
|
|
def main():
|
||
|
|
os.makedirs(OUT, exist_ok=True)
|
||
|
|
with tempfile.TemporaryDirectory() as tmp:
|
||
|
|
print("body")
|
||
|
|
write("body", [(hexcol(c), render(g, tmp)) for c, g in BODY])
|
||
|
|
for seg, parts in LEG.items():
|
||
|
|
print("leg", seg)
|
||
|
|
groups = [(hexcol(c), render(g, tmp)) for c, g in parts]
|
||
|
|
write(f"leg_left_{seg}", groups)
|
||
|
|
write(f"leg_right_{seg}", [(c, mirror_y(t)) for c, t in groups])
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|