exp+fix: commit2 增益重扫(gain2.2 到达 50.8%,n=512)+ 浏览器安全层三点修复(限位夹紧/部分前缀/离散认证)
- gain 1.8/2.2/2.5/2.8 = 50.4/52.0/49.6/47.7%(n=256),2.2 最佳;n=512 复核 50.8%/1.4% 碰撞 - 积分反馈与反馈隔次复用均为负结论(−4.0/−5.1 pp) - JS:tick/候选投影夹 LIM(原越限后 inBox 永久拒绝→刹停);safety_partial 移植;segCert 开关(默认严格) - bench nc10 --seg 0:刹车率 79.5→48.1%、planP50 56.1→33.5ms、ok 0→5/10
This commit is contained in:
@@ -447,6 +447,36 @@
|
||||
- **新历史最佳(50 Hz 目标节奏,commit=2)= 49.2% / 碰撞 2.3% / ratio2s 0.351 / time_ok 26.2%**;非实时上限(commit=1)= 57.4%(n=256,待 n=512 复核)。
|
||||
- **下一步**:① n=512 复核 commit=1;② 积分反馈(fb_ki,针对慢变偏差)在 commit=2 上叠加;③ 浏览器 JS 端同步解析 J;④ commit=2 的 40 ms 周期实测规划 P99(Python/JS)。
|
||||
|
||||
## 轮次 27:解析 J 后重扫增益 + 积分/缓存反馈(负结论)+ 浏览器安全层对齐(exp: `exp/dagger-deploy`)
|
||||
|
||||
- **目标**:解析雅可比改变了反馈噪声结构,重扫增益;同时修浏览器部署路径的安全层(轮次 25 遗留:bench 刹车 47%、到达 0)。
|
||||
- **增益重扫(commit=2,empty,analytic,n=256)**:
|
||||
|
||||
| gain | 到达 | time_ok_1p3 | 碰撞 | ratio2s | 刹停/题 |
|
||||
|---|---|---|---|---|---|
|
||||
| 1.8 | 50.4% | 27.0% | 2.0% | 0.287 | 22.1 |
|
||||
| **2.2** | **52.0%** | 29.7% | 1.6% | 0.329 | 35.3 |
|
||||
| 2.5 | 49.6% | 34.4% | 1.2% | 0.334 | 47.9 |
|
||||
| 2.8 | 47.7% | **34.4%** | 2.0% | 0.362 | 59.8 |
|
||||
|
||||
- **n=512 复核 gain 2.2(commit=2)**:**到达 50.8% / time_ok 28.9% / 碰撞 1.4% / ratio2s 0.343 / 刹停 35.9**。**新历史最佳(50 Hz 节奏)= gain 2.2 + commit=2**。
|
||||
- **负结论**:
|
||||
- 积分反馈(fb_ki=0.3,decay0.9,限幅0.5·STEP):到达 48.0%(gain2.2 单独 52.0%),−4.0 pp。慢变偏差用积分项补偿会过冲,不采用。
|
||||
- 反馈隔次复用(fb_every=2,省 ~3.5 ms/次规划):到达 46.9%(−5.1 pp)。误差方向变化比预期快,缓存不成立。
|
||||
- **浏览器部署修复(有效)**:三个与 Python 仿真不一致的 bug 修掉后(`webapp/js/ik-policy.js`):
|
||||
1. `tick` 不夹关节限位(Python `clamp_box` 每步夹)→ q 一旦越限,认证的 `inBox` 永远拒绝所有候选,原地刹停到超时(certWhy.box 2650 次);现执行与候选投影都夹 LIM。
|
||||
2. 安全层整段拒绝 → 刹停;移植 `safety_partial`:整段不过时依次缩短前缀,执行到第一个不安全步。
|
||||
3. 新增 `segCert` 开关(默认 true):连续区间认证(距离下界递归)~40 ms/次,是 50 Hz 预算的主要超支项;关闭后改为离散判定「每步端点+中点 collideFree」(与 Python 安全层同口径),默认仍走严格认证,bench 用 `--seg 0` 测部署口径。
|
||||
- JS bench(nc10,fb2.2,commit2;`node tests/policy_bench.cjs ... --seg 0`):
|
||||
|
||||
| 集合 | 修复前 ok/reached | 修复后 ok/reached | 刹车率 | safety P50 | plan P50 / P99 |
|
||||
|---|---|---|---|---|---|
|
||||
| near_column_pos | 0/—(轮次 25) | **5/7** | 79.5→**48.1%** | 41.6→**19.8 ms** | 56.1→**33.5 / 61.1 ms** |
|
||||
| near_column_pose | 0/— | **3/3** | 80.4→**31.6%** | 43.4→**22.8 ms** | 57.3→**37.6 / 66.3 ms** |
|
||||
|
||||
- **结论**:实时配置定为 `dag2 + 解析 J + gain2.2 + commit2 + safety_partial + replan=empty`:n=512 到达 **50.8%**、碰撞 **1.4%**。浏览器端从「0 到达 + 47% 刹车」恢复到 nc 集可达,plan P50 33.5/37.6 ms;P99 仍 61–66 ms,commit=2 的 40 ms 队列会偶发饥饿,需继续压 P99(K/安全候选数/JS 特征)。
|
||||
- **下一步**:① Python 测 `safety_try=1`(只认证首选候选)对准确率的影响,若无损则 JS 同步(省多次 collideFree);② K=4/6 的准确率-时延权衡;③ JS `gapMatrix` 同步只算 8 组特征;④ commit=2 异步部署的饥饿率实测。
|
||||
|
||||
## 分支清理(2026-10-20)
|
||||
|
||||
- `feat/fb-mix`(8e5431c):反馈混合方案的中间分支,**已验证是 `exp/dagger-deploy` 的祖先**(所有提交都在后者历史里),本地/远端分支删除,内容保留在 exp/dagger-deploy。
|
||||
|
||||
+3
-1
@@ -42,6 +42,7 @@ def main():
|
||||
ap.add_argument('--ki', type=float, default=0.0)
|
||||
ap.add_argument('--kdecay', type=float, default=0.9)
|
||||
ap.add_argument('--kimax', type=float, default=0.5)
|
||||
ap.add_argument('--fb_every', type=int, default=1)
|
||||
ap.add_argument('--select', default='consist')
|
||||
ap.add_argument('--replan', default='r3')
|
||||
ap.add_argument('--safety', type=int, default=1, help='1=安全层+部分前缀;0=关')
|
||||
@@ -62,7 +63,8 @@ def main():
|
||||
t0 = time.perf_counter()
|
||||
r = run(model, kin, tasks, a.device, K=a.K, safety=bool(a.safety), seed=a.seed, max_sec=a.max_sec,
|
||||
safety_partial=(a.commit if (a.safety and a.partial) else 0), commit_n=a.commit,
|
||||
select=a.select, replan=a.replan, fb_gain=a.fb, fb_ki=a.ki, fb_decay=a.kdecay, fb_imax=a.kimax)
|
||||
select=a.select, replan=a.replan, fb_gain=a.fb, fb_ki=a.ki, fb_decay=a.kdecay, fb_imax=a.kimax,
|
||||
fb_every=a.fb_every)
|
||||
wall = time.perf_counter() - t0
|
||||
print('ckpt=%s n=%d K=%d commit=%d fb=%.2f ki=%.2f select=%s replan=%s safety=%d' %
|
||||
(a.ckpt, r['n'], a.K, a.commit, a.fb, a.ki, a.select, a.replan, a.safety))
|
||||
|
||||
@@ -153,7 +153,7 @@ class TeacherOracle:
|
||||
def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, return_traj=False, use_sub=True, sub_override=None,
|
||||
qp0=None, steps=1, z_scale=1.0, select='consist', oracle=None, replan='r3', sub_rule='cross',
|
||||
fb_gain=0.0, fb_scale=0.0, fb_near=0.2, fb_fallback=0.0, safety_partial=0, commit_n=3, collect=None,
|
||||
fb_exec=0.0, exec_safety=0, fb_ki=0.0, fb_decay=0.9, fb_imax=0.5):
|
||||
fb_exec=0.0, exec_safety=0, fb_ki=0.0, fb_decay=0.9, fb_imax=0.5, fb_every=1):
|
||||
"""use_sub=False:不用子目标(sf=0);sub_override=(subs [B,M,6] 绝对关节角, n [B] 个数 或 need [B] bool):冷启动时用给定途经点代替子目标头;
|
||||
qp0 [B,6]:起点的上一帧(默认 = q0,即静止起步)。steps:MeanFlow 采样段数;z_scale:噪声缩放;
|
||||
select:consist(部署口径:冷启动价值前瞻、之后与上一条已提交计划的一致性)/ value(每次都价值前瞻)/ first(不选,取第 0 条)。
|
||||
@@ -173,6 +173,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
queue = torch.zeros(B, 0, 6, device=device)
|
||||
last = None; last_ok = torch.zeros(B, dtype=torch.bool, device=device)
|
||||
fb_i = torch.zeros(B, 6, device=device) # 积分项:累积持续同向的目标反馈(慢变偏差)
|
||||
fb_cache = None; pit = 0 # fb_every>1:复用上一次规划的目标反馈(误差方向慢变)
|
||||
subs = torch.zeros(B, 2, 6, device=device); nsub = torch.zeros(B, dtype=torch.long, device=device)
|
||||
sub_t = torch.zeros(B, device=device); sub_prev = q0.clone()
|
||||
done = torch.zeros(B, dtype=torch.bool, device=device); t_done = torch.full((B,), float('nan'), device=device)
|
||||
@@ -204,7 +205,11 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
act, val, sub = model.sample_k(feat, K, z, steps=steps)
|
||||
inc = act.float().clamp(-1, 1) * astep
|
||||
if fb_gain > 0: # 实验(2026-10-20):目标反馈加在所有候选上(安全层也评估带反馈的候选)
|
||||
fb = dls_steps(kin, qe, T, R, n=1)
|
||||
if fb_cache is None or fb_every <= 1 or pit % fb_every == 0:
|
||||
fb = dls_steps(kin, qe, T, R, n=1); fb_cache = fb
|
||||
else:
|
||||
fb = fb_cache
|
||||
pit += 1
|
||||
if fb_ki > 0:
|
||||
fb_i = fb_decay * fb_i + fb[:, 0] * (~done)[:, None]
|
||||
lim = step * fb_imax
|
||||
|
||||
@@ -14,7 +14,7 @@ const fs = require('fs'), path = require('path');
|
||||
const argv = process.argv.slice(2);
|
||||
const flag = k => argv.includes(k);
|
||||
const opt = k => { const i = argv.indexOf(k); return i >= 0 ? argv[i + 1] : null; };
|
||||
const VALOPT = new Set(['--out', '--nc-n', '--route-n', '--r8-n', '--fb', '--commit']);
|
||||
const VALOPT = new Set(['--out', '--nc-n', '--route-n', '--r8-n', '--fb', '--commit', '--seg']);
|
||||
const pos = argv.filter((a, i) => !a.startsWith('--') && !(i > 0 && VALOPT.has(argv[i - 1])));
|
||||
const MODEL = pos[0] || path.join(__dirname, '../webapp/models/traj_policy.onnx');
|
||||
const SETS = (pos[1] || 'nc,route').split(',').concat(flag('--nc1000') ? ['nc1000'] : []);
|
||||
@@ -71,7 +71,7 @@ const pct = (a, p) => { if (!a.length) return null; const s = a.slice().sort((x,
|
||||
for (let ci = 0; ci < cases.length; ci++) {
|
||||
const c = cases[ci];
|
||||
const R = mode === 'pose' ? c.R : null;
|
||||
const r = new POL.Runner(S, infer, { seed: 1000 + ci, actScale: meta.act_scale || 1, fbGain: +opt('--fb') || 0, commit: Math.max(1, +opt('--commit') || 3) }); // 2026-10-19:动作尺度随模型
|
||||
const r = new POL.Runner(S, infer, { seed: 1000 + ci, actScale: meta.act_scale || 1, fbGain: +opt('--fb') || 0, commit: Math.max(1, +opt('--commit') || 3), segCert: opt('--seg') !== '0' }); // 2026-10-19:动作尺度随模型
|
||||
r.reset(c.q0); r.setGoal(c.T, R);
|
||||
let prev = c.q0.slice(), cf = 0, tLock = null;
|
||||
for (let k = 0; k < 500 && !r.done; k++) {
|
||||
|
||||
+58
-13
@@ -138,7 +138,19 @@ function refineStep(S, q, T, Rt, maxScale) {
|
||||
return r.concat([0.5 * (M[7] - M[5]) * 100, 0.5 * (M[2] - M[6]) * 100, 0.5 * (M[3] - M[1]) * 100]);
|
||||
};
|
||||
const e0 = e(q), m = e0.length, h = 1e-3, J = [];
|
||||
if (S.jac) {
|
||||
/* 解析几何雅可比(与 ml/sim/closed_loop.py 的 dls_steps(analytic=True) 同口径):
|
||||
有限差分在 float32/双精度下都会给反馈方向注入 ~1–4% 噪声,闭环里会被慢变误差放大。
|
||||
J[j][i] = −∂err_i/∂q_j:位置 err=T−p → +Jv;姿态 err=100·rotvec → +100·Jw。 */
|
||||
const jv = S.jac(q);
|
||||
for (let j = 0; j < 6; j++) {
|
||||
const col = [jv.Jv[0][j], jv.Jv[1][j], jv.Jv[2][j]];
|
||||
if (m === 6) col.push(100 * jv.Jw[0][j], 100 * jv.Jw[1][j], 100 * jv.Jw[2][j]);
|
||||
J.push(col);
|
||||
}
|
||||
} else {
|
||||
for (let j = 0; j < 6; j++) { const qa = q.slice(); qa[j] += h; const e1 = e(qa); J.push(e1.map((v, i) => -(v - e0[i]) / h)); }
|
||||
}
|
||||
// Δq = Jᵀ (J Jᵀ + λ²I)⁻¹ e,m ≤ 6
|
||||
const lam = 0.5, A = [];
|
||||
for (let i = 0; i < m; i++) { A.push([]); for (let k = 0; k < m; k++) { let s = i === k ? lam * lam : 0; for (let j = 0; j < 6; j++) s += J[j][i] * J[j][k]; A[i].push(s); } }
|
||||
@@ -179,14 +191,14 @@ class Runner {
|
||||
this.o = Object.assign({ K: 8, commit: 3, margin: 5, /* 2026-10-18:运行时投影余量保持 5 mm(真机护栏,不迁就数据分布);数据侧另行过滤认证间隙 < 3 mm 的老师轨迹,APG 训练余量 3 mm */
|
||||
projMax: 0.5, refineMm: 5, arriveMm: 1, arriveDeg: 0.5,
|
||||
stuckPlans: 5, lockSec: 1.0, subReachDeg: 5, subTimeout: 1.5, subRule: 'cross', subLeadSec: 0.02, seed: 12345, lookahead: true,
|
||||
actScale: 1, certBudget: 40, safetyMs: 30, firstMs: 45, fbGain: 0 }, opts || {});
|
||||
actScale: 1, certBudget: 40, safetyMs: 30, firstMs: 45, fbGain: 0, segCert: true }, opts || {});
|
||||
/* 2026-10-19:actScale = 模型 meta.act_scale(动作标签按 act_scale·STEP 归一化,旧模型 1);增量 = clamp(act, ±1)·actScale·STEP,
|
||||
与 ml/sim/closed_loop.py 同口径。subRule:'cross'(默认,越过顶点即切,与训练标签同口径;见 _subAdvance)/ 'timeout'(旧:< subReachDeg 或 subTimeout 超时)。 */ // 每条候选最多 40 次距离查询;安全层总预算 30 ms,第 1 条候选可用到 45 ms(超出按刹停处理)
|
||||
this.cert = makeCert(S, { leverTable: this.o.leverTable, cap: 20 });
|
||||
this.rng = makeRng(this.o.seed);
|
||||
/* 2026-10-18 第六轮(审查 B-1/B-2):耗时拆开记——inferMs 主推理、lookaheadMs 前瞻(第二次推理 + 8 份特征)、safetyMs 只含认证、
|
||||
planMs 整次规划(R9 口径:必须 < 60 ms 周期);certQ 每次规划新增的距离查询数,certHit 记忆命中数,certWhy 认证失败原因计数 */
|
||||
this.stats = { plans: 0, inferMs: [], lookaheadMs: [], safetyMs: [], planMs: [], planWarmMs: [], planColdMs: [], certQ: [], certHit: 0, certWhy: {}, brakes: 0, refine: 0, lookahead: 0, locks: 0, projected: 0 };
|
||||
this.stats = { plans: 0, inferMs: [], lookaheadMs: [], safetyMs: [], planMs: [], planWarmMs: [], planColdMs: [], certQ: [], certHit: 0, certWhy: {}, brakes: 0, refine: 0, lookahead: 0, locks: 0, projected: 0, partial: 0 };
|
||||
this.reset(null);
|
||||
}
|
||||
reset(q0) {
|
||||
@@ -213,7 +225,10 @@ class Runner {
|
||||
if (!this.q) return null;
|
||||
const d = this.queue.shift();
|
||||
this.qPrev = this.q.slice();
|
||||
if (d) this.q = this.q.map((v, j) => v + d[j]);
|
||||
if (d) this.q = this.q.map((v, j) => { // 与 ml/sim/closed_loop.py 的 clamp_box 同口径:限位轴执行时夹到 LIM
|
||||
const id = this.S.ORDER[j], L = this.S.LIM[id], x = v + d[j];
|
||||
return L.cont ? x : Math.max(L.lo, Math.min(L.hi, x));
|
||||
});
|
||||
this.t += DT;
|
||||
if (this.goal && !this.queue.length) {
|
||||
const { e, oe } = this.err(this.q);
|
||||
@@ -250,16 +265,39 @@ class Runner {
|
||||
}
|
||||
if (adv) { this.subPrev = this.subs.shift(); this.subT = this.t; }
|
||||
}
|
||||
_certCommit(qe, steps) { // 认证:提交步 + 刹停段(先用离散判定快速剔除,再做距离下界认证)
|
||||
_certCommit(qe, steps, partial) { // 认证:提交步 + 刹停段(先用离散判定快速剔除,再做距离下界认证)
|
||||
/* partial:与 ml/sim/closed_loop.py 的 safety_partial 同口径——整段不过时依次缩短前缀,
|
||||
执行到第一个不安全步为止(而不是整段拒绝 → 刹停)。2026-10-20:浏览器 bench 刹车率 47% 的主因是整段拒绝。 */
|
||||
const tryPath = n => {
|
||||
const use = steps.slice(0, n);
|
||||
const pts = [qe.slice()]; let q = qe.slice();
|
||||
for (const d of steps) { q = q.map((v, j) => v + d[j]); pts.push(q.slice()); }
|
||||
for (let i = 1; i < pts.length; i++) if (!this.S.collideFree(pts[i])) return { ok: false, tail: [] };
|
||||
const tail = brakeSteps(steps.length ? steps[steps.length - 1] : [0, 0, 0, 0, 0, 0]);
|
||||
for (const d of use) { q = q.map((v, j) => v + d[j]); pts.push(q.slice()); }
|
||||
if (!this.o.segCert) {
|
||||
/* 离散判定(步点 + 中点,与 ml/sim/closed_loop.py 安全层同口径):capsule 模型余量已含在 collideFree。
|
||||
连续区间认证(segCert)~40 ms/次,是 50 Hz 部署的主要瓶颈;关闭后 planP50 预计 <30 ms。 */
|
||||
let prev = qe.slice();
|
||||
for (const p of pts.slice(1)) {
|
||||
const mid = prev.map((v, j) => 0.5 * (v + p[j]));
|
||||
if (!this.S.collideFree(mid) || !this.S.collideFree(p)) return null;
|
||||
prev = p;
|
||||
}
|
||||
const tail = brakeSteps(use.length ? use[use.length - 1] : [0, 0, 0, 0, 0, 0]);
|
||||
let qq = q;
|
||||
if (tail.length) { for (const d of tail) qq = qq.map((v, j) => v + d[j]); if (!this.S.collideFree(qq)) return null; }
|
||||
return { ok: true, tail: tail, steps: use };
|
||||
}
|
||||
for (let i = 1; i < pts.length; i++) if (!this.S.collideFree(pts[i])) return null;
|
||||
const tail = brakeSteps(use.length ? use[use.length - 1] : [0, 0, 0, 0, 0, 0]);
|
||||
/* 刹停段各步同向等比减速 → 关节空间里是一条直线,认证终点一段即可 */
|
||||
if (tail.length) { for (const d of tail) q = q.map((v, j) => v + d[j]); pts.push(q.slice()); }
|
||||
let qq = q;
|
||||
if (tail.length) { for (const d of tail) qq = qq.map((v, j) => v + d[j]); pts.push(qq.slice()); }
|
||||
const c = this.cert.path(pts, this.o.certBudget, this._until);
|
||||
if (!c.ok) this.stats.certWhy[c.why || 'collide'] = (this.stats.certWhy[c.why || 'collide'] || 0) + 1;
|
||||
return { ok: c.ok, tail };
|
||||
return { ok: c.ok, tail: tail, steps: use };
|
||||
};
|
||||
if (!partial) { const r = tryPath(steps.length); return r || { ok: false, tail: [] }; }
|
||||
for (let n = steps.length; n >= 1; n--) { const r = tryPath(n); if (r && r.ok) return r; }
|
||||
return { ok: false, tail: [] };
|
||||
}
|
||||
async plan() {
|
||||
if (!this.needPlan()) return;
|
||||
@@ -316,7 +354,9 @@ class Runner {
|
||||
const adj = [];
|
||||
for (const d of steps) {
|
||||
let qn = q.map((v, j) => v + d[j]);
|
||||
const qp = project(S, qn, o.margin, o.projMax, near);
|
||||
let qp = project(S, qn, o.margin, o.projMax, near);
|
||||
/* 限位轴先夹到 LIM(与执行 clamp_box 同口径),否则候选会被认证的 inBox 整段拒绝 → 永远刹停 */
|
||||
qp = qp.map((v, j) => { const L = S.LIM[S.ORDER[j]]; return L.cont ? v : Math.max(L.lo, Math.min(L.hi, v)); });
|
||||
if (qp !== qn) this.stats.projected++;
|
||||
adj.push(qp.map((v, j) => v - q[j])); q = qp;
|
||||
}
|
||||
@@ -366,16 +406,21 @@ class Runner {
|
||||
collideFree 1.5 ms、criticalPairs 3.1 ms,推理本身 1.5 ms——耗时在几何查询,不在模型。此前截止只在候选之间检查,
|
||||
单条候选最多 40 次查询(≈150 ms)不受限。现在墙钟截止下沉到认证内部:第 1 条候选截止 t1+firstMs,其余共享到 t1+safetyMs;
|
||||
到点判不过 → 沿上次已认证的刹停段停,安全性不变,只是可能多刹停。 */
|
||||
let picked = null;
|
||||
let picked = null, pickedSteps = null;
|
||||
for (const c of pool) {
|
||||
if (c !== pool[0] && now() - t1 > o.safetyMs) break;
|
||||
this._until = t1 + (c === pool[0] ? Math.max(o.firstMs, o.safetyMs) : o.safetyMs);
|
||||
const v = this._certCommit(qe, c.steps); if (v.ok) { picked = c; this.brakeTail = v.tail; break; }
|
||||
const v = this._certCommit(qe, c.steps, true);
|
||||
if (v.ok) { picked = c; pickedSteps = v.steps; this.brakeTail = v.tail; break; }
|
||||
}
|
||||
this._until = undefined;
|
||||
this.stats.safetyMs.push(now() - t1);
|
||||
if (picked) {
|
||||
this.queue.push(...picked.steps); this.lastPlan = [qe.slice()].concat(picked.fut); this.brakeRun = 0;
|
||||
if (pickedSteps.length < picked.steps.length) this.stats.partial++;
|
||||
this.queue.push(...pickedSteps);
|
||||
/* 部分前缀时实际执行步数 < commit,一致性对齐的偏移不再成立 → 置空(Python 侧同轮次 14 的部分前缀满分对齐留待后续统一) */
|
||||
this.lastPlan = pickedSteps.length === picked.steps.length ? [qe.slice()].concat(picked.fut) : null;
|
||||
this.brakeRun = 0;
|
||||
} else {
|
||||
this.stats.brakes++; this.brakeRun++;
|
||||
if (this.brakeTail.length) { this.queue.push(...this.brakeTail); this.brakeTail = []; }
|
||||
|
||||
+18
-1
@@ -731,6 +731,23 @@ function make(cfg) {
|
||||
for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R));
|
||||
return M;
|
||||
}
|
||||
/* 几何雅可比(对度):Jv[i][j] = d p_i/d q_j (mm/deg),Jw[i][j] = 世界角速度分量/deg。
|
||||
J4/J5 同轴不影响正确性:两者各贡献同一世界轴。 */
|
||||
function jac(q) {
|
||||
const { R: Rs, T: Ts } = cumXf(q);
|
||||
const p = fk(q, 0);
|
||||
const Jv = [[], [], []], Jw = [[], [], []];
|
||||
for (let j = 0; j < 6; j++) {
|
||||
const id = ORDER[j], Rj = Rs[j], tj = Ts[j], pj = P[id];
|
||||
const a = mv3(Rj, D[id]);
|
||||
const lever = [p[0] - (Rj[0] * pj[0] + Rj[1] * pj[1] + Rj[2] * pj[2] + tj[0]),
|
||||
p[1] - (Rj[3] * pj[0] + Rj[4] * pj[1] + Rj[5] * pj[2] + tj[1]),
|
||||
p[2] - (Rj[6] * pj[0] + Rj[7] * pj[1] + Rj[8] * pj[2] + tj[2])];
|
||||
const v = cross3(a, lever);
|
||||
for (let i = 0; i < 3; i++) { Jv[i][j] = v[i] * D2R; Jw[i][j] = a[i] * D2R; }
|
||||
}
|
||||
return { Jv: Jv, Jw: Jw };
|
||||
}
|
||||
/* M5 第 3 列(工具轴)列向量传播:J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转,
|
||||
只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致(≤1e-15)。 */
|
||||
const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2];
|
||||
@@ -3233,7 +3250,7 @@ function make(cfg) {
|
||||
return { poseHit: pHit, poseTried: pTried, poseErr: pErr, poseMs: pTried ? pMs / pTried : 0 };
|
||||
}
|
||||
|
||||
return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn,
|
||||
return { fk: fk, fkChain: fkChain, jac: jac, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn,
|
||||
solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree, PST: PST, unwrapTo: unwrapTo, rtWarm: rtWarm, rtReset: () => { RT = null; }, rmPlan: rmPlan, rmSmooth: rmSmooth, modelFingerprint: modelFingerprint, RM: RM, wd: wd, classGap: classGap, classFingerprint: classFingerprint, P2D: P2D,
|
||||
mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
|
||||
branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
|
||||
|
||||
Reference in New Issue
Block a user