cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(layout): src/ 收成顶层包 arm/，入口统一 python -m；提升承担构建的 scratch 脚本

Browse Source 
单文件约束取消后，Python 侧没必要再靠 16 处 sys.path.insert 维持扁平模块互认。
现在按职责分包：
  arm/paths.py 路径唯一来源（含新增 CACHE、TPL）
  arm/constants.py 实测接口数值（原 armcore 的 M/C/CLR/FIT/MOTOR_TOL 段）
  arm/cad/{helpers,arm_build,part_tools,parts/} 几何与装配
  arm/kinematics.py（原 human_layout.py，唯一运动学事实源）
  arm/analysis/{sweep_v2,strength_check,torque_balance,finalize_travel,ws_sweep,gen_env_mesh}
  arm/dataio/{screw_mesh,draw_sheet}   arm/viewer/{build_viewer,shot}
  arm/build_model.py（原 arm_model.py）

顺带修掉的既有缺陷（都是实测撞出来的，不是预防性改动）：
- arm_model 第二次 sys.path 跳到 gitignore 的 scratch/ 去 import 构建必需的包络脚本：
  ws_sweep/gen_env_mesh 提升进 arm/analysis/，缓存挪到 data/cache/（gitignore，可重生成）。
- finalize_travel / draw_sheet 顶层就是"算完写文件"，一次 importlib 冒烟测试就把
  docs/travel.json 重写了一遍；现在被 import 直接抛错。
- strength_check 用 OUT.replace("/out","/web") 靠字符串形状推路径，改成直接用 WEB。
- build_viewer 里 WORKSPACE 使用点漏改导致 NameError —— 已由构建关卡抓到并修掉。

os._exit 的处置与计划里的假设相反：实测光是 `python -c "import arm.cad.helpers"`
就会在解释器退出时段错误（exit 139，OCC 静态析构），全流程跑完也一样。所以入口
脚本尾部保留 flush + os._exit，并把原因写进注释；被 import 时不生效。

关卡（tests/）：
- verify.py 一条命令跑三层：产物 md5、IK 数值 exact 段、截图。
- 截图基线重做并分级：同一构建连跑两轮实测 102 张逐字节可复现（要求 md5 相同），
  3 张线束 x-ray 不可复现（mean<=0.0017、无像素差>8）改为与参考像素比容差；
  这 3 张不再入库，免得每次跑关卡都脏 git。
- 新增 ?view=sheet / ?sheet=N / ?theme / ?guide=N 四个 URL 参数：工程图与装配向导
  占了约一半 CSS 和 700 行 JS，原先没有任何截图覆盖，拆它们等于没有关卡。
- 关卡灵敏度已验：改坏 IK 基线一个数 -> FAIL；换旧版求解器 -> 5 处 exact 差异。

实测：python -m arm.build_model 退出码 0，全部产物 md5 与基线一致，
105 视图截图全过，IK exact 段逐字节一致。
This commit is contained in:
cbb
2026-10-15 03:31:20 +08:00
parent b32d44efe0
commit 649bd69810
57 changed files with 843 additions and 453 deletions
Download Patch File Download Diff File Expand all files Collapse all files
arm/analysis/__init__.py
+3
View File
@@ -0,0 +1,3 @@
# -*- coding: utf-8 -*-
"""行程与干涉扫描、强度与力矩核算、可达包络采样与网格注入。
"""
arm/analysis/finalize_travel.py
+71
View File
@@ -0,0 +1,71 @@
# -*- coding: utf-8 -*-
"""从 travel_detail.json 的单关节硬限位重新生成 docs/travel.json。
滑块范围 = 单关节硬限位（任意单关节推满零干涉，含 1° 安全余量）；
组合折叠约束保留在 travel_detail.json 的 combos 字段里，供报告引用。
注意 note 的措辞：单轴整周无干涉 **不等于** 任意组合姿态无干涉。sweep_v2 的组合扫描
已经在 J1/J3/J6 上找到了真实卡死（工具法兰 P09 撞固定立柱 P01_A），例如 J3≈±162°。
所以连续轴的 note 必须写明"单轴"并指向 combos，否则这份文件会误导人——查看器运行期
另有胶囊碰撞模型按姿态夹取（ik-glue.js 的 dynLimit），但静态滑块范围按设计不压缩。
"""
import json
import math
import os
import sys
from arm.analysis.sweep_v2 import CHAIN
from arm.paths import DOCS
# 这两个脚本的顶层就是"算完并写文件"，被 import 时会静默重写 travel/mesh 派生产物
# （实测：一次 importlib 冒烟测试就把 docs/travel.json 重写了一遍）。这里改成直接炸。
if __name__ != "__main__":
raise RuntimeError(
"finalize_travel 是脚本：顶层代码一执行就重写产物，不能 import。"
"要用就跑 python -m arm.analysis.finalize_travel。")
d = json.load(open(DOCS + "/travel_detail.json", encoding="utf-8"))
# 组合扫描中，每个被扫轴真实出现卡死的最小绝对角
combo_warn = {}
for c in d.get("combos", []):
if not c.get("jams"):
continue
ax = c["swept"]
for a in c["jams"]:
try:
v = abs(float(a))
except (TypeError, ValueError):
continue
combo_warn[ax] = min(combo_warn.get(ax, 1e9), v)
out = []
for t in CHAIN[1:]:
s = d["singles"][t]
lo, hi = s["lo"], s["hi"]
warn = combo_warn.get(t)
warn_txt = ("；组合折叠姿态下（其余关节同时处于各自极限）约 ±%.0f° 起与躯干占位"
"（台架 P01 立柱）干涉，见 travel_detail.combos，"
"运行期由查看器碰撞模型按姿态夹取" % warn) if warn is not None else ""
if lo <= -179.9 and hi >= 179.9:
out.append(dict(axis=t, lo=-180.0, hi=180.0, continuous=True, zero=0.0,
combo_warn_deg=warn,
note="单轴整周无干涉（滑块按单轴限位给出）%s" % (warn_txt or
"；组合扫描未发现该轴卡死")))
else:
lo_out = float(math.ceil(lo + 1.0)) if lo < -0.5 else 0.0
hi_out = float(math.floor(hi - 1.0)) if hi > 0.5 else 0.0
out.append(dict(axis=t, lo=lo_out, hi=hi_out, continuous=False, zero=0.0,
combo_warn_deg=warn,
note="单轴正向 +%d° / 反向 %d°，总行程 %d°（单关节硬限位，含相邻件与电机）%s"
% (hi_out, lo_out, hi_out - lo_out, warn_txt)))
print("%-3s %7.1f .. %7.1f -> %s .. %s%s"
% (t, lo, hi, out[-1]["lo"], out[-1]["hi"],
(" 组合卡死阈值 ±%.0f°" % warn) if warn is not None else ""))
json.dump(out, open(DOCS + "/travel.json", "w", encoding="utf-8"),
ensure_ascii=False, indent=1)
d["final"] = out
json.dump(d, open(DOCS + "/travel_detail.json", "w", encoding="utf-8"),
ensure_ascii=False, indent=1)
print("rewrote docs/travel.json (single-joint hard stops) + updated travel_detail.json")
arm/analysis/gen_env_mesh.py
+100
View File
@@ -0,0 +1,100 @@
# -*- coding: utf-8 -*-
"""生成可达外包络的 ENV 零件（经纬网单件）追加进 web/mesh_arm.json：
ENV_WL_包络经纬网 —— kind=line，LineSegments 渲染（经向 10°/条、纬向 8mm/环）
包络 = rmax(x) 母线绕 J1 轴（平行 X，过 (y=128, z=610)）整周回转；grp=root 不随关节动。"""
import json
import math
import os
import sys
import numpy as np
from arm.paths import WEB, CACHE
from arm import kinematics as HL
Y_AX = HL.Y_AXIS # 128
Z_AX = HL.Z_RL # 610
def main():
d = np.load(CACHE + "/ws_cloud.npz")
xs, rmax = d["xs"], d["rmax"]
ok = ~np.isnan(rmax)
xs, rmax = xs[ok], rmax[ok]
def grid(step_x, dphi_deg):
"""经纬参数网格 -> verts(NX*NP,3), segs(扁平索引对), tris。"""
xs_e = np.arange(xs.min(), xs.max() + 1e-9, step_x)
rm_e = np.interp(xs_e, xs, rmax)
nph = int(round(360.0 / dphi_deg))
phg = np.linspace(0.0, 2.0 * math.pi, nph, endpoint=False)
nx = len(xs_e)
V = np.empty((nx * nph, 3))
for i, x in enumerate(xs_e):
r = rm_e[i]
for j, ph in enumerate(phg):
V[i * nph + j] = (x, Y_AX + r * math.cos(ph), Z_AX + r * math.sin(ph))
segs = []
for i in range(nx - 1): # 经线（沿 x）
for j in range(nph):
segs += [i * nph + j, (i + 1) * nph + j]
for i in range(nx): # 纬线（沿 φ）
for j in range(nph):
j2 = (j + 1) % nph
segs += [i * nph + j, i * nph + j2]
tris = []
for i in range(nx - 1):
for j in range(nph):
j2 = (j + 1) % nph
a, b = i * nph + j, i * nph + j2
c, e = (i + 1) * nph + j, (i + 1) * nph + j2
tris += [a, c, b, b, c, e]
return V, segs, tris
# 经纬网单件：经向 10°一条、纬向 8mm一环（无需贴片面，线自带罩体感）
Vl, sl, _tl = grid(8.0, 10.0)
# 自证：包络尺寸应等于 ws_metrics 极值。
# 【2026-10-05c 修正】原来是 <1.0 的硬比较，但网格 x 是从 xs.min() 起按 step_x
# 对齐的（np.arange），最后一个采样点最多比真极值短一个步长 —— 一旦
# (x_fwd - xs.min()) 不是 step_x 的整数倍，断言就会假失败（实测差 6.46mm，
# 正好 < 8.0），导致包络注不进去、查看器里"可达包络"整个消失。改为
# "不得超出、且至多少一个步长"这个真正成立的判据。
step_x = 8.0
met = json.load(open(CACHE + "/ws_metrics.json", encoding="utf-8"))
assert Vl[:, 0].min() >= met["x_back"] - 1e-6, (Vl[:, 0].min(), met["x_back"])
assert Vl[:, 0].min() <= met["x_back"] + step_x + 1e-6, (Vl[:, 0].min(), met["x_back"])
assert Vl[:, 0].max() <= met["x_fwd"] + 1e-6, (Vl[:, 0].max(), met["x_fwd"])
assert Vl[:, 0].max() >= met["x_fwd"] - step_x - 1e-6, (Vl[:, 0].max(), met["x_fwd"])
rl = np.hypot(Vl[:, 1] - Y_AX, Vl[:, 2] - Z_AX)
# rl.max() 就是 rmax(x) 网格插值后的峰值，与 met["r_max"]（原始点云极值）之间
# 只允许有插值/取整量级的差（实测 0.044mm），故给 1.0mm 容差。
assert abs(rl.max() - met["r_max"]) < 1.0, (rl.max(), met["r_max"])
assert np.all((Vl[:, 1] - Y_AX) ** 2 + (Vl[:, 2] - Z_AX) ** 2
<= (np.interp(Vl[:, 0], xs, rmax) + 1e-6) ** 2), "line"
print("包络经纬网: 顶点%d 线段%d（经10° x 纬8mm）" % (len(Vl), len(sl) // 2))
# ---- 追加进 mesh_arm.json ----
path = WEB + "/mesh_arm.json"
data = json.load(open(path, encoding="utf-8"))
data["parts"] = [p for p in data["parts"] if not p["name"].startswith("ENV_")]
mx = max(p["idx"] for p in data["parts"])
note_ws = ("可达外包络（运动学，docs/travel.json 滑块限位下的全组合 FK 包络）："
"r=rmax(x) 绕 J1 轴整周回转。高 %.0f~%.0f mm，前伸 %.0f / 后伸 %.0f mm，"
"离 J1 轴最远 %.0f mm；包络内含死区（肩下折不进），多关节同极位由控制器避碰。"
% (met["z_min"], met["z_max"], met["x_fwd"], met["x_back"], met["r_max"]))
line = dict(name="ENV_WL_包络经纬网", cn="包络经纬网", v=[list(p) for p in Vl],
s=sl, color=0x60a5fa, printed=False, vol=round(met["vol_dm3"] * 1e6, 1),
note=note_ws + " 经纬线：经向 10°/条，纬向 8mm/环。",
grp="root", jaw=0.0, ex=[0.0, 0.0, 0.0], idx=mx + 1,
kind="line")
data["parts"] += [line]
tmp = path + ".tmp"
with open(tmp, "w", encoding="utf-8") as f:
json.dump(data, f, ensure_ascii=False, separators=(",", ":"))
os.replace(tmp, path)
print("已写入", path, "大小 %.1f MB" % (os.path.getsize(path) / 1e6))
if __name__ == "__main__":
main()
arm/analysis/strength_check.py
+494
View File
@@ -0,0 +1,494 @@
# -*- coding: utf-8 -*-
"""Structural check of the printed arm, using section properties measured from the SOLIDS.
Nothing here is hand-waved from a sketch dimension: for each critical cut plane the part is
boolean-cut and the freshly created planar face is taken and integrated with
BRepGProp.SurfaceProperties_s, so area, centroid and the second moments are the real
section of the real printed part (including whatever the lightening windows left behind).
Why the allowables are so low: a printed part is not the material datasheet. Bending
across layers, an un-filleted internal corner (Kt ~ 2.5 measured on square corners), and
PLA creep under permanent static load together cut 35-50 MPa down to single digits, which
is exactly the failure mode to design against. See docs/strength.md.
Run: python strength.py -> docs/strength.md (+ console table)
"""
import io
import json
import math
import os
import re
import sys
try:
sys.stdout.reconfigure(encoding="utf-8")
except Exception:
pass
import cadquery as cq
from arm.paths import OUT, WEB, DOCS
from arm.cad.helpers import solids_of, bb, mesh_of
from OCP.BRepAlgoAPI import BRepAlgoAPI_Common
from OCP.BRepGProp import BRepGProp
from OCP.GProp import GProp_GProps
from OCP.BRepPrimAPI import BRepPrimAPI_MakeBox
from OCP.gp import gp_Pnt, gp_Dir, gp_Ax2, gp_Vec
from arm import kinematics as HL
RHO = {"PLA": 1.24e-3, "motor_g": 210.0} # g/mm3 for printed, g per module
# a slicer does not print solid: 3 perimeters + 30% infill lands near this fraction of
# the CAD envelope, and using the solid mass would blame the part for plastic that is not there
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
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):
if name not in _cache:
_cache[name] = cq.importers.importStep("%s/parts_step/%s.step" % (OUT, name)).val()
return _cache[name]
def volume(sh):
p = GProp_GProps()
BRepGProp.VolumeProperties_s(sh.wrapped, p)
return p.Mass()
def _caps_at(sh, n, o, t):
"""The section faces of (solid ∩ thin slab), taken on the +normal side of the slab.
Two things this replaces. (1) 'Zero extent along the normal' on a HALF-SPACE cut also
matches the part's own horizontal faces (top chord, gusset crowns) sitting anywhere above
the plane - that over-read the shoulder fork by 2.4x (480 mm2 true vs 1155 reported).
(2) The half-space Common outright fails on the filleted fork at z=600 (returns an empty
shape) while a thin slab of the same solid cuts fine.
A slab is also self-checking: its volume / t must equal the integrated area.
"""
c0 = sum(n[k] * o[k] for k in range(3))
lo, hi = [], []
for k in range(3):
if n[k] > 0:
lo.append(o[k] - t / 2.0); hi.append(o[k] + t / 2.0)
elif n[k] < 0:
lo.append(o[k] - t / 2.0); hi.append(o[k] + t / 2.0)
else:
lo.append(o[k] - 400.0); hi.append(o[k] + 400.0)
box = BRepPrimAPI_MakeBox(gp_Pnt(*lo), gp_Pnt(*hi)).Shape()
cut = BRepAlgoAPI_Common(sh.wrapped, box)
cut.Build()
if not cut.IsDone():
return []
off = c0 + t / 2.0
out = []
for f in cq.Shape(cut.Shape()).Faces():
if f.geomType() != "PLANE" or f.Area() <= 1e-6:
continue
cx, cy, cz = f.Center().toTuple()
if abs(n[0] * cx + n[1] * cy + n[2] * cz - off) > 1e-4:
continue
out.append(f)
return out
def section_props(sh, normal, origin, bend=2, size=400.0):
"""Cut the solid with a thin slab and integrate the section face.
Returns (area mm2, Ixx mm4, Iyy mm4, cmax mm, bbox) about the face centroid, with the
in-plane axes taken as (u, v) where u x v = normal.
"""
n = [float(x) for x in normal]
o = [float(x) for x in origin]
caps = []
for t in (0.2, 0.05, 1.0):
for nudge in (0.0, 0.05, -0.05, 0.25, -0.25):
oo = [o[k] + nudge * n[k] for k in range(3)]
caps = _caps_at(sh, n, oo, t)
if caps:
break
if caps:
break
if not caps:
return None
plane_ax = [k for k in (0, 1, 2) if abs(n[k]) < 0.5]
p_ax, q_ax = plane_ax
bend_ax = bend if bend in plane_ax else q_ax
far_ax = p_ax if bend_ax == q_ax else q_ax
A = Sv = Suu = Svv = S = 0.0
for f in caps:
verts, tris = mesh_of(f, 0.02)
fa = fu = fv = fuu = fvv = 0.0
for t in tris:
(u1, v1), (u2, v2), (u3, v3) = [(verts[k][p_ax], verts[k][q_ax]) for k in t]
ar = ((u2 - u1) * (v3 - v1) - (u3 - u1) * (v2 - v1)) / 2.0
fa += ar # SIGNED: inner wires (holes) cancel
fu += ar * (u1 + u2 + u3) / 3.0
fv += ar * (v1 + v2 + v3) / 3.0
fvv += ar * (v1**2 + v2**2 + v3**2 + v1*v2 + v2*v3 + v3*v1) / 6.0
fuu += ar * (u1**2 + u2**2 + u3**2 + u1*u2 + u2*u3 + u3*u1) / 6.0
if fa < 0: # face normal opposed to the cut normal
fa, fu, fv, fvv, fuu = -fa, -fu, -fv, -fvv, -fuu
A += fa; S += fu; Sv += fv; Svv += fvv; Suu += fuu
if A <= 1e-9:
return None
uc, vc = S / A, Sv / A
best = max(caps, key=lambda f: f.Area()) # only for the bounding box below
allb = [f.BoundingBox() for f in caps]
lo = [min(getattr(b, ("xmin", "ymin", "zmin")[k]) for b in allb) for k in range(3)]
hi = [max(getattr(b, ("xmax", "ymax", "zmax")[k]) for b in allb) for k in range(3)]
cmax = max(hi[far_ax] - uc if far_ax == p_ax else hi[far_ax] - vc,
uc - lo[far_ax] if far_ax == p_ax else vc - lo[far_ax]) / 1.0
# distance from the neutral axis to the extreme fibre, measured in the bending direction
ext = (hi[far_ax] - lo[far_ax])
cen = uc if far_ax == p_ax else vc
cmax = max(hi[far_ax] - cen, cen - lo[far_ax])
I = (Svv - A * vc * vc) if far_ax == q_ax else (Suu - A * uc * uc)
return dict(A=A, I=I, c=cmax, bend=bend_ax, centroid=(uc, vc))
def arm_masses():
"""Everything distal to each joint, in grams, from the model itself."""
meta = json.load(io.open(WEB + "/mesh_arm.json", encoding="utf-8"))
TREE = ["root", "J1", "J2", "J3", "J4", "J5", "J6"]
tot = {}
for p in meta["parts"]:
if not os.path.exists("%s/parts_step/%s.step" % (OUT, p["name"])):
continue # 螺钉实例 / 可达包络网格等非 CAD 构件没有 STEP，跳过
sh = load(p["name"])
v = volume(sh) / 1000.0
g = (v * RHO["PLA"] * 1000.0 * INFILL) if p["printed"] else RHO["motor_g"]
tot[p["name"]] = (g, p["grp"])
out = {}
for k, jid in enumerate(TREE[1:], 1):
down = set(TREE[k:])
out[jid] = sum(g for g, grp in tot.values() if grp in down)
out["all"] = sum(g for g, _ in tot.values())
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(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)
bend axis: 0=X (roll of the shoulder/forearm), 1=Y (every pitch joint), 2=Z."""
Y = HL.Y_AXIS
return [
("吊臂根座 @z=480", "P03_肩吊臂_螺旋体35%_墙5底5_竖打", (0, 0, 1), (0.0, 55.0, 480.0), "J1", 1),
("吊臂颊板 @z=560", "P03_肩吊臂_螺旋体35%_墙5底5_竖打", (0, 0, 1), (0.0, 100.0, 560.0), "J1", 1),
("肩叉腹板 @z=560", "P04_肩叉_螺旋体30%_墙5底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 560.0), "J2", 1),
("肩叉腹板 @z=600", "P04_肩叉_螺旋体30%_墙5底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 600.0), "J2", 1),
("大臂旋转座横梁 @z=480", "P05_大臂旋转座_螺旋体30%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 480.0), "J2", 1),
("大臂旋转座套筒 @z=440", "P05_大臂旋转座_螺旋体30%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 440.0), "J3", 1),
("大臂骨架中段 @z=350", "P06_大臂骨架_螺旋体25%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 350.0), "J3", 1),
("大臂骨架抱箍 @z=325", "P06_大臂骨架_螺旋体25%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 325.0), "J4", 1),
# 【2026-10-02 补】P06 原减重窗在 Z358~374，原有剖面(350/325)全在它下面，
# 根本校核不到被挖的区域。z=365 穿过该段过渡块，承载同 J3。
# 【2026-10-03】减重窗已取消、该段恢复实心，此剖面保留为该高度的安全裕度监控点。
("大臂骨架过渡块 @z=365", "P06_大臂骨架_螺旋体25%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 365.0), "J3", 1),
("小臂骨架中段 @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),
# 【2026-10-02 补】P07 减重槽在 Z255~280，同理原有剖面(245/195)没覆盖到。
# z=270 穿过槽体本身，承载同 J4；z=280 是槽口所在的顶板上边缘，壁最薄。
("小臂骨架减重槽 @z=270", "P07_小臂骨架_螺旋体30%_墙5底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 270.0), "J4", 1),
("小臂骨架减重槽口 @z=280", "P07_小臂骨架_螺旋体30%_墙5底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 280.0), "J4", 1),
("腕偏转座根部 @z=150", "P08_腕偏转座_螺旋体25%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 150.0), "J5", 1),
# P09 包络 z=93..121，剖面取 107（法兰中截面）。原 -55 落在零件之外，
# 剖切返回空，工具法兰其实一直没被校核过。
("工具法兰 @z=107", "P09_工具法兰_螺旋体30%_墙4底5_竖打", (0, 0, 1), (HL.X_ARM, Y, 107.0), "J6", 1),
]
MOMENT = { # worst reachable bending moment per joint, N*mm
# computed in main() from the reachable pose + masses
}
def main():
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):
lab, part, n, o, jid, bend = c_item
sh = load(part)
sp = section_props(sh, n, o, bend)
if not sp:
return (lab, part, None, jid)
# 剖面形心的三维重建 + 弯矩所绕的轴
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"]
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 MPa 末端负载 %g kg 动载系数 %g"
% (MAT, ALLOW[MAT], EMOD[MAT], PAYLOAD, DYN))
print("载荷模型：每个零件按真实质心（网格体积加权）随姿态变换，在可达姿态网格上取弯矩最大值；"
"参考点为剖面形心\n")
print("打印件按 %d%% 实体率计质量（壁+填充），电机按 %.0f g/台" % (INFILL * 100, RHO["motor_g"]))
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("%-24s %-9s 剖切失败" % (lab, part))
continue
Zreq = d["M"] / ALLOW[MAT] * 1.5 # section modulus x1.5 SF
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("合计 %.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))
# OCC/cadquery 的静态析构在解释器正常退出时必定段错误（实测：光是
# `python -c "import arm.cad.helpers"` 就返回 139，与本项目代码无关）。所以入口
# 脚本跑完后显式刷缓冲再 os._exit —— 否则全流程已经成功、exit code 却是 139,
# 会把上游脚本的 set -e 判成失败。只在 __main__ 出口生效, 被 import 时不触发。
sys.stdout.flush()
os._exit(0)
arm/analysis/sweep_v2.py
+726
View File
@@ -0,0 +1,726 @@
# -*- coding: utf-8 -*-
"""全对全动态行程扫描与碰撞验证（权威版 v2，多进程并行）
与旧 travel_sweep.py / real_arm_collision_test.py 的区别（旧脚本漏掉了用户实测到的干涉）：
1. 直接在进程内构建当前模型（arm_model.build()），杜绝 STEP/网格过期；
2. 关节轴取自 Joint.rear/.d（human_layout 唯一事实源），与网页查看器逐位一致；
3. 检查【全部】零件对 —— 包括相邻关节的零件对与电机模组（旧脚本跳过相邻对，
而 P04xP05 / P07xP08 恰恰在关节贴面处被卡死）；
4. 单关节双向精扫（粗扫 + 二分）得出真实硬限位；
5. 组合姿态扫描：每个关节置于其极限，再扫其余关节，收敛出滑块安全范围；
6. 写 docs/travel.json（查看器滑块范围）+ docs/travel_detail.json（逐对明细）。
网格化预筛（用户要求，2026-09）：
- 每零件先按 0.5mm 容差网格化取顶点云（凸包顶点集）；
- 候选对先跑 GJK（Minkowski 差 + 0.8mm 安全外扩）：只有"凸包外扩后仍分离"
才允许跳过精确布尔 —— 该判据数学上保守（网格内缩 ≤0.5mm < 外扩 0.8mm），
其余情况（可能干涉/数值退化）一律回落 OCCT 实体会交，结果与纯布尔完全一致；
- --no-gjk 可关闭预筛（对比校验用）。
并行化（用户要求）：
- mp.Pool 每个工作进程独立构建一次模型，角度任务批量分发（imap 保序）；
- OCCT 布尔运算 SetRunParallel(True)；
- 预剪枝：按关节轴计算每对零件的轴向/径向区间，区间不相交的零件对在该关节的
任何角度都不可能碰撞，直接跳过（对单关节扫描是精确安全的）。
台架（P01 立柱 / P02 底盘）代表躯干与机身的连接界面，默认【参与】碰撞计算——臂撞立柱
就是臂撞躯干，是真约束。--no-stand 仅用于诊断：排除台架后重扫，给出"不含躯干约束"的
自由空间行程，用来分辨某个限位是臂自身折叠产生的还是躯干挡的（台架位置本身是假设，
改它等于改测试工装，不等于改设计）。
运行：python -m arm.analysis.sweep_v2 [--workers 20] [--step 2] [--combo-step 6]
[--no-combos] [--joints J2,J4] [--no-gjk] [--no-stand]
"""
import json
import math
import os
import sys
import time
import numpy as np
try:
sys.stdout.reconfigure(encoding="utf-8")
except Exception:
pass
import multiprocessing as mp
from arm.cad import arm_build as AB
import cadquery as cq
from arm import kinematics as HL
from arm.paths import DOCS
from arm.cad.helpers import place, bb
VOL_MIN = 2.0 # mm3，超过即计为干涉
CHAIN = ["root", "J1", "J2", "J3", "J4", "J5", "J6"]
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"),
}
# 网格化预筛参数：外扩必须 ≥ 网格化偏差，判"分离"才保守可信
MESH_TOL = 0.5 # mm，零件网格化容差
MESH_MARGIN = 0.8 # mm，凸包安全外扩（> MESH_TOL 保证网格内缩不致漏判）
USE_GJK = True # 进程内全局；工作进程由 initializer 传入
INCLUDE_FIXTURE = True # True=把 P01 立柱/P02 底盘（代表躯干）算进碰撞；--no-stand 置 False
# ---------------------------------------------------------------- 网格化 + GJK
def tessellate_verts(shape):
"""零件 → 顶点云 (N,3)。凸包 GJK 的支撑函数直接在顶点云上取极值（等价于其凸包），
第二步做精确凸包精简（scipy Qhull）：电机 STEP 网格 15 万顶点 → 约千余点。
顶点均为真实表面上的点，亦用作"顶点侵入"快证的采样点。"""
try:
vs, _ts = shape.tessellate(MESH_TOL)
v = np.asarray([(q.x, q.y, q.z) for q in vs], dtype=np.float64)
if v.size == 0:
return None
v = np.unique(v, axis=0)
try:
from scipy.spatial import ConvexHull
v = v[ConvexHull(v).vertices]
except Exception:
pass # 精简失败则用全顶点云（仍正确，仅慢）
return np.ascontiguousarray(v)
except Exception:
return None
def gjk_separated(va, vb, margin=MESH_MARGIN, max_iter=40):
"""凸包 GJK（带 margin）：va/vb 顶点云。返回 True 仅当
conv(va)⊕Ball(margin) 与 conv(vb) 确证分离 —— 保守，可安全跳过精确布尔。
可能干涉 / 数值退化 / 迭代超限 一律返回 False，回落 OCCT 精确实体求交。"""
d = vb.mean(axis=0) - va.mean(axis=0)
nd = np.linalg.norm(d)
d = np.array([1.0, 0.0, 0.0]) if nd < 1e-9 else d / nd
s = [] # 单纯形顶点，s[-1] 为最新
for _ in range(max_iter):
pa = va[np.argmax(va @ d)]
pb = vb[np.argmax(vb @ (-d))]
w = pa - pb + margin * d # ⊕Ball(margin) 的支撑
if w @ d < 0.0: # Minkowski 差不含原点 → 确证分离
return True
s.append(w)
d, state = _do_simplex(s)
if state != "go":
return False # 'hit' 可能干涉 / 'stop' 退化 → 精确布尔
nd = np.linalg.norm(d)
if nd < 1e-9:
return False # 原点贴着单纯形 → 交给精确布尔
d = d / nd
return False
def _do_simplex(s):
"""GJK 单纯形约化（Muratori 形式）。返回 (下一方向 d, state)：
'go' 继续 / 'hit' 原点可能在内（→ 精确布尔）/ 'stop' 数值退化（→ 精确布尔）。"""
n = len(s)
if n == 1:
return -s[0], "go"
if n == 2:
a, b = s[1], s[0]
ab, ao = b - a, -a
if ab @ ao > 0.0:
abv = np.cross(np.cross(ab, ao), ab)
return (None, "stop") if abv @ abv < 1e-18 else (abv, "go")
del s[0]
return -a, "go"
if n == 3:
a, b, c = s[2], s[1], s[0]
ab, ac, ao = b - a, c - a, -a
abv = np.cross(np.cross(ab, ao), ab)
acv = np.cross(np.cross(ac, ao), ac)
if abv @ ao > 0.0: # AB 边外侧
if ab @ ao > 0.0:
del s[0]
return (None, "stop") if abv @ abv < 1e-18 else (abv, "go")
s[:] = [a]
return -a, "go"
if acv @ ao > 0.0: # AC 边外侧
if ac @ ao > 0.0:
del s[1]
return (None, "stop") if acv @ acv < 1e-18 else (acv, "go")
s[:] = [a]
return -a, "go"
nrm = np.cross(ab, ac)
if nrm @ nrm < 1e-18:
return None, "stop"
return (nrm if nrm @ ao > 0.0 else -nrm), "go" # 三角面上/下方
# n == 4 四面体：原点与被排除顶点异侧的面即"外侧"，约化到该面
a3, b3, c3, d3 = s[3], s[2], s[1], s[0]
for tri, e in (((a3, b3, c3), d3), ((a3, b3, d3), c3),
((a3, c3, d3), b3), ((b3, c3, d3), a3)):
u, v, w = tri
nrm = np.cross(v - u, w - u)
if nrm @ nrm < 1e-18:
continue
so = (-u) @ nrm
sd = (e - u) @ nrm
if abs(so) < 1e-12:
return None, "hit"
if so * sd < 0.0:
s[:] = [u, v, w]
return (nrm if so > 0.0 else -nrm), "go"
return None, "hit" # 原点在四面体内 → 可能干涉
# ---------------------------------------------------------------- 模型与上下文
def load_model(include_fixture=True):
"""include_fixture=False 时把 P01 立柱 / P02 底盘（固定测试台架，非本体）排除在
碰撞配对之外——用来得到真机工作空间而不是台架工作空间。见 human_layout.FIXTURE_PREFIX。"""
from arm import build_model as AM
JS = AM.build()
parts = []
for p in AB.PARTS:
if not include_fixture and HL.is_fixture(p["name"]):
continue
g = CHAIN.index(AB.GRP.get(p["name"], "root"))
lo, hi = bb(p["shape"])
corners = [(x, y, z) for x in (lo[0], hi[0]) for y in (lo[1], hi[1]) for z in (lo[2], hi[2])]
# 包围球（局部坐标）：球心=包围盒中心，半径=半对角线。零件 ⊆ 包围球 恒成立，
# 球心随刚体变换、半径不变 —— 旋转下依旧精确保守，是最廉价的确定性预筛。
ctr = [(lo[k] + hi[k]) / 2.0 for k in range(3)]
rad = 0.5 * math.dist(lo, hi) + 0.21
verts = tessellate_verts(p["shape"])
parts.append(dict(name=p["name"], shape=p["shape"], g=g, c8=corners, box=(lo, hi),
ctr=ctr, rad=rad, verts=verts))
joints = {t: (JS[t].rear, JS[t].d) for t in CHAIN[1:]}
return parts, joints
def _trsf_verts(verts, t):
"""顶点云按 gp_Trsf 刚体变换（3x4 矩阵）。"""
m = np.array([[t.Value(r, c) for c in (1, 2, 3, 4)] for r in (1, 2, 3)])
return verts @ m[:, :3].T + m[:, 3]
def axis_intervals(parts, rear, d):
"""每个零件沿关节轴的 (轴向区间, 径向最小/最大距离)。要求 d 为坐标轴方向。"""
out = []
for p in parts:
lo, hi = p["box"]
if abs(d[0]) > 0.5:
ax = (lo[0] - rear[0], hi[0] - rear[0])
u = (lo[1], hi[1], rear[1]); v = (lo[2], hi[2], rear[2])
elif abs(d[1]) > 0.5:
ax = (lo[1] - rear[1], hi[1] - rear[1])
u = (lo[0], hi[0], rear[0]); v = (lo[2], hi[2], rear[2])
else:
ax = (lo[2] - rear[2], hi[2] - rear[2])
u = (lo[0], hi[0], rear[0]); v = (lo[1], hi[1], rear[1])
du = max(u[0] - u[2], 0.0, u[2] - u[1])
dv = max(v[0] - v[2], 0.0, v[2] - v[1])
rmin = math.hypot(du, dv)
rmax = max(math.hypot(a - u[2], b - v[2]) for a in (u[0], u[1]) for b in (v[0], v[1]))
out.append((ax, rmin, rmax))
return out
def make_ctx(include_fixture=True):
parts, joints = load_model(include_fixture)
impossible = {}
for k in range(1, len(CHAIN)):
rear, d = joints[CHAIN[k]]
iv = axis_intervals(parts, rear, d)
s = set()
for i in range(len(parts)):
for j in range(i + 1, len(parts)):
ga, gb = parts[i]["g"], parts[j]["g"]
lo, hi = (ga, gb) if ga < gb else (gb, ga)
if lo == hi or not (lo < k <= hi):
continue
ax_i, r_i = iv[i][0], (iv[i][1], iv[i][2])
ax_j, r_j = iv[j][0], (iv[j][1], iv[j][2])
if ax_i[1] < ax_j[0] or ax_j[1] < ax_i[0] or \
r_i[1] < r_j[0] or r_j[1] < r_i[0]:
s.add((i, j))
impossible[k] = s
return dict(parts=parts, joints=joints, impossible=impossible)
# ---------------------------------------------------------------- 姿态测试
def accum_trsf(joints, angles_by_idx):
"""链式累积变换：acc[i] = acc[i-1] ∘ R_i（与查看器嵌套 pivot 完全一致）。"""
acc = [None] * len(CHAIN)
for i in range(1, len(CHAIN)):
ang = angles_by_idx.get(i, 0.0)
if abs(ang) < 1e-9:
acc[i] = acc[i - 1]
continue
rear, d = joints[CHAIN[i]]
r = AB.R(ang, d[0], d[1], d[2], rear[0], rear[1], rear[2])
acc[i] = r if acc[i - 1] is None else acc[i - 1].Multiplied(r)
return acc
def xf_c8(c8, t):
from OCP.gp import gp_Pnt
gpt = [gp_Pnt(*c).Transformed(t) for c in c8]
lo = [min(p.X() for p in gpt), min(p.Y() for p in gpt), min(p.Z() for p in gpt)]
hi = [max(p.X() for p in gpt), max(p.Y() for p in gpt), max(p.Z() for p in gpt)]
return (lo, hi)
def common_vol_center(a, b):
"""交集体积 + 干涉质心；布尔失败返回 (-1, None)。"""
try:
from OCP.BRepAlgoAPI import BRepAlgoAPI_Common
c = BRepAlgoAPI_Common(a.wrapped, b.wrapped)
c.SetRunParallel(True)
c.Build()
if not c.IsDone() or c.Shape().IsNull():
return 0.0, None
sh = cq.Shape(c.Shape())
v = AB.vol_of(sh)
if v <= VOL_MIN:
return v, None
(lo, hi) = bb(sh)
return v, tuple(round((lo[i] + hi[i]) / 2.0, 1) for i in range(3))
except Exception:
return -1.0, None
def test_posture(ctx, angles_by_idx, want_errors=False, first_only=True):
"""返回 (violations, errors)：violations = [(na, nb, vol, 干涉质心)]。
first_only：证实第一对干涉即返回（粗扫/二分只需要"卡没卡"的二元答案，卡死姿态
通常 12+ 对干涉、每对布尔 0.5~12s，全算完要 40s+；命中即返回降到 1~2s）。
零位诊断（want_errors=True）保持全对扫描以给出完整清单。"""
parts, joints, impossible = ctx["parts"], ctx["joints"], ctx["impossible"]
active = [k for k, a in angles_by_idx.items() if abs(a) > 1e-9]
acc = accum_trsf(joints, angles_by_idx)
boxes = []
sph = []
for p in parts:
t = acc[p["g"]]
boxes.append(p["box"] if t is None else xf_c8(p["c8"], t))
if t is None:
c = p["ctr"]
else: # 球心随组变换，半径旋转不变
c = _trsf_verts(np.array([p["ctr"]], dtype=np.float64), t)[0]
sph.append((c[0], c[1], c[2], p["rad"]))
shapes = {}
wverts = {} # 顶点云世界坐标缓存（本姿态内复用）
viol, errs = [], []
n = len(parts)
for i in range(n):
gi = parts[i]["g"]
for j in range(i + 1, n):
gj = parts[j]["g"]
lo, hi = (gi, gj) if gi < gj else (gj, gi)
if lo == hi:
continue
na, nb = parts[i]["name"], parts[j]["name"]
if na.startswith("M") and nb.startswith("M"):
continue
if (na, nb) in MOUNT_PAIRS or (nb, na) in MOUNT_PAIRS:
continue
span = [k for k in active if lo < k <= hi]
# 关键：无活动关节（零位）时必须测试全部零件对 —— 相对位姿不变不代表不干涉！
if active and not span:
continue
if len(span) == 1 and (i, j) in impossible[span[0]]:
continue
bi, bj = boxes[i], boxes[j]
if any(bi[1][k] < bj[0][k] - 0.1 or bj[1][k] < bi[0][k] - 0.1 for k in range(3)):
continue
sa, sb = sph[i], sph[j] # 包围球预筛：零件 ⊆ 球，球分离 ⇒ 零件必分离
dxx = sa[0] - sb[0]; dyy = sa[1] - sb[1]; dzz = sa[2] - sb[2]
rr = sa[3] + sb[3] + 0.1
if dxx * dxx + dyy * dyy + dzz * dzz > rr * rr:
continue
if USE_GJK: # 网格化预筛：确证分离则免精确布尔
for key, gk in ((i, gi), (j, gj)):
if key not in wverts:
t = acc[gk]
lv = parts[key]["verts"]
wverts[key] = None if lv is None else (
lv if t is None else _trsf_verts(lv, t))
vi, vj = wverts[i], wverts[j]
if vi is not None and vj is not None and gjk_separated(vi, vj):
continue
si = shapes.get(i)
if si is None:
t = acc[gi]
si = parts[i]["shape"] if t is None else place(parts[i]["shape"], t)
shapes[i] = si
sj = shapes.get(j)
if sj is None:
t = acc[gj]
sj = parts[j]["shape"] if t is None else place(parts[j]["shape"], t)
shapes[j] = sj
# 顶点侵入快证：一方的网格顶点（真实表面上的点）落入对方实体内部即实锤干涉，
# 免掉 4~12s 的 NURBS 布尔。深入重叠时前几十个采样点即命中早退；分类器按
# 零件缓存（每姿态构建一次）。判"自由"无证明力 —— 无命中则照旧精确布尔。
jam = False
try:
from OCP.BRepClass3d import BRepClass3d_SolidClassifier
from OCP.gp import gp_Pnt
from OCP.TopAbs import TopAbs_State
for pi_, pj_ in ((i, j), (j, i)):
pts = wverts.get(pi_)
if pts is None or not len(pts):
continue
ckey = (pj_, id(acc[parts[pj_]["g"]]))
if ckey not in classifiers:
classifiers[ckey] = BRepClass3d_SolidClassifier(shapes[pj_].wrapped)
cls = classifiers[ckey]
stepk = max(1, len(pts) // 240)
for k in range(0, len(pts), stepk):
p = pts[k]
# Perform(pnt, tol=1.0)：距边界 <1mm 的点归为 ON（配合贴面不算），
# 只有深入 >1mm 的顶点才实锤干涉 —— 与 VOL_MIN 语义对齐
cls.Perform(gp_Pnt(p[0], p[1], p[2]), 1.0)
if cls.State() == TopAbs_State.TopAbs_IN:
jam = True
break
if jam:
break
except Exception:
jam = False
if jam:
viol.append((parts[i]["name"], parts[j]["name"], -1.0, None))
if first_only and not want_errors:
return viol
continue
v, center = common_vol_center(si, sj)
if v < 0:
errs.append((parts[i]["name"], parts[j]["name"]))
continue
if v > VOL_MIN:
viol.append((parts[i]["name"], parts[j]["name"], round(v, 1), center))
if first_only and not want_errors:
return viol
return (viol, errs) if want_errors else viol
# ---------------------------------------------------------------- 工作进程
_CTX = None
_POOL = None
_WORKERS = 4
def _init_worker(use_gjk=True, include_fixture=True):
global _CTX, USE_GJK
USE_GJK = bool(use_gjk)
_CTX = make_ctx(include_fixture)
def _task(task):
angles = {int(k): v for k, v in task["a"].items()}
return task["id"], test_posture(_CTX, angles) # 带id回传，配合 imap_unordered 实时进度
def get_pool(workers):
global _POOL, _WORKERS
_WORKERS = workers
if _POOL is None and workers > 1:
_POOL = mp.Pool(workers, initializer=_init_worker, initargs=(USE_GJK, INCLUDE_FIXTURE))
return _POOL
def eval_angles(angles_list, workers, chunk=None, label="姿态"):
"""批量评估姿态列表 [{idx: ang}]，按输入顺序返回 violations 列表。
进度展示：imap_unordered 按完成序实时回报，≥4 个姿态的批次每 2s 滚动打印一次
真实完成数（短批次与二分不打扰输出）。"""
tasks = [dict(id=i, a={str(k): v for k, v in a.items()}) for i, a in enumerate(angles_list)]
n = len(tasks)
if workers <= 1:
ctx = _ctx_single()
out = []
t0 = time.time()
for i, a in enumerate(angles_list):
out.append(test_posture(ctx, a))
if n >= 4 and ((i + 1) % max(1, n // 10) == 0 or i + 1 == n):
print(" %s %d/%d [%.0fs]" % (label, i + 1, n, time.time() - t0),
end="\r" if i + 1 < n else "\n", flush=True)
return out
pool = get_pool(workers)
out = [None] * n
cs = chunk or max(1, n // (workers * 4) or 1)
t0, t_last, done_n = time.time(), time.time(), 0
for id_, viol in pool.imap_unordered(_task, tasks, chunksize=cs):
out[id_] = viol
done_n += 1
now = time.time()
if n >= 4 and (now - t_last >= 2.0 or done_n == n):
print(" %s %d/%d [%.0fs]" % (label, done_n, n, now - t0),
end="\r" if done_n < n else "\n", flush=True)
t_last = now
return out
def _ctx_single():
global _CTX
if _CTX is None:
_CTX = make_ctx(INCLUDE_FIXTURE)
return _CTX
# ---------------------------------------------------------------- 扫描逻辑
def bisect(workers, k, a_free, a_jam, tol=0.2):
n = 0
while abs(a_jam - a_free) > tol:
n += 1
mid = 0.5 * (a_free + a_jam)
print(" %s 二分#%d 区间 [%.2f, %.2f]°" % (CHAIN[k], n, a_free, a_jam), flush=True)
if eval_angles([{k: mid}], workers, label="%s二分" % CHAIN[k])[0]:
a_jam = mid
else:
a_free = mid
return a_free
def sweep_single(k, step, workers):
t_start = time.time()
jams = {}
lo = hi = 0.0
tag = CHAIN[k]
for sign in (+1, -1):
angles = [sign * step * i for i in range(1, int(180.0 / step) + 1)]
stop, a_free = None, 0.0
B = max(workers, 8) # 小批次：首个卡死角即停，少算深度卡死姿态
for s in range(0, len(angles), B):
batch = angles[s:s + B]
results = eval_angles([{k: a} for a in batch], workers,
label="%s%s向粗扫" % (tag, "正" if sign > 0 else "反"))
done = min(s + B, len(angles))
print(" %s %s向 %d/%d 角度（%2.0f%%）[已用 %.0fs]"
% (tag, "正" if sign > 0 else "反", done, len(angles),
100.0 * done / len(angles), time.time() - t_start), flush=True)
for a, viol in zip(batch, results):
if viol:
jams[a] = viol
print(" %s %s向在 %.1f° 首次干涉：%s"
% (tag, "正" if sign > 0 else "反", a,
"; ".join("%sx%s(%s)" % (p, q, "顶点侵入" if v < 0 else "%.0fmm3" % v)
for p, q, v, _c in viol[:3])),
flush=True)
# 二分区间 = [本方向最后自由角, 首个卡死角] —— 修复原 sign*(a-step)
# 在负方向算出未测过的另一侧角度、把二分踢进远端慢区的问题
stop = bisect(workers, k, a_free, a)
break
a_free = a
if stop is not None:
break
if stop is None:
stop = a_free
if sign > 0:
hi = stop
else:
lo = stop
print(" => %s 硬限位 [%7.2f, %7.2f]，耗时 %.0fs" % (tag, lo, hi, time.time() - t_start),
flush=True)
return dict(lo=lo, hi=hi, jams=jams)
def sweep_combo(k_hold, e_hold, j_sweep, a_lo, a_hi, step, workers):
jams = {}
res = {}
tag = "%s@%.0f° 扫%s" % (CHAIN[k_hold], e_hold, CHAIN[j_sweep])
for sign, lim in ((+1, a_hi), (-1, a_lo)):
angles = [sign * step * i for i in range(1, int(abs(lim) / step) + 1)] if abs(lim) > 1e-6 else []
stop, a_free = None, 0.0
B = max(workers, 8) # 小批次：首个卡死角即停，少算深度卡死姿态
for s in range(0, len(angles), B):
batch = angles[s:s + B]
results = eval_angles([{k_hold: e_hold, j_sweep: a} for a in batch], workers,
label=tag)
done = min(s + B, len(angles))
print(" %s %s向 %d/%d（%2.0f%%）"
% (tag, "正" if sign > 0 else "反", done, len(angles),
100.0 * done / max(len(angles), 1)), flush=True)
for a, viol in zip(batch, results):
if viol:
jams[a] = viol
lo_f, hi_j = a_free, a # 同 sweep_single：用本方向最后自由角
while abs(hi_j - lo_f) > 0.5:
mid = 0.5 * (lo_f + hi_j)
if eval_angles([{k_hold: e_hold, j_sweep: mid}], workers,
label="%s组合二分" % tag)[0]:
hi_j = mid
else:
lo_f = mid
stop = lo_f
break
a_free = a
if stop is not None:
break
if stop is None:
stop = a_free
res[sign] = stop
return res[+1], res[-1], jams
def main():
t0 = time.time()
step, combo_step = 2.0, 6.0
do_combos = "--no-combos" not in sys.argv
want_joints = None
if "--no-gjk" in sys.argv:
global USE_GJK
USE_GJK = False
if "--no-stand" in sys.argv:
global INCLUDE_FIXTURE
INCLUDE_FIXTURE = False
# 内存感知的默认进程数：每个工作进程预留 ~2.5GB 峰值（建模+布尔瞬时开销）
avail_gb = 16.0
try:
import ctypes
class _MS(ctypes.Structure):
_fields_ = [("dwLength", ctypes.c_uint32), ("dwMemoryLoad", ctypes.c_uint32),
("ullTotalPhys", ctypes.c_uint64), ("ullAvailPhys", ctypes.c_uint64),
("ullTotalPageFile", ctypes.c_uint64), ("ullAvailPageFile", ctypes.c_uint64),
("ullTotalVirtual", ctypes.c_uint64), ("ullAvailVirtual", ctypes.c_uint64),
("ullAvailExtendedVirtual", ctypes.c_uint64)]
_ms = _MS()
_ms.dwLength = ctypes.sizeof(_MS)
ctypes.windll.kernel32.GlobalMemoryStatusEx(ctypes.byref(_ms))
avail_gb = _ms.ullAvailPhys / 2**30
except Exception:
pass
mem_cap = max(2, int(avail_gb / 2.5))
workers = max(1, min(12, (os.cpu_count() or 4) - 2, mem_cap))
for i, a in enumerate(sys.argv):
if a == "--step":
step = float(sys.argv[i + 1])
if a == "--combo-step":
combo_step = float(sys.argv[i + 1])
if a == "--workers":
workers = int(sys.argv[i + 1])
if a == "--joints":
want_joints = set(sys.argv[i + 1].split(","))
print("=" * 78)
print("全对全动态行程扫描 v2 并行版（workers=%d / 可用内存 %.0fGB / 轴=human_layout / 含相邻对与电机 / GJK预筛=%s）"
% (workers, avail_gb, "开" if USE_GJK else "关"))
print("=" * 78, flush=True)
print("启动 %d 个工作进程并行建模（每进程独立构建全模型+凸包网格，约 20~60s）..." % workers,
flush=True)
if workers > 1:
get_pool(workers) # 启动工作进程（各自并行建模，父进程不重复建模）
# 零位检查
v0 = eval_angles([{}], workers)[0]
print("[零位检查] %s" % ("0 对干涉 ✓" if not v0 else "干涉: %s" % v0), flush=True)
singles = {}
print("\n[单关节硬限位] 粗扫 %.1f° + 二分 0.2°" % step, flush=True)
for k in range(1, len(CHAIN)):
tag = CHAIN[k]
if want_joints and tag not in want_joints:
continue
r = sweep_single(k, step, workers)
singles[tag] = r
jam_s = "; ".join(
"%s%.1f°:%s" % ("+" if a > 0 else "", a,
",".join("%sx%s@%s(%s)" % (p, q, c,
"侵入" if v < 0 else "%.0f" % v)
for p, q, v, c in vs[:2]))
for a, vs in sorted(r["jams"].items())[:4])
print(" %-3s lo=%7.2f hi=%7.2f %s [%.0fs]" % (tag, r["lo"], r["hi"],
("卡死: " + jam_s) if jam_s else "整周无干涉 → 360° 连续", time.time() - t0), flush=True)
if want_joints:
json.dump(dict(generated=time.strftime("%Y-%m-%d %H:%M:%S"),
singles={t: dict(lo=round(singles[t]["lo"], 2),
hi=round(singles[t]["hi"], 2),
jams={str(a): v for a, v in singles[t]["jams"].items()})
for t in singles}),
open(DOCS + "/travel_probe.json", "w", encoding="utf-8"),
ensure_ascii=False, indent=1)
print("\n[诊断模式] 明细已写出 docs/travel_probe.json [%.0fs]" % (time.time() - t0))
return
limits = {t: [singles[t]["lo"], singles[t]["hi"]] for t in CHAIN[1:]}
combos_log = []
if do_combos:
print("\n[组合姿态收敛] 保持一个关节于极限，扫其余关节（步长 %.1f°）" % combo_step, flush=True)
for pass_i in range(2):
changed = False
for k in range(1, len(CHAIN)):
tag_k = CHAIN[k]
for e in (limits[tag_k][0], limits[tag_k][1]):
if abs(e) < 5.0:
continue
for j in range(1, len(CHAIN)):
if j == k:
continue
tag_j = CHAIN[j]
lo_j, hi_j = limits[tag_j]
if abs(lo_j) < 5.0 and abs(hi_j) < 5.0:
continue
pos, neg, jams = sweep_combo(k, e, j, lo_j, hi_j, combo_step, workers)
for sign, free, lim in ((+1, pos, hi_j), (-1, neg, lo_j)):
new = free - 1.0
if sign > 0 and new < limits[tag_j][1] - 0.01:
combos_log.append(dict(pass_=pass_i, hold=tag_k, hold_at=e,
swept=tag_j, direction="hi",
old=limits[tag_j][1], new=max(new, 0.0),
jams={str(a): v for a, v in jams.items()}))
limits[tag_j][1] = max(new, 0.0)
changed = True
if sign < 0 and new > limits[tag_j][0] + 0.01:
combos_log.append(dict(pass_=pass_i, hold=tag_k, hold_at=e,
swept=tag_j, direction="lo",
old=limits[tag_j][0], new=min(new, 0.0),
jams={str(a): v for a, v in jams.items()}))
limits[tag_j][0] = min(new, 0.0)
changed = True
print(" hold %s done [%.0fs]" % (tag_k, time.time() - t0), flush=True)
print(" pass %d: %s" % (pass_i + 1,
"收紧 " + str({t: [round(limits[t][0], 1), round(limits[t][1], 1)] for t in CHAIN[1:]}
if changed else "无变化")), flush=True)
if not changed:
break
# 滑块范围 = 单关节验证硬限位（任意单关节推满保证零干涉）。
# 组合折叠约束（某关节保持极限时扫其它关节发现的卡死）只写入明细/报告，
# 不压缩滑块 —— 否则手臂会被静态限位锁死（真实机器人由控制器做避碰）。
final = {}
print("\n[最终滑块范围]（单关节硬限位，含 1° 安全余量）")
for t in CHAIN[1:]:
lo, hi = singles[t]["lo"], singles[t]["hi"]
if lo <= -179.9 and hi >= 179.9:
final[t] = dict(axis=t, lo=-180.0, hi=180.0, continuous=True, zero=0.0,
note="整周无干涉，可 360° 连续旋转（全对全动态扫描验证）")
else:
lo_out = float(math.ceil(lo + 1.0)) if lo < -0.5 else 0.0
hi_out = float(math.floor(hi - 1.0)) if hi > 0.5 else 0.0
final[t] = dict(axis=t, lo=lo_out, hi=hi_out, continuous=False, zero=0.0,
note="正向 +%d° / 反向 %d°，总行程 %d°（全对全动态扫描验证，含相邻件与电机）"
% (hi_out, lo_out, hi_out - lo_out))
print(" %-3s %7.1f .. %7.1f %s" % (t, final[t]["lo"], final[t]["hi"],
"360°连续" if final[t]["continuous"] else ""))
os.makedirs(DOCS, exist_ok=True)
json.dump([final[t] for t in CHAIN[1:]],
open(DOCS + "/travel.json", "w", encoding="utf-8"), ensure_ascii=False, indent=1)
detail = dict(generated=time.strftime("%Y-%m-%d %H:%M:%S"),
vol_min=VOL_MIN, step=step, combo_step=combo_step, workers=workers,
fixture_included=INCLUDE_FIXTURE,
singles={t: dict(lo=round(singles[t]["lo"], 2), hi=round(singles[t]["hi"], 2),
jams={str(a): v for a, v in singles[t]["jams"].items()})
for t in singles},
combos=combos_log,
final=[final[t] for t in CHAIN[1:]])
json.dump(detail, open(DOCS + "/travel_detail.json", "w", encoding="utf-8"),
ensure_ascii=False, indent=1)
print("\n已写出 docs/travel.json（查看器滑块）+ docs/travel_detail.json（明细） [%.0fs]"
% (time.time() - t0))
if _POOL is not None:
_POOL.terminate()
if __name__ == "__main__":
main()
# OCC/cadquery 的静态析构在解释器正常退出时必定段错误（实测：光是
# `python -c "import arm.cad.helpers"` 就返回 139，与本项目代码无关）。所以入口
# 脚本跑完后显式刷缓冲再 os._exit —— 否则全流程已经成功、exit code 却是 139,
# 会把上游脚本的 set -e 判成失败。只在 __main__ 出口生效, 被 import 时不触发。
sys.stdout.flush()
os._exit(0)
arm/analysis/torque_balance.py
+213
View File
@@ -0,0 +1,213 @@
# -*- coding: utf-8 -*-
"""关节电机力矩与负载能力校核。
与旧版的区别（这些是旧版实际出错的地方）：
1. 拓扑。旧版把关节表手抄成 "J3 肘俯仰 / J4 前臂自转 / J5 腕俯仰 / J6 夹爪"，
并据此虚构了一个夹爪机构和 135mm 大臂——与 human_layout.py 的实际拓扑
（J3 大臂自转 / J4 肘俯仰 / J6 腕自转）完全不符，整份报告的结论都是错的。
本版不再复制拓扑：零件表、关节轴、运动学链全部复用 strength_check，
而 strength_check 的轴又直接来自 human_layout，单一事实源。
2. 力臂。旧版把每个关节的质量与力臂写成硬编码常数（J2 = 1.75kg @ 464mm），
几何一改就失效且不会报错。本版按每个零件的真实质心、在可达姿态网格上
搜索最大重力力矩。
3. 惯量。旧版用一组手填的 kg·m²。本版按 Σm·d²（对关节轴的平行轴距离）
在同一姿态网格上取最大值。零件自身转动惯量未计入，对大臂量级的关节
是小修正，已在报告中标注。
电机参数：仓库内没有原始数据手册，以下为工程假定，可用环境变量覆盖。
ARMT_CONT 连续额定输出力矩 (N·m)
ARMT_PEAK 峰值堵转输出力矩 (N·m)
ARMT_ALLOW 封闭塑料外壳热衰减后的许用连续力矩 (N·m)
ARMT_ALPHA 角加速度 (rad/s²)
ARMPAY 末端负载 (kg)
输出：docs/torque_balance.json + docs/torque_report.md
"""
import io
import json
import math
import os
import sys
try:
sys.stdout.reconfigure(encoding="utf-8")
except Exception:
pass
from arm import kinematics as HL
from arm.paths import DOCS
from arm.analysis import strength_check as SC
G = 9.81
T_CONT = float(os.environ.get("ARMT_CONT", "2.5"))
T_PEAK = float(os.environ.get("ARMT_PEAK", "8.0"))
T_ALLOW = float(os.environ.get("ARMT_ALLOW", "2.0"))
ALPHA = float(os.environ.get("ARMT_ALPHA", "5.0"))
PAYLOAD = float(os.environ.get("ARMPAY", "0.5"))
# 关节名与轴向，全部取自 human_layout（不手抄）
JOINT_CN = {t[0]: t[1] for t in HL.JOINTS}
JOINT_D = {t[0]: t[3] for t in HL.JOINTS}
def _axis_kind(tag):
d = JOINT_D[tag]
if abs(d[2]) == 1:
return "roll(绕Z)"
if abs(d[0]) == 1:
return "roll(绕X)"
return "pitch(绕Y)"
def analyze(payload_kg=PAYLOAD):
parts = SC.part_table(payload_kg)
jl = SC.joint_loads(parts)
jl0 = SC.joint_loads(SC.part_table(0.0)) # 去掉末端负载，用于反推负载能力
masses = {"J%d" % j: sum(m for _, m, _, g in parts if g >= j) for j in range(1, 7)}
arm_mass = {k: v - payload_kg * 1000.0 for k, v in masses.items()}
res = {"assumptions": {
"motor": "60mm 方形标准关节电机模组，2 级行星减速",
"T_cont_Nm": T_CONT, "T_peak_Nm": T_PEAK, "T_allow_Nm": T_ALLOW,
"alpha_rad_s2": ALPHA, "payload_kg": payload_kg,
"note": "电机参数为工程假定，仓库内无数据手册；结论对此敏感，见报告第 4 节",
}, "joints": {}}
for j in range(1, 7):
tag = "J%d" % j
r = jl[tag]
tau_g = r["tau"] / 1000.0 # N*m，重力
tau_dyn = r["I"] * ALPHA # N*m，惯性
tau_g_arm = jl0[tag]["tau"] / 1000.0 # 去掉负载后的重力力矩
req_static = tau_g
req_peak = tau_g + tau_dyn
res["joints"][tag] = {
"cn": JOINT_CN[tag], "axis": _axis_kind(tag),
"distal_mass_kg": round(masses[tag] / 1000.0, 3),
"arm_mass_kg": round(arm_mass[tag] / 1000.0, 3),
"weight_N": round(r["F"], 1),
"reach_tip_mm": round(r["Lp"], 0),
"tau_gravity_Nm": round(tau_g, 3),
"tau_gravity_arm_only_Nm": round(tau_g_arm, 3),
"I_distal_kgm2": round(r["I"], 5),
"tau_dynamic_Nm": round(tau_dyn, 3),
"tau_required_static_Nm": round(req_static, 3),
"tau_required_peak_Nm": round(req_peak, 3),
"duty_vs_Tcont_pct": round(100.0 * req_static / T_CONT, 1),
"duty_vs_Tallow_pct": round(100.0 * req_static / T_ALLOW, 1),
"peak_margin_x": round(T_PEAK / req_peak, 2) if req_peak > 1e-3 else None,
"status": "OK" if (req_static <= T_ALLOW and req_peak <= T_PEAK) else "OVERLOAD",
}
return res, jl0, jl
def max_payload(res):
"""在许用连续力矩下，末端还能带多少 kg（按各关节取最小）。
线性化：tau_g(p) ≈ tau_g_arm + p·g·L。用的是该关节实测的最大 TCP 力臂，
即最不利姿态下的力臂。
"""
out = {}
for tag, d in res["joints"].items():
L = d["reach_tip_mm"] / 1000.0
if L <= 0:
out[tag] = None
continue
head = T_ALLOW - d["tau_gravity_arm_only_Nm"]
out[tag] = round(max(head, 0.0) / (G * L), 3) if head > 0 else 0.0
return out
def generate_report(res, mp):
L = []
L.append("# 6 自由度机械臂 · 关节电机力矩与负载能力校核\n")
a = res["assumptions"]
L.append("## 1. 前提\n")
L.append("- 驱动：%s" % a["motor"])
L.append("- 连续额定输出力矩 `T_cont` = %.2f N·m；峰值堵转 `T_peak` = %.2f N·m"
% (a["T_cont_Nm"], a["T_peak_Nm"]))
L.append("- 封闭打印外壳热衰减后许用连续力矩 `T_allow` = %.2f N·m；末端负载 %.2f kg；"
"角加速度 %.1f rad/s²" % (a["T_allow_Nm"], a["payload_kg"], a["alpha_rad_s2"]))
L.append("- **%s**\n" % a["note"])
L.append("载荷模型：每个零件按网格体积加权质心定位，随姿态做刚体变换，在可达姿态"
"网格（俯仰轴取行程端点/中值，自转轴取 0/±60/±120/180°）上取重力力矩最大值。"
"力矩为绕**关节自身轴**的分量——这才是电机要出的力。\n")
L.append("## 2. 各关节校核结果\n")
L.append("| 关节 | 名称 | 轴 | 以下质量(kg) | TCP力臂(mm) | 重力力矩(N·m) | 惯性力矩(N·m) | 峰值需求(N·m) | 负荷率(对T_allow) | 峰值裕度 | 判定 |")
L.append("|:---:|:---|:---|---:|---:|---:|---:|---:|---:|---:|:---:|")
for tag, d in res["joints"].items():
L.append("| %s | %s | %s | %.2f | %.0f | **%.2f** | %.2f | %.2f | **%.0f%%** | %s | %s |"
% (tag, d["cn"], d["axis"], d["distal_mass_kg"], d["reach_tip_mm"],
d["tau_gravity_Nm"], d["tau_dynamic_Nm"], d["tau_required_peak_Nm"],
d["duty_vs_Tallow_pct"],
("%.2f×" % d["peak_margin_x"]) if d["peak_margin_x"] else "—", d["status"]))
L.append("")
L.append("## 3. 许用连续工况下的末端负载能力\n")
L.append("去掉末端负载后各关节仍需 `tau_gravity_arm_only` 的重力力矩，余额除以该关节"
"实测最大 TCP 力臂，即为还能挂的负载：\n")
L.append("| 关节 | 仅臂身重力力矩(N·m) | 余额(N·m) | 可挂负载(kg) |")
L.append("|:---:|---:|---:|---:|")
for tag, d in res["joints"].items():
head = T_ALLOW - d["tau_gravity_arm_only_Nm"]
L.append("| %s | %.2f | %+.2f | %s |"
% (tag, d["tau_gravity_arm_only_Nm"], head,
("%.2f" % mp[tag]) if mp[tag] is not None else "—"))
ok = [t for t, v in mp.items() if v is not None]
worst = min(ok, key=lambda t: mp[t]) if ok else None
if worst:
L.append("")
L.append("**整臂瓶颈：%s，可挂末端负载 %.2f kg。**\n" % (worst, mp[worst]))
L.append("## 4. 结论要点\n")
over = [t for t, d in res["joints"].items() if d["status"] == "OVERLOAD"]
if over:
L.append("- **超载关节：%s**。" % "、".join(over))
for t in over:
d = res["joints"][t]
L.append(" - %s（%s）重力 %.2f N·m，超 `T_allow` %.0f%%；即使按峰值堵转算，"
"裕度也只有 %s。" % (t, d["cn"], d["tau_gravity_Nm"],
d["duty_vs_Tallow_pct"] - 100.0,
("%.2f×" % d["peak_margin_x"]) if d["peak_margin_x"] else "—"))
L.append("- 结论对电机参数高度敏感：若实际连续额定高于假定值，"
"请用 `ARMT_CONT` / `ARMT_ALLOW` 覆盖后重跑本脚本。")
else:
L.append("- 全部关节在许用连续力矩内。")
L.append("- 惯量按 Σm·d² 估，**未计入零件自身转动惯量**，对肩/肘是大臂量级的关节"
"约低估一成，惯性力矩一栏应视为下限。")
L.append("- 本版不含被动弹簧平衡。旧版报告提出的 J2 拉簧预补偿（2.48 N·m）在几何上"
"**尚未实现**；若要保留该方案，需要在 P04/P05 之间增加拉簧连杆机构。")
return "\n".join(L)
def main():
res, jl0, jl = analyze(PAYLOAD)
mp = max_payload(res)
io.open(DOCS + "/torque_balance.json", "w", encoding="utf-8").write(
json.dumps(res, ensure_ascii=False, indent=2))
io.open(DOCS + "/torque_report.md", "w", encoding="utf-8").write(generate_report(res, mp))
print("末端负载 %.2f kg T_cont=%.2f T_allow=%.2f T_peak=%.2f alpha=%.1f\n"
% (PAYLOAD, T_CONT, T_ALLOW, T_PEAK, ALPHA))
print("%-4s %-8s %-12s %8s %8s %8s %9s %8s %s"
% ("关节", "名称", "轴", "重力N·m", "惯性N·m", "峰值N·m", "负荷率", "峰值裕度", "判定"))
for tag, d in res["joints"].items():
print("%-4s %-8s %-12s %8.2f %8.2f %8.2f %8.0f%% %8s %s"
% (tag, d["cn"], d["axis"], d["tau_gravity_Nm"], d["tau_dynamic_Nm"],
d["tau_required_peak_Nm"], d["duty_vs_Tallow_pct"],
("%.2fx" % d["peak_margin_x"]) if d["peak_margin_x"] else "-",
d["status"]))
print("\n许用连续力矩下可挂末端负载 (kg):")
for tag in res["joints"]:
print(" %-4s %s" % (tag, ("%.2f" % mp[tag]) if mp[tag] is not None else "—"))
ok = [t for t, v in mp.items() if v is not None]
if ok:
w = min(ok, key=lambda t: mp[t])
print(" 整臂瓶颈 %s -> %.2f kg" % (w, mp[w]))
print("\n已写出 docs/torque_balance.json + docs/torque_report.md")
if __name__ == "__main__":
main()
# OCC/cadquery 的静态析构在解释器正常退出时必定段错误（实测：光是
# `python -c "import arm.cad.helpers"` 就返回 139，与本项目代码无关）。所以入口
# 脚本跑完后显式刷缓冲再 os._exit —— 否则全流程已经成功、exit code 却是 139,
# 会把上游脚本的 set -e 判成失败。只在 __main__ 出口生效, 被 import 时不触发。
sys.stdout.flush()
os._exit(0)
arm/analysis/ws_sweep.py
+143
View File
@@ -0,0 +1,143 @@
# -*- coding: utf-8 -*-
"""关节角组合可达空间（工具尖点）计算：FK 采样 + J1 旋转对称包络 + 体积估计"""
import json
import math
import os
import sys
import numpy as np
from arm import kinematics as HL
from arm.paths import DOCS, CACHE
TRAVEL = {t["axis"]: (t["lo"], t["hi"]) for t in
json.load(open(DOCS + "/travel.json", encoding="utf-8"))}
TIP = (HL.X_ARM, HL.Y_AXIS, HL.Z_TIP) # P09 法兰底面中心（工具安装面）
J1_AX = (HL.Y_AXIS, HL.Z_RL) # J1 转轴: 过 Y=128, Z=610, 平行 X
def rot_about(p, k, ang):
"""绕轴 (p 点, k 方向) 旋转 ang(弧度, 可为数组M,)；返回 Rm(M,3,3), t(M,3)。
p' = Rm @ (p_pt - p) + p，与 AB.R 的 gp_Trsf.SetRotation 右手定则一致。"""
k = np.asarray(k, dtype=float)
k = k / np.linalg.norm(k)
ang = np.atleast_1d(np.asarray(ang, dtype=float))
px, py, pz = p
K = np.array([[0.0, -k[2], k[1]], [k[2], 0.0, -k[0]], [-k[1], k[0], 0.0]])
K2 = K @ K
Rm = np.eye(3)[None, :, :] + np.sin(ang)[:, None, None] * K[None, :, :] \
+ (1.0 - np.cos(ang))[:, None, None] * K2[None, :, :]
pp = np.array([px, py, pz])
t = pp[None, :] - np.einsum("mij,j->mi", Rm, pp)
return Rm, t
def fk_batch(angles, tip=TIP):
"""angles: dict idx->弧度数组(M,)；返回 (M,3) 世界坐标。链式 acc 与 sweep_v2 一致：
父关节变换在最外层 p' = R1∘R2∘…∘R5 (p)，各 R_i 绕其零位世界轴（human_layout 唯一事实源）。"""
pts = np.tile(np.asarray(tip, dtype=float), (angles[1].shape[0], 1))
for i in range(5, 0, -1): # J6 跳过：尖点在 J6 轴上，其转角不改变尖点位置
rear, d = HL.JOINTS[i - 1][2], HL.JOINTS[i - 1][3]
Rm, t = rot_about(rear, d, angles[i])
pts = np.einsum("mij,mj->mi", Rm, pts) + t
return pts
def sweep(step=2.0):
j2 = np.radians(np.arange(TRAVEL["J2"][0], TRAVEL["J2"][1] + 1e-9, step))
j4 = np.radians(np.arange(TRAVEL["J4"][0], TRAVEL["J4"][1] + 1e-9, step))
j5 = np.radians(np.arange(TRAVEL["J5"][0], TRAVEL["J5"][1] + 1e-9, step))
g2, g4, g5 = np.meshgrid(j2, j4, j5, indexing="ij")
M = g2.size
angles = {1: np.zeros(M), 2: g2.ravel(), 3: np.zeros(M),
4: g4.ravel(), 5: g5.ravel()}
pts = fk_batch(angles)
# 自证1：零位尖点必须回到 (18.5,128,93)
z0 = fk_batch({i: np.zeros(1) for i in range(1, 6)})
assert np.allclose(z0[0], TIP, atol=1e-9), z0[0]
# 自证2：J1/J6 旋转不改变 (x, r) 与极值位置——J1 取中值扫一遍，r 分布应与 J1=0 相同
for phi in (0.5, 1.2, 2.4):
v1 = np.full(M, phi)
p1 = fk_batch({1: v1, 2: g2.ravel(), 3: np.zeros(M),
4: g4.ravel(), 5: g5.ravel()})
r1 = np.hypot(p1[:, 1] - J1_AX[0], p1[:, 2] - J1_AX[1])
r0 = np.hypot(pts[:, 1] - J1_AX[0], pts[:, 2] - J1_AX[1])
assert np.allclose(np.sort(r0), np.sort(r1), atol=1e-9), "J1 对称性被破坏"
# 自证3：J3 旋转（大臂自转）不改变尖点位置——关节轴过尖点所在纵线
print("J3 对称性: 尖点 x=%.1f 与 J3 轴 x=%.1f 一致=%s, y=%.1f 与轴 y=%.1f 一致=%s"
% (TIP[0], HL.X_ARM, abs(TIP[0] - HL.X_ARM) < 1e-9,
TIP[1], HL.Y_AXIS, abs(TIP[1] - HL.Y_AXIS) < 1e-9))
print("J6 含于轴: 尖点 (%.1f,%.1f) vs 轴 (%.1f,%.1f)"
% (TIP[0], TIP[1], HL.JOINTS[5][2][0], HL.JOINTS[5][2][1]))
return pts
def envelope(pts, grid=1.0):
"""(x, r) 半平面栅格化占用 + 体积（绕 J1 轴回转）。"""
x = pts[:, 0]
r = np.hypot(pts[:, 1] - J1_AX[0], pts[:, 2] - J1_AX[1])
x0, x1 = math.floor(x.min()), math.ceil(x.max())
r0, r1 = 0.0, math.ceil(r.max())
nx = int(round((x1 - x0) / grid)) + 1
nr = int(round((r1 - r0) / grid)) + 1
xi = np.clip(((x - x0) / grid).round().astype(int), 0, nx - 1)
ri = np.clip((r / grid).round().astype(int), 0, nr - 1)
occ = np.zeros((nx, nr), dtype=bool)
occ[xi, ri] = True
# 每个 x 列只保留 r <= 该列可达最大 r（回转体沿 r 连续填充）
for i in range(nx):
j = np.nonzero(occ[i])[0]
if j.size:
occ[i, : j.max() + 1] = True
rr = r0 + np.arange(nr) * grid
vol = float((occ * (2.0 * math.pi * rr)[None, :] * grid * grid).sum())
return dict(occ=occ, x0=x0, grid=grid, rm_axes=rr, vol_mm3=vol,
nx=nx, nr=nr)
def main():
pts = sweep()
x, y, z = pts[:, 0], pts[:, 1], pts[:, 2]
r = np.hypot(y - J1_AX[0], z - J1_AX[1])
env = envelope(pts)
# 每个 x 的 r 包络（侧视/俯视剖面用）
xs = env["x0"] + np.arange(env["nx"]) * env["grid"]
rmax_x, rmin_x = [], []
for i in range(env["nx"]):
j = np.nonzero(env["occ"][i])[0]
if j.size:
rmax_x.append(env["rm_axes"][j.max()])
rmin_x.append(env["rm_axes"][j.min()])
else:
rmax_x.append(np.nan)
rmin_x.append(np.nan)
os.makedirs(CACHE, exist_ok=True)
np.savez_compressed(CACHE + "/ws_cloud.npz",
pts=pts, xs=np.array(xs),
rmax=np.array(rmax_x), rmin=np.array(rmin_x))
met = dict(
z_max=float(z.max()), z_min=float(z.min()),
x_fwd=float(x.max()), x_back=float(x.min()),
y_lo=float(y.min()), y_hi=float(y.max()),
r_max=float(r.max()), r_min=float(r.min()),
reach_from_j1_axis=float(r.max()),
spread=float(z.max() - z.min()),
vol_dm3=env["vol_mm3"] / 1e6,
tip=TIP, j1_axis=list(J1_AX),
limits={k: list(v) for k, v in TRAVEL.items()},
)
# 自证4：极值点姿态可解释（打印取极值的角度组合索引）
imax = int(np.argmax(z)); imin = int(np.argmin(z))
ifwd = int(np.argmax(x)); iback = int(np.argmin(x))
met["pose_extreme"] = dict(
zmax_idx=imax, zmin_idx=imin, xfwd_idx=ifwd, xback_idx=iback)
print(json.dumps({k: v for k, v in met.items()
if k not in ("limits", "pose_extreme")},
ensure_ascii=False, indent=1))
json.dump(met, open(CACHE + "/ws_metrics.json", "w", encoding="utf-8"),
ensure_ascii=False, indent=1)
if __name__ == "__main__":
main()