cbb
34019c1a34
- 撤回一条自己的假收益：先前"合并几何查询→24.5ms"不成立。①第一版把 xf= 传给含5行的 gaps， 只有第0行走完宽相位，认证点全部退回截顶40.0（少干活）；②真实顺序 特征→推理→候选→认证， 认证点依赖网络输出，故"特征间隙与认证点合并一次调用"实现上做不到 - 可实现并已落地：gm_groups(..., xf=) 复用 features 已算的 cum_xf，B=1 交错实测 P50 −3.4ms； 256题重跑 22 项指标与轮次33 base 逐项相同 - 新增 --cert_batch（默认0不变）：K条候选并成1次 gaps 调用。合成夹具(B=2)断言选择/轨迹/证书逐位一致， 变异检查（换回错误索引）2项测试立即失败；256题重跑同样 22 项指标逐位相同 实测交换：典型 33.5→37.6ms，最坏（前面候选被拒）80.2→37.1ms，P99 64.7→48.3ms - FP16/BF16 实测不比 FP32 快（推理 7.68→8.31→8.58ms），整周期不变而 P99 更差，动作偏差 0.005°/0.038° → 小批量下不是算力受限，半精度不划算，推理保持 FP32 - 仍不达 20ms：下一刀是流水线复用（下次规划的特征间隙＝本次已认证的末点），可整块省掉特征 ~13ms - 测试 32→34 项
62 lines
2.4 KiB
Python
62 lines
2.4 KiB
Python
"""策略输入特征（83 维）—— 与 webapp/js/ik-policy.js features() 逐项一致。改动须两边同步并升 FEAT_VER。"""
|
||
import math
|
||
import torch
|
||
|
||
FEAT_VER = 1
|
||
VMAX = [60., 60., 90., 90., 120., 180.]
|
||
DT = 0.02
|
||
STEP = [v * DT for v in VMAX]
|
||
ACC = 5.0
|
||
H, NJ, PREFIX = 16, 6, 3
|
||
GP = [(0, 2), (0, 3), (0, 4), (0, 5), (0, 6), (2, 4), (2, 5), (3, 5)]
|
||
GAP_CAP = 40.0
|
||
F_DIM = 83
|
||
D2R = math.pi / 180.0
|
||
ROT6 = [0, 3, 6, 1, 4, 7] # 行主序 3×3 的前两列
|
||
|
||
|
||
def wrap180(x):
|
||
return torch.remainder(x + 180.0, 360.0) - 180.0
|
||
|
||
|
||
def features(kin, q, q_prev, T, R, prefix, prefix_len, sub, sub_flag, gm=None, xf=None):
|
||
"""全部为 batch 张量：
|
||
q, q_prev [B,6] 度；T [B,3] mm；R [B,9]（pos 模式全 0）；pose 标志由 R 是否全 0 判定
|
||
prefix [B,3,6] 增量（度），prefix_len [B]；sub [B,6]，sub_flag [B]（0/1）
|
||
gm：kin.gaps(q, 40)['gm'] 可传入复用。"""
|
||
B = q.shape[0]
|
||
dt = q.dtype
|
||
step = torch.tensor(STEP, dtype=dt, device=q.device)
|
||
xf = xf if xf is not None else kin.cum_xf(q)
|
||
out = []
|
||
rad = q * D2R
|
||
out.append(torch.stack([torch.sin(rad), torch.cos(rad)], -1).reshape(B, 12))
|
||
lo, hi = kin.lo.to(dt), kin.hi.to(dt)
|
||
idx = [1, 3, 4]
|
||
out.append(2 * (q[:, idx] - lo[idx]) / (hi[idx] - lo[idx]) - 1)
|
||
d = torch.where(kin.cont, wrap180(q - q_prev), q - q_prev)
|
||
out.append(d / step)
|
||
p = kin.fk(q, xf); Rq = kin.orient(q, xf).reshape(B, 9)
|
||
out.append(p / 600)
|
||
out.append(Rq[:, ROT6])
|
||
out.append(T / 600)
|
||
pose = (R.abs().sum(-1) > 0).to(dt)
|
||
out.append(R[:, ROT6] * pose[:, None])
|
||
out.append((T - p) / 600)
|
||
out.append(((T - p) / 50).clamp(-1, 1))
|
||
out.append(torch.stack([1 - pose, pose], -1))
|
||
m = (torch.arange(PREFIX, device=q.device)[None, :] < prefix_len[:, None]).to(dt) # [B,3]
|
||
out.append((prefix / step * m[:, :, None]).reshape(B, 18))
|
||
out.append(m)
|
||
sd = torch.where(kin.cont, wrap180(sub - q), sub - q) / 180 * sub_flag[:, None].to(dt)
|
||
out.append(sd)
|
||
out.append(sub_flag[:, None].to(dt))
|
||
if gm is None:
|
||
out.append(kin.gm_groups(q, GP, GAP_CAP, xf=xf) / GAP_CAP) # 2026-10-20：只算特征用到的 8 组；xf 复用不重复算 FK
|
||
gm = None
|
||
if gm is not None:
|
||
out.append(torch.stack([gm[:, a * 7 + b].clamp(max=GAP_CAP) / GAP_CAP for a, b in GP], -1))
|
||
f = torch.cat(out, -1)
|
||
assert f.shape[1] == F_DIM, f.shape
|
||
return f
|