feat: 增加组合折叠姿态干涉扫描、力矩实测与结构强度复核数据
This commit is contained in:
+253
-32
@@ -17,6 +17,7 @@ import io
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||||
@@ -40,6 +41,8 @@ RHO = {"PLA": 1.24e-3, "motor_g": 210.0} # g/mm3 for printed, g per module
|
||||
INFILL = float(os.environ.get("ARMINFILL", "0.38"))
|
||||
# allowable bending stress across the print layers, after notch + creep derating
|
||||
ALLOW = {"PLA": 12.0, "PETG": 20.0, "PA_CF": 30.0} # MPa
|
||||
# Young's modulus of the printed lattice, for the deflection estimate only
|
||||
EMOD = {"PLA": 3000.0, "PETG": 2100.0, "PA_CF": 8000.0} # MPa
|
||||
MAT = os.environ.get("ARMMAT", "PLA")
|
||||
PAYLOAD = float(os.environ.get("ARMPAY", "0.5")) # kg held at the fingertip
|
||||
DYN = float(os.environ.get("ARMDYN", "2.0")) # dynamic factor on the moving mass
|
||||
@@ -48,6 +51,7 @@ G981 = 9.81
|
||||
GRIP_T = 12.0 # N*mm per motor is far too small to matter; the grip force
|
||||
# case is carried separately below as a normal force at the pad
|
||||
_cache = {}
|
||||
GRP = {"root": 0, "J1": 1, "J2": 2, "J3": 3, "J4": 4, "J5": 5, "J6": 6}
|
||||
|
||||
|
||||
def load(name):
|
||||
@@ -173,6 +177,210 @@ def arm_masses():
|
||||
return out, tot
|
||||
|
||||
|
||||
# ------------------------------------------------------------------ 零件表：质量 + 零位世界质心
|
||||
def _mesh_vol_cg(v, t):
|
||||
"""四面体法求闭合三角网格的体积与体积加权质心。
|
||||
|
||||
网格顶点已在零位世界系（mesh_arm.json 由 arm_model 直接落盘），所以质心就是零件
|
||||
在装配中的真实位置。实测体积与 STEP/BOM 差 <0.1%（电机包络 1.7%），可作载荷输入。
|
||||
"""
|
||||
vol = mx = my = mz = 0.0
|
||||
for a, b, c in t:
|
||||
x1, y1, z1 = v[a]
|
||||
x2, y2, z2 = v[b]
|
||||
x3, y3, z3 = v[c]
|
||||
det = x1 * (y2 * z3 - y3 * z2) - x2 * (y1 * z3 - y3 * z1) + x3 * (y1 * z2 - y2 * z1)
|
||||
vol += det
|
||||
mx += (x1 + x2 + x3) * det
|
||||
my += (y1 + y2 + y3) * det
|
||||
mz += (z1 + z2 + z3) * det
|
||||
if vol <= 0.0:
|
||||
return 0.0, None
|
||||
V = vol / 6.0
|
||||
return V, (mx / (24.0 * V), my / (24.0 * V), mz / (24.0 * V))
|
||||
|
||||
|
||||
def part_table(payload_kg=None):
|
||||
"""[(name, mass_g, cg_xyz, grp)]，含末端负载这个集中质量点。"""
|
||||
meta = json.load(io.open(OUT.replace("/out", "/web") + "/mesh_arm.json", encoding="utf-8"))
|
||||
rows = []
|
||||
for p in meta["parts"]:
|
||||
nm = p["name"]
|
||||
if nm.startswith("ENV_") or not p.get("v") or not p.get("t") or p.get("vol", 0) <= 0:
|
||||
continue # 可达包络网格 / 无实体螺钉实例，不承载
|
||||
V, cg = _mesh_vol_cg(p["v"], p["t"])
|
||||
if V <= 0.0 or cg is None:
|
||||
continue
|
||||
if p.get("printed"):
|
||||
m = V * RHO["PLA"] * INFILL
|
||||
elif re.match(r"^M\d", nm):
|
||||
m = RHO["motor_g"]
|
||||
else:
|
||||
continue
|
||||
rows.append([nm, m, cg, GRP.get(p["grp"], 0)])
|
||||
if payload_kg:
|
||||
rows.append(["PAYLOAD", payload_kg * 1000.0,
|
||||
(HL.X_ARM, HL.Y_AXIS, HL.Z_TIP), 6])
|
||||
return rows
|
||||
|
||||
|
||||
# ------------------------------------------------------------------ 运动学：产品外参链
|
||||
def _rodrigues(deg, d, p):
|
||||
"""绕世界轴 d 过点 p 转 deg -> (R, t)，使 x -> R x + t。"""
|
||||
n = math.sqrt(sum(c * c for c in d)) or 1.0
|
||||
x, y, z = d[0] / n, d[1] / n, d[2] / n
|
||||
a = math.radians(deg)
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
t1 = 1.0 - c
|
||||
R = [[t1 * x * x + c, t1 * x * y - s * z, t1 * x * z + s * y],
|
||||
[t1 * x * y + s * z, t1 * y * y + c, t1 * y * z - s * x],
|
||||
[t1 * x * z - s * y, t1 * y * z + s * x, t1 * z * z + c]]
|
||||
t = [p[i] - (R[i][0] * p[0] + R[i][1] * p[1] + R[i][2] * p[2]) for i in range(3)]
|
||||
return R, t
|
||||
|
||||
|
||||
def chain(ang):
|
||||
"""acc[g] = (R, t)：第 g 组零件的刚体变换。
|
||||
|
||||
acc[g] = acc[g-1] o R_g（产品外参，右侧先作用），与 sweep_v2.accum_trsf、
|
||||
查看器嵌套 pivot 三者一致。
|
||||
"""
|
||||
R = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
|
||||
t = [0.0, 0.0, 0.0]
|
||||
out = [(R, t)]
|
||||
for j in range(1, 7):
|
||||
ax = HL.AXIS["J%d" % j]
|
||||
Rj, tj = _rodrigues(ang.get(j, 0.0), ax["d"], ax["rear"])
|
||||
Rn = [[sum(R[i][k] * Rj[k][j2] for k in range(3)) for j2 in range(3)] for i in range(3)]
|
||||
tn = [sum(R[i][k] * tj[k] for k in range(3)) + t[i] for i in range(3)]
|
||||
R, t = Rn, tn
|
||||
out.append((R, t))
|
||||
return out
|
||||
|
||||
|
||||
def _xf(T, c):
|
||||
R, t = T
|
||||
return [sum(R[i][k] * c[k] for k in range(3)) + t[i] for i in range(3)]
|
||||
|
||||
|
||||
def _dir(T, v):
|
||||
"""只取旋转部分——方向向量绝不能带平移，否则投影出的"力矩"会被平移量放大。"""
|
||||
R = T[0]
|
||||
return [sum(R[i][k] * v[k] for k in range(3)) for i in range(3)]
|
||||
|
||||
|
||||
def _cross(a, b):
|
||||
return [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
|
||||
|
||||
|
||||
def joint_loads(parts, payload_kg=None):
|
||||
"""在可达姿态网格上搜索，返回每个关节的：
|
||||
|
||||
F 该关节以下的总重力 (N)（与姿态无关）
|
||||
Mxyz 重力对关节轴点取矩后，三个全局分量各自的最大绝对值 (N*mm)
|
||||
tau 重力绕该关节自身轴的最大力矩 (N*mm) —— 电机真正要出的力
|
||||
Lp 出现 tau 最大值时，TCP 到该关节轴点的距离 (mm)
|
||||
|
||||
为什么搜索姿态：力臂不是常数。J1 是绕 X 的自转轴，手臂在轴正下方时重力对它
|
||||
只产生绕 X 的扭矩（不产生弯曲），手臂摆到侧方时扭矩最大。旧算法把全部 distal
|
||||
质量当作集中在指尖，量级完全错掉。
|
||||
"""
|
||||
trv = {t["axis"]: t for t in json.load(io.open(DOCS + "/travel.json", encoding="utf-8"))}
|
||||
|
||||
def picks(j):
|
||||
t = trv.get("J%d" % j, {"lo": -180.0, "hi": 180.0})
|
||||
if t.get("continuous"):
|
||||
return [0.0, 60.0, -60.0, 120.0, -120.0, 180.0]
|
||||
lo, hi = t["lo"], t["hi"]
|
||||
return sorted({lo, 0.0 if lo < 0 < hi else lo, hi, 0.5 * (lo + hi)})
|
||||
|
||||
grid = [picks(j) for j in range(1, 7)]
|
||||
res = {}
|
||||
for j in range(1, 7):
|
||||
res["J%d" % j] = dict(F=0.0, Mxyz=[0.0, 0.0, 0.0], tau=0.0, Lp=0.0, I=0.0, Iarm=0.0)
|
||||
|
||||
import itertools
|
||||
for combo in itertools.product(*grid):
|
||||
ang = {k + 1: v for k, v in enumerate(combo)}
|
||||
acc = chain(ang)
|
||||
pos = [(nm, m, _xf(acc[g], cg), g) for nm, m, cg, g in parts]
|
||||
tcp = None
|
||||
for nm, m, c, g in pos:
|
||||
if nm == "PAYLOAD":
|
||||
tcp = c
|
||||
for j in range(1, 7):
|
||||
ax = HL.AXIS["J%d" % j]
|
||||
pj = _xf(acc[j - 1], ax["rear"])
|
||||
dj = _dir(acc[j - 1], ax["d"])
|
||||
F = M = [0.0, 0.0, 0.0]
|
||||
wsum = 0.0
|
||||
I = Iarm = 0.0
|
||||
for _nm, m, c, g in pos:
|
||||
if g < j:
|
||||
continue
|
||||
w = m * G981 / 1000.0
|
||||
wsum += w
|
||||
r = [c[i] - pj[i] for i in range(3)]
|
||||
proj = sum(r[i] * dj[i] for i in range(3))
|
||||
perp2 = sum(r[i] * r[i] for i in range(3)) - proj * proj
|
||||
I += m * perp2 / 1e9 # g*mm^2 -> kg*m^2
|
||||
if _nm != "PAYLOAD":
|
||||
Iarm += m * perp2 / 1e9
|
||||
rc = _cross(r, [0.0, 0.0, -w])
|
||||
for i in range(3):
|
||||
M[i] += rc[i]
|
||||
r = res["J%d" % j]
|
||||
r["F"] = max(r["F"], wsum)
|
||||
for i in range(3):
|
||||
r["Mxyz"][i] = max(r["Mxyz"][i], abs(M[i]))
|
||||
tau = abs(M[0] * dj[0] + M[1] * dj[1] + M[2] * dj[2])
|
||||
if tau > r["tau"]:
|
||||
r["tau"] = tau
|
||||
r["Lp"] = math.dist(tcp, pj) if tcp else 0.0
|
||||
r["I"] = max(r["I"], I)
|
||||
r["Iarm"] = max(r["Iarm"], Iarm)
|
||||
return res
|
||||
|
||||
|
||||
def section_moment(jid, bend_axis, ref, refgrp, parts):
|
||||
"""截面绕自身形心、沿 bend_axis 的弯矩最大值 (N*mm)，以及该姿态下 TCP 到该
|
||||
截面的距离 (mm)（挠度估算用）。
|
||||
|
||||
参考点取【剖面形心】而不是关节轴：这两个参考点差得很远。P03 在 z=480 的剖面
|
||||
形心 x≈-111，而臂身所有零件质心 x≈+19.5，真实力臂 130mm；若误用关节轴点
|
||||
(J1 在 x=-51)只会得到 70mm，把弯矩低估 1.85 倍。
|
||||
"""
|
||||
trv = {t["axis"]: t for t in json.load(io.open(DOCS + "/travel.json", encoding="utf-8"))}
|
||||
jj = int(jid[1])
|
||||
|
||||
def picks(j):
|
||||
t = trv.get("J%d" % j, {"lo": -180.0, "hi": 180.0})
|
||||
if t.get("continuous"):
|
||||
return [0.0, 60.0, -60.0, 120.0, -120.0, 180.0]
|
||||
lo, hi = t["lo"], t["hi"]
|
||||
return sorted({lo, 0.0 if lo < 0 < hi else lo, hi, 0.5 * (lo + hi)})
|
||||
|
||||
import itertools
|
||||
best = 0.0
|
||||
best_L = 0.0
|
||||
for combo in itertools.product(*[picks(j) for j in range(1, 7)]):
|
||||
acc = chain({k + 1: v for k, v in enumerate(combo)})
|
||||
rw = _xf(acc[refgrp], ref)
|
||||
s = 0.0
|
||||
tcp = rw
|
||||
for nm, m, cg, g in parts:
|
||||
if g < jj:
|
||||
continue
|
||||
c = _xf(acc[g], cg)
|
||||
if nm == "PAYLOAD":
|
||||
tcp = c
|
||||
s += m * (c[bend_axis] - rw[bend_axis])
|
||||
if abs(s) > best:
|
||||
best = abs(s)
|
||||
best_L = math.dist(tcp, rw)
|
||||
return best * G981 / 1000.0, best_L
|
||||
|
||||
|
||||
# critical cuts: (label, part, plane normal, plane origin, which load case owns it)
|
||||
def cuts():
|
||||
"""(label, part, cut-plane normal, point, joint whose load it carries, bend axis)
|
||||
@@ -190,7 +398,9 @@ def cuts():
|
||||
("小臂骨架中段 @z=245", "P07_小臂骨架_螺旋体30%_墙5底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 245.0), "J4", 1),
|
||||
("小臂骨架抱箍 @z=195", "P07_小臂骨架_螺旋体30%_墙5底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 195.0), "J5", 1),
|
||||
("腕偏转座根部 @z=150", "P08_腕偏转座_螺旋体25%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 150.0), "J5", 1),
|
||||
("工具法兰 @z=55", "P09_工具法兰_螺旋体100%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 55.0), "J6", 0),
|
||||
# P09 包络 z=93..121，剖面取 107（法兰中截面）。原 -55 落在零件之外，
|
||||
# 剖切返回空，工具法兰其实一直没被校核过。
|
||||
("工具法兰 @z=107", "P09_工具法兰_螺旋体100%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 107.0), "J6", 1),
|
||||
]
|
||||
|
||||
|
||||
@@ -200,16 +410,12 @@ MOMENT = { # worst reachable bending moment per joint, N*mm
|
||||
|
||||
|
||||
def main():
|
||||
masses, per = arm_masses()
|
||||
# Moment arms DERIVED from the swept travel (docs/travel.json), not hand-typed: they were
|
||||
# stale (J4 said 24 deg while the arm now sweeps 360, and J1 was missing entirely), which
|
||||
# silently under-loaded those sections. A continuous or >90 deg axis is capped at 90 deg
|
||||
# because that is where a gravity load sits furthest off the axis.
|
||||
TRV = json.load(io.open(DOCS + "/travel.json", encoding="utf-8"))
|
||||
dev = {t["axis"]: min(max(abs(t["hi"]), abs(t["lo"])), 90.0) for t in TRV}
|
||||
# distance from each joint axis out to the fingertip load, mm
|
||||
dist = {"J1": 564.0, "J2": 444.0, "J3": 380.0, "J4": 264.0, "J5": 129.0, "J6": 60.0}
|
||||
reach = {k: dist[k] * math.sin(math.radians(dev.get(k, 90.0))) for k in dist}
|
||||
parts = part_table(PAYLOAD)
|
||||
grpof = {nm: g for nm, _, _, g in parts}
|
||||
jl = joint_loads(parts)
|
||||
masses = {"J%d" % j: sum(m for _, m, _, g in parts if g >= j) for j in range(1, 7)}
|
||||
masses["all"] = sum(m for _, m, _, _ in parts)
|
||||
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
def compute_cut(c_item):
|
||||
@@ -218,46 +424,61 @@ def main():
|
||||
sp = section_props(sh, n, o, bend)
|
||||
if not sp:
|
||||
return (lab, part, None, jid)
|
||||
m_g = masses[jid] * DYN + PAYLOAD * 1000.0 * DYN
|
||||
F = m_g * G981 / 1000.0 # N
|
||||
# the section carries everything distal to its own joint, at that joint's worst
|
||||
# reachable moment arm, plus the grip reaction on the finger root
|
||||
M = F * reach[jid] # N*mm
|
||||
if jid == "J6":
|
||||
M = max(M, GRIP_T * 60.0)
|
||||
I = sp["I"] # already the real bend axis, not min(Iu,Iv)
|
||||
sigma = M * sp["c"] / max(I, 1e-6)
|
||||
# 剖面形心的三维重建 + 弯矩所绕的轴
|
||||
nvec = [float(x) for x in n]
|
||||
p_ax, q_ax = [k for k in (0, 1, 2) if abs(nvec[k]) < 0.5]
|
||||
bend_ax = sp["bend"]
|
||||
far_ax = p_ax if bend_ax == q_ax else q_ax # I 绕 bend_ax、c 沿 far_ax
|
||||
ref = list(o)
|
||||
ref[p_ax] += sp["centroid"][0]
|
||||
ref[q_ax] += sp["centroid"][1]
|
||||
M, L = section_moment(jid, far_ax, ref, grpof.get(part, 0), parts)
|
||||
M *= DYN
|
||||
F = jl[jid]["F"] * DYN
|
||||
I, c = sp["I"], sp["c"]
|
||||
sigma = M * c / max(I, 1e-6)
|
||||
tau = 1.5 * F / sp["A"]
|
||||
return (lab, part, dict(sp, M=M, F=F, sigma=sigma, tau=tau,
|
||||
SF=ALLOW[MAT] / sigma if sigma > 0 else 99.0), jid)
|
||||
defl = M * L * L / (2.0 * EMOD[MAT] * I) if I > 1e-6 and L > 0 else 0.0
|
||||
return (lab, part, dict(sp, M=M, F=F, sigma=sigma, tau=tau, L=L, defl=defl,
|
||||
bend_axis=bend_ax, SF=ALLOW[MAT] / sigma if sigma > 0 else 99.0), jid)
|
||||
|
||||
with ThreadPoolExecutor(max_workers=min(13, os.cpu_count() or 4)) as executor:
|
||||
rows = list(executor.map(compute_cut, cuts()))
|
||||
print("材料 %s 许用弯曲 %g MPa（跨层、含缺口与蠕变折减） 末端负载 %g kg 动载系数 %g\n"
|
||||
% (MAT, ALLOW[MAT], PAYLOAD, DYN))
|
||||
print("材料 %s 许用弯曲 %g MPa（跨层、含缺口与蠕变折减） 弹性模量 %g MPa 末端负载 %g kg 动载系数 %g"
|
||||
% (MAT, ALLOW[MAT], EMOD[MAT], PAYLOAD, DYN))
|
||||
print("载荷模型：每个零件按真实质心（网格体积加权）随姿态变换，在可达姿态网格上取弯矩最大值；"
|
||||
"参考点为剖面形心\n")
|
||||
print("打印件按 %d%% 实体率计质量（壁+填充），电机按 %.0f g/台" % (INFILL * 100, RHO["motor_g"]))
|
||||
print("%-26s %-10s %7s %9s %6s %8s %7s %5s %9s" %
|
||||
("截面", "零件", "面积mm2", "Imin mm4", "c mm", "M N*mm", "sigma", "SF", "需Z mm3"))
|
||||
print("%-24s %-9s %7s %10s %6s %9s %7s %6s %7s %7s"
|
||||
% ("截面", "零件", "面积mm2", "I mm4", "c mm", "M N*mm", "sigma", "SF", "挠度mm", "需Z mm3"))
|
||||
bad = []
|
||||
for lab, part, d, jid in rows:
|
||||
if not d:
|
||||
print("%-26s %-10s 剖切失败" % (lab, part))
|
||||
print("%-24s %-9s 剖切失败" % (lab, part))
|
||||
continue
|
||||
Zreq = d["M"] / ALLOW[MAT] * 1.5 # section modulus x1.5 SF
|
||||
print("%-26s %-10s %7.0f %9.0f %6.1f %8.0f %7.1f %5.2f %9.0f%s" %
|
||||
(lab, part, d["A"], d["I"], d["c"], d["M"], d["sigma"], d["SF"], Zreq,
|
||||
" <<< 不足" if d["SF"] < 1.5 else ""))
|
||||
print("%-24s %-9s %7.0f %10.0f %6.1f %9.0f %7.2f %6.2f %7.1f %7.0f%s"
|
||||
% (lab, part[:9], d["A"], d["I"], d["c"], d["M"], d["sigma"], d["SF"], d["defl"],
|
||||
Zreq, " <<< 不足" if d["SF"] < 1.5 else ""))
|
||||
if d["SF"] < 1.5:
|
||||
bad.append((lab, part, d))
|
||||
print("\n各关节以下质量(g, 含电机): " + " ".join("%s=%.0f" % (k, v) for k, v in masses.items() if k != "all"))
|
||||
print("整机质量(含 6 电机) %.0f g" % masses["all"])
|
||||
print("\n各关节以下质量(g, 含电机与末端负载): "
|
||||
+ " ".join("%s=%.0f" % (k, v) for k, v in masses.items() if k != "all"))
|
||||
print("合计 %.0f g （其中末端负载 %.0f g）" % (masses["all"], PAYLOAD * 1000))
|
||||
print("\n各关节电机需出的重力力矩（绕自身轴，姿态搜索最大值）:")
|
||||
for j in range(1, 7):
|
||||
r = jl["J%d" % j]
|
||||
print(" J%d %6.2f N·m （该关节以下总重 %.1f N，力臂 %.0f mm）"
|
||||
% (j, r["tau"] / 1000.0, r["F"], r["Lp"]))
|
||||
io.open(DOCS + "/strength.json", "w", encoding="utf-8").write(
|
||||
json.dumps([dict(截面=a, 零件=b, 关节=d, **(c or {})) for a, b, c, d in rows],
|
||||
ensure_ascii=False, indent=1, default=float))
|
||||
return bad
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
b = main()
|
||||
print("\n不合格截面 %d 处" % len(b))
|
||||
sys.stdout.flush() # os._exit 不刷缓冲，重定向到文件时否则输出全丢
|
||||
os._exit(0)
|
||||
|
||||
Reference in New Issue
Block a user