refactor(layout): src/ 收成顶层包 arm/，入口统一 python -m；提升承担构建的 scratch 脚本
单文件约束取消后，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:
@@ -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()
|
||||
Reference in New Issue
Block a user