100 lines
4.9 KiB
Python
100 lines
4.9 KiB
Python
|
# -*- 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()
|