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

feat: 增加组合折叠姿态干涉扫描、力矩实测与结构强度复核数据

Browse Source
This commit is contained in:
cbb
2026-10-02 12:06:23 +08:00
parent 6fe97fc643
commit b593e47c76
21 changed files with 1587 additions and 1030 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/arm_build.py
+13 -9
View File
@@ -211,17 +211,21 @@ class Joint:
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 tapped holes on the motor rear metal endplate:
(±17.55, ±20.46) on the rear face at motor-local z = 0.
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 sx in (-17.55, 17.55):
for sy in (-20.46, 20.46):
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))
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):
src/arm_model.py
+62 -15
View File
@@ -794,13 +794,15 @@ def p_girdle(j1):
s = s.cut(Xcyl(1.95, X_COL - TENON / 2.0 - 1.0, X_COL - TENON / 2.0 + 14.0, Y_COL, Z_LOCK))
# 4x M3 连接螺孔（螺丝刀通道直接走两竖板之间的开放腔体，无需再钻 Ø8.5 直通隧道）
for dy in (-17.55, 17.55):
for dz in (-20.46, 20.46):
py = Y_ARM + dy
pz = Z_ARM + dz
th = Xcyl(1.7, X_BACK - 0.5, X_MOUNT + 4.0, py, pz)
cb = Xcyl(3.25, X_BACK - 0.5, X_BACK + 4.5, py, pz)
s = s.cut(th).cut(cb)
# 孔位 = 电机尾端 4x M3 螺纹径向槽实测位（电机.STEP）：BCD55、相位 39.9°+90k。
# 旧 35.1×40.9 矩形孔距与实际槽位差 ~4.7mm（对角两颗螺钉无处咬合），
# 2026-10-02 用户指出"螺钉没有对齐"后按 STEP 实测修正。
for dy, dz in ((21.06, 17.65), (-17.65, 21.06), (-21.06, -17.65), (17.65, -21.06)):
py = Y_ARM + dy
pz = Z_ARM + dz
th = Xcyl(1.7, X_BACK - 0.5, X_MOUNT + 4.0, py, pz)
cb = Xcyl(3.25, X_BACK - 0.5, X_BACK + 4.5, py, pz)
s = s.cut(th).cut(cb)
# 中央走线孔 (Ø26mm)
wire_hole = Xcyl(13.0, X_COL - 40.0, X_MOUNT + 2.0, Y_ARM, Z_ARM)
@@ -883,6 +885,11 @@ def p_fork(j1, j2):
2. J1 从动法兰圆盘已删除(盘面在 column 前缘形成鼓楞)：J1 由 boss(Ø35.6)贯通 P03 前盖中心孔直连电机输出盘；column 顶边保持 Z=646.5 与 P03 环齐高，R=18mm 大圆弧顺滑过渡；
3. 头部转接凸台加长贯通 P03 前盖中心孔 (R=19.5mm)，直连 J1 旋转法兰盘；
4. M2 保留后端安装板(R=35mm, 带 rear_mount_holes 螺钉孔)，前端定位环鼓出圆盘已删除(前端定位由 column 端面 pocket 承担)，中间镂空散热；
4b. 【2026-10-02 新增】M2 前端压盖：原方案电机只被后端安装板(S02 4xM3x10)单面锁死，
大扭矩下电机绕 Y 轴在 Ø60.3 座孔里点头/翘起，固定不牢。现在前端盖外端面
(Y=156.0) 再加一枚同径 R35 压盖，用 4x M3x8 在电机前端盖 BCD55 上把电机
从前端夹一次 —— 电机被前后两块同心 Ø60.3 端面双向夹紧，倾斜自由度归零；
螺钉沉在压盖沉孔内不外凸，不占用 J2 轴向间隙（P05 法兰盘已相应后移）；
5. 严格满足 single-solid 流形拓扑与 360° 全范围零干涉 (COLLISION pairs = 0)。
"""
# 1. M2 后端安装法兰盘 (Y: 92..100, R=35mm)
@@ -892,6 +899,28 @@ def p_fork(j1, j2):
front_collar = Ycyl(35.0, Y_COVER - 6.5, Y_COVER - 0.5, X_ARM, Z_SH)
s = rear_plate.fuse(front_collar)
# 2b. 【2026-10-02 新增】M2 前端压盖 (Y: 155.5..161.0, R=35，与后端板同径)
# 根部与 front_collar 前端面 (Y_COVER-0.5=155.5) 共面熔接成一体，无缝隙、无悬浮面。
# 压盖背面 (Y_COVER=156.0) 与电机前端盖外端面 0.0 间隙面压贴合；Ø37 中央让位孔
# 让出 P05 的 J2 直锁凸台 (R17.8) 与 3xS01 输出螺钉头；Ø60.3 让位槽把压盖背面
# 精确落在 Y=156.0，不让根部那 0.5mm 啃到电机前端盖 R30.15 的外圆。
FP_Y0 = Y_COVER - 0.5 # 155.5 压盖根部（= front_collar 前端面，保证熔接）
FP_Y1 = Y_COVER + 5.0 # 161.0 压盖前端面（与 P05 法兰盘 162.0 留 1.0mm 动间隙）
FP_BORE = 27.5 # 4xM3 分布圆半径 (BCD55，与 P03→M1 的 S04 同约定)
fp = Ycyl(35.0, FP_Y0, FP_Y1, X_ARM, Z_SH)
fp = fp.cut(Ycyl(18.5, FP_Y0 - 0.5, FP_Y1 + 0.5, X_ARM, Z_SH)) # Ø37 中央让位孔
fp = fp.cut(Ycyl(30.15, FP_Y0 - 0.5, Y_COVER, X_ARM, Z_SH)) # Ø60.3 让位槽
for deg in (17.8, 107.8, 197.8, 287.8):
rad = math.radians(deg)
fu, fz = X_ARM + FP_BORE * math.cos(rad), Z_SH + FP_BORE * math.sin(rad)
# Ø6.0 沉孔深 3.0 (自前端面 FP_Y1 往 -Y 沉到 Y=158.0)：M3 圆柱头(Ø5.5x3.0)
# 全沉入压盖，头面 Y=158.0 压在 Ø3.4 孔口台阶上；压盖背面(156.0)压电机前端盖，
# 中间 2.0mm 压盖本体受压 —— 夹紧力全部走面压，螺钉只提供压紧力不提供定位。
fp = fp.cut(Ycyl(3.0, Y_COVER + 2.0, FP_Y1 + 0.5, fu, fz))
fp = fp.cut(Ycyl(1.7, Y_COVER - 0.5, Y_COVER + 2.0, fu, fz))
# 压盖在第 7 步 M2 接口成型（座孔/让位槽）之后才熔接，避免被那些 R30.15~30.5 的
# 圆柱刀具蹭掉根部一圈 0.1mm 薄边。
# 3. J1 从动法兰圆盘已删除：原 R=36.5 圆盘盘面在 column 前缘形成圆盘鼓楞，
# J1 与 P04 的连接由 boss(Ø35.6 贯通 P03 前盖中心孔 R=19.5)直连电机输出盘
# 承担，螺钉走 boss 端面 4xB 孔，圆盘无传力功能已去除。
@@ -960,6 +989,10 @@ def p_fork(j1, j2):
c_m2 = [cyl(BORE_R, -0.6, COVER_Z - 3.5), cyl(CB_R, COVER_Z - 3.5, COVER_Z + 0.6)]
s = s.cut(*[j2.L(item) for item in c_m2])
# 7b. M2 前端压盖熔接（见 2b）：必须在 6/7 步所有 R30.15~30.5 圆柱刀具走完之后，
# 否则压盖根部会被 front_collar_pocket / c_m2 啃掉一圈 0.1mm 薄边。
s = s.fuse(fp)
# 8. 头部转接凸台加长（贯通 P03 前盖中心孔，跨越装配缝直连 J1 输出盘）
boss = cyl(17.8, 54.942, 61.5 + X_FORK_GAP)
s = s.fuse(j1.L(boss))
@@ -977,17 +1010,22 @@ def p_fork(j1, j2):
def make_pitch_to_roll_bracket(j_pitch, j_roll, z_pitch, z_r, name="bracket"):
def make_pitch_to_roll_bracket(j_pitch, j_roll, z_pitch, z_r, name="bracket", hub_gap=1.0):
"""Half-wrapped (半包) 双曲面有机流线型 Pitch-to-Roll 结构座 (P05 & P08):
1. 顶部从动法兰与前侧 Loft 自然相切顺接；
2. 严格清除 z_plate 顶面以上与上级旋转件贴面的多余材料 (cut_top_solid)，保证关节大范围摆动动态 0 干涉；
3. 下方电机实行半包散热设计，-Y侧敞开，+Y侧背脊与圆柱一体化融合；
4. 严格满足 single-solid 流形拓扑与零干涉 (COLLISION pairs = 0)。
hub_gap = 从动法兰盘背面与上级电机前端盖之间的轴向让位间隙。
默认 1.0（贴合防磨）。P05 传 6.0：J2 处 P04 新增了 M2 前端压盖
（Y 155.5..161.0），必须让出这 6.0mm 才能装下；见 p_shoulder_roll_base。
"""
z_bot = z_r - 56.0 # roll 电机底端 (front collar)
z_plate = z_r + 7.0 # roll 电机顶端安装板 (rear plate top)
y_out = HL.Y_HUB + HL.PLATE_T # 174.0 (俯仰直连法兰外侧面)
y_hub_in = HL.Y_COVER + 1.0 # 157.0 (从动法兰内侧面，预留 1.0mm 动间隙)
y_hub_in = HL.Y_COVER + hub_gap # 157.0 / (P05) 162.0 从动法兰内侧面
z_hub_in = y_hub_in - HL.Y_REAR # 法兰盘背面的电机局部 z（凸台长度跟着它走）
y_cyl_max = HL.Y_AXIS + 35.0 # 163.0 (下方电机外轮廓极值)
# 1. 基础立柱：Ø70mm 实心圆柱 (Z: z_bot .. z_plate, R=35mm)
@@ -1065,8 +1103,10 @@ def make_pitch_to_roll_bracket(j_pitch, j_roll, z_pitch, z_r, name="bracket"):
# 9. 电机定子配合空腔
s = s.cut(cyl(30.5, z_r - 49.5, z_r - 0.5, HL.X_ARM, HL.Y_AXIS))
# 10. 俯仰轴直连法兰结构（凸台延伸至 59.5 跨过间隙与法兰盘牢固熔接，保持单实体）
boss = cyl(17.8, 54.942, 59.5)
# 10. 俯仰轴直连法兰结构（凸台延伸至法兰盘背面之后 2.5mm，跨过间隙与法兰盘牢固熔接，
# 保持单实体）。凸台长度跟着 y_hub_in 走：法兰盘后移时凸台同步加长，
# 3xS01 输出螺钉的头面台阶(局部 z=58)、杆长、咬合量与改前完全一致。
boss = cyl(17.8, 54.942, z_hub_in + 2.5)
s = s.fuse(j_pitch.L(boss))
s = s.cut(j_pitch.L(cyl(3.5, 54.0, 74.0 + 30.0)))
for a in AB.HUB_M2_CLOCK:
@@ -1089,8 +1129,15 @@ def make_pitch_to_roll_bracket(j_pitch, j_roll, z_pitch, z_r, name="bracket"):
def p_shoulder_roll_base(j2, j3, half=None):
"""P05_大臂旋转座_螺旋体30%_墙4底5_竖打（深度圆角优化版）"""
return make_pitch_to_roll_bracket(j2, j3, Z_SH, Z_R3, name="P05_大臂旋转座_螺旋体30%_墙4底5_竖打")
"""P05_大臂旋转座_螺旋体30%_墙4底5_竖打（深度圆角优化版）
hub_gap=6.0：J2 处 P04 新增了 M2 前端压盖（前端夹紧电机，消除大扭矩下的倾斜），
压盖占 Y 155.5..161.0，所以从动法兰盘背面必须从 Y=157 后移到 Y=162（留 1.0mm 动间隙）。
P08 保持默认 1.0 —— J5 处 P07 没有前端压盖，不需要这段轴向空间。
"""
return make_pitch_to_roll_bracket(j2, j3, Z_SH, Z_R3,
name="P05_大臂旋转座_螺旋体30%_墙4底5_竖打",
hub_gap=6.0)
def p_humerus_bone(j3, j4):
@@ -1394,9 +1441,9 @@ def build():
("P03_肩吊臂_螺旋体35%_墙5底5_竖打", "肩关节安装悬臂", lambda: p_girdle(j1), 0x4b5563,
"轻量化板式桁架版：主干由 66mm 实心块改为双侧 6mm 桁架竖板+帐篷减重窗(斜面≥52°自支撑)+R4内角，外轮廓与旧版并集完全一致，实心 728->286cm³；底部 9mm 承力鞍座(Ø3.9 安装孔)+12mm 中位龙骨筋(自鞍座斜拉至法兰下缘)+40~45°外扩斜撑；保留 10mm 后置法兰、侧开式电机托架、Ø60.7 直通电机舱(平孔无台阶)(M1 从+Y滑入,后端 4xM3x16 锁死)、大散热窗与对流槽；无封闭空腔、免支撑"),
("P04_肩叉_螺旋体30%_墙5底5_竖打", "肩叉", lambda: p_fork(j1, j2), 0x2563eb,
"齐平插槽版：叉板背面与P03托架端面(11.5)0.0硬贴平（用户要求外观齐平），P03侧插槽独立加长3mm，M1电机可倾斜卡入，J1输出凸台贯通直连电机，J1从动法兰圆盘鼓楞已删（接缝区与P03止脱环齐径齐高R36.5/顶646.5），单实体全角度零干涉"),
"齐平插槽+ M2 前端压盖版：叉板背面与P03托架端面(11.5)0.0硬贴平（用户要求外观齐平），P03侧插槽独立加长3mm，M1电机可倾斜卡入，J1输出凸台贯通直连电机，J1从动法兰圆盘鼓楞已删（接缝区与P03止脱环齐径齐高R36.5/顶646.5），单实体全角度零干涉；【2026-10-02 新增】M2 前端压盖(R35, Y155.5..161.0)：电机前端盖外端面加 4xM3x8 沉孔(BCD55)夹紧，与后端安装板 4xM3x10 形成前后双向面压，消除大扭矩下电机在座孔里的倾斜"),
("P05_大臂旋转座_螺旋体30%_墙4底5_竖打", "大臂旋转座", lambda: p_shoulder_roll_base(j2, j3, None), 0x059669,
"V3圆角：Loft外轮廓R=10mm，L型转角内部2mm倒角，后缘凸包两角90°竖棱磨R2圆角（结构/后缘高度不变，打印无悬挂），保证J2全角度0干涉"),
"V3圆角+ J2前端夹紧配合：Loft外轮廓R=10mm，L型转角内部2mm倒角，后缘凸包两角90°竖棱磨R2圆角（结构/后缘高度不变，打印无悬挂）；J2 从动法兰盘背面由 Y=157 后移至 Y=162（hub_gap=6.0，给 P04 新增的 M2 前端压盖让位，J2 直锁凸台同步加长到 Y=165.5，3xS01 输出螺钉咬合量不变）；保证 J2 全角度 0 干涉"),
("P06_大臂骨架_螺旋体25%_墙4底5_竖打", "大臂骨架", lambda: p_humerus_bone(j3, j4), 0x0d9488,
"Loft有机过渡版：借鉴P05圆弧变方形放样算法，顶端Ø70正圆与J3平齐无台阶，底端渐变融入M4抱紧套筒与壳体，单实体0干涉"),
("P07_小臂骨架_螺旋体30%_墙5底5_竖打", "小臂骨架", lambda: p_forearm_bone(j4, j5, None), 0x7c3aed,
src/finalize_travel.py
+32 -4
View File
@@ -3,6 +3,11 @@
滑块范围 = 单关节硬限位（任意单关节推满零干涉，含 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
@@ -13,20 +18,43 @@ sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from sweep_v2 import CHAIN, DOCS
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,
note="整周无干涉，可 360° 连续旋转（全对全动态扫描验证）"))
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,
note="正向 +%d° / 反向 %d°，总行程 %d°（全对全动态扫描验证，含相邻件与电机）"
% (hi_out, lo_out, hi_out - lo_out)))
print("%-3s %7.1f .. %7.1f -> %s .. %s" % (t, lo, hi, out[-1]["lo"], out[-1]["hi"]))
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)
src/human_layout.py
+20 -1
View File
@@ -79,7 +79,26 @@ Y_M6 = Y_AXIS
Y_CHEST = 62.0 # mast half width
X_CHEST = 6.0 # chest plate half thickness
# ------------------------------------------------------------------ joint frames
# ------------------------------------------------------------------ 台架（非常规构件）
# P01 立柱 / P02 底盘 是【固定测试台架】，不是机器人本体的一部分（用户 2026-10-02
# 确认）；它在扫描中代表机械臂与**躯干/机身连接界面**的那块实体。因此：
#
# - 臂撞立柱 = 臂撞躯干，是真约束，不能忽略。sweep_v2 报出的组合卡死（J3 ±162°、
# J4 ±18°、J5 ±36° 等，全部是工具法兰 P09 撞立柱 P01_A）就是这类真约束。
# - 但台架的**位置与截面是一个假设**。躯干侧工作空间只和这个假设一样准：真机躯干
# 挪后一点或外形不同，这些限位就变。所以台架本身不要为了放宽行程去改（那是改测试
# 工装，不是改设计）；要改的是"真机躯干占位"这个输入。
# - `--no-stand` 仅用于诊断：给出不含躯干约束的自由空间行程，用来分辨某个限位到底是
# 臂自身折叠产生的、还是躯干挡的。默认不排除。
FIXTURE_PREFIX = ("P01_", "P02_")
def is_fixture(name):
"""该零件是否属于固定测试台架（代表躯干/安装界面，非机器人本体）。"""
return name.startswith(FIXTURE_PREFIX)
# ------------------------------------------------------------------ 关节表
# tag, cn, rear, d, xdir, holder of the motor, driven part
# J1 roll +X fore-aft, in the shoulder base -> drives P04 肩叉
# J2 pitch +Y outboard in the fork -> drives P05 大臂旋转座
src/screw_mesh.py
+28 -14
View File
@@ -13,7 +13,7 @@ docs/screws.json 是紧固件清单的唯一事实源（规格 / 长度 / 数量
台阶 z=63(J1, p_fork th 54..63) / 58(其余, th 54..58)；头深藏在 Ø5.6 螺丝刀通道内
S02 P04~P08 4xM3 ：下级电机 rear_mount_holes：沉孔 Ø5.5 从 -t_plate-1 到 -t_plate+4，
台阶 z = -t_plate+4 = -4（t_plate=8）/ -3（t_plate=7），头沉入 1mm
S03 P03 后法兰 4xM3 ：J1 局部 (±17.55, ±20.46, -5.5) （沉孔 Ø6.5 台阶 x=-56.5，法兰外端面 -61）
S03 P03 后法兰 4xM3 ：J1 局部 (R27.5 @39.9°+90k, -5.5) （沉孔 Ø6.5 台阶 x=-56.5，法兰外端面 -61）
S04 P03 前置锁紧凸台 4xM3 ：J1 局部 (R27.5 @17.8/107.8/197.8/287.8°, 62.5)
（沉孔 Ø6.0 在 Ø10 凸台端面 x=11.5，M3x10 旋入电机前端面咬合 3.5mm）
S05 P01 分段对锁 8xM4x16：Ø3.9 直孔 x∈{-131,-93}, z∈{180,329}（母口壁与公榫壁同径），
@@ -61,6 +61,12 @@ PROX = {"J2": "P04 肩叉", "J3": "P05 大臂旋转座", "J4": "P06 大臂骨架
COLOR = 0xc3cad6 # 不锈钢冷色
LMAX = {"S08": 17.0, "S10": 22.0} # 可见段截断：螺杆伸到底座底面(z=0)即止（台面/预埋螺母未建模）
# 电机尾端 4x M3 螺纹径向槽的实测孔位（电机.STEP）：BCD55、相位 39.9°+90k，
# 槽宽 2.5、径向可调约 ±1.25mm。S02（P04~P08 后端板）与 S03（P03 后法兰）共用；
# 旧 35.1×40.9 矩形孔距对角两颗偏出螺纹槽 ~4.7mm（2026-10-02 用户指出"螺钉没有对齐"）。
REAR_SLOTS = tuple((27.5 * math.cos(math.radians(39.9 + 90.0 * k)),
27.5 * math.sin(math.radians(39.9 + 90.0 * k))) for k in range(4))
# ---------------------------------------------------------------- 局部坐标系工具
def _norm(v):
@@ -127,8 +133,9 @@ def _geom(spec):
nom = float(str(spec["螺纹"]).lstrip("Mm"))
rs, L = nom / 2.0, float(spec["长度mm"])
head = spec.get("头型", "")
if "沉头" in head: # 沉头内六角：90° 锥，最宽处在承载面
return dict(kind="csk", rs=rs, L=L, rh=2.0 * nom)
if "沉头" in head: # 沉头内六角：90° 锥，最宽处(刃口)在承载面
rh = {3.0: 3.15, 4.0: 3.5, 6.0: 5.5}.get(nom, 1.05 * nom) # 头径≈沉孔座位径（GB dk）
return dict(kind="csk", rs=rs, L=L, rh=rh)
if "外六角" in head: # 外六角：M6 对边 10，高 4
af = {3.0: 5.5, 4.0: 7.0, 5.0: 8.0, 6.0: 10.0}.get(nom, 1.7 * nom)
hh = max(3.0, 0.7 * nom)
@@ -142,9 +149,10 @@ def _mesh(g):
"""局部网格：头部 z ∈ [-hh,0]（0 = 承载头面），杆部 z ∈ [0, L]。"""
rs, L = g["rs"], g["L"]
if g["kind"] == "csk":
hh = g["rh"] - rs
head = _merge(_side(rs, g["rh"], -hh, 0.0, 14), _cap(rs, -hh, 14, up=False),
_cap(g["rh"], 0.0, 14, up=True))
hh = g["rh"] - rs # 90° 锥：头高 = (dk-d2)/2
head = _merge(_side(g["rh"], rs, 0.0, hh, 14), # 锥面自承载面收进材料（沉入沉孔座位）
_cap(g["rh"], 0.0, 14, up=False), # 承载端外露环形面
_cap(rs, hh, 14, up=True))
elif g["kind"] == "hex":
hh, rh = g["hh"], 0.98 * g["rh"]
head = _merge(_side(rh, rh, -hh, 0.0, 6), _cap(rh, 0.0, 6, up=True),
@@ -186,10 +194,9 @@ def instances(m2_bcd, m2_clock):
for tag, tp in (("J2", 8.0), ("J3", 7.0), ("J4", 8.0), ("J5", 8.0), ("J6", 7.0)):
d = frame(tag)[1]
zt = -tp + 4.0 # 沉孔台阶 = -t_plate + cb_depth(4)
for u in (-17.55, 17.55):
for v in (-20.46, 20.46):
out.append(("S02", to_global(tag, u, v, zt), tuple(-c for c in d), GRP_PAR[tag],
"%s 后端板" % PROX[tag], "孔位(%.2f,%.2f) -> %s 电机尾端面" % (u, v, tag)))
for u, v in REAR_SLOTS:
out.append(("S02", to_global(tag, u, v, zt), tuple(-c for c in d), GRP_PAR[tag],
"%s 后端板" % PROX[tag], "孔位(%.2f,%.2f) -> %s 电机尾端面" % (u, v, tag)))
d1 = frame("J1")[1]
for deg in (17.8, 107.8, 197.8, 287.8):
@@ -197,10 +204,17 @@ def instances(m2_bcd, m2_clock):
v = 27.5 * math.sin(math.radians(deg))
out.append(("S04", to_global("J1", u, v, 62.5), d1, "root",
"P03 前置锁紧凸台", "BCD55 @%.1f° -> M1 前端面" % deg))
for u in (-17.55, 17.55):
for v in (-20.46, 20.46):
out.append(("S03", to_global("J1", u, v, -5.5), tuple(-c for c in d1), "root",
"P03 后置法兰", "35.1×40.9 孔距 -> M1 尾端面"))
for u, v in REAR_SLOTS:
out.append(("S03", to_global("J1", u, v, -5.5), tuple(-c for c in d1), "root",
"P03 后置法兰", "BCD55 @39.9° 螺纹槽 -> M1 尾端面"))
# S12 P04 前端压盖 -> M2 前端盖（J2）：沉孔台阶 Y=158（局部 z=58），与 S04 同 BCD55 相位
d2 = frame("J2")[1]
for deg in (17.8, 107.8, 197.8, 287.8):
u = 27.5 * math.cos(math.radians(deg))
v = 27.5 * math.sin(math.radians(deg))
out.append(("S12", to_global("J2", u, v, 58.0), d2, "J1",
"P04 前端压盖", "BCD55 @%.1f° -> M2 前端面（前端夹紧）" % deg))
for x in (X_COL - 16.0, X_COL + 22.0):
for z in (180.0, 329.0):
src/strength_check.py
+253 -32
View File
@@ -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)
src/sweep_v2.py
+23 -8
View File
@@ -23,8 +23,13 @@
- 预剪枝：按关节轴计算每对零件的轴向/径向区间，区间不相交的零件对在该关节的
任何角度都不可能碰撞，直接跳过（对单关节扫描是精确安全的）。
台架（P01 立柱 / P02 底盘）代表躯干与机身的连接界面，默认【参与】碰撞计算——臂撞立柱
就是臂撞躯干，是真约束。--no-stand 仅用于诊断：排除台架后重扫，给出"不含躯干约束"的
自由空间行程，用来分辨某个限位是臂自身折叠产生的还是躯干挡的（台架位置本身是假设，
改它等于改测试工装，不等于改设计）。
运行：python src/sweep_v2.py [--workers 20] [--step 2] [--combo-step 6]
[--no-combos] [--joints J2,J4] [--no-gjk]
[--no-combos] [--joints J2,J4] [--no-gjk] [--no-stand]
"""
import json
import math
@@ -44,6 +49,7 @@ import multiprocessing as mp
import arm_build as AB
import cadquery as cq
import human_layout as HL
from armcore import place, bb, DOCS
VOL_MIN = 2.0 # mm3，超过即计为干涉
@@ -61,6 +67,7 @@ MOUNT_PAIRS = {
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
@@ -165,11 +172,15 @@ def _do_simplex(s):
# ---------------------------------------------------------------- 模型与上下文
def load_model():
def load_model(include_fixture=True):
"""include_fixture=False 时把 P01 立柱 / P02 底盘（固定测试台架，非本体）排除在
碰撞配对之外——用来得到真机工作空间而不是台架工作空间。见 human_layout.FIXTURE_PREFIX。"""
import arm_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])]
@@ -212,8 +223,8 @@ def axis_intervals(parts, rear, d):
return out
def make_ctx():
parts, joints = load_model()
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]]
@@ -395,10 +406,10 @@ _POOL = None
_WORKERS = 4
def _init_worker(use_gjk=True):
def _init_worker(use_gjk=True, include_fixture=True):
global _CTX, USE_GJK
USE_GJK = bool(use_gjk)
_CTX = make_ctx()
_CTX = make_ctx(include_fixture)
def _task(task):
@@ -410,7 +421,7 @@ 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,))
_POOL = mp.Pool(workers, initializer=_init_worker, initargs=(USE_GJK, INCLUDE_FIXTURE))
return _POOL
@@ -448,7 +459,7 @@ def eval_angles(angles_list, workers, chunk=None, label="姿态"):
def _ctx_single():
global _CTX
if _CTX is None:
_CTX = make_ctx()
_CTX = make_ctx(INCLUDE_FIXTURE)
return _CTX
@@ -555,6 +566,9 @@ def main():
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:
@@ -691,6 +705,7 @@ def main():
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},
src/torque_balance.py
+187 -279
View File
@@ -1,27 +1,28 @@
# -*- coding: utf-8 -*-
"""Motor Torque Balance and Dynamic Capacity Analysis.
"""关节电机力矩与负载能力校核。
This module evaluates static and dynamic torque demand across all 6 joints (J1 to J6)
for the 6-DOF compact robotic arm driven by the standardized 60mm gearmotor module.
与旧版的区别（这些是旧版实际出错的地方）：
Motor Module Parameters (measured from 电机.STEP):
- Reduction: 2-stage planetary 25:1
- Mass: 210g per unit
- Continuous Rated Torque: T_cont = 2.5 N·m
- Peak Stall Torque: T_peak = 8.0 N·m
- Safe Continuous Allowable (in 3D printed housing): T_allow = 2.0 N·m
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²（对关节轴的平行轴距离）
在同一姿态网格上取最大值。零件自身转动惯量未计入，对大臂量级的关节
是小修正，已在报告中标注。
Load Cases Evaluated:
1. Horizontal Outstretched (Worst-Case Static Overhang):
theta_2 = 90°, theta_3 = 0°, theta_5 = 0°, payload = 0.5 kg (tip)
2. Standard Working Posture (90° Elbow Flexion):
theta_2 = 45°, theta_3 = 90°, payload = 0.5 kg
3. Dynamic Acceleration (worst-case sweep @ alpha = 5 rad/s^2)
4. Spring Gravity Compensation (Passive Assist at J2)
电机参数：仓库内没有原始数据手册，以下为工程假定，可用环境变量覆盖。
ARMT_CONT 连续额定输出力矩 (N·m)
ARMT_PEAK 峰值堵转输出力矩 (N·m)
ARMT_ALLOW 封闭塑料外壳热衰减后的许用连续力矩 (N·m)
ARMT_ALPHA 角加速度 (rad/s²)
ARMPAY 末端负载 (kg)
Outputs:
- docs/torque_balance.json
- docs/torque_report.md
输出：docs/torque_balance.json + docs/torque_report.md
"""
import io
import json
@@ -30,277 +31,184 @@ import os
import sys
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from armcore import DOCS, WEB, OUT
try:
sys.stdout.reconfigure(encoding="utf-8")
except Exception:
pass
import human_layout as HL
from armcore import DOCS
import strength_check as SC
G = 9.81
T_CONT = 2.5 # N·m
T_PEAK = 8.0 # N·m
T_ALLOW = 2.0 # N·m derated thermal continuous for enclosed plastic housing
PAYLOAD_KG = 0.5 # 500g rated tip payload
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"))
# Link geometric parameters (measured from human_layout / arm_model)
# Joint stations (mm):
# J1: Z=652 (Torso roll)
# J2: Z=560 (Shoulder pitch, offset from J1 by 92mm in Z)
# J3: Z=425 (Elbow pitch, L_upper = 135mm)
# J4: Z=300 (Forearm roll, internal coaxial)
# J5: Z=267 (Wrist pitch, L_fore = 158mm)
# J6: Z=171 (Gripper tip, L_hand = 96mm, total wrist-to-tip = 171mm)
# 关节名与轴向，全部取自 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 analyze_torque(masses=None):
if masses is None:
# Default mass distribution (in kg) based on 38% infill PLA + 210g motor per joint
# Distal mass carried by each joint (including its driven link + all distal motors/links)
masses = {
"J1": 2.15, # carries entire arm (J2..J6 motors + links)
"J2": 1.75, # carries humerus + forearm + hand + payload
"J3": 1.20, # carries forearm + wrist + hand + payload
"J4": 0.85, # carries internal forearm rotor + wrist + hand + payload
"J5": 0.65, # carries palm + gripper + payload
"J6": 0.25 # gripper jaws + payload
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",
}
# Moment arms to distal center of mass and payload in horizontal outstretched pose (meters)
# L_cg: distance from joint axis to combined distal link CG
# L_tip: distance from joint axis to tip payload (0.5 kg)
dist_tip = {
"J1": 0.547,
"J2": 0.464,
"J3": 0.329,
"J4": 0.171,
"J5": 0.171,
"J6": 0.060
}
# Distance to the distal mass CG (excluding payload)
dist_cg = {
"J1": 0.280,
"J2": 0.235,
"J3": 0.165,
"J4": 0.085,
"J5": 0.085,
"J6": 0.030
}
# --- Case 1: Worst-Case Horizontal Outstretched ---
# In horizontal pose:
# J2 (pitch) and J3 (pitch) and J5 (pitch) bear full gravity moment.
# J1 (roll) sees zero gravity bending torque if arm is purely along pitch plane,
# but if arm swings 90° sideways, J1 sees the full horizontal moment.
# J4 (roll) has axis aligned with forearm, so gravity torque on J4 is near 0.
# Masses excluding payload
m_arm_only = {k: masses[k] - PAYLOAD_KG for k in masses}
horiz_static = {}
for j in ["J1", "J2", "J3", "J4", "J5", "J6"]:
if j in ["J1", "J2", "J3", "J5"]:
# Arm gravity moment + Payload gravity moment
m_arm = m_arm_only[j]
t_arm = m_arm * G * dist_cg[j]
t_pay = PAYLOAD_KG * G * dist_tip[j]
t_tot = t_arm + t_pay
elif j == "J4":
# Forearm roll: minimal eccentricity (< 10mm)
t_tot = (m_arm_only[j] + PAYLOAD_KG) * G * 0.008
else: # J6 gripper
# Grip clamping force is normal, axial reaction torque:
t_tot = 0.08
horiz_static[j] = t_tot
# --- Passive Spring Gravity Assist at J2 ---
# Mounting lugs placed on shoulder fork and elbow
# Designed counter-moment: Ts(theta) = T_spring_max * sin(theta_2)
# Target: compensate ~65% of horizontal gravity torque (approx 2.48 N·m)
t_spring_j2 = 2.48
horiz_net_j2 = horiz_static["J2"] - t_spring_j2
# --- Case 2: Natural Working Posture (Elbow 90° flexed, shoulder 45°) ---
# Shoulder angle = 45°, elbow angle = 90°
# The effective moment arm to payload is reduced from 464mm to ~210mm
# The effective moment arm of humerus is 135mm * sin(45°) = 95mm
work_static = {}
# J2 in working pose:
# Humerus mass (~0.55kg) at 48mm: 0.55 * 9.81 * 0.048 = 0.26 N·m
# Forearm + Hand + Payload (~1.20kg) hanging vertically at L_upper*sin(45°) = 95mm:
# 1.20 * 9.81 * 0.095 = 1.12 N·m
# Total J2 static working torque: ~1.38 N·m (without spring), with spring (~1.75 N·m counter): net is ~0.37 N·m
work_static["J1"] = 0.65
work_static["J2"] = 1.38
work_static["J3"] = 0.88
work_static["J4"] = 0.05
work_static["J5"] = 0.52
work_static["J6"] = 0.08
work_net_j2 = abs(work_static["J2"] - t_spring_j2 * math.sin(math.radians(45)))
# --- Dynamic Acceleration Load (alpha = 5 rad/s^2) ---
# Approximate moment of inertia I_distal = sum(m_i * r_i^2)
inertia = {
"J1": 0.085, # kg·m^2
"J2": 0.072, # kg·m^2
"J3": 0.031, # kg·m^2
"J4": 0.003, # kg·m^2
"J5": 0.004, # kg·m^2
"J6": 0.001 # kg·m^2
}
alpha = 5.0 # rad/s^2 (corresponds to high speed reach)
dyn_torque = {k: inertia[k] * alpha for k in inertia}
# Total peak combined torque during fast horizontal move
peak_combined = {}
for k in masses:
if k == "J2":
peak_combined[k] = horiz_net_j2 + dyn_torque[k]
else:
peak_combined[k] = horiz_static[k] + dyn_torque[k]
results = {
"motor_specs": {
"module": "电机.STEP (60mm x 56mm)",
"ratio": "25:1 2-stage planetary",
"rated_continuous_Nm": T_CONT,
"peak_stall_Nm": T_PEAK,
"allowable_thermal_Nm": T_ALLOW
},
"payload_kg": PAYLOAD_KG,
"joints": {}
}
for j in ["J1", "J2", "J3", "J4", "J5", "J6"]:
t_raw = horiz_static[j]
t_work = work_static[j]
t_dyn = dyn_torque[j]
t_opt = horiz_net_j2 if j == "J2" else t_raw
duty_raw = (t_raw / T_CONT) * 100.0
duty_opt = (t_opt / T_CONT) * 100.0
margin_peak = T_PEAK / max(peak_combined[j], 0.01)
results["joints"][j] = {
"distal_mass_kg": round(masses[j], 3),
"reach_tip_mm": int(dist_tip[j] * 1000),
"static_horizontal_raw_Nm": round(t_raw, 2),
"static_horizontal_opt_Nm": round(t_opt, 2),
"static_working_pose_Nm": round(work_static[j], 2),
"dynamic_accel_Nm": round(t_dyn, 2),
"peak_combined_Nm": round(peak_combined[j], 2),
"duty_cycle_raw_pct": round(duty_raw, 1),
"duty_cycle_opt_pct": round(duty_opt, 1),
"peak_stall_safety_factor": round(margin_peak, 2),
"status": "PASS" if duty_opt <= 100.0 and margin_peak >= 1.5 else "WARN"
}
return results
def generate_report(res):
lines = []
lines.append("# 6自由度机械臂关节电机力矩平衡校核与优化分析报告\n")
lines.append("## 1. 电机性能基准与工况参数")
lines.append("- **驱动电机规格**: 标准化一体化行星减速关节电机 (`电机.STEP`)")
lines.append("- **外形尺寸**: 直径 Ø60.0 mm × 长度 56.0 mm，方形端盖 63.6 mm")
lines.append("- **传动形式**: 2级行星减速器 (减速比 25:1)")
lines.append("- **连续额定扭矩 ($T_{rated}$)**: 2.50 N·m")
lines.append("- **峰值堵转扭矩 ($T_{peak}$)**: 8.00 N·m")
lines.append("- **封闭塑料外壳热衰减后容许连续扭矩 ($T_{allow}$)**: 2.00 N·m")
lines.append("- **末端负载 ($m_{load}$)**: 0.50 kg (500g 全行程工况)")
lines.append("- **角加速度 ($\alpha$)**: 5.0 rad/s²\n")
lines.append("## 2. 各关节力矩校核与平衡计算表")
lines.append("| 关节编号 | 运动自由度 | 下属质量(kg) | 极限量臂(mm) | 极限水平静力矩(N·m) | 弹簧平衡后力矩(N·m) | 工作姿态静力矩(N·m) | 额定负荷率 | 峰值安全裕度 | 状态 |")
lines.append("|:---|:---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|")
labels = {
"J1": "肩抬举 (Roll)",
"J2": "肩俯仰 (Pitch)",
"J3": "肘俯仰 (Pitch)",
"J4": "前臂自转 (Roll)",
"J5": "腕俯仰 (Pitch)",
"J6": "手爪开合 (Grip)"
}
for jid, d in res["joints"].items():
spring_note = f"**{d['static_horizontal_opt_Nm']:.2f}**" if jid == "J2" else "-"
lines.append(f"| {jid} | {labels[jid]} | {d['distal_mass_kg']:.2f} | {d['reach_tip_mm']} | "
f"{d['static_horizontal_raw_Nm']:.2f} | {spring_note} | {d['static_working_pose_Nm']:.2f} | "
f"**{d['duty_cycle_opt_pct']:.1f}%** | {d['peak_stall_safety_factor']:.2f}x | {d['status']} |")
lines.append("\n## 3. 关键瓶颈分析与优化措施落地")
lines.append("### 3.1 瓶颈定位：J2 肩俯仰关节点")
lines.append("- **原始状态**: 在全臂水平伸直极限工况下，下属总质量达 1.75 kg（含大臂、小臂、腕部电机及0.5kg末端负载），质心距离达 464 mm。由此产生的重力力矩高达 **3.78 N·m**。")
lines.append("- **风险**: 3.78 N·m 超过电机连续额定值 2.5 N·m（负荷率 151%）。长时间悬停会导致电机过热保护甚至失步；同时 3D 打印结构受长期持续弯矩容易产生塑性蠕变下垂。")
lines.append("\n### 3.2 优化方案一：被动式拉簧重力预补偿 (Spring Gravity Balance)")
lines.append("- **设计**: 在 J2 肩叉 (P04) 与大臂 (P05) 之间布置平行拉伸弹簧连杆机构。")
lines.append("- **力矩对冲**: 水平展开时拉簧产生反向补偿力矩 $T_s = -2.48\\text{ N}\\cdot\\text{m}$。")
lines.append("- **效果**: J2 电机实际所需静扭矩骤降至 **1.30 N·m**，额定负荷率从 **151% 降至 52.0%**，安全落入连续工况安全区（小于 $T_{allow} = 2.0\\text{ N}\\cdot\\text{m}$）。")
lines.append("\n### 3.3 优化方案二：四点角接触薄壁轴承解耦 (QJ204 Moment Decoupling)")
lines.append("- **设计**: J2 驱动侧配置薄壁四点接触轴承 (Ø20×Ø47×14mm)。")
lines.append("- **效果**: 臂身产生的 38 N·m 侧向倾覆弯矩完全由外圈刚性轴承与肩叉支架承担，电机行星减速箱输出轴端面齿仅承受纯扭矩载荷，彻底杜绝行星齿轮偏磨卡死。")
lines.append("\n### 3.4 优化方案三：闭口箱涵与内腔纵向加强筋 (Closed-Box Beam Stiffness)")
lines.append("- **设计**: 大臂与小臂由原始开缝受弯槽钢结构升级为**带中肋闭口箱型梁**，壁厚 3.0mm，外廓 66mm。")
lines.append("- **效果**: 截面抗弯惯性矩 $I_{min}$ 从 $8.25\\times 10^4\\text{ mm}^4$ 提升至 $3.12\\times 10^5\\text{ mm}^4$（**刚度提升 3.78 倍**），彻底消除悬臂颤振与末端抖动。")
lines.append("\n### 3.5 优化方案四：典型作业姿态规划 (Elbow Flexion Workload)")
lines.append("- 在日常搬运抓取姿态下（肘部弯曲 90°），末端质心力臂由 464 mm 缩短至 210 mm。")
lines.append("- 各关节电机工作负荷均在 0.37~1.38 N·m 之间（负荷率 15%~55%），发热温升处于环境温标以内，保证长时间稳定运行。")
return "\n".join(lines)
return res, jl0, jl
def get_actual_arm_masses():
json_path = WEB + "/mesh_arm.json"
if not os.path.exists(json_path):
return None
d = json.load(io.open(json_path, encoding="utf-8"))
TREE = ["root", "J1", "J2", "J3", "J4", "J5", "J6"]
tot = {}
for p in d.get("parts", []):
nm = p["name"]
if p.get("printed", False):
v_cm3 = p.get("vol", 0.0) / 1000.0
g = v_cm3 * 1.24 * 0.38
elif nm.startswith("M"):
g = 210.0 # motor module
elif "半联轴器" in nm:
g = 14.0 # aluminum face spline half-coupling
elif "抱箍" in nm:
g = 8.0 # aluminum clamp band
else:
g = 20.0
tot[nm] = (g, p.get("grp", "root"))
m_calc = {}
for k, jid in enumerate(TREE[1:], 1):
down = set(TREE[k:])
m_calc[jid] = (sum(g for g, grp in tot.values() if grp in down) + PAYLOAD_KG * 1000.0) / 1000.0
return m_calc
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():
masses = get_actual_arm_masses()
if masses:
print("Accurate physical mass distribution (including 0.5kg payload):")
for k, v in masses.items():
print(f" {k}: {v:.2f} kg (arm: {v - PAYLOAD_KG:.2f} kg + payload: {PAYLOAD_KG:.2f} kg)")
res = analyze_torque(masses)
# Save JSON
out_json = DOCS + "/torque_balance.json"
io.open(out_json, "w", encoding="utf-8").write(json.dumps(res, ensure_ascii=False, indent=2))
print(f"Torque balance JSON written to: {out_json}")
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))
# Save Markdown report
rep = generate_report(res)
out_md = DOCS + "/torque_report.md"
io.open(out_md, "w", encoding="utf-8").write(rep)
print(f"Torque report MD written to: {out_md}")
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")
sys.stdout.flush()
print("\n" + "="*80)
print("TORQUE BALANCE SUMMARY (WITH PASSIVE SPRING & STIFFNESS OPTIMIZATION)")
print("="*80)
for jid, d in res["joints"].items():
print(f"[{jid}] Horiz Raw: {d['static_horizontal_raw_Nm']:5.2f} N·m | "
f"Opt: {d['static_horizontal_opt_Nm']:5.2f} N·m | "
f"Duty: {d['duty_cycle_opt_pct']:5.1f}% | "
f"Peak Margin: {d['peak_stall_safety_factor']:4.2f}x | Status: {d['status']}")
print("="*80)
os._exit(0)
if __name__ == "__main__":
main()
os._exit(0)