refactor(pose_3d): rimuovi agent/chat, aggiungi cloud+diagnostics+camera polso
Rimosso il modulo agent (LLM chat) e la relativa config OpenRouter. La tab Pose 3D ora: espone i giunti reali via G1 /diagnostics per specchiare l'URDF senza IK; acquisisce point cloud Nuwa+D435i (/api/pose_3d/clouds); mostra il modello 3D della camera sul polso; degrada gracefully a 200 (wrist=null) quando il robot è offline. Il vecchio renderer canvas-2D (app.js view3d + diagnostic3d.js) è stato consolidato nel viewport Three.js di pose_3d.js. 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
9d30073d57
commit
6c2452fe20
20 files changed
+152757
-2714
No files matched your search
+769
-39
@@ -47,6 +47,147 @@ function makeLabelSprite(text, color = '#ffffff') {
|
||||
return sprite;
|
||||
}
|
||||
|
||||
// Carica la mesh STL della Nuwa HP60C come child del polso destro: segue
|
||||
// rigidamente il wrist frame, posizione/orientamento applicati da config.
|
||||
// Aggiunge un UCS (assi colorati) all'origine di ogni link URDF. I helper
|
||||
// vengono memorizzati in state.joint_ucses per show/hide e resize rapidi.
|
||||
// I link "*_hand_*" vengono saltati (mani gia' nascoste).
|
||||
function addJointUCSes(robot) {
|
||||
const helpers = [];
|
||||
const visited = new Set();
|
||||
robot.traverse((obj) => {
|
||||
if (!obj.name || visited.has(obj.name)) return;
|
||||
if (obj.name.startsWith('left_hand_') || obj.name.startsWith('right_hand_')) return;
|
||||
if (obj.isURDFLink !== true) return;
|
||||
visited.add(obj.name);
|
||||
const ucs = makeUCS(0.10); // unit length; resize tramite .scale
|
||||
ucs.userData.isJointUCS = true;
|
||||
ucs.userData.linkName = obj.name;
|
||||
ucs.visible = false; // di default nascosto, abilita dal flag
|
||||
obj.add(ucs);
|
||||
helpers.push(ucs);
|
||||
});
|
||||
console.log(`[pose_3d] joint UCSes attached: ${helpers.length}`);
|
||||
return helpers;
|
||||
}
|
||||
|
||||
function setJointUCSVisible(visible) {
|
||||
for (const h of state.joint_ucses || []) h.visible = visible;
|
||||
}
|
||||
|
||||
function setJointUCSScale(sizeM) {
|
||||
const s = sizeM / 0.10;
|
||||
for (const h of state.joint_ucses || []) h.scale.setScalar(s);
|
||||
}
|
||||
|
||||
// UCS sempre visibile per ogni sensore fisso modellato nell'URDF
|
||||
// (RealSense D435i frontale, Livox MID360 lidar). I link esistono come
|
||||
// frame senza mesh; aggiungiamo qui un UCS + label per visualizzarli.
|
||||
function addFixedSensorMarkers(robot) {
|
||||
const sensors = [
|
||||
{ link: 'd435_link', label: 'D435i', color: '#ffd54f', size: 0.06 },
|
||||
{ link: 'mid360_link', label: 'MID360', color: '#9b59b6', size: 0.05 },
|
||||
];
|
||||
for (const s of sensors) {
|
||||
const link = (robot.links && robot.links[s.link])
|
||||
|| (() => { let f = null; robot.traverse(o => { if (o.name === s.link) f = o; }); return f; })();
|
||||
if (!link) continue;
|
||||
const ucs = makeUCS(s.size);
|
||||
link.add(ucs);
|
||||
const label = makeLabelSprite(s.label, s.color);
|
||||
label.position.set(0, 0, s.size + 0.02);
|
||||
link.add(label);
|
||||
}
|
||||
}
|
||||
|
||||
// Nasconde i mesh delle mani (palm + tutte le dita) lasciando i joint nel
|
||||
// grafo. Riconosce i nomi che iniziano per `left_hand_` / `right_hand_`.
|
||||
function hideHandMeshes(robot) {
|
||||
let n = 0;
|
||||
robot.traverse((obj) => {
|
||||
const name = obj.name || '';
|
||||
if (name.startsWith('left_hand_') || name.startsWith('right_hand_')) {
|
||||
obj.visible = false;
|
||||
n++;
|
||||
}
|
||||
});
|
||||
console.log(`[pose_3d] hand meshes hidden: ${n}`);
|
||||
}
|
||||
|
||||
// Trova il link URDF al quale la Nuwa e' fissata rigidamente (ultimo link
|
||||
// della catena polso destro). Ritorna null se l'URDF non e' ancora caricato.
|
||||
function getRightWristUrdfLink() {
|
||||
const r = state.robot;
|
||||
if (!r) return null;
|
||||
if (r.links && r.links['right_wrist_yaw_link']) return r.links['right_wrist_yaw_link'];
|
||||
let found = null;
|
||||
r.traverse((obj) => { if (obj.name === 'right_wrist_yaw_link') found = obj; });
|
||||
return found;
|
||||
}
|
||||
|
||||
// Aggancia la camera al link URDF: cosi' segue rotazione+posizione del joint,
|
||||
// non solo la posizione della sfera marker.
|
||||
function reparentNuwaToUrdf() {
|
||||
const cam = state.nuwa_mesh;
|
||||
const link = getRightWristUrdfLink();
|
||||
if (!cam || !link) return;
|
||||
if (cam.parent === link) return;
|
||||
link.add(cam); // THREE.Object3D.add() rimuove automaticamente dal vecchio parent
|
||||
applyNuwaTransform();
|
||||
}
|
||||
|
||||
function loadNuwaMesh(parent, meshUrl) {
|
||||
return new Promise((resolve, reject) => {
|
||||
const loader = new STLLoader();
|
||||
loader.load(
|
||||
meshUrl,
|
||||
(geom) => {
|
||||
geom.computeVertexNormals();
|
||||
const mesh = new THREE.Mesh(
|
||||
geom,
|
||||
new THREE.MeshStandardMaterial({
|
||||
color: 0xb8c1cb, metalness: 0.2, roughness: 0.5,
|
||||
emissive: 0x223040, emissiveIntensity: 0.6,
|
||||
}),
|
||||
);
|
||||
mesh.userData.isCustom = true; // protetto dal toggle "nascondi robot"
|
||||
mesh.scale.setScalar(0.001); // STL in mm, scena in metri
|
||||
// UCS visibile sull'origine della camera (debug orientamento)
|
||||
const cucs = makeUCS(0.04);
|
||||
const cam = new THREE.Group();
|
||||
cam.add(mesh);
|
||||
cam.add(cucs);
|
||||
// Default visibile anche prima che /state arrivi col config
|
||||
cam.position.set(0, 0, 0.1);
|
||||
parent.add(cam);
|
||||
resolve(cam);
|
||||
},
|
||||
undefined,
|
||||
(e) => { console.warn('[pose_3d] STLLoader error', e); reject(e); },
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
function applyNuwaTransform() {
|
||||
const cfg = state.nuwa_cfg;
|
||||
const cam = state.nuwa_mesh;
|
||||
if (!cam) return;
|
||||
if (cfg && cfg.enabled === false) { cam.visible = false; return; }
|
||||
cam.visible = true;
|
||||
// Se cfg non e' ancora arrivato, lascia i default impostati al load.
|
||||
if (!cfg) return;
|
||||
const [ox = 0, oy = 0, oz = 0.1] = cfg.offset_m || [];
|
||||
cam.position.set(ox, oy, oz);
|
||||
const [rd = 0, pd = 0, yd = 0] = cfg.rpy_deg || [];
|
||||
const e = new THREE.Euler(
|
||||
rd * Math.PI / 180,
|
||||
pd * Math.PI / 180,
|
||||
yd * Math.PI / 180,
|
||||
'XYZ',
|
||||
);
|
||||
cam.quaternion.setFromEuler(e);
|
||||
}
|
||||
|
||||
function makeWristMarker(color) {
|
||||
const group = new THREE.Group();
|
||||
const sphere = new THREE.Mesh(
|
||||
@@ -76,6 +217,11 @@ const state = {
|
||||
live_max_step_m: 0.15,
|
||||
robot: null, // URDF robot loaded via urdf-loader (null se non caricato)
|
||||
arm_links_m: null, // {upper_m, fore_m} computati da URDF al load
|
||||
nuwa_mesh: null, // mesh camera Nuwa montata sul polso destro (null se disabilitata)
|
||||
nuwa_cfg: null, // {enabled, mesh_url, offset_m, rpy_deg} dal config
|
||||
joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug)
|
||||
clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config
|
||||
cloud_points: { nuwa: null, d435: null }, // THREE.Points obj per camera
|
||||
};
|
||||
|
||||
function checkWorkspace(side, position_m) {
|
||||
@@ -149,6 +295,17 @@ async function loadG1Urdf(scene) {
|
||||
state.arm_links_m = measureArmLinkLengths(robot);
|
||||
console.log('[pose_3d] G1 URDF caricato:', robot.name || 'g1',
|
||||
'arm links (m):', state.arm_links_m);
|
||||
// Nascondi visivamente i link delle mani (palm, thumb, index, middle,
|
||||
// ring, pinky). I joint restano nel grafo per non rompere la catena
|
||||
// cinematica; viene oscurato solo il rendering del visual.
|
||||
hideHandMeshes(robot);
|
||||
// UCS per ogni link (di default nascosti, attivabili da checkbox)
|
||||
state.joint_ucses = addJointUCSes(robot);
|
||||
applyJointUCSUIState();
|
||||
// UCS permanenti per le camere fisse (D435i, MID360 lidar)
|
||||
addFixedSensorMarkers(robot);
|
||||
// Se la Nuwa e' gia' caricata, riparent sul link URDF cosi' segue i joint
|
||||
reparentNuwaToUrdf();
|
||||
resolve(robot);
|
||||
},
|
||||
undefined,
|
||||
@@ -233,6 +390,28 @@ function solveTwoBoneIK(shoulderWorldPos, wristTargetWorldPos, L1, L2, poleHintD
|
||||
};
|
||||
}
|
||||
|
||||
// Imposta direttamente i giunti URDF dai valori reali (q in rad) per nome.
|
||||
// Salta silenziosamente i nomi non presenti nell'URDF (joint fisso o
|
||||
// non modellato). Usa il suffisso "_joint" se richiesto da URDFLoader.
|
||||
function applyJointsToUrdf(joints) {
|
||||
const robot = state.robot;
|
||||
if (!robot || !joints) return 0;
|
||||
let n = 0;
|
||||
for (const name in joints) {
|
||||
const q = joints[name];
|
||||
if (!Number.isFinite(q)) continue;
|
||||
let j = robot.joints && (robot.joints[name] || robot.joints[`${name}_joint`]);
|
||||
if (!j) continue;
|
||||
try {
|
||||
robot.setJointValue(j.name, q);
|
||||
n++;
|
||||
} catch (e) {
|
||||
// joint fisso o fuori limiti — ignora
|
||||
}
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
function applyIKToUrdf(side, target_position, target_quaternion) {
|
||||
// Aggiorna i joint dell'URDF G1 per posizionare il polso `side` al target.
|
||||
// No-op se l'URDF non e' caricato.
|
||||
@@ -325,11 +504,18 @@ function initScene() {
|
||||
scene.add(wrist);
|
||||
state.arms[side] = wrist;
|
||||
|
||||
// Etichetta DX / SX sopra la sfera
|
||||
const label = makeLabelSprite(side === 'right' ? 'DX' : 'SX',
|
||||
side === 'right' ? '#4da3ff' : '#ff5e7e');
|
||||
label.position.set(0, 0, 0.08); // 8cm sopra la sfera (locale al gruppo)
|
||||
wrist.add(label);
|
||||
// Nuwa camera rigida sul polso destro (eye-in-hand): mesh STL, trasformata
|
||||
// letta da pose_3d.right_wrist_camera (offset+rpy in wrist frame).
|
||||
if (side === 'right') {
|
||||
loadNuwaMesh(wrist, '/static/assets/nuwa/nuwa-hp60c.stl')
|
||||
.then((cam) => {
|
||||
state.nuwa_mesh = cam;
|
||||
applyNuwaTransform();
|
||||
// Se l'URDF e' gia' caricato, sposta la mesh sul link URDF
|
||||
reparentNuwaToUrdf();
|
||||
})
|
||||
.catch((e) => console.warn('[pose_3d] STL Nuwa non caricato:', e.message));
|
||||
}
|
||||
|
||||
// Linea shoulder → wrist
|
||||
const lineGeom = new THREE.BufferGeometry().setFromPoints([
|
||||
@@ -427,15 +613,44 @@ function rpyDegToQuat(roll, pitch, yaw) {
|
||||
return [qx, qy, qz, qw];
|
||||
}
|
||||
|
||||
// Trova il link URDF del palmo per `side`. Usato per posizionare la sfera
|
||||
// marker selezionabile esattamente sul TCP visivo (= palm_link del modello).
|
||||
function getPalmLink(side) {
|
||||
if (!state.robot) return null;
|
||||
const name = `${side}_hand_palm_link`;
|
||||
if (state.robot.links && state.robot.links[name]) return state.robot.links[name];
|
||||
let found = null;
|
||||
state.robot.traverse(o => { if (o.name === name) found = o; });
|
||||
return found;
|
||||
}
|
||||
|
||||
// ----- state apply to scene -----
|
||||
function updateRealWrists() {
|
||||
const tmpV = new THREE.Vector3();
|
||||
const tmpQ = new THREE.Quaternion();
|
||||
for (const side of ['right', 'left']) {
|
||||
const pose = state.current[side];
|
||||
if (!pose) continue;
|
||||
const arm = state.arms[side];
|
||||
arm.position.set(pose.position[0], pose.position[1], pose.position[2]);
|
||||
arm.quaternion.set(pose.quaternion[0], pose.quaternion[1],
|
||||
pose.quaternion[2], pose.quaternion[3]);
|
||||
if (!arm) continue;
|
||||
// Preferiamo la pose world del palm link URDF (cosi' la sfera coincide
|
||||
// sempre col TCP visivo dato dai giunti reali via /diagnostics).
|
||||
const palm = getPalmLink(side);
|
||||
if (palm) {
|
||||
palm.updateWorldMatrix(true, false);
|
||||
palm.getWorldPosition(tmpV);
|
||||
palm.getWorldQuaternion(tmpQ);
|
||||
arm.position.copy(tmpV);
|
||||
arm.quaternion.copy(tmpQ);
|
||||
} else {
|
||||
// Fallback: TCP da /state (G1 wrist), usato quando URDF non e' carico
|
||||
const pose = state.current[side];
|
||||
if (!pose) continue;
|
||||
arm.position.set(pose.position[0], pose.position[1], pose.position[2]);
|
||||
arm.quaternion.set(pose.quaternion[0], pose.quaternion[1],
|
||||
pose.quaternion[2], pose.quaternion[3]);
|
||||
}
|
||||
// URDF: i giunti reali arrivano da pollState e li applichiamo li' (vedi
|
||||
// applyJointsToUrdf). Niente IK qui: l'IK e' un'approssimazione e
|
||||
// l'approccio diretto via /diagnostics e' esatto.
|
||||
// aggiorna line shoulder→wrist
|
||||
const line = state.links[side];
|
||||
const pts = [SHOULDER[side].clone(), arm.position.clone()];
|
||||
@@ -547,10 +762,20 @@ let pollTimer = null;
|
||||
async function pollState() {
|
||||
try {
|
||||
const s = await jsonFetch('/api/pose_3d/state');
|
||||
state.current.right = s.wrist.right;
|
||||
state.current.left = s.wrist.left;
|
||||
if (s.wrist) {
|
||||
state.current.right = s.wrist.right;
|
||||
state.current.left = s.wrist.left;
|
||||
}
|
||||
state.saved_poses = s.poses || {};
|
||||
state.live_max_step_m = s.live_max_step_m || 0.15;
|
||||
state.nuwa_cfg = s.right_wrist_camera || null;
|
||||
state.clouds_cfg = s.clouds || null;
|
||||
applyNuwaTransform();
|
||||
updateNuwaInputsFromCfg();
|
||||
updateD435InputsFromCfg();
|
||||
applyCalibratedD435UCS();
|
||||
// Applica TUTTI i giunti reali sul modello URDF (29 motori G1)
|
||||
if (s.joints) applyJointsToUrdf(s.joints);
|
||||
updateRealWrists();
|
||||
syncGhostsToTargetOrReal();
|
||||
updateReadout();
|
||||
@@ -563,16 +788,21 @@ async function pollState() {
|
||||
}
|
||||
|
||||
function refreshPoseSelect() {
|
||||
const sel = $('p3PoseSelect');
|
||||
if (!sel) return;
|
||||
const cur = sel.value;
|
||||
sel.innerHTML = '<option value="">(seleziona)</option>';
|
||||
for (const name of Object.keys(state.saved_poses).sort()) {
|
||||
const opt = document.createElement('option');
|
||||
opt.value = name; opt.textContent = name;
|
||||
sel.appendChild(opt);
|
||||
// Popola i due dropdown filtrati per side (right → DX, left → SX).
|
||||
for (const side of ['right', 'left']) {
|
||||
const sel = $(POSE_UI[side].select);
|
||||
if (!sel) continue;
|
||||
const cur = sel.value;
|
||||
sel.innerHTML = '<option value="">(seleziona)</option>';
|
||||
for (const name of Object.keys(state.saved_poses).sort()) {
|
||||
const p = state.saved_poses[name];
|
||||
if (!p || p.side !== side) continue;
|
||||
const opt = document.createElement('option');
|
||||
opt.value = name; opt.textContent = name;
|
||||
sel.appendChild(opt);
|
||||
}
|
||||
sel.value = cur;
|
||||
}
|
||||
sel.value = cur;
|
||||
}
|
||||
|
||||
// ----- selezione polso via click -----
|
||||
@@ -872,17 +1102,24 @@ function bindJogAutoApply() {
|
||||
});
|
||||
}
|
||||
|
||||
// ----- named poses -----
|
||||
async function onSavePose() {
|
||||
const name = ($('p3PoseName').value || '').trim();
|
||||
// ----- named poses (sezioni separate DX e SX) -----
|
||||
const POSE_UI = {
|
||||
right: { select: 'p3PoseSelectR', go: 'p3PoseGoBtnR', name: 'p3PoseNameR',
|
||||
save: 'p3PoseSaveBtnR', del: 'p3PoseDelBtnR', label: 'DX' },
|
||||
left: { select: 'p3PoseSelectL', go: 'p3PoseGoBtnL', name: 'p3PoseNameL',
|
||||
save: 'p3PoseSaveBtnL', del: 'p3PoseDelBtnL', label: 'SX' },
|
||||
};
|
||||
|
||||
async function onSavePose(side) {
|
||||
const ui = POSE_UI[side];
|
||||
const name = ($(ui.name).value || '').trim();
|
||||
if (!name) { alert('Inserisci un nome'); return; }
|
||||
if (!/^[A-Za-z0-9_-]{1,40}$/.test(name)) {
|
||||
alert('Nome: solo [A-Za-z0-9_-], 1-40 caratteri');
|
||||
return;
|
||||
}
|
||||
const side = state.active_side;
|
||||
const pose = state.target[side] || state.current[side];
|
||||
if (!pose) { alert('Nessuna posa da salvare'); return; }
|
||||
if (!pose) { alert(`Nessuna posa ${ui.label} da salvare`); return; }
|
||||
if (state.saved_poses[name]) {
|
||||
if (!confirm(`Posa "${name}" esiste già — sovrascrivere?`)) return;
|
||||
}
|
||||
@@ -893,19 +1130,20 @@ async function onSavePose() {
|
||||
side, position: pose.position, quaternion: pose.quaternion,
|
||||
}),
|
||||
});
|
||||
$('p3PoseName').value = '';
|
||||
$(ui.name).value = '';
|
||||
await pollState(); // refresh dropdown
|
||||
$('p3PoseSelect').value = name;
|
||||
$(ui.select).value = name;
|
||||
} catch (e) {
|
||||
showError(`Salvataggio fallito: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
async function onGoPose() {
|
||||
const name = $('p3PoseSelect').value;
|
||||
if (!name) { alert('Seleziona una posa dal dropdown'); return; }
|
||||
async function onGoPose(side) {
|
||||
const ui = POSE_UI[side];
|
||||
const name = $(ui.select).value;
|
||||
if (!name) { alert(`Seleziona una posa ${ui.label} dal dropdown`); return; }
|
||||
if (state.mode === 'jog') {
|
||||
if (!confirm(`Eseguire posa "${name}" SUL ROBOT REALE? (Jog mode)`)) return;
|
||||
if (!confirm(`Eseguire posa "${name}" (${ui.label}) SUL ROBOT REALE?`)) return;
|
||||
}
|
||||
const dry_run = (state.mode === 'editor');
|
||||
clearError();
|
||||
@@ -921,7 +1159,7 @@ async function onGoPose() {
|
||||
if (r.reached_target === false || r.stalled === true) {
|
||||
showError(`Vai a "${name}": residual ${(r.position_error_mm||0).toFixed(1)} mm`);
|
||||
} else {
|
||||
$('p3Status').textContent = `✓ Posa "${name}" applicata` +
|
||||
$('p3Status').textContent = `✓ Posa "${name}" (${ui.label}) applicata` +
|
||||
(dry_run ? ' (simulato)' : '');
|
||||
$('p3Status').className = 'p3-status ok';
|
||||
const pose = state.saved_poses[name];
|
||||
@@ -932,9 +1170,10 @@ async function onGoPose() {
|
||||
}
|
||||
}
|
||||
|
||||
async function onDeletePose() {
|
||||
const name = $('p3PoseSelect').value;
|
||||
if (!name) { alert('Seleziona una posa dal dropdown'); return; }
|
||||
async function onDeletePose(side) {
|
||||
const ui = POSE_UI[side];
|
||||
const name = $(ui.select).value;
|
||||
if (!name) { alert(`Seleziona una posa ${ui.label} dal dropdown`); return; }
|
||||
if (!confirm(`Cancellare definitivamente la posa "${name}"?`)) return;
|
||||
try {
|
||||
await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
|
||||
@@ -947,9 +1186,12 @@ async function onDeletePose() {
|
||||
}
|
||||
|
||||
function bindPoseButtons() {
|
||||
$('p3PoseSaveBtn').addEventListener('click', onSavePose);
|
||||
$('p3PoseGoBtn').addEventListener('click', onGoPose);
|
||||
$('p3PoseDelBtn').addEventListener('click', onDeletePose);
|
||||
for (const side of ['right', 'left']) {
|
||||
const ui = POSE_UI[side];
|
||||
$(ui.save).addEventListener('click', () => onSavePose(side));
|
||||
$(ui.go).addEventListener('click', () => onGoPose(side));
|
||||
$(ui.del).addEventListener('click', () => onDeletePose(side));
|
||||
}
|
||||
}
|
||||
|
||||
async function onUnlock() {
|
||||
@@ -967,6 +1209,490 @@ function bindUnlockButton() {
|
||||
$('p3UnlockBtn').addEventListener('click', onUnlock);
|
||||
}
|
||||
|
||||
// UCS al pose hand-eye calibrato della D435i (frame pelvis).
|
||||
// La matrice in config e' la convenzione `T @ p_pelvis = p_optical` (cioe'
|
||||
// trasforma punti DAL pelvis AL frame ottico camera). Per posizionare la
|
||||
// camera nel pelvis frame ci serve l'INVERSA.
|
||||
function applyCalibratedD435UCS() {
|
||||
if (!state.three) return;
|
||||
const cfg = state.clouds_cfg && state.clouds_cfg.d435;
|
||||
const m4 = cfg && cfg.transform_pelvis_4x4;
|
||||
if (!Array.isArray(m4) || m4.length !== 4) return;
|
||||
|
||||
const M_pelvis_to_optical = new THREE.Matrix4();
|
||||
M_pelvis_to_optical.set(
|
||||
m4[0][0], m4[0][1], m4[0][2], m4[0][3],
|
||||
m4[1][0], m4[1][1], m4[1][2], m4[1][3],
|
||||
m4[2][0], m4[2][1], m4[2][2], m4[2][3],
|
||||
m4[3][0], m4[3][1], m4[3][2], m4[3][3],
|
||||
);
|
||||
// Pose della camera in pelvis frame = inversa.
|
||||
const M_optical_in_pelvis = M_pelvis_to_optical.clone().invert();
|
||||
|
||||
if (!state.d435_calib_ucs) {
|
||||
const grp = new THREE.Group();
|
||||
// UCS con assi piu' spessi/lunghi per distinguerlo da quelli URDF
|
||||
const ucs = makeUCS(0.12);
|
||||
grp.add(ucs);
|
||||
// Pallina centrale arancione = identifica visivamente la D435i calibrata
|
||||
const dot = new THREE.Mesh(
|
||||
new THREE.SphereGeometry(0.012, 12, 8),
|
||||
new THREE.MeshBasicMaterial({ color: 0xff6f4d }),
|
||||
);
|
||||
grp.add(dot);
|
||||
const label = makeLabelSprite('D435i hand-eye', '#ff6f4d');
|
||||
label.scale.set(0.14, 0.06, 1); // un po' piu' larga del default
|
||||
label.position.set(0, 0, 0.16);
|
||||
grp.add(label);
|
||||
grp.matrixAutoUpdate = false;
|
||||
state.three.scene.add(grp);
|
||||
state.d435_calib_ucs = grp;
|
||||
}
|
||||
state.d435_calib_ucs.matrix.copy(M_optical_in_pelvis);
|
||||
state.d435_calib_ucs.matrixWorldNeedsUpdate = true;
|
||||
applyD435CalibUCSVisibility();
|
||||
}
|
||||
|
||||
function applyD435CalibUCSVisibility() {
|
||||
if (!state.d435_calib_ucs) return;
|
||||
const visible = $('p3ShowD435CalibUCS')?.checked !== false;
|
||||
state.d435_calib_ucs.visible = visible;
|
||||
}
|
||||
|
||||
// ----- Cloud cross-camera (Nuwa + D435i) -----
|
||||
const CLOUD_COLORS = { nuwa: 0xffd54f, d435: 0x4dd0e1 };
|
||||
|
||||
function readCloudSize(camKey) {
|
||||
// Valore UI in mm; PointsMaterial in modalita' sizeAttenuation:true usa
|
||||
// world units (metri) → divide per 1000.
|
||||
const id = camKey === 'nuwa' ? 'p3NuwaCloudSize' : 'p3D435CloudSize';
|
||||
const mm = parseFloat($(id)?.value);
|
||||
return Number.isFinite(mm) && mm > 0 ? mm / 1000 : 0.005;
|
||||
}
|
||||
|
||||
function applyCloudSize(camKey) {
|
||||
const pts = state.cloud_points[camKey];
|
||||
if (!pts) return;
|
||||
pts.material.size = readCloudSize(camKey);
|
||||
pts.material.needsUpdate = true;
|
||||
}
|
||||
|
||||
function getCloudParent(camKey) {
|
||||
// D435i in modalita' hand-eye: child della scene (pelvis = origin scena)
|
||||
if (camKey === 'd435' && useD435Handeye()) {
|
||||
return state.three && state.three.scene;
|
||||
}
|
||||
if (!state.robot) return null;
|
||||
const linkName = camKey === 'nuwa' ? 'right_wrist_yaw_link' : 'd435_link';
|
||||
if (state.robot.links && state.robot.links[linkName]) return state.robot.links[linkName];
|
||||
let found = null;
|
||||
state.robot.traverse(o => { if (o.name === linkName) found = o; });
|
||||
return found;
|
||||
}
|
||||
|
||||
function useD435Handeye() {
|
||||
return $('p3D435UseHandeye')?.checked === true;
|
||||
}
|
||||
|
||||
// Restituisce la Matrix4 M_optical_in_pelvis dalla calibrazione hand-eye,
|
||||
// oppure null se la matrice non e' presente in config.
|
||||
function getD435HandeyeMatrix() {
|
||||
const cfg = state.clouds_cfg && state.clouds_cfg.d435;
|
||||
const m4 = cfg && cfg.transform_pelvis_4x4;
|
||||
if (!Array.isArray(m4) || m4.length !== 4) return null;
|
||||
const M = new THREE.Matrix4().set(
|
||||
m4[0][0], m4[0][1], m4[0][2], m4[0][3],
|
||||
m4[1][0], m4[1][1], m4[1][2], m4[1][3],
|
||||
m4[2][0], m4[2][1], m4[2][2], m4[2][3],
|
||||
m4[3][0], m4[3][1], m4[3][2], m4[3][3],
|
||||
);
|
||||
return M.invert(); // pelvis→optical → optical pose in pelvis
|
||||
}
|
||||
|
||||
// Trasformazione optical→link da config: per nuwa eredita il
|
||||
// right_wrist_camera (offset+rpy), per d435 usa clouds.d435.
|
||||
function getCloudTransform(camKey) {
|
||||
const c = state.clouds_cfg || {};
|
||||
if (camKey === 'nuwa') {
|
||||
const nuwa = state.nuwa_cfg || {};
|
||||
const ex = (c.nuwa && c.nuwa.extra_offset_m) || [0,0,0];
|
||||
const er = (c.nuwa && c.nuwa.extra_rpy_deg) || [0,0,0];
|
||||
const base_off = nuwa.offset_m || [0,0,0.1];
|
||||
const base_rpy = nuwa.rpy_deg || [0,0,0];
|
||||
return {
|
||||
offset_m: [base_off[0]+ex[0], base_off[1]+ex[1], base_off[2]+ex[2]],
|
||||
rpy_deg: [base_rpy[0]+er[0], base_rpy[1]+er[1], base_rpy[2]+er[2]],
|
||||
};
|
||||
}
|
||||
const d = c.d435 || {};
|
||||
return { offset_m: d.offset_m || [0,0,0], rpy_deg: d.rpy_deg || [-90,0,-90] };
|
||||
}
|
||||
|
||||
function buildOrUpdateCloud(camKey, pointsFlat) {
|
||||
const parent = getCloudParent(camKey);
|
||||
if (!parent) { console.warn('[pose_3d] cloud parent link non trovato per', camKey); return null; }
|
||||
const N = pointsFlat.length;
|
||||
const flat = new Float32Array(N * 3);
|
||||
const colors = new Float32Array(N * 3);
|
||||
const hasRgb = pointsFlat.length > 0 && pointsFlat[0].length >= 6;
|
||||
for (let i = 0; i < N; i++) {
|
||||
flat[3*i] = pointsFlat[i][0];
|
||||
flat[3*i+1] = pointsFlat[i][1];
|
||||
flat[3*i+2] = pointsFlat[i][2];
|
||||
if (hasRgb) {
|
||||
colors[3*i] = pointsFlat[i][3] / 255.0;
|
||||
colors[3*i+1] = pointsFlat[i][4] / 255.0;
|
||||
colors[3*i+2] = pointsFlat[i][5] / 255.0;
|
||||
}
|
||||
}
|
||||
let pts = state.cloud_points[camKey];
|
||||
if (pts) {
|
||||
pts.geometry.dispose();
|
||||
pts.geometry = new THREE.BufferGeometry();
|
||||
pts.geometry.setAttribute('position', new THREE.BufferAttribute(flat, 3));
|
||||
if (hasRgb) pts.geometry.setAttribute('color', new THREE.BufferAttribute(colors, 3));
|
||||
pts.material.vertexColors = hasRgb;
|
||||
pts.material.needsUpdate = true;
|
||||
} else {
|
||||
const geom = new THREE.BufferGeometry();
|
||||
geom.setAttribute('position', new THREE.BufferAttribute(flat, 3));
|
||||
if (hasRgb) geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
|
||||
// RGB reali se disponibili, altrimenti colore camera-specific come fallback.
|
||||
// sizeAttenuation:true → la dimensione e' in metri e scala con la distanza.
|
||||
const mat = new THREE.PointsMaterial({
|
||||
color: hasRgb ? 0xffffff : CLOUD_COLORS[camKey],
|
||||
size: readCloudSize(camKey), sizeAttenuation: true,
|
||||
vertexColors: hasRgb,
|
||||
});
|
||||
pts = new THREE.Points(geom, mat);
|
||||
pts.userData.camKey = camKey;
|
||||
state.cloud_points[camKey] = pts;
|
||||
}
|
||||
// Log diagnostico
|
||||
let xmin=Infinity,xmax=-Infinity,ymin=Infinity,ymax=-Infinity,zmin=Infinity,zmax=-Infinity;
|
||||
for (let i = 0; i < N; i++) {
|
||||
const x=flat[3*i], y=flat[3*i+1], z=flat[3*i+2];
|
||||
if (x<xmin)xmin=x; if (x>xmax)xmax=x;
|
||||
if (y<ymin)ymin=y; if (y>ymax)ymax=y;
|
||||
if (z<zmin)zmin=z; if (z>zmax)zmax=z;
|
||||
}
|
||||
console.log(`[pose_3d] cloud ${camKey}: ${N} pts parent=${parent.name}`,
|
||||
`X[${xmin.toFixed(2)},${xmax.toFixed(2)}]`,
|
||||
`Y[${ymin.toFixed(2)},${ymax.toFixed(2)}]`,
|
||||
`Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`);
|
||||
// (Ri-)attacca al parent corretto e applica transform optical→link
|
||||
if (pts.parent !== parent) parent.add(pts);
|
||||
applyCloudTransform(camKey);
|
||||
applyCloudVisibility();
|
||||
return pts;
|
||||
}
|
||||
|
||||
function applyCloudVisibility() {
|
||||
const showNuwa = $('p3ShowNuwaCloud')?.checked === true;
|
||||
const showD435 = $('p3ShowD435Cloud')?.checked === true;
|
||||
if (state.cloud_points.nuwa) state.cloud_points.nuwa.visible = showNuwa;
|
||||
if (state.cloud_points.d435) state.cloud_points.d435.visible = showD435;
|
||||
}
|
||||
|
||||
// Applica position+rpy+mirror sui punti del cloud `camKey`. Il mirror (sign
|
||||
// flip per asse) avviene IN FRAME OTTICO grazie all'ordine T·R·S di Three.js
|
||||
// (lo scale e' la prima operazione applicata al punto locale).
|
||||
function applyCloudTransform(camKey) {
|
||||
const pts = state.cloud_points[camKey];
|
||||
if (!pts) return;
|
||||
const sv = readCloudScaleVec(camKey); // flip × scale (frame ottico)
|
||||
|
||||
// Modalita' hand-eye per D435i: usa direttamente la matrice 4x4 calibrata
|
||||
if (camKey === 'd435' && useD435Handeye()) {
|
||||
const M = getD435HandeyeMatrix();
|
||||
if (M) {
|
||||
pts.matrixAutoUpdate = false;
|
||||
const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]);
|
||||
pts.matrix.copy(M).multiply(S);
|
||||
pts.matrixWorldNeedsUpdate = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Modalita' URDF link + offset/rpy (default)
|
||||
pts.matrixAutoUpdate = true;
|
||||
const t = getCloudTransform(camKey);
|
||||
pts.position.set(t.offset_m[0], t.offset_m[1], t.offset_m[2]);
|
||||
const e = new THREE.Euler(
|
||||
t.rpy_deg[0] * Math.PI/180,
|
||||
t.rpy_deg[1] * Math.PI/180,
|
||||
t.rpy_deg[2] * Math.PI/180,
|
||||
'XYZ',
|
||||
);
|
||||
pts.quaternion.setFromEuler(e);
|
||||
pts.scale.set(sv[0], sv[1], sv[2]);
|
||||
}
|
||||
|
||||
function readCloudFlip(camKey) {
|
||||
const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435';
|
||||
return [
|
||||
$(`p3Flip${tag}X`)?.checked ? -1 : 1,
|
||||
$(`p3Flip${tag}Y`)?.checked ? -1 : 1,
|
||||
$(`p3Flip${tag}Z`)?.checked ? -1 : 1,
|
||||
];
|
||||
}
|
||||
|
||||
// Scale per asse (workaround bug intrinsics-mismatch). Per D435i applichiamo
|
||||
// fx_ir/fx_rgb ≈ 0.65 su X e Y; Z corretto. Per Nuwa nessuna correzione.
|
||||
function readCloudScale(camKey) {
|
||||
if (camKey !== 'd435') return [1, 1, 1];
|
||||
const v = parseFloat($('p3D435ScaleXY')?.value);
|
||||
const s = Number.isFinite(v) && v > 0 ? v : 1;
|
||||
return [s, s, 1];
|
||||
}
|
||||
|
||||
// Combina flip (±1) e correzione scale per asse.
|
||||
function readCloudScaleVec(camKey) {
|
||||
const f = readCloudFlip(camKey);
|
||||
const sc = readCloudScale(camKey);
|
||||
return [f[0] * sc[0], f[1] * sc[1], f[2] * sc[2]];
|
||||
}
|
||||
|
||||
async function acquireClouds() {
|
||||
// Clamp dei parametri: step oltre ~32 fa 422 sul server pointcloud.
|
||||
let step = parseInt($('p3CloudStep').value, 10);
|
||||
if (!Number.isFinite(step) || step < 1) step = 8;
|
||||
if (step > 32) step = 32;
|
||||
$('p3CloudStep').value = String(step);
|
||||
let max_depth_mm = parseInt($('p3CloudMaxDepth').value, 10);
|
||||
if (!Number.isFinite(max_depth_mm) || max_depth_mm < 100) max_depth_mm = 2000;
|
||||
if (max_depth_mm > 5000) max_depth_mm = 5000;
|
||||
$('p3CloudMaxDepth').value = String(max_depth_mm);
|
||||
$('p3CloudStatus').textContent = 'acquisizione…';
|
||||
$('p3CloudStatus').style.color = 'var(--dim)';
|
||||
try {
|
||||
const r = await jsonFetch(
|
||||
`/api/pose_3d/clouds?step=${step}&max_depth_mm=${max_depth_mm}`,
|
||||
);
|
||||
const parts = [];
|
||||
for (const cam of ['nuwa', 'd435']) {
|
||||
const c = r[cam];
|
||||
if (!c || c.error) {
|
||||
parts.push(`${cam}: ${c?.error || 'no data'}`);
|
||||
continue;
|
||||
}
|
||||
buildOrUpdateCloud(cam, c.points);
|
||||
parts.push(`${cam}: ${c.n} pts`);
|
||||
}
|
||||
$('p3CloudStatus').textContent = parts.join(' · ');
|
||||
$('p3CloudStatus').style.color = 'var(--ok)';
|
||||
} catch (e) {
|
||||
$('p3CloudStatus').textContent = `errore: ${e.message}`;
|
||||
$('p3CloudStatus').style.color = 'var(--danger)';
|
||||
}
|
||||
}
|
||||
|
||||
function clearClouds() {
|
||||
for (const cam of ['nuwa', 'd435']) {
|
||||
const pts = state.cloud_points[cam];
|
||||
if (pts) {
|
||||
pts.parent?.remove(pts);
|
||||
pts.geometry.dispose();
|
||||
state.cloud_points[cam] = null;
|
||||
}
|
||||
}
|
||||
$('p3CloudStatus').textContent = '—';
|
||||
$('p3CloudStatus').style.color = '';
|
||||
}
|
||||
|
||||
function bindCloudControls() {
|
||||
$('p3CloudAcquireBtn')?.addEventListener('click', acquireClouds);
|
||||
$('p3CloudClearBtn')?.addEventListener('click', clearClouds);
|
||||
$('p3ShowNuwaCloud')?.addEventListener('change', applyCloudVisibility);
|
||||
$('p3ShowD435Cloud')?.addEventListener('change', applyCloudVisibility);
|
||||
// Mirror toggles: applicano lo scale in frame ottico senza ri-acquisire
|
||||
for (const tag of ['Nuwa', 'D435']) {
|
||||
const key = tag === 'Nuwa' ? 'nuwa' : 'd435';
|
||||
for (const axis of ['X', 'Y', 'Z']) {
|
||||
$(`p3Flip${tag}${axis}`)?.addEventListener('change', () => applyCloudTransform(key));
|
||||
}
|
||||
}
|
||||
// Swap modalita' D435i: riparenting + ricalcolo trasformazione
|
||||
$('p3D435UseHandeye')?.addEventListener('change', () => {
|
||||
const pts = state.cloud_points.d435;
|
||||
if (!pts) return;
|
||||
const newParent = getCloudParent('d435');
|
||||
if (newParent && pts.parent !== newParent) newParent.add(pts);
|
||||
applyCloudTransform('d435');
|
||||
});
|
||||
// Size live per camera (pixel size)
|
||||
for (const ev of ['input', 'change']) {
|
||||
$('p3NuwaCloudSize')?.addEventListener(ev, () => applyCloudSize('nuwa'));
|
||||
$('p3D435CloudSize')?.addEventListener(ev, () => applyCloudSize('d435'));
|
||||
}
|
||||
// Scale XY workaround D435i (intrinsics-mismatch fix)
|
||||
for (const ev of ['input', 'change']) {
|
||||
$('p3D435ScaleXY')?.addEventListener(ev, () => applyCloudTransform('d435'));
|
||||
}
|
||||
}
|
||||
|
||||
// ----- joint UCS debug toggle -----
|
||||
function applyJointUCSUIState() {
|
||||
const visible = $('p3ShowJointUCS')?.checked === true;
|
||||
const sizeMm = parseFloat($('p3JointUCSSize')?.value || '40');
|
||||
setJointUCSVisible(visible);
|
||||
setJointUCSScale((Number.isFinite(sizeMm) ? sizeMm : 40) / 1000);
|
||||
}
|
||||
|
||||
function bindJointUCSControls() {
|
||||
$('p3ShowJointUCS')?.addEventListener('change', applyJointUCSUIState);
|
||||
$('p3JointUCSSize')?.addEventListener('change', applyJointUCSUIState);
|
||||
$('p3JointUCSSize')?.addEventListener('input', applyJointUCSUIState);
|
||||
$('p3ShowD435CalibUCS')?.addEventListener('change', applyD435CalibUCSVisibility);
|
||||
$('p3ShowRobotBody')?.addEventListener('change', applyRobotBodyVisibility);
|
||||
}
|
||||
|
||||
// Toggla solo i mesh "nativi" dell'URDF, lasciando UCS giunti / sensor / Nuwa
|
||||
// (taggati con userData.isCustom=true) e label/sprite sempre visibili.
|
||||
function applyRobotBodyVisibility() {
|
||||
if (!state.robot) return;
|
||||
const visible = $('p3ShowRobotBody')?.checked !== false;
|
||||
state.robot.traverse((obj) => {
|
||||
if (obj.isMesh && !obj.userData.isCustom) {
|
||||
obj.visible = visible;
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// ----- Nuwa camera params (offset+rpy in wrist frame) -----
|
||||
const NUWA_DEFAULTS = { enabled: true, offset_mm: [0, 0, 100], rpy_deg: [0, 0, -90] };
|
||||
|
||||
function updateNuwaInputsFromCfg() {
|
||||
// Popola solo al primo poll. Poi l'utente controlla i campi finche' non
|
||||
// clicca Default (che bypassa questa funzione).
|
||||
if (state.nuwa_inputs_populated) return;
|
||||
const cfg = state.nuwa_cfg;
|
||||
if (!cfg) return;
|
||||
const enabled = cfg.enabled !== false;
|
||||
const [ox = 0, oy = 0, oz = 0.1] = cfg.offset_m || [];
|
||||
const [rd = 0, pd = 0, yd = 0] = cfg.rpy_deg || [];
|
||||
const elE = $('p3NuwaEnabled');
|
||||
if (elE) elE.checked = enabled;
|
||||
const fields = [
|
||||
['p3NuwaX', Math.round(ox * 1000)],
|
||||
['p3NuwaY', Math.round(oy * 1000)],
|
||||
['p3NuwaZ', Math.round(oz * 1000)],
|
||||
['p3NuwaR', Math.round(rd)],
|
||||
['p3NuwaP', Math.round(pd)],
|
||||
['p3NuwaW', Math.round(yd)],
|
||||
];
|
||||
for (const [id, val] of fields) {
|
||||
const el = $(id);
|
||||
if (el) el.value = String(val);
|
||||
}
|
||||
state.nuwa_inputs_populated = true;
|
||||
}
|
||||
|
||||
async function onNuwaApply() {
|
||||
const num = (id) => Number($(id).value);
|
||||
const offset_m = [num('p3NuwaX') / 1000, num('p3NuwaY') / 1000, num('p3NuwaZ') / 1000];
|
||||
const rpy_deg = [num('p3NuwaR'), num('p3NuwaP'), num('p3NuwaW')];
|
||||
const enabled = $('p3NuwaEnabled').checked;
|
||||
const patch = {
|
||||
pose_3d: {
|
||||
right_wrist_camera: { enabled, offset_m, rpy_deg },
|
||||
},
|
||||
};
|
||||
try {
|
||||
const r = await fetch('/api/config', {
|
||||
method: 'PUT',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(patch),
|
||||
});
|
||||
if (!r.ok) throw new Error(`HTTP ${r.status}`);
|
||||
// riapplica subito senza aspettare il prossimo poll
|
||||
state.nuwa_cfg = { ...state.nuwa_cfg, enabled, offset_m, rpy_deg };
|
||||
applyNuwaTransform();
|
||||
$('p3Status').textContent = '✓ Nuwa applicata';
|
||||
$('p3Status').className = 'p3-status ok';
|
||||
} catch (e) {
|
||||
showError(`Nuwa apply fallito: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
function onNuwaReset() {
|
||||
$('p3NuwaEnabled').checked = NUWA_DEFAULTS.enabled;
|
||||
$('p3NuwaX').value = String(NUWA_DEFAULTS.offset_mm[0]);
|
||||
$('p3NuwaY').value = String(NUWA_DEFAULTS.offset_mm[1]);
|
||||
$('p3NuwaZ').value = String(NUWA_DEFAULTS.offset_mm[2]);
|
||||
$('p3NuwaR').value = String(NUWA_DEFAULTS.rpy_deg[0]);
|
||||
$('p3NuwaP').value = String(NUWA_DEFAULTS.rpy_deg[1]);
|
||||
$('p3NuwaW').value = String(NUWA_DEFAULTS.rpy_deg[2]);
|
||||
}
|
||||
|
||||
function bindNuwaControls() {
|
||||
$('p3NuwaApplyBtn')?.addEventListener('click', onNuwaApply);
|
||||
$('p3NuwaResetBtn')?.addEventListener('click', onNuwaReset);
|
||||
}
|
||||
|
||||
// ----- D435i URDF-mode offset (offset_m + rpy_deg vs d435_link) -----
|
||||
const D435_DEFAULTS = { offset_mm: [0, 0, 0], rpy_deg: [-90, 90, 0] };
|
||||
|
||||
function updateD435InputsFromCfg() {
|
||||
// Popola solo al primo poll: dopo non si tocca, l'utente controlla.
|
||||
if (state.d435_inputs_populated) return;
|
||||
const cfg = state.clouds_cfg && state.clouds_cfg.d435;
|
||||
if (!cfg) return;
|
||||
const [ox = 0, oy = 0, oz = 0] = cfg.offset_m || [];
|
||||
const [rd = -90, pd = 90, yd = 0] = cfg.rpy_deg || [];
|
||||
const fields = [
|
||||
['p3D435X', Math.round(ox * 1000)],
|
||||
['p3D435Y', Math.round(oy * 1000)],
|
||||
['p3D435Z', Math.round(oz * 1000)],
|
||||
['p3D435R', Math.round(rd)],
|
||||
['p3D435P', Math.round(pd)],
|
||||
['p3D435W', Math.round(yd)],
|
||||
];
|
||||
for (const [id, val] of fields) {
|
||||
const el = $(id);
|
||||
if (el) el.value = String(val);
|
||||
}
|
||||
state.d435_inputs_populated = true;
|
||||
}
|
||||
|
||||
async function onD435Apply() {
|
||||
const num = (id) => Number($(id).value);
|
||||
const offset_m = [num('p3D435X') / 1000, num('p3D435Y') / 1000, num('p3D435Z') / 1000];
|
||||
const rpy_deg = [num('p3D435R'), num('p3D435P'), num('p3D435W')];
|
||||
const patch = {
|
||||
pose_3d: { clouds: { d435: { offset_m, rpy_deg } } },
|
||||
};
|
||||
try {
|
||||
const r = await fetch('/api/config', {
|
||||
method: 'PUT',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(patch),
|
||||
});
|
||||
if (!r.ok) throw new Error(`HTTP ${r.status}`);
|
||||
state.clouds_cfg = state.clouds_cfg || {};
|
||||
state.clouds_cfg.d435 = { ...state.clouds_cfg.d435, offset_m, rpy_deg };
|
||||
applyCloudTransform('d435');
|
||||
$('p3Status').textContent = '✓ D435i applicata';
|
||||
$('p3Status').className = 'p3-status ok';
|
||||
} catch (e) {
|
||||
showError(`D435i apply fallito: ${e.message}`);
|
||||
}
|
||||
}
|
||||
|
||||
function onD435Reset() {
|
||||
$('p3D435X').value = String(D435_DEFAULTS.offset_mm[0]);
|
||||
$('p3D435Y').value = String(D435_DEFAULTS.offset_mm[1]);
|
||||
$('p3D435Z').value = String(D435_DEFAULTS.offset_mm[2]);
|
||||
$('p3D435R').value = String(D435_DEFAULTS.rpy_deg[0]);
|
||||
$('p3D435P').value = String(D435_DEFAULTS.rpy_deg[1]);
|
||||
$('p3D435W').value = String(D435_DEFAULTS.rpy_deg[2]);
|
||||
}
|
||||
|
||||
function bindD435Controls() {
|
||||
$('p3D435ApplyBtn')?.addEventListener('click', onD435Apply);
|
||||
$('p3D435ResetBtn')?.addEventListener('click', onD435Reset);
|
||||
}
|
||||
|
||||
// ----- init triggered on first activation -----
|
||||
let inited = false;
|
||||
function ensureInit() {
|
||||
@@ -984,6 +1710,10 @@ function ensureInit() {
|
||||
bindJogAutoApply();
|
||||
bindPoseButtons();
|
||||
bindUnlockButton();
|
||||
bindNuwaControls();
|
||||
bindD435Controls();
|
||||
bindJointUCSControls();
|
||||
bindCloudControls();
|
||||
bindReadoutInputs();
|
||||
pollState();
|
||||
pollTimer = setInterval(pollState, POLL_MS);
|
||||
|
||||
Reference in new issue
Block a user