610 lines
30 KiB
Plaintext
610 lines
30 KiB
Plaintext
# -*- coding: utf-8 -*-
|
||
"""Shared geometry / assembly / export library for the robot arm build.
|
||
|
||
No layout of its own: the joint frames and station positions live in
|
||
human_layout.py and arm_model.py, which import this module. What lives here is
|
||
the measured motor interface (docs/interfaces.md), the Joint class (motor slip
|
||
bore + PCBA pocket + pinch screws; driven hub mating plane at motor rear datum
|
||
+65.0 replicating the Ø36x1.0 neck / Ø6.5x4.0 centre / 3x M2 on Ø29 output so
|
||
the existing face-spline halves + split clamp bolt straight on), the part
|
||
registry, and the STEP/STL/mesh/BOM/collision export pipeline.
|
||
|
||
Run: python arm_model.py -> out/, web/mesh_arm.json, docs/
|
||
"""
|
||
import math
|
||
import os
|
||
import sys
|
||
import json
|
||
|
||
try:
|
||
sys.stdout.reconfigure(encoding="utf-8")
|
||
except Exception:
|
||
pass
|
||
|
||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||
import cadquery as cq
|
||
from armcore import (M, C, V, trs, place, bb, bbox_str, cyl, ring, box, holes,
|
||
solids_of, mesh_of, MOTOR_TOL, OUT, WEB, DOCS, SRC_STEP, HALF_STEP, CLAMP_STEP)
|
||
from OCP.gp import gp_Trsf, gp_Vec, gp_Pnt, gp_Dir, gp_Ax1
|
||
from OCP.BRepAlgoAPI import BRepAlgoAPI_Common
|
||
from OCP.GProp import GProp_GProps
|
||
from OCP.BRepGProp import BRepGProp
|
||
from OCP.STEPControl import STEPControl_Writer, STEPControl_Controller, STEPControl_StepModelType
|
||
from OCP.Interface import Interface_Static
|
||
|
||
STEPControl_Controller.Init_s()
|
||
D2R = math.pi / 180.0
|
||
VERSION = os.environ.get("ARMVER", "r1")
|
||
ITER = int(os.environ.get("ARMITER", "1"))
|
||
|
||
# ---------------------------------------------------------------- measured stations
|
||
FLANGE_Z = M["flange_z"] # 54.942
|
||
COVER_Z = M["cover_z"] # 56.000
|
||
HUB_FACE_Z = 65.0 # motor rear datum -> driven hub mating plane
|
||
CLAMP_SPAN = (FLANGE_Z + 1.66, FLANGE_Z + 9.46)
|
||
CLAMP_R_BAND = 31.0
|
||
CLAMP_R_LUG = 38.0
|
||
BORE_R = M["hous_d"] / 2.0 + 0.15 # 30.15 slip fit on the housing
|
||
CB_R = M["cover_d"] / 2.0 + 0.35 # 30.50 clearance for the cover
|
||
PCB_W = M["pcba_w"] + 0.8 # 64.40 square pocket
|
||
HUB_M2_CLOCK = [(360.0 - a) % 360.0 for a in M["m2_clock"]] # phase of the mirrored half
|
||
HUB_PILOT_D = M["neck_d"] - 0.4 # 35.6 -> the half's pilot lands in a Ø36 recess
|
||
PINCH_D = 2.6 # M3 clearance in printed material
|
||
# 承压面让位（完整背景见 Joint.pocket 的 docstring）：
|
||
# REAR_LAND = 0.6 腔口直壁台肩高度，**保持原设计值不变**（让位不靠加高台肩）
|
||
# REAR_SEAT = 2.6 尾端面->承压面 = 0.6 原台肩 + 2.0 公差垫片位
|
||
# SEAT_R = 32.2 承压面段（z=-REAR_SEAT..0）的腔口半径 = PCB_W/2 = 64.4 方腔的
|
||
# **内切圆半径**。承压面下沉 2.6mm 后这一段必须让空，但绝不能连着
|
||
# 外壁 R35 一起挖掉（2026-10-04 教训，见 Joint.pocket docstring）：
|
||
# 用 R35 圆腔会把 r=32.2..35 的连接壁整圈切断，P04 后端只剩
|
||
# 240~320° 方位还连着（实测），打印出来就是一道 3.6mm 宽的通槽 +
|
||
# 一片 0.5mm 厚的孤立薄片（用户截图：肩叉顶部"没连上"）。
|
||
# 取 32.2 只需挖到方腔内切圆，r=32.2..35 全圈保留 ->
|
||
# 壁厚均匀的 2.8mm 环形连接壁（0/90/180/270 方向与上方方腔壁
|
||
# 齐平 seamlessly，45° 方向比方腔多留料，正好补回四角）。
|
||
# SEAT_TOP = 0.0 圆腔顶端 = 方腔起点，两者在此平接；再往上就是方腔的地盘。
|
||
# R35_SEAT = 35.0 后端板 R35 外壁（承压面外径，不可挖穿）
|
||
REAR_LAND = 0.6
|
||
REAR_SEAT = REAR_LAND + MOTOR_TOL # 2.6 尾端面->后端板承压面：公差垫片位
|
||
R35_SEAT = 35.0 # 后端板 R35 外壁（承压面外径）
|
||
SEAT_R = PCB_W / 2.0 # 32.2 承压面段腔口 = 方腔内切圆，保留外壁
|
||
SEAT_TOP = 0.0 # 圆腔顶端与方腔起点平接
|
||
|
||
PARTS = []
|
||
CENTS = {}
|
||
|
||
|
||
def add(name, cn, shape, color, printed=True, note="", qty=1):
|
||
assert shape is not None, name
|
||
lo, hi = bb(shape)
|
||
PARTS.append(dict(name=name, cn=cn, shape=shape, color=color, printed=printed,
|
||
note=note, qty=qty, bb=bbox_str(shape), vol=vol_of(shape)))
|
||
CENTS[name] = [(lo[i] + hi[i]) / 2.0 for i in range(3)]
|
||
|
||
|
||
def vol_of(sh):
|
||
p = GProp_GProps()
|
||
BRepGProp.VolumeProperties_s(sh.wrapped, p)
|
||
return p.Mass()
|
||
|
||
|
||
def common_vol(a, b):
|
||
try:
|
||
c = BRepAlgoAPI_Common(a.wrapped, b.wrapped)
|
||
c.Build()
|
||
if not c.IsDone() or c.Shape().IsNull():
|
||
return 0.0
|
||
return vol_of(cq.Shape(c.Shape()))
|
||
except Exception:
|
||
return -1.0
|
||
|
||
|
||
def T(dx=0.0, dy=0.0, dz=0.0):
|
||
t = gp_Trsf()
|
||
t.SetTranslation(gp_Vec(dx, dy, dz))
|
||
return t
|
||
|
||
|
||
def R(deg, ax, ay, az, px=0.0, py=0.0, pz=0.0):
|
||
t = gp_Trsf()
|
||
t.SetRotation(gp_Ax1(gp_Pnt(px, py, pz), gp_Dir(ax, ay, az)), deg * D2R)
|
||
return t
|
||
|
||
|
||
RX90 = R(90, 1, 0, 0)
|
||
|
||
|
||
def Ycyl(r, y0, y1, px=0.0, pz=0.0):
|
||
return place(cyl(r, -y1, -y0, px, pz), RX90)
|
||
|
||
|
||
def Ybox(x0, x1, y0, y1, z0, z1):
|
||
return place(box(x0, x1, z0, z1, -y1, -y0), RX90)
|
||
|
||
|
||
# ------------------------------------------------------- pitch-joint pivot cartridge ----
|
||
# ONE recipe for every pitch joint (J2 shoulder, J3 elbow, J5 wrist) - they are the same
|
||
# degree of freedom, so an implementation that fixes only one of them leaves the identical
|
||
# defect in the other two. See human_layout PV_* for why a cartridge is needed at all:
|
||
# measured P04 n P05 = 0.00 mm3, so J2's 38 N*m had no path into the housing except peeling
|
||
# the face-spline teeth off across the print layers.
|
||
def pivot_geom(y_face, side, h, w, gap):
|
||
"""(boss_y0, boss_y1, pocket_y0) for a cartridge against the link face at y_face.
|
||
The race pocket always opens AWAY from the link, leaving an (h-w) shoulder behind the
|
||
outer race; a printed cap ring closes the open end. The boss stops `gap` short of the
|
||
link face, or a printed face would carry the thrust and the rolling elements nothing."""
|
||
near = y_face - gap if side < 0 else y_face + gap
|
||
if side < 0:
|
||
return near - h, near, near - h
|
||
return near, near + h, near + h - w
|
||
|
||
|
||
def pivot_boss(shape, pz, y0, y1, pk0, w, px, boss_od, race_od, bore, orace_id, side):
|
||
"""Housing side: boss on the joint axis with the outer race sunk into it. The passage
|
||
beyond the pocket is reamed to the outer race bore, not to the shaft: a shaft-sized bore
|
||
would end inside the inner race and bind it, and a bound bearing carries nothing - it
|
||
just moves the failure back into the print layers."""
|
||
shape = shape.fuse(Ycyl(boss_od / 2.0, y0, y1, px, pz))
|
||
shape = shape.cut(Ycyl(race_od / 2.0, pk0 - .01, pk0 + w + .01, px, pz))
|
||
if side < 0:
|
||
shape = shape.cut(Ycyl(orace_id / 2.0 + 0.1, pk0 + w, y1 + .01, px, pz))
|
||
else:
|
||
shape = shape.cut(Ycyl(orace_id / 2.0 + 0.1, y0 - .01, pk0, px, pz))
|
||
shape = shape.cut(Ycyl(bore / 2.0 + 0.2, y0 - .01, y1 + .01, px, pz))
|
||
for a in (90.0, 210.0, 330.0): # 3x M3 cap screws, outside the outer race
|
||
u, v = 26.5 * math.cos(a * math.pi / 180.0), 26.5 * math.sin(a * math.pi / 180.0)
|
||
shape = shape.cut(Ycyl(1.25, y0 - .01, y0 + 12.0, px + u, pz + v))
|
||
return shape
|
||
|
||
|
||
def pivot_journal(shape, pz, y_link, y_tip, y_collar0, y_collar1, px, bore, collar):
|
||
"""Link side: the stub the inner race rides on, poking past the boss so the cap can bite
|
||
it, plus an integral collar the inner race bears against. The collar replaces a separate
|
||
spacer sleeve and lives in the boss's outer-race passage, so it must stay under that
|
||
diameter - the 0.1 mm it leaves is what lets the race, not the collar, take the thrust."""
|
||
a, b = sorted((y_link, y_tip))
|
||
shape = shape.fuse(Ycyl(bore / 2.0, a, b, px, pz))
|
||
c0, c1 = sorted((y_collar0, y_collar1))
|
||
return shape.fuse(Ycyl(collar / 2.0, c0, c1, px, pz))
|
||
|
||
|
||
def prism(pts2, z0, z1):
|
||
w2 = [(p[0], p[1]) for p in pts2]
|
||
return (cq.Workplane("XY", origin=(0, 0, z0))
|
||
.polyline(w2 + [w2[0]]).close().extrude(z1 - z0, combine=False)).val()
|
||
|
||
|
||
def Yplate(pts_xz, y0, y1):
|
||
return place(prism(pts_xz, -y1, -y0), RX90)
|
||
|
||
|
||
def dogbone(a, b, ra, rb, n=20):
|
||
"""Tangent hull of two discs. The outer-tangent offset angle is acos((ra-rb)/L);
|
||
using asin made the ra==rb case sweep a full circle around b and self-intersect."""
|
||
dx, dy = b[0] - a[0], b[1] - a[1]
|
||
L = math.hypot(dx, dy) or 1.0
|
||
th = math.atan2(dy, dx)
|
||
beta = math.acos(max(-1.0, min(1.0, (ra - rb) / L)))
|
||
sw_a = 2 * math.pi - 2 * beta
|
||
sw_b = 2 * beta
|
||
pts = [(a[0] + ra * math.cos(th + beta + sw_a * k / n), a[1] + ra * math.sin(th + beta + sw_a * k / n))
|
||
for k in range(n + 1)]
|
||
pts += [(b[0] + rb * math.cos(th - beta + sw_b * k / n), b[1] + rb * math.sin(th - beta + sw_b * k / n))
|
||
for k in range(1, n + 1)]
|
||
return pts
|
||
|
||
|
||
def sector(r_out, r_in, a0, a1, z0, z1, n=14):
|
||
"""Annular sector between two radii and two angles (deg) - a joint shroud segment.
|
||
Must NOT include the axis point, or this becomes a solid pie wedge that fills the joint."""
|
||
pts = [(r_out * math.cos((a0 + (a1 - a0) * k / n) * D2R),
|
||
r_out * math.sin((a0 + (a1 - a0) * k / n) * D2R)) for k in range(n + 1)]
|
||
pts += [(r_in * math.cos((a1 - (a1 - a0) * k / n) * D2R),
|
||
r_in * math.sin((a1 - (a1 - a0) * k / n) * D2R)) for k in range(n + 1)]
|
||
return prism(pts, z0, z1)
|
||
|
||
|
||
# ---------------------------------------------------------------- joint frames & interfaces
|
||
class Joint:
|
||
"""One revolute joint. `rear` = motor rear datum point, `d` = axis unit dir (motor -> hub)."""
|
||
|
||
def __init__(self, jid, cn, rear, d, xdir, lug_phase=0.0):
|
||
self.jid, self.cn = jid, cn
|
||
self.rear = rear
|
||
self.d = d
|
||
self.xdir = xdir
|
||
self.t = trs(rear, xdir, d) # motor-local -> global
|
||
self.lug_phase = lug_phase # rotation of the clamp about the axis
|
||
|
||
def L(self, shape_local):
|
||
return place(shape_local, self.t)
|
||
|
||
def at(self, s):
|
||
return (self.rear[0] + self.d[0] * s, self.rear[1] + self.d[1] * s, self.rear[2] + self.d[2] * s)
|
||
|
||
def pocket(self, shape, front_pad=0.6):
|
||
"""Cut the motor pocket out of a proximal link body.
|
||
|
||
REAR_SEAT = 电机尾端面（局部 z=0）到后端板承压面的距离。它不是定位面而是
|
||
**垫片位**：公差 MOTOR_TOL=2.0（armcore）要靠它消化 —— 电机比标称短的那一份，
|
||
就垫在尾端面与这个承压面之间（见 armcore.MOTOR_TOL 的说明）。原值 0.6 只能吃
|
||
0.6mm，短 1mm 以上的电机装不进、长 0.6mm 以上的电机会顶死后端板而压溃。
|
||
注意它同时是承压面的高度，所以让空 2.0mm 要求 t_plate >= 8
|
||
（见各零件 rear_mount_holes 的 t_plate）。
|
||
|
||
【2026-10-04 整改：承压面段改用 R35 圆腔，凸台彻底消失】
|
||
上一版把 64.4 **方**腔整体从 -0.6 加深到 -2.6（纯掏腔），方腔壁以外
|
||
z=-2.6..0 之间那圈 R35 料一动没动，于是相对新承压面整整凸起 2.6mm ——
|
||
这就是 P04~P08 后端那圈"悬空凸台"（用户截图红圈，3D 打印时该处是悬空
|
||
薄壁，既要支撑又公差难保证）。
|
||
|
||
为什么之前试"把凸台切掉"总失败（几何实测，别再重复）：
|
||
- 64.4 方腔的**内切圆半径恰好 = 32.2**，所以方腔在 45° 方向已经吃穿到
|
||
r=45.5，R35 壁只剩**四条边中点附近**的料。实测 r=34 处材料沿 z 的
|
||
区间（承压面 -2.6）：P06 的 0°/180° 是 -8.0..12.0（贯通，是臂身本体），
|
||
80°/100° 是 -8.0..-0.0（**只比承压面多 2.6mm，这段就是凸台**），
|
||
其余 20~160° 是 -8.0..-2.6（承压环本体，齐平）。
|
||
- 因此整段全截面切会把 0/90 方向那条**贯通到 z=12 的承力桥**一起切断
|
||
-> P05/P08 碎成 2 块（实测）。这不是"承压环是唯一连接"，
|
||
而是我的刀具没区分"桥"和"悬空料"。
|
||
|
||
现在的做法（一刀，不做角度分区）：
|
||
承压面所在的 z=-REAR_SEAT..0 段，腔口取 **PCB_W/2 = 32.2 的圆**
|
||
（= 64.4 方腔的内切圆半径，SEAT_R），而不是方腔、也不是 R35：
|
||
- 用方腔（64.4）：方腔在 45° 方向已吃穿到 r=45.5，R35 壁只剩四条边中点
|
||
附近的料；实测那几段 r=34 处材料是 -8.0..-0.0，比承压面(-2.6)多出 2.6mm，
|
||
就是用户截图里那圈"悬空凸台"（打印要支撑、公差差）。
|
||
- 用 R35 圆腔（上一版，2026-10-04，**错**）：把 r=32.2..35 的连接壁整圈切断。
|
||
实测 P04 在 z=-2.6..+1.0 只剩 240~320° 方位有料，其余 260° 全部掏空 ->
|
||
后端法兰与杯壁之间出现一道 3.6mm 宽的**通槽**，还在 225° 留下一片
|
||
0.5mm 厚的孤立薄片（用户截图：肩叉顶部"没连上"）。且承压面整个悬空。
|
||
- 用 32.2 圆腔（当前）：承压面段整圈挖到 -2.6，凸台消失；同时 r=32.2..35
|
||
这圈 2.8mm 厚的壁完整保留，把后端板与上方臂身连成一体。0/90/180/270
|
||
方向该壁与方腔壁齐平，45° 方向比方腔多留料把四角补实。
|
||
PCBA 方腔本体仍从 z=0 起（z<0 那段由 32.2 圆腔负责，两者在此平接）。
|
||
"""
|
||
half = PCB_W / 2.0
|
||
c = [cyl(BORE_R, 5.5, COVER_Z - 3.5), cyl(CB_R, COVER_Z - 3.5, COVER_Z + front_pad),
|
||
# 承压面段：SEAT_R(=32.2 方腔内切圆) 圆腔，从承压面挖到 z=SEAT_TOP(=0)
|
||
# 与方腔平接。挖到 32.2 而不是 35 —— 35 会把外壁整圈切断（见 docstring）
|
||
cyl(SEAT_R, -REAR_SEAT, SEAT_TOP),
|
||
# PCBA 方腔本体：从 z=0 起（承压面段那 2.6mm 由上面的圆腔负责）
|
||
box(-half, half, -half, half, 0.0, M["pcba_t"] + 0.4)]
|
||
return shape.cut(*[self.L(s) for s in c])
|
||
|
||
def rear_mount_holes(self, shape, t_plate=8.0, hole_d=2.8, cb_d=5.5, cb_depth=4.0):
|
||
"""Cut the 4x rear mounting bolt holes into a proximal link body.
|
||
Matches the 4x M3 tapped radial slots on the motor rear endplate
|
||
(电机.STEP 实测)：BCD55、相位 39.9°+90k，槽宽 2.5、径向可调约 ±1.25mm。
|
||
旧版误用 35.1x40.9 矩形孔距：对角两颗偏出螺纹槽 ~4.7mm 无处咬合
|
||
（2026-10-02 用户指出"螺钉没有对齐"，按 STEP 实测槽位修正）。
|
||
"""
|
||
cuts = []
|
||
for k in range(4):
|
||
a = math.radians(39.9 + 90.0 * k)
|
||
sx = 27.5 * math.cos(a)
|
||
sy = 27.5 * math.sin(a)
|
||
h = cyl(hole_d / 2.0, -t_plate - 1.0, 3.0, sx, sy)
|
||
cuts.append(self.L(h))
|
||
if cb_d > 0 and cb_depth > 0:
|
||
cb = cyl(cb_d / 2.0, -t_plate - 1.0, -t_plate + cb_depth, sx, sy)
|
||
cuts.append(self.L(cb))
|
||
return shape.cut(*cuts)
|
||
|
||
def front_collar_pocket(self, shape, r_bore=30.25, z0=50.0, z1=56.5):
|
||
"""Cut front locating collar pocket for the motor front rim."""
|
||
p = cyl(r_bore, z0, z1)
|
||
return shape.cut(self.L(p))
|
||
|
||
def pinch(self, shape, fracs=(0.26, 0.58, 0.86), r_out=48.0, n=2):
|
||
"""Radial M3 pinch screws through the boss wall onto the housing."""
|
||
cuts = []
|
||
for fr in fracs[:n]:
|
||
for a in (self.lug_phase, self.lug_phase + 180.0):
|
||
c = cyl(PINCH_D / 2.0, -1.0, r_out, 0, 0)
|
||
c = place(c, R(a, 0, 0, 1))
|
||
c = place(c, T(0, 0, fr * COVER_Z))
|
||
cuts.append(self.L(c))
|
||
return shape.cut(*cuts)
|
||
|
||
def hub_face(self):
|
||
return self.at(HUB_FACE_Z)
|
||
|
||
def cut_hub(self, shape, depth=24.0, r_clear=32.0):
|
||
"""Cut the motor-output replica into the driven hub. The mating plane sits at
|
||
motor-local z = HUB_FACE_Z, and the recesses open there and run into the driven
|
||
material (away from the motor), so that half B's pilot (0.60) and spigot (4.06)
|
||
land in them."""
|
||
H = HUB_FACE_Z
|
||
# measured on the delivered half B: past the mating plane it is Ø35.6 for 1.4 mm
|
||
# then a Ø6.3 spigot to 4.06 - the nominal neck_depth of 1.0 is 0.4 too shallow,
|
||
# which showed up as a ~160 mm3 interference at every driven hub
|
||
cuts = [cyl(M["neck_d"] / 2.0, H - 0.05, H + 1.70),
|
||
cyl(M["centre_d"] / 2.0, H - 0.05, H + 5.05),
|
||
# probed on the delivered half: its Ø54 back face sits ~0.06 mm PAST the
|
||
# nominal plane, so the driven face gets a 0.35 deep, Ø54.6 relief. That is
|
||
# also the printed-part tolerance land the joint wants.
|
||
cyl(27.30, H - 0.05, H + 0.35)]
|
||
for a in HUB_M2_CLOCK:
|
||
u = M["m2_bcd"] / 2.0 * math.cos(a * D2R)
|
||
v = M["m2_bcd"] / 2.0 * math.sin(a * D2R)
|
||
cuts.append(cyl(M["m2_tap_d"] / 2.0, H - 0.05, H + M["m2_depth"], u, v))
|
||
return shape.cut(*[self.L(s) for s in cuts])
|
||
|
||
def hub_island(self, shape, r=30.0, t=9.5):
|
||
"""Solid disc at the joint centre (the lightening dish must not break into the
|
||
interface recesses), then the output replica is cut into it. Starts exactly on the
|
||
mating plane: half B's Ø54 body ends there, so any overlap is a hard clash."""
|
||
shape = shape.fuse(self.L(cyl(r, HUB_FACE_Z, HUB_FACE_Z + t)))
|
||
shape = self.cut_hub(shape)
|
||
# two arc shrouds over the clamp band: the joint reads as a closed drum, and the
|
||
# sectors are placed so the M3 lug bolts on the joint's local +-X stay reachable
|
||
for (a0, a1) in ((34.0, 146.0), (214.0, 326.0)):
|
||
shape = shape.fuse(self.L(sector(38.0, 31.6, a0, a1, FLANGE_Z + 1.4, HUB_FACE_Z)))
|
||
return shape
|
||
|
||
def place_hardware(self, half, clamp, tag):
|
||
"""Place the existing coupling halves + split clamp for this joint.
|
||
|
||
The delivered half is authored with z=0 at the motor flange face and spans
|
||
z -3.00..8.06. B = flip 180 deg about the local X axis, then shift by
|
||
2*PART_Z, which puts its flat back face exactly on the hub mating plane
|
||
(motor-local 65.00) - verified against the measured feature bounds.
|
||
"""
|
||
tf = trs(self.at(FLANGE_Z), self.xdir, self.d)
|
||
hA = place(half, tf)
|
||
hb = place(half, R(180, 1, 0, 0))
|
||
hb = place(hb, T(0, 0, 2 * C["part_z"]))
|
||
hB = place(hb, tf)
|
||
cA = place(clamp, tf)
|
||
cB = place(place(clamp, R(180, 0, 0, 1)), tf) # the two C pieces are one ring split
|
||
# along a diametral plane: 180 deg about the JOINT axis, so both keep the same
|
||
# axial span 56.602..64.402 and sit on opposite sides of the band.
|
||
return [("%s_半联轴器A" % tag, hA), ("%s_半联轴器B" % tag, hB),
|
||
("%s_抱箍A" % tag, cA), ("%s_抱箍B" % tag, cB)]
|
||
|
||
|
||
# ---------------------------------------------------------------- printed part builders
|
||
def bx(x0, x1, y0, y1, z0, z1):
|
||
return box(min(x0, x1), max(x0, x1), min(y0, y1), max(y0, y1), min(z0, z1), max(z0, z1))
|
||
|
||
|
||
GRP = { # part -> the joint group that carries it (kinematic tree)
|
||
"P01_底座板": "root", "P02_立柱": "root", "M1_J1": "root",
|
||
"J1_半联轴器A": "root", "J1_抱箍A": "root", "J1_抱箍B": "root",
|
||
"P03_转塔": "J1", "J1_半联轴器B": "J1",
|
||
"P04_肩支架": "J1", "M2_J2": "J1", "J2_半联轴器A": "J1",
|
||
"J2_抱箍A": "J1", "J2_抱箍B": "J1",
|
||
"P05_大臂": "J2", "J2_半联轴器B": "J2", "M3_J3": "J2",
|
||
"J3_半联轴器A": "J2", "J3_抱箍A": "J2", "J3_抱箍B": "J2",
|
||
"P06_小臂": "J3", "J3_半联轴器B": "J3", "M4_J4": "J3",
|
||
"J4_半联轴器A": "J3", "J4_抱箍A": "J3", "J4_抱箍B": "J3",
|
||
"P07_腕板": "J4", "P11_爪壳": "J4", "J4_半联轴器B": "J4", "M5_J5": "J4",
|
||
"P08_指A": "J4", "P09_指B": "J4", "P10_曲柄": "J5",
|
||
}
|
||
JAW = {"P08_指A": 1.0, "P09_指B": -1.0} # sign of the jaw travel along the J5 local +X
|
||
|
||
|
||
def explode_vec(p, JS, cents, gmean):
|
||
"""Explosion offset: coupling hardware separates along its joint axis, everything
|
||
else moves radially out of the assembly centroid."""
|
||
nm = p["name"]
|
||
c = cents[nm]
|
||
for jid in ("J1", "J2", "J3", "J4"):
|
||
if nm.startswith(jid + "_半联轴器B"):
|
||
return [JS[jid].d[i] * 24.0 for i in range(3)]
|
||
if nm.startswith(jid + "_半联轴器A") or nm.startswith(jid + "_抱箍"):
|
||
k = -26.0 if "抱箍" in nm else -16.0
|
||
return [JS[jid].d[i] * k for i in range(3)]
|
||
v = [c[i] - gmean[i] for i in range(3)]
|
||
m = math.sqrt(sum(x * x for x in v)) or 1.0
|
||
return [v[i] / m * 34.0 for i in range(3)]
|
||
|
||
|
||
# ---------------------------------------------------------------- assembly
|
||
def write_step(items, path):
|
||
w = STEPControl_Writer()
|
||
for nm, sh in items:
|
||
Interface_Static.SetCVal_s('write.step.product.name', nm)
|
||
w.Transfer(sh.wrapped, STEPControl_StepModelType.STEPControl_AsIs)
|
||
w.Write(path.replace("/", "\\"))
|
||
return os.path.getsize(path)
|
||
|
||
|
||
import threading
|
||
import time
|
||
from concurrent.futures import ThreadPoolExecutor
|
||
|
||
def export(JS):
|
||
for d in (OUT, OUT + "/parts_step", OUT + "/print_stl", OUT + "/assembly", WEB, DOCS):
|
||
os.makedirs(d, exist_ok=True)
|
||
|
||
print("=" * 80, flush=True)
|
||
print("【第二阶段 2/5】并行导出 STEP / STL 制造模型 (out/parts_step, out/print_stl)...", flush=True)
|
||
print("=" * 80, flush=True)
|
||
|
||
# 1. 多线程并行导出 STEP 与 STL（复用已有标准电机 STEP，极速提速）
|
||
printed_parts = [p for p in PARTS if p["printed"]]
|
||
total_exp = len(printed_parts)
|
||
done_exp = 0
|
||
exp_lock = threading.Lock()
|
||
|
||
def export_one_part(p):
|
||
nonlocal done_exp
|
||
t0 = time.time()
|
||
fn = "%s/%s.step" % (OUT + "/parts_step", p["name"])
|
||
# 如果是非打印标准已有件且已存在，跳过冗余写盘
|
||
if not p["printed"] and os.path.exists(fn) and os.path.getsize(fn) > 1000:
|
||
pass
|
||
else:
|
||
write_step([(p["name"], p["shape"])], fn)
|
||
if p["printed"]:
|
||
stl = "%s/%s.stl" % (OUT + "/print_stl", p["name"])
|
||
cq.exporters.export(cq.Workplane(obj=p["shape"]), stl, exportType="STL")
|
||
with exp_lock:
|
||
done_exp += 1
|
||
dt = time.time() - t0
|
||
print(f" [文件导出 {done_exp}/{total_exp}] {p['name']}.step / .stl 导出完成 ({dt:.2f}s) ✓", flush=True)
|
||
return p["name"]
|
||
|
||
workers = 1
|
||
with ThreadPoolExecutor(max_workers=workers) as ex:
|
||
list(ex.map(export_one_part, PARTS))
|
||
|
||
print(f"-> 制造模型导出全部完成！已保存在 {OUT}/parts_step 与 {OUT}/print_stl\n", flush=True)
|
||
|
||
# 2. 多线程并行生成网页 3D Mesh
|
||
print("=" * 80, flush=True)
|
||
print("【第三阶段 3/5】多线程并行生成网页 3D 轻量化几何网格 (web/mesh_arm.json)...", flush=True)
|
||
print("=" * 80, flush=True)
|
||
|
||
done_mesh = 0
|
||
mesh_lock = threading.Lock()
|
||
total_parts = len(PARTS)
|
||
|
||
def mesh_one_part(p):
|
||
nonlocal done_mesh
|
||
t0 = time.time()
|
||
key = "motor" if p["name"].startswith("M") else ("coupl" if "联轴器" in p["name"] or "抱箍" in p["name"] else p["name"])
|
||
defl = 0.85 if p["name"].startswith("M") else (0.20 if key == "coupl" else 0.055)
|
||
v, t = mesh_of(p["shape"], defl)
|
||
with mesh_lock:
|
||
done_mesh += 1
|
||
n_v = len(v) // 3
|
||
n_t = len(t) // 3
|
||
dt = time.time() - t0
|
||
print(f" [网格细分 {done_mesh:2d}/{total_parts}] {p['name']:<14} 顶点: {n_v:5d} | 面片: {n_t:5d} ({dt:.2f}s) ✓", flush=True)
|
||
return (p, v, t)
|
||
|
||
with ThreadPoolExecutor(max_workers=workers) as ex:
|
||
mesh_results = list(ex.map(mesh_one_part, PARTS))
|
||
|
||
parts = []
|
||
for idx, (p, v, t) in enumerate(mesh_results):
|
||
parts.append(dict(name=p["name"], cn=p["cn"], v=v, t=t, color=p["color"],
|
||
printed=p["printed"], vol=round(p["vol"], 0), note=p["note"],
|
||
grp=GRP.get(p["name"], "root"), jaw=JAW.get(p["name"], 0.0),
|
||
ex=[round(x, 1) for x in explode_vec(p, JS, CENTS,
|
||
[sum(CENTS[k][i] for k in CENTS) / float(len(CENTS)) for i in range(3)])
|
||
],
|
||
idx=idx + 1))
|
||
|
||
joints = [dict(id=k, cn=JS[k].cn, rear=JS[k].rear, d=JS[k].d, xdir=JS[k].xdir, hub=JS[k].hub_face())
|
||
for k in JS]
|
||
d3 = lambda a, b: math.sqrt(sum((a[i] - b[i]) ** 2 for i in range(3)))
|
||
json.dump(dict(parts=parts, joints=joints,
|
||
layout=dict(ver=VERSION, it=ITER,
|
||
L1=round(d3(JS["J2"].rear, JS["J4"].rear), 1),
|
||
L2=round(d3(JS["J4"].rear, JS["J5"].rear), 1)),
|
||
measured=dict(M=M, C=C, HUB_FACE_Z=HUB_FACE_Z, CLAMP_SPAN=list(CLAMP_SPAN))),
|
||
open(WEB + "/mesh_arm.json", "w", encoding="utf-8"), ensure_ascii=False)
|
||
bom = [dict(件号=p["name"], 名称=p["cn"], 类型="3D打印" if p["printed"] else "已有/电机",
|
||
数量=1, 体积cm3=round(p["vol"] / 1000.0, 1),
|
||
质量g_PLA=round(p["vol"] * 1.24e-3, 1), 包络=p["bb"], 说明=p["note"]) for p in PARTS]
|
||
json.dump(bom, open(DOCS + "/bom.json", "w", encoding="utf-8"), ensure_ascii=False, indent=1)
|
||
print(f"-> 3D网格与BOM清单已保存至 web/mesh_arm.json 与 docs/bom.json ✓\n", flush=True)
|
||
return bom
|
||
|
||
|
||
def collide():
|
||
ps = [p for p in PARTS if p["vol"] > 1.0]
|
||
pairs = []
|
||
MOUNT_PAIRS = {
|
||
("P03_肩吊臂_螺旋体35%_墙5底5_竖打", "M1_J1"), ("P04_肩叉_螺旋体30%_墙5底5_竖打", "M1_J1"),
|
||
("P04_肩叉_螺旋体30%_墙5底5_竖打", "M2_J2"), ("P05_大臂旋转座_螺旋体30%_墙4底5_竖打", "M2_J2"),
|
||
("P05_大臂旋转座_螺旋体30%_墙4底5_竖打", "M3_J3"), ("P06_大臂骨架_螺旋体25%_墙4底5_竖打", "M3_J3"),
|
||
("P06_大臂骨架_螺旋体25%_墙4底5_竖打", "M4_J4"), ("P07_小臂骨架_螺旋体30%_墙5底5_竖打", "M4_J4"),
|
||
("P07_小臂骨架_螺旋体30%_墙5底5_竖打", "M5_J5"), ("P08_腕偏转座_螺旋体25%_墙4底5_竖打", "M5_J5"),
|
||
("P08_腕偏转座_螺旋体25%_墙4底5_竖打", "M6_J6"), ("P09_工具法兰_螺旋体30%_墙4底5_竖打", "M6_J6"),
|
||
("P01_A_立柱下段_三角形30%_墙4底5_竖打", "P01_B_立柱中段_三角形30%_墙4底5_竖打"), ("P01_B_立柱中段_三角形30%_墙4底5_竖打", "P01_C_立柱上段_三角形30%_墙4底5_竖打"),
|
||
("P01_A_立柱下段_三角形30%_墙4底5_竖打", "P02_Center_底座中核心_螺旋体30%_墙5底6_平打"),
|
||
("P02_Center_底座中核心_螺旋体30%_墙5底6_平打", "P02_Front_前向防翻力臂_螺旋体30%_墙4底5_平打"),
|
||
("P02_Center_底座中核心_螺旋体30%_墙5底6_平打", "P02_Rear_后向防翻力臂_螺旋体30%_墙4底5_平打"),
|
||
("P01_C_立柱上段_三角形30%_墙4底5_竖打", "P03_肩吊臂_螺旋体35%_墙5底5_竖打"),
|
||
}
|
||
for i in range(len(ps)):
|
||
for j in range(i + 1, len(ps)):
|
||
a, b = ps[i], ps[j]
|
||
if a["name"].startswith("M") and b["name"].startswith("M"):
|
||
continue
|
||
if (a["name"], b["name"]) in MOUNT_PAIRS or (b["name"], a["name"]) in MOUNT_PAIRS:
|
||
continue
|
||
av, bv = bb(a["shape"]), bb(b["shape"])
|
||
if any(av[1][k] < bv[0][k] - 0.05 or bv[1][k] < av[0][k] + 0.05 for k in range(3)):
|
||
continue
|
||
pairs.append((a, b))
|
||
|
||
print("=" * 80, flush=True)
|
||
print(f"【第四阶段 4/5】多线程精确布尔碰撞检测（从 {len(ps)*(len(ps)-1)//2} 对中初筛出 {len(pairs)} 组潜在包络相交对）...", flush=True)
|
||
print("=" * 80, flush=True)
|
||
|
||
done_pair = 0
|
||
pair_lock = threading.Lock()
|
||
|
||
def check_pair(pair):
|
||
nonlocal done_pair
|
||
a, b = pair
|
||
t0 = time.time()
|
||
v = common_vol(a["shape"], b["shape"])
|
||
with pair_lock:
|
||
done_pair += 1
|
||
status = f"干涉 {v:.1f} mm³ ❌" if v > 2.0 else "零干涉 (0.0 mm³) ✓"
|
||
print(f" [干涉检测 {done_pair:2d}/{len(pairs)}] {a['name']:<14} x {b['name']:<14} : {status} ({time.time()-t0:.3f}s)", flush=True)
|
||
if v > 2.0:
|
||
try:
|
||
c = BRepAlgoAPI_Common(a["shape"].wrapped, b["shape"].wrapped)
|
||
c.SetRunParallel(True)
|
||
c.Build()
|
||
cb = cq.Shape(c.Shape())
|
||
lo, hi = bb(cb)
|
||
loc = "@[%.0f %.0f %.0f..%.0f %.0f %.0f]" % (
|
||
lo[0], lo[1], lo[2], hi[0], hi[1], hi[2])
|
||
except Exception:
|
||
loc = "?"
|
||
return (a["name"], b["name"], round(v, 1), loc)
|
||
return None
|
||
|
||
rows = []
|
||
for pair in pairs:
|
||
res = check_pair(pair)
|
||
if res is not None:
|
||
rows.append(res)
|
||
|
||
rows.sort(key=lambda r: -r[2])
|
||
json.dump([r[:3] for r in rows], open(DOCS + "/collision.json", "w", encoding="utf-8"),
|
||
ensure_ascii=False)
|
||
if len(rows) == 0:
|
||
print(f"\n-> 静态碰撞干涉检测通过: 0 对干涉！全部 {len(pairs)} 组接触面对零碰撞！\n", flush=True)
|
||
else:
|
||
print(f"\n-> 发现 {len(rows)} 对干涉！\n", flush=True)
|
||
return rows
|
||
|
||
|
||
def axial_extent(j, shape):
|
||
lo, hi = bb(shape)
|
||
vals = []
|
||
for i in (0, 1):
|
||
for k in range(3):
|
||
p = [lo[0], lo[1], lo[2]]
|
||
p[k] = hi[k] if i else lo[k]
|
||
vals.append(sum((p[q] - j.rear[q]) * j.d[q] for q in range(3)))
|
||
return min(vals), max(vals)
|
||
|
||
|
||
def verify_interfaces(half, clamp, JS):
|
||
"""Numeric self-check of the derived joint stack, before trusting the renders."""
|
||
rows = []
|
||
for k, j in JS.items():
|
||
if k == "J5":
|
||
continue
|
||
parts = dict(j.place_hardware(half, clamp, k))
|
||
a = axial_extent(j, parts["%s_半联轴器A" % k])
|
||
b = axial_extent(j, parts["%s_半联轴器B" % k])
|
||
c = axial_extent(j, parts["%s_抱箍A" % k])
|
||
hub = j.at(HUB_FACE_Z)
|
||
rows.append(dict(jid=k, halfA=[round(x, 3) for x in a], halfB=[round(x, 3) for x in b],
|
||
clamp=[round(x, 3) for x in c], hub_face=round(HUB_FACE_Z, 3),
|
||
spigot_proud=round(b[1] - HUB_FACE_Z, 3),
|
||
clamp_clear=round(c[0] - COVER_Z, 3)))
|
||
return rows
|