refactor: remove visual-servo feature
The marker-relative servo (endpoints /api/grasp/visual_servo/{plan,step,
plane/*}, helpers in grasp_routes.py, the bulk of grasp/visual_servo.py,
web/servo.js, and the Servo Visuale GUI tab) is gone. Plane fitting now
lives in /api/work_plane/* and perception/work_plane.py; Task 5 wires the
new Calibrazione Piano tab.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
1 parent
565c3df2f4
commit
eb53f30e23
5 files changed
+3
-1455
No files matched your search
@@ -14,7 +14,6 @@
|
||||
<button class="tab" data-tab="agent">Assistente AI</button>
|
||||
<button class="tab" data-tab="ai-settings">Impostazioni AI</button>
|
||||
<button class="tab" data-tab="grasp">TrovaUtensile</button>
|
||||
<button class="tab" data-tab="servo">Servo Visuale</button>
|
||||
<button class="tab" data-tab="view3d">Vista 3D</button>
|
||||
<button class="tab" data-tab="ee">Movimento EE</button>
|
||||
</nav>
|
||||
@@ -234,87 +233,6 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== SERVO VISUALE ============================== -->
|
||||
<section class="panel" id="tab-servo">
|
||||
<div class="diag3d-toolbar">
|
||||
<span class="post-health" id="servoStatus">—</span>
|
||||
</div>
|
||||
<div class="grid-2 diag3d-layout">
|
||||
<div>
|
||||
<div id="servoViewport" style="position:relative;width:min(100%,720px);height:440px;overflow:hidden;background:#000;border:1px solid var(--border);border-radius:4px;cursor:grab;user-select:none">
|
||||
<div id="servoPanZoom" style="position:absolute;left:0;top:0;transform-origin:0 0;will-change:transform">
|
||||
<img id="servoImg" alt="Nuwa RGB" style="display:block;max-width:none;pointer-events:none" draggable="false" />
|
||||
<canvas id="servoOverlay" style="position:absolute;left:0;top:0;pointer-events:none"></canvas>
|
||||
</div>
|
||||
</div>
|
||||
<div class="marker-info">RGB live · ciano = marker · giallo = oggetto · wheel zoom · drag pan · doppio click reset</div>
|
||||
<h3 style="margin-top:14px">Vista 3D (frame camera)</h3>
|
||||
<div class="canvas-wrap" style="width:min(100%,720px)">
|
||||
<canvas id="servo3dCanvas" width="720" height="360" style="display:block;background:#0a0d11;border:1px solid var(--border);border-radius:4px"></canvas>
|
||||
</div>
|
||||
<div class="marker-info">origine = camera · piano viola · marker ciano · oggetto giallo · proiezioni più chiare · drag ruota · wheel zoom</div>
|
||||
<div class="diag3d-legend">
|
||||
<div><span class="legend-sw" style="background:#4dd0e1;border-radius:50%"></span> marker polso</div>
|
||||
<div><span class="legend-sw" style="background:#ffd54f;border-radius:50%"></span> oggetto (centro bbox)</div>
|
||||
</div>
|
||||
</div>
|
||||
<div>
|
||||
<h2>Parametri</h2>
|
||||
<div class="ctrl-block">
|
||||
<div class="info-box">
|
||||
Avvicinamento marker-relativo senza hand-eye calibration: la camera vede marker e oggetto nello stesso frame, ogni step sposta il polso della frazione indicata della distanza rimanente. Si ferma a <code>min_distance_m</code>.
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>Classe (filtro) <input type="text" id="svClass" placeholder="(tutte)" /></label>
|
||||
<label>Confidenza <input type="number" id="svConf" value="0.5" step="0.05" min="0" max="1" /></label>
|
||||
<label>min dist (mm) <input type="number" id="svMinDist" value="100" step="10" min="20" max="500" /></label>
|
||||
<label>frazione step <input type="number" id="svStepFrac" value="0.5" step="0.05" min="0.05" max="1.0" /></label>
|
||||
<label>timeout (s) <input type="number" id="svTimeout" value="5" step="0.5" min="1" max="30" /></label>
|
||||
<label>side
|
||||
<select id="svSide">
|
||||
<option value="">(config)</option>
|
||||
<option value="right">right</option>
|
||||
<option value="left">left</option>
|
||||
</select>
|
||||
</label>
|
||||
<label class="inline"><input type="checkbox" id="svDryRun" checked /> dry-run (nessun moto reale)</label>
|
||||
</div>
|
||||
<div class="info-box" style="margin-top:8px">
|
||||
<strong>Orientazione marker rispetto al polso</strong> (rvec in gradi). Se non allineato al frame polso il target risulta spostato: tuna finché marker, oggetto e target sono coerenti.
|
||||
</div>
|
||||
<div class="kv compact">
|
||||
<label>rx (°) <input type="number" id="svRx" value="0" step="5" min="-180" max="180" /></label>
|
||||
<label>ry (°) <input type="number" id="svRy" value="0" step="5" min="-180" max="180" /></label>
|
||||
<label>rz (°) <input type="number" id="svRz" value="0" step="5" min="-180" max="180" /></label>
|
||||
<button id="svSaveRvecBtn" title="Salva l'rvec corrente in config.yaml">Salva rvec in config</button>
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="svPlanBtn" class="primary">Rileva / ricalcola</button>
|
||||
<button id="svStepBtn">▶ Vai al prossimo</button>
|
||||
<button id="svResetBtn">Reset</button>
|
||||
</div>
|
||||
<div class="info-box" style="margin-top:8px">
|
||||
<strong>Piano di lavoro</strong>. Clicca "Seleziona area", poi traccia un rettangolo sull'immagine con shift+drag: il backend calcola il piano dalla depth in quell'area. Marker e oggetto vengono poi proiettati su questo piano.
|
||||
</div>
|
||||
<div class="actions">
|
||||
<button id="svPlaneSelBtn">Seleziona area</button>
|
||||
<button id="svPlaneFitBtn">Calcola piano</button>
|
||||
<button id="svPlaneClearBtn" class="danger">Rimuovi piano</button>
|
||||
</div>
|
||||
<div id="svPlaneInfo" class="marker-info">piano: —</div>
|
||||
</div>
|
||||
|
||||
<h2>Stato</h2>
|
||||
<div id="servoStats" class="ctrl-block">
|
||||
<div class="dim">nessun piano calcolato</div>
|
||||
</div>
|
||||
|
||||
<h2>Dettagli piano</h2>
|
||||
<pre id="servoResult" class="log small">—</pre>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- ============================== VISTA 3D ============================== -->
|
||||
<section class="panel" id="tab-view3d">
|
||||
<div class="grid-2">
|
||||
@@ -419,6 +337,5 @@
|
||||
}
|
||||
</script>
|
||||
<script src="/static/app.js"></script>
|
||||
<script type="module" src="/static/servo.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
-669
@@ -1,669 +0,0 @@
|
||||
/* Visual-servo tab: live Nuwa RGB with marker + object-centre overlays.
|
||||
* Each click: plan → render overlay + stats → user confirms → step → wait →
|
||||
* re-plan → render. No 3D viewer in this tab — it's an image-space tool.
|
||||
*/
|
||||
|
||||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
|
||||
|
||||
const $ = (id) => document.getElementById(id);
|
||||
const LS_KEY = 'presaMano.servoParams.v1';
|
||||
const DEG = Math.PI / 180;
|
||||
|
||||
// ----- 3D scene (camera-frame) ----------------------------------------------
|
||||
let scene3d = null, cam3d = null, renderer3d = null, controls3d = null;
|
||||
let scene3dObjs = {}; // named overlays to clear/update on each plan
|
||||
|
||||
function boot3d() {
|
||||
if (scene3d) return;
|
||||
const canvas = $('servo3dCanvas');
|
||||
if (!canvas) return;
|
||||
scene3d = new THREE.Scene();
|
||||
scene3d.background = new THREE.Color(0x0a0d11);
|
||||
// Camera looks from outside the scene; world frame == Nuwa camera frame
|
||||
// (OpenCV: +X right, +Y down, +Z forward). We orbit around the origin.
|
||||
cam3d = new THREE.PerspectiveCamera(45, canvas.width / canvas.height, 0.05, 20);
|
||||
cam3d.position.set(0.8, -0.6, -0.2);
|
||||
cam3d.up.set(0, -1, 0); // Y-down so CV axes match visually
|
||||
renderer3d = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
renderer3d.setPixelRatio(window.devicePixelRatio || 1);
|
||||
renderer3d.setSize(canvas.width, canvas.height, false);
|
||||
controls3d = new OrbitControls(cam3d, canvas);
|
||||
controls3d.target.set(0, 0, 1.2);
|
||||
controls3d.update();
|
||||
|
||||
scene3d.add(new THREE.AmbientLight(0xffffff, 0.6));
|
||||
const dir = new THREE.DirectionalLight(0xffffff, 0.6);
|
||||
dir.position.set(1, -1, 1);
|
||||
scene3d.add(dir);
|
||||
|
||||
// Camera-origin marker (small axes triad)
|
||||
const ax = new THREE.AxesHelper(0.2);
|
||||
scene3d.add(ax);
|
||||
// Small grid at Z=1.5 to anchor the eye when there's no plane yet.
|
||||
const grid = new THREE.GridHelper(2, 10, 0x222a33, 0x1a2029);
|
||||
grid.rotation.x = Math.PI / 2;
|
||||
grid.position.z = 1.5;
|
||||
scene3d.add(grid);
|
||||
|
||||
// Frustum wireframe (approximate FOV lines up to 2 m depth).
|
||||
const fh = 0.7, fw = 1.0, fz = 2.0;
|
||||
const frPts = [
|
||||
0,0,0, fw, fh, fz,
|
||||
0,0,0, -fw, fh, fz,
|
||||
0,0,0, fw, -fh, fz,
|
||||
0,0,0, -fw, -fh, fz,
|
||||
fw, fh, fz, fw, -fh, fz,
|
||||
fw, -fh, fz, -fw, -fh, fz,
|
||||
-fw, -fh, fz, -fw, fh, fz,
|
||||
-fw, fh, fz, fw, fh, fz,
|
||||
];
|
||||
const frGeo = new THREE.BufferGeometry();
|
||||
frGeo.setAttribute('position', new THREE.Float32BufferAttribute(frPts, 3));
|
||||
const frMat = new THREE.LineBasicMaterial({ color: 0x2a3340 });
|
||||
scene3d.add(new THREE.LineSegments(frGeo, frMat));
|
||||
|
||||
animate3d();
|
||||
}
|
||||
|
||||
function animate3d() {
|
||||
requestAnimationFrame(animate3d);
|
||||
if (controls3d) controls3d.update();
|
||||
if (renderer3d && scene3d && cam3d) renderer3d.render(scene3d, cam3d);
|
||||
}
|
||||
|
||||
function clear3dOverlays() {
|
||||
for (const k of Object.keys(scene3dObjs)) {
|
||||
const o = scene3dObjs[k];
|
||||
if (!o) continue;
|
||||
scene3d.remove(o);
|
||||
if (o.geometry) o.geometry.dispose();
|
||||
if (o.material) (Array.isArray(o.material) ? o.material : [o.material]).forEach(m => m.dispose());
|
||||
scene3dObjs[k] = null;
|
||||
}
|
||||
}
|
||||
|
||||
function addSphere3d(key, xyz, color, radius = 0.025) {
|
||||
const geom = new THREE.SphereGeometry(radius, 24, 16);
|
||||
const mat = new THREE.MeshStandardMaterial({ color, emissive: color, emissiveIntensity: 0.25 });
|
||||
const m = new THREE.Mesh(geom, mat);
|
||||
m.position.set(xyz[0], xyz[1], xyz[2]);
|
||||
scene3d.add(m);
|
||||
scene3dObjs[key] = m;
|
||||
}
|
||||
|
||||
function addLine3d(key, a, b, color) {
|
||||
const g = new THREE.BufferGeometry().setFromPoints([
|
||||
new THREE.Vector3(...a), new THREE.Vector3(...b),
|
||||
]);
|
||||
const m = new THREE.Line(g, new THREE.LineBasicMaterial({ color }));
|
||||
scene3d.add(m);
|
||||
scene3dObjs[key] = m;
|
||||
}
|
||||
|
||||
function addPlaneMesh3d(key, normal, offset, anchor, size = 1.2, color = 0x9b6dff) {
|
||||
const n = new THREE.Vector3(...normal).normalize();
|
||||
// Build two in-plane orthonormal vectors.
|
||||
const up = Math.abs(n.y) < 0.9 ? new THREE.Vector3(0, 1, 0) : new THREE.Vector3(1, 0, 0);
|
||||
const u = new THREE.Vector3().crossVectors(up, n).normalize();
|
||||
const v = new THREE.Vector3().crossVectors(n, u).normalize();
|
||||
// Centre the mesh on the anchor point (foot of camera onto plane if given).
|
||||
const anchorVec = new THREE.Vector3(...anchor);
|
||||
// Pull anchor onto the plane to be safe.
|
||||
const s = anchorVec.dot(n) + offset;
|
||||
anchorVec.addScaledVector(n, -s);
|
||||
const geom = new THREE.BufferGeometry();
|
||||
const h = size / 2;
|
||||
const corners = [
|
||||
anchorVec.clone().addScaledVector(u, -h).addScaledVector(v, -h),
|
||||
anchorVec.clone().addScaledVector(u, h).addScaledVector(v, -h),
|
||||
anchorVec.clone().addScaledVector(u, h).addScaledVector(v, h),
|
||||
anchorVec.clone().addScaledVector(u, -h).addScaledVector(v, h),
|
||||
];
|
||||
const pos = [];
|
||||
for (const c of corners) pos.push(c.x, c.y, c.z);
|
||||
geom.setAttribute('position', new THREE.Float32BufferAttribute(pos, 3));
|
||||
geom.setIndex([0, 1, 2, 0, 2, 3]);
|
||||
geom.computeVertexNormals();
|
||||
const mat = new THREE.MeshStandardMaterial({
|
||||
color, opacity: 0.35, transparent: true, side: THREE.DoubleSide,
|
||||
});
|
||||
const mesh = new THREE.Mesh(geom, mat);
|
||||
scene3d.add(mesh);
|
||||
scene3dObjs[key] = mesh;
|
||||
// Edges
|
||||
const edgeGeom = new THREE.EdgesGeometry(geom);
|
||||
const edgeMat = new THREE.LineBasicMaterial({ color });
|
||||
const edges = new THREE.LineSegments(edgeGeom, edgeMat);
|
||||
scene3d.add(edges);
|
||||
scene3dObjs[key + '_edges'] = edges;
|
||||
}
|
||||
|
||||
function redraw3d() {
|
||||
if (!scene3d) return;
|
||||
clear3dOverlays();
|
||||
const p = lastPlan && lastPlan.plan;
|
||||
// Plane (from stored planeInfo, independent of current plan)
|
||||
if (planeInfo) {
|
||||
const anchor = planeInfo.centroid || [0, 0, 1.5];
|
||||
addPlaneMesh3d('plane', planeInfo.normal, planeInfo.offset, anchor);
|
||||
}
|
||||
if (!p) return;
|
||||
addSphere3d('marker', p.marker_position_cam, 0x4dd0e1, 0.022);
|
||||
addSphere3d('object', p.object_position_cam, 0xffd54f, 0.028);
|
||||
addLine3d('mo_line', p.marker_position_cam, p.object_position_cam, 0xbbbbbb);
|
||||
if (p.plane) {
|
||||
addSphere3d('marker_foot', p.plane.marker_foot_cam, 0x80ecf7, 0.018);
|
||||
addSphere3d('object_foot', p.plane.object_foot_cam, 0xffe998, 0.020);
|
||||
addLine3d('m_drop', p.marker_position_cam, p.plane.marker_foot_cam, 0x4dd0e1);
|
||||
addLine3d('o_drop', p.object_position_cam, p.plane.object_foot_cam, 0xffd54f);
|
||||
addLine3d('foot_line', p.plane.marker_foot_cam, p.plane.object_foot_cam, 0x9b6dff);
|
||||
}
|
||||
}
|
||||
|
||||
let lastPlan = null;
|
||||
let busy = false;
|
||||
let imgRefreshTimer = null;
|
||||
// Image native dimensions (from /api/intrinsics or from the first load).
|
||||
let imgW = 0, imgH = 0;
|
||||
// Pan/zoom state: scale and translate applied to the inner pan-zoom wrapper.
|
||||
const view = { scale: 1, tx: 0, ty: 0 };
|
||||
let drag = null;
|
||||
// Work plane selection state.
|
||||
let planeInfo = null; // last response from /plane/fit or /plane GET
|
||||
let roiMode = false; // true when user clicked "Seleziona area"
|
||||
let roi = null; // {u0,v0,u1,v1} in image coords (committed)
|
||||
let roiDrag = null; // in-progress drag
|
||||
|
||||
function setStatus(msg, cls) {
|
||||
const el = $('servoStatus');
|
||||
if (!el) return;
|
||||
el.textContent = msg;
|
||||
el.style.color = cls === 'err' ? 'var(--danger)' : cls === 'ok' ? 'var(--ok)' : 'var(--dim)';
|
||||
}
|
||||
|
||||
function readParams() {
|
||||
const side = $('svSide').value || null;
|
||||
const cls = $('svClass').value.trim();
|
||||
const conf = $('svConf').value.trim();
|
||||
const body = {
|
||||
min_distance_m: (+$('svMinDist').value) / 1000,
|
||||
step_fraction: +$('svStepFrac').value,
|
||||
};
|
||||
if (cls) body.class_name = cls;
|
||||
if (conf) body.conf = +conf;
|
||||
if (side) body.side = side;
|
||||
const rx = +$('svRx').value, ry = +$('svRy').value, rz = +$('svRz').value;
|
||||
if (rx || ry || rz) {
|
||||
body.wrist_marker_rvec = [rx * DEG, ry * DEG, rz * DEG];
|
||||
}
|
||||
return body;
|
||||
}
|
||||
|
||||
function persistParams() {
|
||||
try {
|
||||
localStorage.setItem(LS_KEY, JSON.stringify({
|
||||
cls: $('svClass').value, conf: $('svConf').value,
|
||||
minDist: $('svMinDist').value, step: $('svStepFrac').value,
|
||||
timeout: $('svTimeout').value, side: $('svSide').value,
|
||||
dryRun: $('svDryRun').checked,
|
||||
rx: $('svRx').value, ry: $('svRy').value, rz: $('svRz').value,
|
||||
}));
|
||||
} catch {}
|
||||
}
|
||||
|
||||
function restoreParams() {
|
||||
try {
|
||||
const raw = localStorage.getItem(LS_KEY);
|
||||
if (!raw) return;
|
||||
const p = JSON.parse(raw);
|
||||
const setV = (id, v) => { if ($(id) && v != null) $(id).value = v; };
|
||||
setV('svClass', p.cls); setV('svConf', p.conf);
|
||||
setV('svMinDist', p.minDist); setV('svStepFrac', p.step);
|
||||
setV('svTimeout', p.timeout); setV('svSide', p.side);
|
||||
setV('svRx', p.rx); setV('svRy', p.ry); setV('svRz', p.rz);
|
||||
if ($('svDryRun') && p.dryRun != null) $('svDryRun').checked = !!p.dryRun;
|
||||
} catch {}
|
||||
}
|
||||
|
||||
async function api(method, path, body = null) {
|
||||
const opts = { method, headers: {} };
|
||||
if (body !== null) {
|
||||
opts.headers['Content-Type'] = 'application/json';
|
||||
opts.body = JSON.stringify(body);
|
||||
}
|
||||
const r = await fetch(path, opts);
|
||||
const text = await r.text();
|
||||
let data; try { data = JSON.parse(text); } catch { data = text; }
|
||||
if (!r.ok) throw new Error(`${r.status} ${(data && data.detail) || text}`);
|
||||
return data;
|
||||
}
|
||||
|
||||
async function saveRvecToConfig() {
|
||||
const rx = +$('svRx').value, ry = +$('svRy').value, rz = +$('svRz').value;
|
||||
const rvec = [rx * DEG, ry * DEG, rz * DEG];
|
||||
try {
|
||||
await api('PUT', '/api/config', {
|
||||
grasp: { visual_servo: { wrist_marker_rvec: rvec } },
|
||||
});
|
||||
setStatus(`rvec salvato: [${rx}°, ${ry}°, ${rz}°]`, 'ok');
|
||||
} catch (e) {
|
||||
setStatus('salvataggio rvec fallito: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
function refreshImg() {
|
||||
const img = $('servoImg');
|
||||
if (!img) return;
|
||||
// Cache-bust so we always see the current frame.
|
||||
img.src = `/api/calibration/rgb.jpg?t=${Date.now()}`;
|
||||
}
|
||||
|
||||
function resizeOverlay() {
|
||||
const img = $('servoImg');
|
||||
const cvs = $('servoOverlay');
|
||||
if (!img || !cvs) return;
|
||||
if (img.naturalWidth) { imgW = img.naturalWidth; imgH = img.naturalHeight; }
|
||||
if (!imgW || !imgH) return;
|
||||
// Match canvas pixel size to the native image size; CSS transform on the
|
||||
// wrapper handles zoom/pan, keeping overlay and image perfectly aligned.
|
||||
const dpr = window.devicePixelRatio || 1;
|
||||
cvs.width = Math.round(imgW * dpr);
|
||||
cvs.height = Math.round(imgH * dpr);
|
||||
cvs.style.width = imgW + 'px';
|
||||
cvs.style.height = imgH + 'px';
|
||||
img.style.width = imgW + 'px';
|
||||
img.style.height = imgH + 'px';
|
||||
// Fit-to-viewport on first sizing.
|
||||
if (view.scale === 1 && view.tx === 0 && view.ty === 0) fitView();
|
||||
else applyTransform();
|
||||
redrawOverlay();
|
||||
}
|
||||
|
||||
function applyTransform() {
|
||||
const wrap = $('servoPanZoom');
|
||||
if (!wrap) return;
|
||||
wrap.style.transform = `translate(${view.tx}px, ${view.ty}px) scale(${view.scale})`;
|
||||
}
|
||||
|
||||
function fitView() {
|
||||
const vp = $('servoViewport');
|
||||
if (!vp || !imgW || !imgH) return;
|
||||
const r = vp.getBoundingClientRect();
|
||||
const s = Math.min(r.width / imgW, r.height / imgH);
|
||||
view.scale = s;
|
||||
view.tx = (r.width - imgW * s) / 2;
|
||||
view.ty = (r.height - imgH * s) / 2;
|
||||
applyTransform();
|
||||
}
|
||||
|
||||
function zoomAt(clientX, clientY, factor) {
|
||||
const vp = $('servoViewport');
|
||||
const r = vp.getBoundingClientRect();
|
||||
const px = clientX - r.left, py = clientY - r.top;
|
||||
// Image coords under the cursor BEFORE scaling:
|
||||
const ix = (px - view.tx) / view.scale;
|
||||
const iy = (py - view.ty) / view.scale;
|
||||
const newScale = Math.min(16, Math.max(0.1, view.scale * factor));
|
||||
// Keep (ix, iy) under the cursor after the new scale.
|
||||
view.tx = px - ix * newScale;
|
||||
view.ty = py - iy * newScale;
|
||||
view.scale = newScale;
|
||||
applyTransform();
|
||||
}
|
||||
|
||||
function clientToImage(clientX, clientY) {
|
||||
const vp = $('servoViewport');
|
||||
const r = vp.getBoundingClientRect();
|
||||
const px = clientX - r.left, py = clientY - r.top;
|
||||
return [(px - view.tx) / view.scale, (py - view.ty) / view.scale];
|
||||
}
|
||||
|
||||
function wireViewport() {
|
||||
const vp = $('servoViewport');
|
||||
if (!vp) return;
|
||||
vp.addEventListener('wheel', (e) => {
|
||||
e.preventDefault();
|
||||
const factor = e.deltaY < 0 ? 1.15 : 1 / 1.15;
|
||||
zoomAt(e.clientX, e.clientY, factor);
|
||||
}, { passive: false });
|
||||
vp.addEventListener('pointerdown', (e) => {
|
||||
// ROI selection takes priority when enabled, OR when user holds Shift.
|
||||
if (roiMode || e.shiftKey) {
|
||||
const [ix, iy] = clientToImage(e.clientX, e.clientY);
|
||||
roiDrag = { u0: ix, v0: iy, u1: ix, v1: iy, id: e.pointerId };
|
||||
vp.setPointerCapture(e.pointerId);
|
||||
redrawOverlay();
|
||||
return;
|
||||
}
|
||||
drag = { x: e.clientX, y: e.clientY, tx: view.tx, ty: view.ty };
|
||||
vp.setPointerCapture(e.pointerId);
|
||||
vp.style.cursor = 'grabbing';
|
||||
});
|
||||
vp.addEventListener('pointermove', (e) => {
|
||||
if (roiDrag) {
|
||||
const [ix, iy] = clientToImage(e.clientX, e.clientY);
|
||||
roiDrag.u1 = ix; roiDrag.v1 = iy;
|
||||
redrawOverlay();
|
||||
return;
|
||||
}
|
||||
if (!drag) return;
|
||||
view.tx = drag.tx + (e.clientX - drag.x);
|
||||
view.ty = drag.ty + (e.clientY - drag.y);
|
||||
applyTransform();
|
||||
});
|
||||
const endDrag = (e) => {
|
||||
if (roiDrag) {
|
||||
try { vp.releasePointerCapture(e.pointerId); } catch {}
|
||||
const r = roiDrag;
|
||||
const u0 = Math.max(0, Math.min(r.u0, r.u1));
|
||||
const v0 = Math.max(0, Math.min(r.v0, r.v1));
|
||||
const u1 = Math.min(imgW || r.u1, Math.max(r.u0, r.u1));
|
||||
const v1 = Math.min(imgH || r.v1, Math.max(r.v0, r.v1));
|
||||
if (u1 - u0 > 5 && v1 - v0 > 5) {
|
||||
roi = { u0: u0|0, v0: v0|0, u1: u1|0, v1: v1|0 };
|
||||
}
|
||||
roiDrag = null;
|
||||
roiMode = false;
|
||||
vp.style.cursor = 'grab';
|
||||
setStatus(roi ? `ROI ${roi.u1-roi.u0}x${roi.v1-roi.v0} px · premi "Calcola piano"` : 'ROI troppo piccola', roi ? 'ok' : 'err');
|
||||
redrawOverlay();
|
||||
return;
|
||||
}
|
||||
if (drag) { try { vp.releasePointerCapture(e.pointerId); } catch {} }
|
||||
drag = null;
|
||||
vp.style.cursor = 'grab';
|
||||
};
|
||||
vp.addEventListener('pointerup', endDrag);
|
||||
vp.addEventListener('pointercancel', endDrag);
|
||||
vp.addEventListener('dblclick', () => fitView());
|
||||
}
|
||||
|
||||
function drawCross(ctx, x, y, color) {
|
||||
// Sizes expressed in IMAGE pixels so they stay legible after CSS zoom.
|
||||
const r = 18;
|
||||
ctx.strokeStyle = color;
|
||||
ctx.lineWidth = 3;
|
||||
ctx.beginPath();
|
||||
ctx.arc(x, y, r, 0, Math.PI * 2);
|
||||
ctx.stroke();
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(x - r - 6, y); ctx.lineTo(x + r + 6, y);
|
||||
ctx.moveTo(x, y - r - 6); ctx.lineTo(x, y + r + 6);
|
||||
ctx.stroke();
|
||||
}
|
||||
|
||||
function drawRect(ctx, u0, v0, u1, v1, color, label) {
|
||||
ctx.strokeStyle = color;
|
||||
ctx.lineWidth = 2;
|
||||
ctx.setLineDash([6, 4]);
|
||||
ctx.strokeRect(u0, v0, u1 - u0, v1 - v0);
|
||||
ctx.setLineDash([]);
|
||||
ctx.fillStyle = 'rgba(0,0,0,0.35)';
|
||||
ctx.fillRect(u0, v0, u1 - u0, v1 - v0);
|
||||
if (label) {
|
||||
ctx.font = 'bold 16px ui-monospace, monospace';
|
||||
const w = ctx.measureText(label).width + 10;
|
||||
ctx.fillStyle = color;
|
||||
ctx.fillRect(u0, v0, w, 20);
|
||||
ctx.fillStyle = '#000';
|
||||
ctx.fillText(label, u0 + 5, v0 + 15);
|
||||
}
|
||||
}
|
||||
|
||||
function redrawOverlay() {
|
||||
const cvs = $('servoOverlay');
|
||||
if (!cvs) return;
|
||||
const ctx = cvs.getContext('2d');
|
||||
const dpr = window.devicePixelRatio || 1;
|
||||
ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
|
||||
ctx.clearRect(0, 0, imgW || cvs.width, imgH || cvs.height);
|
||||
if (!imgW || !imgH) return;
|
||||
|
||||
// Committed ROI
|
||||
if (roi) drawRect(ctx, roi.u0, roi.v0, roi.u1, roi.v1, '#9b6dff', 'ROI piano');
|
||||
// In-progress drag
|
||||
if (roiDrag) {
|
||||
const u0 = Math.min(roiDrag.u0, roiDrag.u1);
|
||||
const v0 = Math.min(roiDrag.v0, roiDrag.v1);
|
||||
const u1 = Math.max(roiDrag.u0, roiDrag.u1);
|
||||
const v1 = Math.max(roiDrag.v0, roiDrag.v1);
|
||||
drawRect(ctx, u0, v0, u1, v1, '#9b6dff', null);
|
||||
}
|
||||
|
||||
if (!lastPlan) return;
|
||||
const p = lastPlan.plan;
|
||||
const mx = p.marker_pixel[0], my = p.marker_pixel[1];
|
||||
const ox = p.object_pixel[0], oy = p.object_pixel[1];
|
||||
ctx.strokeStyle = 'rgba(255,255,255,0.6)';
|
||||
ctx.setLineDash([8, 6]);
|
||||
ctx.lineWidth = 2;
|
||||
ctx.beginPath(); ctx.moveTo(mx, my); ctx.lineTo(ox, oy); ctx.stroke();
|
||||
ctx.setLineDash([]);
|
||||
drawCross(ctx, mx, my, '#4dd0e1');
|
||||
drawCross(ctx, ox, oy, '#ffd54f');
|
||||
}
|
||||
|
||||
async function planeSelectStart() {
|
||||
roiMode = true;
|
||||
setStatus('traccia un rettangolo sull\'immagine (shift+drag o dopo questo click, drag)', 'ok');
|
||||
}
|
||||
|
||||
async function planeFit() {
|
||||
if (!roi) { setStatus('seleziona prima un\'area', 'err'); return; }
|
||||
try {
|
||||
setStatus('fit piano…');
|
||||
const body = {
|
||||
u_min: roi.u0, v_min: roi.v0,
|
||||
u_max: roi.u1, v_max: roi.v1,
|
||||
};
|
||||
const r = await api('POST', '/api/grasp/visual_servo/plane/fit', body);
|
||||
planeInfo = r.plane;
|
||||
updatePlaneInfo();
|
||||
redraw3d();
|
||||
setStatus(`piano OK · rms=${r.plane.fit_rms_mm.toFixed(1)} mm · ${r.plane.n_inliers} punti`, 'ok');
|
||||
} catch (e) {
|
||||
setStatus('fit fallito: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
async function planeClear() {
|
||||
try {
|
||||
await api('DELETE', '/api/grasp/visual_servo/plane');
|
||||
planeInfo = null;
|
||||
roi = null;
|
||||
updatePlaneInfo();
|
||||
redrawOverlay();
|
||||
redraw3d();
|
||||
setStatus('piano rimosso', 'ok');
|
||||
} catch (e) {
|
||||
setStatus('rimozione piano fallita: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
function updatePlaneInfo() {
|
||||
const el = $('svPlaneInfo');
|
||||
if (!el) return;
|
||||
if (!planeInfo) { el.textContent = 'piano: —'; return; }
|
||||
const n = planeInfo.normal.map(v => v.toFixed(3)).join(', ');
|
||||
el.innerHTML = `piano: n=[${n}] · offset=${planeInfo.offset.toFixed(3)} m · rms=${planeInfo.fit_rms_mm.toFixed(1)} mm · ${planeInfo.n_inliers} pts`;
|
||||
}
|
||||
|
||||
async function planeLoadCurrent() {
|
||||
try {
|
||||
const r = await api('GET', '/api/grasp/visual_servo/plane');
|
||||
if (r.set) {
|
||||
planeInfo = r.plane;
|
||||
roi = { u0: r.plane.roi[0], v0: r.plane.roi[1], u1: r.plane.roi[2], v1: r.plane.roi[3] };
|
||||
updatePlaneInfo();
|
||||
redrawOverlay();
|
||||
redraw3d();
|
||||
}
|
||||
} catch {}
|
||||
}
|
||||
|
||||
function fmtMm(m) { return (m * 1000).toFixed(1) + ' mm'; }
|
||||
|
||||
function renderStats(resp) {
|
||||
const p = resp.plan;
|
||||
const cls = p.detection && p.detection.class;
|
||||
const conf = p.detection && p.detection.confidence;
|
||||
const reproj = p.marker_reprojection_error_px;
|
||||
const doneBadge = p.done
|
||||
? '<span style="color:var(--ok);font-weight:600">RAGGIUNTO</span>'
|
||||
: '<span>in avvicinamento</span>';
|
||||
$('servoStats').innerHTML = `
|
||||
<table class="kvtable">
|
||||
<tbody>
|
||||
<tr><td>stato</td><td>${doneBadge}</td></tr>
|
||||
<tr><td>marker · pixel</td><td>(${p.marker_pixel[0].toFixed(0)}, ${p.marker_pixel[1].toFixed(0)})</td></tr>
|
||||
<tr><td>marker · Z / dist / scala</td><td>${fmtMm(p.marker_depth_z_m)} · ${fmtMm(p.marker_distance_cam_m)} · ${p.marker_mm_per_px ? p.marker_mm_per_px.toFixed(2) + ' mm/px' : '—'}</td></tr>
|
||||
<tr><td>oggetto · pixel</td><td>(${p.object_pixel[0].toFixed(0)}, ${p.object_pixel[1].toFixed(0)})</td></tr>
|
||||
<tr><td>oggetto · Z / dist / scala</td><td>${fmtMm(p.object_depth_z_m)} · ${fmtMm(p.object_distance_cam_m)} · ${p.object_mm_per_px ? p.object_mm_per_px.toFixed(2) + ' mm/px' : '—'}</td></tr>
|
||||
<tr><td>marker ↔ oggetto (3D)</td><td><strong>${fmtMm(p.distance_m)}</strong></td></tr>
|
||||
${p.plane ? `
|
||||
<tr><td>piano · rms / punti</td><td>${p.plane.fit_rms_mm.toFixed(1)} mm · ${planeInfo ? planeInfo.n_inliers : '—'}</td></tr>
|
||||
<tr><td>marker · altezza dal piano</td><td><strong>${(p.plane.marker_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.marker_signed_distance_m))}</strong> <span class="dim">(${p.plane.marker_signed_distance_m >= 0 ? 'sopra' : 'sotto'})</span></td></tr>
|
||||
<tr><td>oggetto · altezza dal piano</td><td><strong>${(p.plane.object_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.object_signed_distance_m))}</strong> <span class="dim">(${p.plane.object_signed_distance_m >= 0 ? 'sopra' : 'sotto'})</span></td></tr>
|
||||
<tr><td>Δ altezza marker−oggetto</td><td>${fmtMm(Math.abs(p.plane.marker_signed_distance_m - p.plane.object_signed_distance_m))}</td></tr>
|
||||
<tr><td>in-piano (perp-foot)</td><td><strong>${fmtMm(p.plane.in_plane_distance_m)}</strong></td></tr>
|
||||
<tr><td>in-piano (pixel-hit)</td><td>${fmtMm(p.plane.in_plane_pixel_distance_m)}</td></tr>
|
||||
` : ''}
|
||||
<tr><td>prossimo step (base)</td><td>${p.target_position_base ? fmtMm(p.planned_step_m) : '—'}</td></tr>
|
||||
<tr><td>limite stop</td><td>${fmtMm(resp.min_distance_m)}</td></tr>
|
||||
<tr><td>frazione step</td><td>${resp.step_fraction}</td></tr>
|
||||
<tr><td>lato</td><td>${resp.side}</td></tr>
|
||||
<tr><td>oggetto · classe</td><td>${cls || '—'} ${conf != null ? `(${(conf*100).toFixed(0)}%)` : ''}</td></tr>
|
||||
<tr><td>punti bbox</td><td>${p.n_object_points}</td></tr>
|
||||
<tr><td>reproj err marker</td><td>${reproj != null ? reproj.toFixed(2) + ' px' : '—'}</td></tr>
|
||||
<tr><td>sorgente posizione marker</td><td>${p.marker_translation_source || '—'}</td></tr>
|
||||
<tr><td>sorgente punto oggetto</td><td>${p.object_point_source || '—'}</td></tr>
|
||||
<tr><td>note</td><td>${p.reason || '—'}</td></tr>
|
||||
</tbody>
|
||||
</table>`;
|
||||
$('servoResult').textContent = JSON.stringify({
|
||||
wrist_position_base: p.wrist_position_base,
|
||||
marker_position_cam: p.marker_position_cam,
|
||||
object_position_cam: p.object_position_cam,
|
||||
target_position_base: p.target_position_base,
|
||||
distance_m: p.distance_m,
|
||||
}, null, 2);
|
||||
}
|
||||
|
||||
async function doPlan() {
|
||||
if (busy) return;
|
||||
busy = true;
|
||||
persistParams();
|
||||
try {
|
||||
setStatus('rilevamento…');
|
||||
refreshImg();
|
||||
const body = readParams();
|
||||
const resp = await api('POST', '/api/grasp/visual_servo/plan', body);
|
||||
lastPlan = resp;
|
||||
renderStats(resp);
|
||||
redrawOverlay();
|
||||
redraw3d();
|
||||
setStatus(resp.plan.done ? 'target raggiunto' : 'piano pronto · premi "Vai al prossimo"', 'ok');
|
||||
} catch (e) {
|
||||
setStatus('errore plan: ' + e.message, 'err');
|
||||
} finally { busy = false; }
|
||||
}
|
||||
|
||||
async function doStep() {
|
||||
if (busy) return;
|
||||
busy = true;
|
||||
persistParams();
|
||||
const dryRun = $('svDryRun').checked;
|
||||
try {
|
||||
setStatus(dryRun ? 'step (dry-run)…' : 'movimento in corso…');
|
||||
refreshImg();
|
||||
const body = readParams();
|
||||
body.dry_run = dryRun;
|
||||
body.timeout_s = +$('svTimeout').value;
|
||||
const resp = await api('POST', '/api/grasp/visual_servo/step', body);
|
||||
lastPlan = resp;
|
||||
renderStats(resp);
|
||||
redrawOverlay();
|
||||
if (!resp.moved) {
|
||||
setStatus('target raggiunto, nessun moto', 'ok');
|
||||
return;
|
||||
}
|
||||
setStatus('ricalcolo post-moto…');
|
||||
refreshImg();
|
||||
const replan = await api('POST', '/api/grasp/visual_servo/plan', readParams());
|
||||
lastPlan = replan;
|
||||
renderStats(replan);
|
||||
redrawOverlay();
|
||||
redraw3d();
|
||||
setStatus(replan.plan.done ? 'target raggiunto' : 'pronto per il prossimo step', 'ok');
|
||||
} catch (e) {
|
||||
setStatus('errore step: ' + e.message, 'err');
|
||||
} finally { busy = false; }
|
||||
}
|
||||
|
||||
function doReset() {
|
||||
lastPlan = null;
|
||||
const ctx = $('servoOverlay').getContext('2d');
|
||||
ctx.clearRect(0, 0, $('servoOverlay').width, $('servoOverlay').height);
|
||||
$('servoStats').innerHTML = '<div class="dim">nessun piano calcolato</div>';
|
||||
$('servoResult').textContent = '—';
|
||||
setStatus('reset');
|
||||
}
|
||||
|
||||
function startLiveImg() {
|
||||
stopLiveImg();
|
||||
imgRefreshTimer = setInterval(refreshImg, 500);
|
||||
refreshImg();
|
||||
}
|
||||
function stopLiveImg() {
|
||||
if (imgRefreshTimer) { clearInterval(imgRefreshTimer); imgRefreshTimer = null; }
|
||||
}
|
||||
|
||||
document.addEventListener('DOMContentLoaded', () => {
|
||||
if (!$('servoImg')) return;
|
||||
restoreParams();
|
||||
['svClass','svConf','svMinDist','svStepFrac','svTimeout','svSide','svDryRun',
|
||||
'svRx','svRy','svRz']
|
||||
.forEach(id => { const el = $(id); if (el) el.addEventListener('change', persistParams); });
|
||||
$('svPlanBtn').addEventListener('click', doPlan);
|
||||
$('svStepBtn').addEventListener('click', doStep);
|
||||
$('svResetBtn').addEventListener('click', doReset);
|
||||
$('svSaveRvecBtn').addEventListener('click', saveRvecToConfig);
|
||||
$('svPlaneSelBtn').addEventListener('click', planeSelectStart);
|
||||
$('svPlaneFitBtn').addEventListener('click', planeFit);
|
||||
$('svPlaneClearBtn').addEventListener('click', planeClear);
|
||||
boot3d();
|
||||
planeLoadCurrent();
|
||||
|
||||
const img = $('servoImg');
|
||||
img.addEventListener('load', () => {
|
||||
imgW = img.naturalWidth; imgH = img.naturalHeight;
|
||||
resizeOverlay();
|
||||
});
|
||||
wireViewport();
|
||||
window.addEventListener('resize', () => { resizeOverlay(); });
|
||||
});
|
||||
|
||||
function resize3d() {
|
||||
const canvas = $('servo3dCanvas');
|
||||
if (!canvas || !renderer3d || !cam3d) return;
|
||||
const rect = canvas.getBoundingClientRect();
|
||||
const w = Math.max(100, Math.round(rect.width));
|
||||
const h = Math.max(60, Math.round(rect.height));
|
||||
renderer3d.setSize(w, h, false);
|
||||
cam3d.aspect = w / h;
|
||||
cam3d.updateProjectionMatrix();
|
||||
}
|
||||
|
||||
// Only stream the RGB while the servo tab is active (avoid hammering Nuwa).
|
||||
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
|
||||
const active = btn.dataset.tab === 'servo';
|
||||
if (active) {
|
||||
startLiveImg();
|
||||
// The 3D canvas may have rendered at 0x0 while the panel was display:none.
|
||||
// Re-boot (no-op if already ready) and resize once visible.
|
||||
boot3d();
|
||||
setTimeout(() => { resizeOverlay(); resize3d(); redraw3d(); }, 100);
|
||||
} else { stopLiveImg(); }
|
||||
}));
|
||||
window.addEventListener('resize', () => { resize3d(); });
|
||||
Reference in new issue
Block a user