AdrianoandClaude Opus 4.8
746662d534
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2238 lines
82 KiB
JavaScript
2238 lines
82 KiB
JavaScript
// Tab "Pose 3D" — viewport Three.js, gizmo translate/rotate, drag dei polsi.
|
||
// Vanilla JS, ES modules.
|
||
|
||
import * as THREE from 'three';
|
||
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
|
||
import { TransformControls } from 'three/addons/controls/TransformControls.js';
|
||
import { STLLoader } from 'three/addons/loaders/STLLoader.js';
|
||
import URDFLoader from 'three/addons/loaders/URDFLoader.js';
|
||
|
||
const $ = (id) => document.getElementById(id);
|
||
const POLL_MS = 1000;
|
||
|
||
// ----- helpers geometria -----
|
||
function makeUCS(size = 0.1) {
|
||
// 3 linee X(rosso) Y(verde) Z(blu) dalla origine
|
||
const g = new THREE.Group();
|
||
const mat = (color) => new THREE.LineBasicMaterial({ color, linewidth: 2 });
|
||
const make = (dir, color) => {
|
||
const geom = new THREE.BufferGeometry().setFromPoints([
|
||
new THREE.Vector3(0, 0, 0),
|
||
new THREE.Vector3(dir.x * size, dir.y * size, dir.z * size),
|
||
]);
|
||
return new THREE.Line(geom, mat(color));
|
||
};
|
||
g.add(make(new THREE.Vector3(1, 0, 0), 0xe74c3c));
|
||
g.add(make(new THREE.Vector3(0, 1, 0), 0x3ecf8e));
|
||
g.add(make(new THREE.Vector3(0, 0, 1), 0x4da3ff));
|
||
return g;
|
||
}
|
||
|
||
function makeLabelSprite(text, color = '#ffffff') {
|
||
const canvas = document.createElement('canvas');
|
||
canvas.width = 128;
|
||
canvas.height = 64;
|
||
const ctx = canvas.getContext('2d');
|
||
ctx.fillStyle = 'rgba(0, 0, 0, 0.6)';
|
||
ctx.fillRect(0, 0, canvas.width, canvas.height);
|
||
ctx.fillStyle = color;
|
||
ctx.font = 'bold 36px sans-serif';
|
||
ctx.textAlign = 'center';
|
||
ctx.textBaseline = 'middle';
|
||
ctx.fillText(text, canvas.width / 2, canvas.height / 2);
|
||
const texture = new THREE.CanvasTexture(canvas);
|
||
const material = new THREE.SpriteMaterial({ map: texture, depthTest: false });
|
||
const sprite = new THREE.Sprite(material);
|
||
sprite.scale.set(0.10, 0.05, 1); // ~10cm wide x 5cm tall in scene units
|
||
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(
|
||
new THREE.SphereGeometry(0.04, 16, 12),
|
||
new THREE.MeshStandardMaterial({ color, metalness: 0.2, roughness: 0.5 }),
|
||
);
|
||
group.add(sphere);
|
||
group.add(makeUCS(0.07));
|
||
group.userData.sphere = sphere;
|
||
group.userData.sphere.userData.kind = 'wrist';
|
||
return group;
|
||
}
|
||
|
||
// ----- state -----
|
||
const state = {
|
||
three: null, // {scene, camera, renderer, orbit, transform}
|
||
arms: { right: null, left: null }, // wrist groups (real)
|
||
ghosts: { right: null, left: null }, // wrist groups (target preview)
|
||
links: { right: null, left: null }, // line shoulder→wrist
|
||
current: { right: null, left: null }, // {position, quaternion}
|
||
target: { right: null, left: null }, // null = nessuna modifica locale
|
||
active_side: 'right',
|
||
mode: 'editor',
|
||
gizmo_mode: 'translate',
|
||
precise: true,
|
||
saved_poses: {},
|
||
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
|
||
align: { enabled: false, id: 0, matrix: null /* THREE.Matrix4 */ },
|
||
tag_locator: { // sezione "Tag locator" nella sidebar Pose 3D
|
||
marker: null, // THREE.LineSegments (crocetta XYZ) o null
|
||
position_m: null, // [x,y,z] in pelvis frame della crocetta corrente
|
||
joints_at_place: null, // snapshot giunti al click (può essere null se G1 offline)
|
||
},
|
||
tags: { // overlay tag per camera (gruppi THREE child del cloud node)
|
||
nuwa: { group: null },
|
||
d435: { group: null },
|
||
},
|
||
};
|
||
|
||
function checkWorkspace(side, position_m) {
|
||
// Controllo client-side: il target e' nella sfera di raggio ARM_MAX_REACH_M
|
||
// centrata sulla spalla? Ritorna {feasible, distance_m, max_m}.
|
||
const sh = SHOULDER[side];
|
||
if (!sh) return { feasible: true, distance_m: 0, max_m: ARM_MAX_REACH_M };
|
||
const dx = position_m[0] - sh.x;
|
||
const dy = position_m[1] - sh.y;
|
||
const dz = position_m[2] - sh.z;
|
||
const distance = Math.sqrt(dx*dx + dy*dy + dz*dz);
|
||
return {
|
||
feasible: distance <= ARM_MAX_REACH_M,
|
||
distance_m: distance,
|
||
max_m: ARM_MAX_REACH_M,
|
||
};
|
||
}
|
||
|
||
// Posizioni torso e spalle hardcoded (riferimento visivo, non IK)
|
||
const TORSO_POS = new THREE.Vector3(0, 0, 0.40);
|
||
// Posizioni di riferimento del G1 in base frame (feet on floor convention,
|
||
// URDF alzato di 0.793m). Costanti approssimate, non vengono usate per IK
|
||
// vera ma per: (1) workspace check (centro = spalla, raggio = braccio teso),
|
||
// (2) fallback delle sfere polso quando /state e' offline (posa zero-joint).
|
||
// Costanti in frame G1 base (pelvi all'origine). Sottratto il lift di 0.793m
|
||
// che prima alzava il robot.
|
||
const SHOULDER = {
|
||
right: new THREE.Vector3(0.0, -0.18, 0.26), // ~26cm sopra pelvi
|
||
left: new THREE.Vector3(0.0, 0.18, 0.26),
|
||
};
|
||
const WRIST_ZERO_POSE = {
|
||
right: new THREE.Vector3(0.0, -0.18, -0.34), // ~34cm sotto pelvi (braccia giu')
|
||
left: new THREE.Vector3(0.0, 0.18, -0.34),
|
||
};
|
||
const ARM_MAX_REACH_M = 0.62; // braccio teso G1 (upper+forearm+palm ~10+25+25 cm)
|
||
|
||
// ----- URDF loader -----
|
||
// Carica il modello G1 reale a partire dal URDF vendored sotto
|
||
// /static/vendor/g1/. Le mesh STL referenziate nel URDF (path relativi tipo
|
||
// "meshes/pelvis.STL") vengono risolti contro lo stesso prefisso.
|
||
async function loadG1Urdf(scene) {
|
||
const loader = new URDFLoader();
|
||
loader.loadMeshCb = function (path, manager, onComplete) {
|
||
// URDFLoader risolve gia' i path relativi rispetto alla URL della URDF,
|
||
// quindi `path` arriva come "/static/vendor/g1/meshes/X.STL". Non
|
||
// ri-prefissare.
|
||
const url = path;
|
||
const stl = new STLLoader(manager);
|
||
stl.load(
|
||
url,
|
||
(geometry) => {
|
||
const mat = new THREE.MeshStandardMaterial({
|
||
color: 0x9099a3, metalness: 0.4, roughness: 0.6,
|
||
transparent: true, opacity: 0.85,
|
||
});
|
||
const mesh = new THREE.Mesh(geometry, mat);
|
||
onComplete(mesh);
|
||
},
|
||
undefined,
|
||
(err) => onComplete(null, err),
|
||
);
|
||
};
|
||
return new Promise((resolve, reject) => {
|
||
loader.load(
|
||
'/static/vendor/g1/g1.urdf',
|
||
(robot) => {
|
||
// Robot rimane all'origine: la pelvi e' lo zero del frame base del G1,
|
||
// coerente con le pose wrist che arrivano da /api/pose_3d/state.
|
||
scene.add(robot);
|
||
state.robot = robot;
|
||
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,
|
||
(err) => reject(err),
|
||
);
|
||
});
|
||
}
|
||
|
||
function measureArmLinkLengths(robot) {
|
||
// Misura le lunghezze dei link "upper arm" e "forearm" leggendo le posizioni
|
||
// dei joint right_shoulder_pitch -> right_elbow -> right_wrist_pitch
|
||
// (proxy per la lunghezza dell'avambraccio fino al polso).
|
||
// Default fallback: ~0.25 m upper, ~0.25 m forearm se i joint non si trovano.
|
||
const get = (n) => robot.joints && robot.joints[n] ? robot.joints[n] : null;
|
||
const shp = get('right_shoulder_pitch');
|
||
const elb = get('right_elbow');
|
||
const wrp = get('right_wrist_pitch');
|
||
if (!shp || !elb || !wrp) return { upper_m: 0.25, fore_m: 0.25 };
|
||
// World positions
|
||
const wShoulder = new THREE.Vector3();
|
||
const wElbow = new THREE.Vector3();
|
||
const wWrist = new THREE.Vector3();
|
||
shp.getWorldPosition(wShoulder);
|
||
elb.getWorldPosition(wElbow);
|
||
wrp.getWorldPosition(wWrist);
|
||
return {
|
||
upper_m: wShoulder.distanceTo(wElbow),
|
||
fore_m: wElbow.distanceTo(wWrist),
|
||
};
|
||
}
|
||
|
||
function solveTwoBoneIK(shoulderWorldPos, wristTargetWorldPos, L1, L2, poleHintDir) {
|
||
// Calcola angolo gomito e direzione shoulder->elbow per posizionare
|
||
// l'end-effector (polso) in `wristTargetWorldPos`.
|
||
// Restituisce {elbow_angle, shoulder_pitch, shoulder_roll, shoulder_yaw}
|
||
// basati su convenzione G1 (Unitree): pitch attorno a Y, roll attorno a X, yaw attorno a Z del frame spalla.
|
||
// I valori sono approssimati, calibrabili in seguito.
|
||
const sw = new THREE.Vector3().subVectors(wristTargetWorldPos, shoulderWorldPos);
|
||
let d = sw.length();
|
||
const dmax = (L1 + L2) * 0.999;
|
||
const dmin = Math.abs(L1 - L2) * 1.001;
|
||
if (d > dmax) d = dmax;
|
||
if (d < dmin) d = dmin;
|
||
sw.normalize();
|
||
|
||
// Angolo del gomito (law of cosines), 0 = teso, pi = ripiegato
|
||
const cosE = (L1*L1 + L2*L2 - d*d) / (2*L1*L2);
|
||
const elbow_angle = Math.PI - Math.acos(Math.max(-1, Math.min(1, cosE)));
|
||
|
||
// Direzione "verso la target" in coordinate base del polso shoulder
|
||
// Per il G1 right arm in zero pose: shoulder X = down (verso il basso), Z = forward.
|
||
// Approssimazione: shoulder_pitch ruota attorno a Y (sinistra-destra), shoulder_roll attorno a X (avanti-indietro).
|
||
// - pitch: angolo nel piano XZ (front/back vs down)
|
||
// - roll: angolo fuori dal piano (verso medial/lateral)
|
||
// - yaw: tipicamente 0 per drag puro (rotazione attorno a all'asse del braccio)
|
||
|
||
// pitch: alzare/abbassare il braccio = ruotare attorno a Y dello shoulder frame
|
||
// In zero pose dx, braccio giu' -> world (0, 0, -1). Vogliamo il braccio puntato lungo `sw`.
|
||
// pitch sposta nel piano X-Z (vista frontale: avanti/dietro)
|
||
// roll sposta lateralmente
|
||
const x = sw.x; // forward axis (X base = +x)
|
||
const y = sw.y; // lateral (sinistra +)
|
||
const z = sw.z; // up (+z)
|
||
|
||
// pitch: angolo nel piano XZ. In zero pose (-Z), pitch=0. pitch>0 alza in avanti (+X).
|
||
// arm pointing direction: from shoulder to wrist, in world frame.
|
||
// Se z=-1 (giù) e x=0,y=0 -> pitch=0, roll=0
|
||
// Se x=+1 (avanti) e z=0 -> pitch = +pi/2
|
||
const horiz = Math.sqrt(x*x + y*y);
|
||
const shoulder_pitch = Math.atan2(x, -z); // 0 = giù, +pi/2 = avanti
|
||
// roll: angolo dalla verticale lungo Y (asse laterale)
|
||
const shoulder_roll = Math.atan2(y, Math.sqrt(x*x + z*z)); // 0 = nel piano sagittale
|
||
// yaw: lasciato a 0 per ora (rotazione attorno al braccio); puo' essere
|
||
// tunato dal quaternion target del polso
|
||
const shoulder_yaw = 0;
|
||
|
||
return {
|
||
elbow_angle,
|
||
shoulder_pitch,
|
||
shoulder_roll,
|
||
shoulder_yaw,
|
||
};
|
||
}
|
||
|
||
// 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.
|
||
if (!state.robot || !state.arm_links_m) return;
|
||
const robot = state.robot;
|
||
// Posizione spalla in world frame (legge dall'URDF live)
|
||
const shoulderName = `${side}_shoulder_pitch`;
|
||
const shJoint = robot.joints && robot.joints[shoulderName];
|
||
if (!shJoint) {
|
||
console.warn('[pose_3d] joint non trovato:', shoulderName);
|
||
return;
|
||
}
|
||
const shoulderWorld = new THREE.Vector3();
|
||
shJoint.getWorldPosition(shoulderWorld);
|
||
// Target in world (i target sono gia' in scene/world frame)
|
||
const wristWorld = new THREE.Vector3(target_position[0], target_position[1], target_position[2]);
|
||
|
||
const { upper_m, fore_m } = state.arm_links_m;
|
||
const ik = solveTwoBoneIK(shoulderWorld, wristWorld, upper_m, fore_m);
|
||
|
||
// Per il braccio sinistro, alcuni segni si invertono (mirror)
|
||
const sign = side === 'right' ? 1 : -1;
|
||
|
||
try {
|
||
robot.setJointValue(`${side}_shoulder_pitch`, ik.shoulder_pitch);
|
||
robot.setJointValue(`${side}_shoulder_roll`, ik.shoulder_roll * sign);
|
||
robot.setJointValue(`${side}_shoulder_yaw`, ik.shoulder_yaw);
|
||
robot.setJointValue(`${side}_elbow`, ik.elbow_angle);
|
||
// Wrist orientation: per ora teniamo 0 (raggiunge la posizione, l'orientazione
|
||
// non e' guidata dal target quaternion). Estensione futura: convertire il
|
||
// quaternion target a roll/pitch/yaw del polso URDF.
|
||
robot.setJointValue(`${side}_wrist_roll`, 0);
|
||
robot.setJointValue(`${side}_wrist_pitch`, 0);
|
||
robot.setJointValue(`${side}_wrist_yaw`, 0);
|
||
} catch (e) {
|
||
console.warn('[pose_3d] setJointValue err:', e.message);
|
||
}
|
||
}
|
||
|
||
// ----- scene init -----
|
||
function initScene() {
|
||
const canvas = $('p3Canvas');
|
||
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||
const rect = canvas.parentElement.getBoundingClientRect();
|
||
renderer.setSize(rect.width, rect.height, false);
|
||
renderer.setPixelRatio(window.devicePixelRatio || 1);
|
||
|
||
const scene = new THREE.Scene();
|
||
scene.background = new THREE.Color(0x0d1014);
|
||
|
||
const camera = new THREE.PerspectiveCamera(45, rect.width / rect.height, 0.05, 20);
|
||
camera.position.set(1, -1, 0.8);
|
||
camera.up.set(0, 0, 1); // Z up (coerente col base frame del robot)
|
||
camera.lookAt(0, 0, 0.3);
|
||
|
||
scene.add(new THREE.HemisphereLight(0xffffff, 0x202030, 0.6));
|
||
const dir = new THREE.DirectionalLight(0xffffff, 0.8);
|
||
dir.position.set(2, 2, 3);
|
||
scene.add(dir);
|
||
|
||
// Grid (pavimento) posizionata sotto i piedi del robot: la pelvi del G1 e'
|
||
// all'origine del frame base, i piedi sono ~0.793m sotto. Il base UCS resta
|
||
// all'origine (centro robot).
|
||
const grid = new THREE.GridHelper(2, 20, 0x444444, 0x222222);
|
||
grid.rotation.x = Math.PI / 2;
|
||
grid.position.z = -0.793;
|
||
scene.add(grid);
|
||
const baseUCS = makeUCS(0.2);
|
||
scene.add(baseUCS);
|
||
|
||
// "FRONT" label sul piano del pavimento (Z=0.01, sotto tutto) per indicare
|
||
// la direzione +X del base frame. Solo marker visivo, NON interagibile
|
||
// (la raycaster considera solo le sfere dei polsi).
|
||
const frontArrow = makeLabelSprite('▲ FRONT (base +X)', '#ffd54f');
|
||
frontArrow.position.set(0.30, 0, -0.783); // appena sopra la griglia abbassata
|
||
frontArrow.scale.set(0.18, 0.045, 1); // più largo ma sottile, "etichetta a terra"
|
||
scene.add(frontArrow);
|
||
|
||
// Modello G1 reale via URDF (best-effort: se il loader fallisce, niente
|
||
// body — la tab funziona comunque coi soli polsi/UCS).
|
||
loadG1Urdf(scene).catch((e) => {
|
||
console.warn('[pose_3d] URDF G1 non caricato:', e.message);
|
||
});
|
||
|
||
// Arms
|
||
for (const side of ['right', 'left']) {
|
||
const wrist = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
|
||
wrist.position.copy(WRIST_ZERO_POSE[side]); // posizione fallback "braccia giù"
|
||
wrist.userData.side = side;
|
||
scene.add(wrist);
|
||
state.arms[side] = wrist;
|
||
|
||
// 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([
|
||
SHOULDER[side].clone(),
|
||
wrist.position.clone(),
|
||
]);
|
||
const line = new THREE.Line(lineGeom, new THREE.LineBasicMaterial({ color: 0xaaaaaa }));
|
||
scene.add(line);
|
||
state.links[side] = line;
|
||
|
||
// Ghost (preview target), inizialmente nascosto
|
||
const ghost = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
|
||
ghost.userData.sphere.material = ghost.userData.sphere.material.clone();
|
||
ghost.userData.sphere.material.transparent = true;
|
||
ghost.userData.sphere.material.opacity = 0.4;
|
||
ghost.visible = false;
|
||
scene.add(ghost);
|
||
state.ghosts[side] = ghost;
|
||
}
|
||
|
||
const orbit = new OrbitControls(camera, renderer.domElement);
|
||
orbit.target.set(0, 0, 0.3);
|
||
orbit.update();
|
||
|
||
const transform = new TransformControls(camera, renderer.domElement);
|
||
transform.setMode('translate');
|
||
transform.size = 0.6;
|
||
// Disable orbit while dragging gizmo
|
||
transform.addEventListener('dragging-changed', (e) => { orbit.enabled = !e.value; });
|
||
// In r160 the visual helper is exposed via getHelper(); fall back to the
|
||
// transform object itself for older versions.
|
||
const transformHelper = typeof transform.getHelper === 'function'
|
||
? transform.getHelper()
|
||
: transform;
|
||
scene.add(transformHelper);
|
||
|
||
state.three = { scene, camera, renderer, orbit, transform };
|
||
|
||
// Resize handler
|
||
const onResize = () => {
|
||
const r = canvas.parentElement.getBoundingClientRect();
|
||
renderer.setSize(r.width, r.height, false);
|
||
camera.aspect = r.width / r.height;
|
||
camera.updateProjectionMatrix();
|
||
};
|
||
window.addEventListener('resize', onResize);
|
||
|
||
// Render loop
|
||
function animate() {
|
||
requestAnimationFrame(animate);
|
||
renderer.render(scene, camera);
|
||
}
|
||
animate();
|
||
}
|
||
|
||
// ----- fetch helpers -----
|
||
async function jsonFetch(url, opts) {
|
||
const r = await fetch(url, opts);
|
||
const body = await r.json().catch(() => ({}));
|
||
if (!r.ok) {
|
||
const err = new Error(body.detail || `HTTP ${r.status}`);
|
||
err.status = r.status;
|
||
throw err;
|
||
}
|
||
return body;
|
||
}
|
||
|
||
// ----- quaternion / RPY conversion (xyzw, deg) -----
|
||
function quatToRPYdeg(q) {
|
||
const [x, y, z, w] = q;
|
||
// ZYX intrinsic = RPY (roll=x, pitch=y, yaw=z)
|
||
const sinr_cosp = 2 * (w * x + y * z);
|
||
const cosr_cosp = 1 - 2 * (x * x + y * y);
|
||
const roll = Math.atan2(sinr_cosp, cosr_cosp);
|
||
const sinp = 2 * (w * y - z * x);
|
||
const pitch = Math.abs(sinp) >= 1 ? Math.sign(sinp) * Math.PI / 2 : Math.asin(sinp);
|
||
const siny_cosp = 2 * (w * z + x * y);
|
||
const cosy_cosp = 1 - 2 * (y * y + z * z);
|
||
const yaw = Math.atan2(siny_cosp, cosy_cosp);
|
||
return [roll * 180 / Math.PI, pitch * 180 / Math.PI, yaw * 180 / Math.PI];
|
||
}
|
||
|
||
function rpyDegToQuat(roll, pitch, yaw) {
|
||
const r = roll * Math.PI / 180;
|
||
const p = pitch * Math.PI / 180;
|
||
const y = yaw * Math.PI / 180;
|
||
const cr = Math.cos(r / 2), sr = Math.sin(r / 2);
|
||
const cp = Math.cos(p / 2), sp = Math.sin(p / 2);
|
||
const cy = Math.cos(y / 2), sy = Math.sin(y / 2);
|
||
// ZYX intrinsic (matches quatToRPYdeg)
|
||
const qx = sr * cp * cy - cr * sp * sy;
|
||
const qy = cr * sp * cy + sr * cp * sy;
|
||
const qz = cr * cp * sy - sr * sp * cy;
|
||
const qw = cr * cp * cy + sr * sp * sy;
|
||
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 arm = state.arms[side];
|
||
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()];
|
||
line.geometry.setFromPoints(pts);
|
||
line.geometry.attributes.position.needsUpdate = true;
|
||
}
|
||
}
|
||
|
||
function syncGhostsToTargetOrReal() {
|
||
for (const side of ['right', 'left']) {
|
||
const ghost = state.ghosts[side];
|
||
if (!ghost) continue;
|
||
if (state.target[side]) {
|
||
// ghost segue il target locale (mosso dal gizmo)
|
||
ghost.visible = true;
|
||
} else if (side === state.active_side) {
|
||
// ghost allineato al reale (preparato per drag).
|
||
// Fallback se /state e' offline: copia dall'arm visibile.
|
||
const real = state.current[side];
|
||
const arm = state.arms[side];
|
||
if (real) {
|
||
ghost.position.set(real.position[0], real.position[1], real.position[2]);
|
||
ghost.quaternion.set(real.quaternion[0], real.quaternion[1],
|
||
real.quaternion[2], real.quaternion[3]);
|
||
} else if (arm) {
|
||
ghost.position.copy(arm.position);
|
||
ghost.quaternion.copy(arm.quaternion);
|
||
}
|
||
} else {
|
||
ghost.visible = false;
|
||
}
|
||
}
|
||
}
|
||
|
||
function setInputIfNotFocused(id, value) {
|
||
const el = $(id);
|
||
if (!el) return;
|
||
if (document.activeElement === el) return; // user is typing, don't stomp
|
||
el.value = value;
|
||
}
|
||
|
||
function updateReadout() {
|
||
const side = state.active_side;
|
||
const pose = state.target[side] || state.current[side];
|
||
if (!pose) {
|
||
setInputIfNotFocused('p3X', '');
|
||
setInputIfNotFocused('p3Y', '');
|
||
setInputIfNotFocused('p3Z', '');
|
||
setInputIfNotFocused('p3R', '');
|
||
setInputIfNotFocused('p3P', '');
|
||
setInputIfNotFocused('p3W', '');
|
||
return;
|
||
}
|
||
const p = pose.position, q = pose.quaternion;
|
||
setInputIfNotFocused('p3X', (p[0] * 1000).toFixed(1));
|
||
setInputIfNotFocused('p3Y', (p[1] * 1000).toFixed(1));
|
||
setInputIfNotFocused('p3Z', (p[2] * 1000).toFixed(1));
|
||
const rpy = quatToRPYdeg(q);
|
||
setInputIfNotFocused('p3R', rpy[0].toFixed(1));
|
||
setInputIfNotFocused('p3P', rpy[1].toFixed(1));
|
||
setInputIfNotFocused('p3W', rpy[2].toFixed(1));
|
||
$('p3Status').textContent = state.target[side] ? '● modificata' : '— polso fermo';
|
||
$('p3Status').className = 'p3-status' + (state.target[side] ? ' modified' : '');
|
||
// Workspace feasibility coloring + indicator
|
||
applyFeasibilityToActiveGhost();
|
||
}
|
||
|
||
function applyFeasibilityToActiveGhost() {
|
||
const side = state.active_side;
|
||
if (!side) return;
|
||
const pose = state.target[side] || state.current[side];
|
||
if (!pose) {
|
||
// Niente posa, niente indicazione
|
||
setFeasibilityIndicator(null);
|
||
return;
|
||
}
|
||
const check = checkWorkspace(side, pose.position);
|
||
const ghost = state.ghosts[side];
|
||
if (ghost && ghost.userData.sphere) {
|
||
// verde se feasible, rosso se fuori workspace; opacita' invariata
|
||
const color = check.feasible ? 0x3ecf8e : 0xe74c3c;
|
||
ghost.userData.sphere.material.color.setHex(color);
|
||
}
|
||
setFeasibilityIndicator(check);
|
||
}
|
||
|
||
function setFeasibilityIndicator(check) {
|
||
const el = $('p3Feasibility');
|
||
if (!el) return;
|
||
if (!check) {
|
||
el.textContent = '';
|
||
el.className = 'p3-feasibility';
|
||
return;
|
||
}
|
||
const dist_mm = (check.distance_m * 1000).toFixed(0);
|
||
const max_mm = (check.max_m * 1000).toFixed(0);
|
||
if (check.feasible) {
|
||
el.textContent = `✓ raggiungibile (dist spalla ${dist_mm}/${max_mm} mm)`;
|
||
el.className = 'p3-feasibility ok';
|
||
} else {
|
||
const over_mm = (check.distance_m - check.max_m) * 1000;
|
||
el.textContent = `⚠ fuori workspace (${dist_mm}/${max_mm} mm — ${over_mm.toFixed(0)} mm oltre)`;
|
||
el.className = 'p3-feasibility error';
|
||
}
|
||
}
|
||
|
||
// ----- polling -----
|
||
let pollTimer = null;
|
||
async function pollState() {
|
||
try {
|
||
const s = await jsonFetch('/api/pose_3d/state');
|
||
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();
|
||
applyCloudParamsFromCfg(s.cloud_params);
|
||
applyCalibratedD435UCS();
|
||
// Applica TUTTI i giunti reali sul modello URDF (29 motori G1)
|
||
state.joints = s.joints || null; // snapshot per tag locator save
|
||
if (s.joints) applyJointsToUrdf(s.joints);
|
||
updateRealWrists();
|
||
syncGhostsToTargetOrReal();
|
||
updateReadout();
|
||
// Aggiorna dropdown pose salvate
|
||
refreshPoseSelect();
|
||
} catch (e) {
|
||
// soft fail: log + segna lo stato
|
||
console.warn('pose_3d /state fail:', e.message);
|
||
}
|
||
// NB: il front-tag overlay NON viene aggiornato qui. La detection ArUco
|
||
// gira solo al cambio dell'immagine (pulsante Acquisisci -> acquireClouds)
|
||
// e al toggle on del flag, non ad ogni tick del poll dello state.
|
||
}
|
||
|
||
function refreshPoseSelect() {
|
||
// 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;
|
||
}
|
||
}
|
||
|
||
// ----- selezione polso via click -----
|
||
// ----- Tag locator -----
|
||
// Workflow:
|
||
// 1) flagga "Modalità tag click" → entra in placement mode (click su URDF
|
||
// mesh piazza la crocetta, click su sfere polso disabilitato).
|
||
// 2) seleziona tag dal dropdown.
|
||
// 3) click sulla mesh URDF → crocetta arancione + gizmo XYZ agganciato per
|
||
// fine-tuning lungo gli assi pelvis.
|
||
// 4) bottone "Salva" → POST /api/pose_3d/tag_offset/save (pelvis frame).
|
||
// TODO(orientation): per ora salviamo solo CdM del tag. Estensione futura:
|
||
// catturare anche orientamento via gizmo di rotazione sulla crocetta, oppure
|
||
// con 3-click (normale + up).
|
||
const TAG_OPTIONS = {
|
||
'10:40:wrist_sx': { tag_id: 10, tag_size_mm: 40, tag_label: 'wrist_sx' },
|
||
'1:20:nuwa_dx': { tag_id: 1, tag_size_mm: 20, tag_label: 'nuwa_dx' },
|
||
};
|
||
|
||
function getActiveTagInfo() {
|
||
const v = $('p3TagSelect')?.value || '10:40:wrist_sx';
|
||
return TAG_OPTIONS[v] || TAG_OPTIONS['10:40:wrist_sx'];
|
||
}
|
||
|
||
function isTagPlacementMode() {
|
||
return $('p3TagPlaceMode')?.checked === true;
|
||
}
|
||
|
||
function makeTagMarker(sizeMm) {
|
||
// 3 segmenti XYZ centrati. Lunghezza = tag_size così la crocetta scala col tag.
|
||
const halfM = (sizeMm / 1000) / 2;
|
||
const geom = new THREE.BufferGeometry();
|
||
const verts = new Float32Array([
|
||
-halfM, 0, 0, +halfM, 0, 0,
|
||
0, -halfM, 0, 0, +halfM, 0,
|
||
0, 0, -halfM, 0, 0, +halfM,
|
||
]);
|
||
geom.setAttribute('position', new THREE.BufferAttribute(verts, 3));
|
||
// Arancione brillante, renderOrder alto + depthTest off → sempre visibile.
|
||
const mat = new THREE.LineBasicMaterial({ color: 0xff8800, depthTest: false });
|
||
const seg = new THREE.LineSegments(geom, mat);
|
||
seg.renderOrder = 999;
|
||
return seg;
|
||
}
|
||
|
||
function attachGizmoToTagMarker() {
|
||
if (!state.tag_locator.marker) return;
|
||
const { transform } = state.three;
|
||
transform.setMode('translate');
|
||
transform.attach(state.tag_locator.marker);
|
||
}
|
||
|
||
function placeTagMarker(positionWorld, sizeMm) {
|
||
if (state.tag_locator.marker) {
|
||
if (state.three.transform.object === state.tag_locator.marker) {
|
||
state.three.transform.detach();
|
||
}
|
||
state.three.scene.remove(state.tag_locator.marker);
|
||
state.tag_locator.marker.geometry.dispose();
|
||
state.tag_locator.marker.material.dispose();
|
||
}
|
||
const m = makeTagMarker(sizeMm);
|
||
m.position.copy(positionWorld);
|
||
state.three.scene.add(m);
|
||
state.tag_locator.marker = m;
|
||
// Snapshot giunti al momento del click (resta valido anche dopo gizmo-drag).
|
||
state.tag_locator.joints_at_place = state.joints ? { ...state.joints } : null;
|
||
updateTagMarkerReadout();
|
||
const sv = $('p3TagSaveBtn'); if (sv) sv.disabled = false;
|
||
const cv = $('p3TagClearBtn'); if (cv) cv.disabled = false;
|
||
const st = $('p3TagStatus');
|
||
if (st) {
|
||
const qInfo = state.tag_locator.joints_at_place
|
||
? ` · q snapshot=${Object.keys(state.tag_locator.joints_at_place).length} giunti`
|
||
: ' · ⚠ G1 offline → niente q snapshot';
|
||
st.textContent = 'crocetta piazzata' + qInfo + ' · gizmo per fine-tune · Salva quando ok';
|
||
st.style.color = '';
|
||
}
|
||
attachGizmoToTagMarker();
|
||
}
|
||
|
||
function updateTagMarkerReadout() {
|
||
if (!state.tag_locator.marker) return;
|
||
const p = state.tag_locator.marker.position;
|
||
state.tag_locator.position_m = [p.x, p.y, p.z];
|
||
$('p3TagPosX').value = (p.x * 1000).toFixed(1);
|
||
$('p3TagPosY').value = (p.y * 1000).toFixed(1);
|
||
$('p3TagPosZ').value = (p.z * 1000).toFixed(1);
|
||
}
|
||
|
||
function tryPlaceTagFromClick(raycaster) {
|
||
if (!state.robot) {
|
||
$('p3TagStatus').textContent = 'URDF non ancora caricata, riprova tra un attimo';
|
||
$('p3TagStatus').style.color = 'var(--danger)';
|
||
return false;
|
||
}
|
||
const hits = raycaster.intersectObject(state.robot, true);
|
||
const meshHit = hits.find(h => h.object && h.object.isMesh && h.object.visible);
|
||
if (!meshHit) {
|
||
$('p3TagStatus').textContent = 'click fuori dalla mesh del robot · clicca direttamente sul modello';
|
||
$('p3TagStatus').style.color = 'var(--dim)';
|
||
return false;
|
||
}
|
||
const info = getActiveTagInfo();
|
||
placeTagMarker(meshHit.point, info.tag_size_mm);
|
||
return true;
|
||
}
|
||
|
||
function clearTagMarker() {
|
||
if (!state.tag_locator.marker) return;
|
||
if (state.three.transform.object === state.tag_locator.marker) {
|
||
state.three.transform.detach();
|
||
}
|
||
state.three.scene.remove(state.tag_locator.marker);
|
||
state.tag_locator.marker.geometry.dispose();
|
||
state.tag_locator.marker.material.dispose();
|
||
state.tag_locator.marker = null;
|
||
state.tag_locator.position_m = null;
|
||
state.tag_locator.joints_at_place = null;
|
||
for (const id of ['p3TagPosX','p3TagPosY','p3TagPosZ']) { const el = $(id); if (el) el.value = ''; }
|
||
$('p3TagSaveBtn').disabled = true;
|
||
$('p3TagClearBtn').disabled = true;
|
||
$('p3TagStatus').textContent = isTagPlacementMode()
|
||
? 'click sul modello 3D per piazzare la crocetta'
|
||
: 'attiva la modalità tag click sopra, poi clicca sul modello 3D';
|
||
$('p3TagStatus').style.color = '';
|
||
}
|
||
|
||
async function onTagSave() {
|
||
if (!state.tag_locator.position_m) return;
|
||
const info = getActiveTagInfo();
|
||
const sv = $('p3TagSaveBtn');
|
||
const st = $('p3TagStatus');
|
||
if (sv) sv.disabled = true;
|
||
if (st) { st.textContent = 'salvataggio…'; st.style.color = 'var(--dim)'; }
|
||
try {
|
||
const res = await jsonFetch('/api/pose_3d/tag_offset/save', {
|
||
method: 'POST',
|
||
headers: { 'Content-Type': 'application/json' },
|
||
body: JSON.stringify({
|
||
tag_id: info.tag_id,
|
||
tag_size_mm: info.tag_size_mm,
|
||
tag_label: info.tag_label,
|
||
position_pelvis_m: state.tag_locator.position_m,
|
||
joints_at_save_rad: state.tag_locator.joints_at_place,
|
||
}),
|
||
});
|
||
if (st) {
|
||
const arch = res.archived_from ? ' · prev → archive' : '';
|
||
const qBit = state.tag_locator.joints_at_place
|
||
? ` · q=${Object.keys(state.tag_locator.joints_at_place).length} giunti`
|
||
: ' · ⚠ niente q (G1 offline)';
|
||
st.textContent = `salvato${qBit}${arch}`;
|
||
st.style.color = 'var(--ok)';
|
||
}
|
||
} catch (e) {
|
||
if (st) { st.textContent = `errore: ${e.message}`; st.style.color = 'var(--danger)'; }
|
||
} finally {
|
||
if (sv) sv.disabled = false;
|
||
}
|
||
}
|
||
|
||
function setupTagLocator() {
|
||
// Toggle modalita' tag click → aggiorna stato + status testo, e aggancia
|
||
// / sgancia il gizmo dalla crocetta esistente.
|
||
$('p3TagPlaceMode')?.addEventListener('change', () => {
|
||
if (isTagPlacementMode()) {
|
||
$('p3TagStatus').textContent = state.tag_locator.marker
|
||
? 'modalità attiva · click per spostare la crocetta · gizmo per fine-tuning'
|
||
: 'modalità attiva · click sul modello 3D per piazzare';
|
||
$('p3TagStatus').style.color = 'var(--ok)';
|
||
if (state.tag_locator.marker) attachGizmoToTagMarker();
|
||
} else {
|
||
$('p3TagStatus').textContent = 'modalità disattivata · attiva sopra per piazzare';
|
||
$('p3TagStatus').style.color = '';
|
||
// se il gizmo era sulla crocetta, lascialo libero (l'utente magari vuole
|
||
// tornare a editare il polso senza dover ri-cliccare).
|
||
if (state.three.transform.object === state.tag_locator.marker) {
|
||
state.three.transform.detach();
|
||
}
|
||
}
|
||
});
|
||
// Cambio tag → ricreo crocetta con nuova dimensione, mantenendo link.
|
||
$('p3TagSelect')?.addEventListener('change', () => {
|
||
if (state.tag_locator.marker && state.tag_locator.position_m) {
|
||
const info = getActiveTagInfo();
|
||
const pos = new THREE.Vector3(...state.tag_locator.position_m);
|
||
placeTagMarker(pos, info.tag_size_mm, state.tag_locator.link_object);
|
||
}
|
||
});
|
||
$('p3TagSaveBtn')?.addEventListener('click', onTagSave);
|
||
$('p3TagClearBtn')?.addEventListener('click', clearTagMarker);
|
||
// Listener gizmo: aggiorna lettura X/Y/Z quando trascini la crocetta lungo gli assi.
|
||
state.three.transform.addEventListener('change', () => {
|
||
if (state.three.transform.object === state.tag_locator.marker) {
|
||
updateTagMarkerReadout();
|
||
}
|
||
});
|
||
}
|
||
|
||
function setupSelection() {
|
||
const { renderer, camera, transform } = state.three;
|
||
const raycaster = new THREE.Raycaster();
|
||
const mouse = new THREE.Vector2();
|
||
const canvas = renderer.domElement;
|
||
|
||
canvas.addEventListener('pointerdown', (e) => {
|
||
// Se sta interagendo con il gizmo, non fare nulla
|
||
if (transform.dragging) return;
|
||
const rect = canvas.getBoundingClientRect();
|
||
mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1;
|
||
mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1;
|
||
raycaster.setFromCamera(mouse, camera);
|
||
// Tag-click mode ATTIVA → click su URDF mesh piazza la crocetta.
|
||
// In questa modalita' i click sulle sfere polso restano disabilitati per
|
||
// non confondere il workflow (l'utente sa che e' in placement mode).
|
||
if (isTagPlacementMode()) {
|
||
tryPlaceTagFromClick(raycaster);
|
||
return;
|
||
}
|
||
// Modalita' normale: click sulle sfere polso → attiva il gizmo sul ghost
|
||
const candidates = [state.arms.right.userData.sphere,
|
||
state.arms.left.userData.sphere];
|
||
const hits = raycaster.intersectObjects(candidates, false);
|
||
console.log('[pose_3d] click on canvas, raycast hits:', hits.length);
|
||
if (hits.length > 0) {
|
||
const sphere = hits[0].object;
|
||
const side = sphere.parent.userData.side;
|
||
console.log('[pose_3d] activating side:', side);
|
||
activateSide(side);
|
||
}
|
||
});
|
||
}
|
||
|
||
function activateSide(side) {
|
||
state.active_side = side;
|
||
// sync radio buttons
|
||
for (const r of document.querySelectorAll('input[name=p3Side]')) {
|
||
r.checked = (r.value === side);
|
||
}
|
||
// Attacca gizmo al ghost (lo creiamo se non esiste, posizionandolo sul reale)
|
||
ensureGhostInit(side);
|
||
const { transform } = state.three;
|
||
transform.attach(state.ghosts[side]);
|
||
console.log('[pose_3d] gizmo attached to ghost', side, 'at', state.ghosts[side].position);
|
||
state.ghosts[side].visible = true;
|
||
updateReadout();
|
||
}
|
||
|
||
function ensureGhostInit(side) {
|
||
// Se non c'è target locale, allinea ghost al reale (preparato per drag).
|
||
// Se /state non e' disponibile (G1 offline), fallback alla posizione
|
||
// dell'arm visibile (che usa SHOULDER come default).
|
||
if (state.target[side]) return;
|
||
const real = state.current[side];
|
||
const arm = state.arms[side];
|
||
if (real) {
|
||
state.ghosts[side].position.set(real.position[0], real.position[1], real.position[2]);
|
||
state.ghosts[side].quaternion.set(real.quaternion[0], real.quaternion[1],
|
||
real.quaternion[2], real.quaternion[3]);
|
||
} else if (arm) {
|
||
state.ghosts[side].position.copy(arm.position);
|
||
state.ghosts[side].quaternion.copy(arm.quaternion);
|
||
}
|
||
}
|
||
|
||
// ----- gizmo mode toggle -----
|
||
function bindGizmoControls() {
|
||
for (const r of document.querySelectorAll('input[name=p3Gizmo]')) {
|
||
r.addEventListener('change', () => {
|
||
state.gizmo_mode = r.value;
|
||
state.three.transform.setMode(r.value);
|
||
});
|
||
}
|
||
// tasti t/r
|
||
window.addEventListener('keydown', (e) => {
|
||
if (!$('tab-pose-3d')?.classList.contains('active')) return;
|
||
if (e.target.tagName === 'INPUT' || e.target.tagName === 'TEXTAREA') return;
|
||
if (e.key === 't') { setGizmoMode('translate'); }
|
||
if (e.key === 'r') { setGizmoMode('rotate'); }
|
||
});
|
||
}
|
||
|
||
function setGizmoMode(mode) {
|
||
state.gizmo_mode = mode;
|
||
state.three.transform.setMode(mode);
|
||
for (const r of document.querySelectorAll('input[name=p3Gizmo]')) {
|
||
r.checked = (r.value === mode);
|
||
}
|
||
}
|
||
|
||
// ----- target tracking durante drag gizmo -----
|
||
function bindGizmoEvents() {
|
||
const { transform } = state.three;
|
||
transform.addEventListener('change', () => {
|
||
// Guardia: questo listener gestisce solo le pose dei polsi (ghost).
|
||
// Quando il gizmo e' attaccato alla crocetta tag locator, c'e' un
|
||
// listener separato in setupTagLocator() che fa l'update giusto.
|
||
const attached = transform.object;
|
||
if (!attached) return;
|
||
if (attached !== state.ghosts.right && attached !== state.ghosts.left) return;
|
||
const side = state.active_side;
|
||
const ghost = state.ghosts[side];
|
||
if (!ghost) return;
|
||
state.target[side] = {
|
||
position: [ghost.position.x, ghost.position.y, ghost.position.z],
|
||
quaternion: [ghost.quaternion.x, ghost.quaternion.y,
|
||
ghost.quaternion.z, ghost.quaternion.w],
|
||
};
|
||
updateReadout();
|
||
});
|
||
}
|
||
|
||
function readReadoutInputs() {
|
||
const num = (id, dflt) => {
|
||
const v = parseFloat($(id).value);
|
||
return Number.isFinite(v) ? v : dflt;
|
||
};
|
||
const side = state.active_side;
|
||
if (!side) return;
|
||
// fallback values: current pose if any, else SHOULDER position via arm
|
||
const fallback = state.current[side] || (state.arms[side] ? {
|
||
position: [state.arms[side].position.x, state.arms[side].position.y, state.arms[side].position.z],
|
||
quaternion: [0, 0, 0, 1],
|
||
} : { position: [0, 0, 0], quaternion: [0, 0, 0, 1] });
|
||
const x_mm = num('p3X', fallback.position[0] * 1000);
|
||
const y_mm = num('p3Y', fallback.position[1] * 1000);
|
||
const z_mm = num('p3Z', fallback.position[2] * 1000);
|
||
const r_deg = num('p3R', 0);
|
||
const p_deg = num('p3P', 0);
|
||
const w_deg = num('p3W', 0);
|
||
const position = [x_mm / 1000, y_mm / 1000, z_mm / 1000];
|
||
const quaternion = rpyDegToQuat(r_deg, p_deg, w_deg);
|
||
state.target[side] = { position, quaternion };
|
||
// Aggiorna ghost in scena
|
||
const ghost = state.ghosts[side];
|
||
if (ghost) {
|
||
ghost.position.set(position[0], position[1], position[2]);
|
||
ghost.quaternion.set(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
|
||
ghost.visible = true;
|
||
}
|
||
// Status
|
||
$('p3Status').textContent = '● modificata (da input)';
|
||
$('p3Status').className = 'p3-status modified';
|
||
}
|
||
|
||
function bindReadoutInputs() {
|
||
for (const id of ['p3X', 'p3Y', 'p3Z', 'p3R', 'p3P', 'p3W']) {
|
||
const el = $(id);
|
||
if (!el) continue;
|
||
el.addEventListener('change', readReadoutInputs); // fire on blur / Enter
|
||
}
|
||
}
|
||
|
||
// ----- side change listener -----
|
||
function bindSideControls() {
|
||
for (const r of document.querySelectorAll('input[name=p3Side]')) {
|
||
r.addEventListener('change', () => {
|
||
state.active_side = r.value;
|
||
updateReadout();
|
||
});
|
||
}
|
||
}
|
||
|
||
// ----- Editor mode actions -----
|
||
async function onApplyClick() {
|
||
if (state.mode !== 'editor') return; // in Jog non c'è Apply
|
||
const side = state.active_side;
|
||
const tgt = state.target[side];
|
||
if (!tgt) return;
|
||
try {
|
||
await jsonFetch('/api/pose_3d/apply', {
|
||
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
||
body: JSON.stringify({
|
||
side, position: tgt.position, quaternion: tgt.quaternion,
|
||
dry_run: true, precise: state.precise,
|
||
}),
|
||
});
|
||
// commit locale: il target diventa il nuovo current (simulato)
|
||
state.current[side] = { position: tgt.position, quaternion: tgt.quaternion };
|
||
state.target[side] = null;
|
||
applyIKToUrdf(side, tgt.position, tgt.quaternion);
|
||
updateRealWrists();
|
||
syncGhostsToTargetOrReal();
|
||
updateReadout();
|
||
$('p3Status').textContent = '✓ Editor: target committato (simulato)';
|
||
$('p3Status').className = 'p3-status ok';
|
||
} catch (e) {
|
||
showError(e.message);
|
||
}
|
||
}
|
||
|
||
function onCancelClick() {
|
||
const side = state.active_side;
|
||
state.target[side] = null;
|
||
syncGhostsToTargetOrReal();
|
||
updateReadout();
|
||
}
|
||
|
||
async function onSyncWithRobot() {
|
||
// Force-refresh dallo stato del robot e ricopia la posa attuale nel target
|
||
// (cosi' gli input riflettono il robot, e l'utente puo' partire da li').
|
||
clearError();
|
||
try {
|
||
await pollState(); // refresha state.current dal backend
|
||
} catch (e) {
|
||
// soft fail; pollState gia' logga internamente
|
||
}
|
||
const side = state.active_side;
|
||
if (!side) return;
|
||
state.target[side] = null;
|
||
// Forza il ghost (e quindi gli input) ad allinearsi sul reale (con fallback arm)
|
||
syncGhostsToTargetOrReal();
|
||
updateReadout();
|
||
$('p3Status').textContent = '✓ Sincronizzato con posa robot';
|
||
$('p3Status').className = 'p3-status ok';
|
||
}
|
||
|
||
function showError(msg) {
|
||
$('p3ErrorBox').style.display = 'block';
|
||
$('p3ErrorText').textContent = msg;
|
||
$('p3Status').textContent = '✗ errore';
|
||
$('p3Status').className = 'p3-status error';
|
||
}
|
||
|
||
function clearError() {
|
||
$('p3ErrorBox').style.display = 'none';
|
||
$('p3ErrorText').textContent = '';
|
||
}
|
||
|
||
function bindEditorButtons() {
|
||
$('p3ApplyBtn').addEventListener('click', onApplyClick);
|
||
$('p3CancelBtn').addEventListener('click', onCancelClick);
|
||
$('p3SyncBtn').addEventListener('click', onSyncWithRobot);
|
||
}
|
||
|
||
// ----- mode toggle -----
|
||
function bindModeControls() {
|
||
for (const r of document.querySelectorAll('input[name=p3Mode]')) {
|
||
r.addEventListener('change', () => {
|
||
state.mode = r.value;
|
||
const badge = $('p3ModeBadge');
|
||
if (state.mode === 'jog') {
|
||
badge.textContent = '🔴 JOG (live)';
|
||
badge.classList.add('jog');
|
||
$('p3EditorActions').style.display = 'none';
|
||
} else {
|
||
badge.textContent = 'Editor';
|
||
badge.classList.remove('jog');
|
||
$('p3EditorActions').style.display = '';
|
||
}
|
||
});
|
||
}
|
||
}
|
||
|
||
function bindPreciseControl() {
|
||
$('p3Precise').addEventListener('change', (e) => {
|
||
state.precise = e.target.checked;
|
||
});
|
||
}
|
||
|
||
// ----- jog auto-apply al mouseup gizmo -----
|
||
function bindJogAutoApply() {
|
||
const { transform } = state.three;
|
||
transform.addEventListener('dragging-changed', async (e) => {
|
||
if (e.value) return; // inizio drag, niente
|
||
if (state.mode !== 'jog') return;
|
||
const side = state.active_side;
|
||
const tgt = state.target[side];
|
||
if (!tgt) return;
|
||
const cur = state.current[side];
|
||
if (!cur) return;
|
||
const dx = tgt.position[0] - cur.position[0];
|
||
const dy = tgt.position[1] - cur.position[1];
|
||
const dz = tgt.position[2] - cur.position[2];
|
||
const delta = Math.sqrt(dx*dx + dy*dy + dz*dz);
|
||
if (delta > state.live_max_step_m) {
|
||
const ok = confirm(
|
||
`Movimento richiesto ${(delta*1000).toFixed(0)} mm > soglia ${(state.live_max_step_m*1000).toFixed(0)} mm.\n` +
|
||
`Procedere comunque?`,
|
||
);
|
||
if (!ok) return;
|
||
}
|
||
clearError();
|
||
try {
|
||
const res = await jsonFetch('/api/pose_3d/apply', {
|
||
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
||
body: JSON.stringify({
|
||
side, position: tgt.position, quaternion: tgt.quaternion,
|
||
dry_run: false, precise: state.precise,
|
||
}),
|
||
});
|
||
const r = res.result || {};
|
||
if (r.reached_target === false || r.stalled === true) {
|
||
showError(`Move non completato: residual ${(r.position_error_mm||0).toFixed(1)} mm. ` +
|
||
`Sblocca braccia e riprova, oppure disattiva Precise.`);
|
||
} else {
|
||
$('p3Status').textContent = `✓ Jog: raggiunto (residual ${(r.position_error_mm||0).toFixed(1)} mm)`;
|
||
$('p3Status').className = 'p3-status ok';
|
||
applyIKToUrdf(side, tgt.position, tgt.quaternion);
|
||
state.target[side] = null;
|
||
syncGhostsToTargetOrReal();
|
||
}
|
||
} catch (e) {
|
||
showError(e.message);
|
||
}
|
||
});
|
||
}
|
||
|
||
// ----- 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 pose = state.target[side] || state.current[side];
|
||
if (!pose) { alert(`Nessuna posa ${ui.label} da salvare`); return; }
|
||
if (state.saved_poses[name]) {
|
||
if (!confirm(`Posa "${name}" esiste già — sovrascrivere?`)) return;
|
||
}
|
||
try {
|
||
await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
|
||
method: 'PUT', headers: { 'Content-Type': 'application/json' },
|
||
body: JSON.stringify({
|
||
side, position: pose.position, quaternion: pose.quaternion,
|
||
}),
|
||
});
|
||
$(ui.name).value = '';
|
||
await pollState(); // refresh dropdown
|
||
$(ui.select).value = name;
|
||
} catch (e) {
|
||
showError(`Salvataggio fallito: ${e.message}`);
|
||
}
|
||
}
|
||
|
||
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}" (${ui.label}) SUL ROBOT REALE?`)) return;
|
||
}
|
||
const dry_run = (state.mode === 'editor');
|
||
clearError();
|
||
try {
|
||
const res = await jsonFetch(
|
||
`/api/pose_3d/poses/${encodeURIComponent(name)}/go`,
|
||
{
|
||
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
||
body: JSON.stringify({ dry_run, precise: state.precise }),
|
||
},
|
||
);
|
||
const r = res.result || {};
|
||
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}" (${ui.label}) applicata` +
|
||
(dry_run ? ' (simulato)' : '');
|
||
$('p3Status').className = 'p3-status ok';
|
||
const pose = state.saved_poses[name];
|
||
if (pose) applyIKToUrdf(pose.side, pose.position, pose.quaternion);
|
||
}
|
||
} catch (e) {
|
||
showError(e.message);
|
||
}
|
||
}
|
||
|
||
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)}`, {
|
||
method: 'DELETE',
|
||
});
|
||
await pollState();
|
||
} catch (e) {
|
||
showError(e.message);
|
||
}
|
||
}
|
||
|
||
function bindPoseButtons() {
|
||
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() {
|
||
clearError();
|
||
try {
|
||
await jsonFetch('/api/pose_3d/unlock', { method: 'POST' });
|
||
$('p3Status').textContent = '✓ Braccia sbloccate';
|
||
$('p3Status').className = 'p3-status ok';
|
||
} catch (e) {
|
||
showError(`Unlock fallito: ${e.message}`);
|
||
}
|
||
}
|
||
|
||
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 (camKey === 'nuwa') {
|
||
applyNuwaCloudPlacement();
|
||
} else {
|
||
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).
|
||
// Matrix4 locale (rispetto al parent getCloudParent('d435')) usata dal cloud
|
||
// D435i: hand-eye (M·S) oppure URDF (T·R·S). Riusata per piazzare la Nuwa
|
||
// allineata nello stesso frame del D435i.
|
||
function d435LocalMatrix() {
|
||
const sv = readCloudScaleVec('d435');
|
||
const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]);
|
||
if (useD435Handeye()) {
|
||
const M = getD435HandeyeMatrix();
|
||
if (M) return M.clone().multiply(S);
|
||
}
|
||
const t = getCloudTransform('d435');
|
||
const T = new THREE.Matrix4().makeTranslation(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');
|
||
const R = new THREE.Matrix4().makeRotationFromEuler(e);
|
||
return T.multiply(R).multiply(S);
|
||
}
|
||
|
||
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]);
|
||
}
|
||
|
||
// Piazza la nuvola Nuwa: in modalità allineata la renderizza nel frame del
|
||
// D435i (parent + pipeline D435i · matrice di allineamento); altrimenti usa il
|
||
// piazzamento URDF normale.
|
||
function applyNuwaCloudPlacement() {
|
||
const pts = state.cloud_points.nuwa;
|
||
if (!pts) return;
|
||
const aligned = state.align.enabled && state.align.matrix;
|
||
if (aligned) {
|
||
const parent = getCloudParent('d435');
|
||
if (parent) {
|
||
if (pts.parent !== parent) parent.add(pts);
|
||
pts.matrixAutoUpdate = false;
|
||
pts.matrix.copy(d435LocalMatrix()).multiply(state.align.matrix);
|
||
pts.matrixWorldNeedsUpdate = true;
|
||
return;
|
||
}
|
||
}
|
||
if (state.align.enabled && !state.align.matrix) {
|
||
const s = $('p3AlignStatus');
|
||
if (s) { s.textContent = 'nessuna calibrazione: premi Allinea'; s.style.color = 'var(--dim)'; }
|
||
}
|
||
// fallback URDF normale
|
||
const parent = getCloudParent('nuwa');
|
||
if (parent && pts.parent !== parent) parent.add(pts);
|
||
pts.matrixAutoUpdate = true;
|
||
applyCloudTransform('nuwa');
|
||
}
|
||
|
||
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]];
|
||
}
|
||
|
||
// Snapshot dei parametri UI Cloud cross-camera. Mantenuto separato da
|
||
// acquireClouds() perche' usato anche dal save persistente (memorizza la
|
||
// configurazione attiva all'ultima Acquisisci).
|
||
function readCloudParamsFromUI() {
|
||
return {
|
||
step: parseInt($('p3CloudStep').value, 10) || 8,
|
||
max_depth_mm: parseInt($('p3CloudMaxDepth').value, 10) || 2000,
|
||
nuwa_size_mm: parseFloat($('p3NuwaCloudSize').value) || 5,
|
||
d435_size_mm: parseFloat($('p3D435CloudSize').value) || 5,
|
||
d435_scale_xy: parseFloat($('p3D435ScaleXY').value) || 1.0,
|
||
flip_nuwa: [
|
||
$('p3FlipNuwaX').checked,
|
||
$('p3FlipNuwaY').checked,
|
||
$('p3FlipNuwaZ').checked,
|
||
],
|
||
flip_d435: [
|
||
$('p3FlipD435X').checked,
|
||
$('p3FlipD435Y').checked,
|
||
$('p3FlipD435Z').checked,
|
||
],
|
||
d435_use_handeye: $('p3D435UseHandeye').checked,
|
||
};
|
||
}
|
||
|
||
async function saveCloudParamsToConfig() {
|
||
const params = readCloudParamsFromUI();
|
||
try {
|
||
const r = await fetch('/api/config', {
|
||
method: 'PUT',
|
||
headers: { 'Content-Type': 'application/json' },
|
||
body: JSON.stringify({ pose_3d: { cloud_params: params } }),
|
||
});
|
||
if (!r.ok) throw new Error(`HTTP ${r.status}`);
|
||
} catch (e) {
|
||
console.warn('[pose_3d] save cloud_params failed:', e?.message || e);
|
||
}
|
||
}
|
||
|
||
// Popola UI Cloud cross-camera dai params salvati. Eseguito una volta sola
|
||
// (state.cloud_params_populated) per non sovrascrivere edit utente in corso.
|
||
function applyCloudParamsFromCfg(cloud_params) {
|
||
if (state.cloud_params_populated) return;
|
||
if (!cloud_params || Object.keys(cloud_params).length === 0) return;
|
||
const set = (id, val) => { const el = $(id); if (el && val !== undefined && val !== null) el.value = String(val); };
|
||
const setCb = (id, val) => { const el = $(id); if (el && typeof val === 'boolean') el.checked = val; };
|
||
set('p3CloudStep', cloud_params.step);
|
||
set('p3CloudMaxDepth', cloud_params.max_depth_mm);
|
||
set('p3NuwaCloudSize', cloud_params.nuwa_size_mm);
|
||
set('p3D435CloudSize', cloud_params.d435_size_mm);
|
||
set('p3D435ScaleXY', cloud_params.d435_scale_xy);
|
||
const fn = cloud_params.flip_nuwa || [];
|
||
setCb('p3FlipNuwaX', fn[0]); setCb('p3FlipNuwaY', fn[1]); setCb('p3FlipNuwaZ', fn[2]);
|
||
const fd = cloud_params.flip_d435 || [];
|
||
setCb('p3FlipD435X', fd[0]); setCb('p3FlipD435Y', fd[1]); setCb('p3FlipD435Z', fd[2]);
|
||
setCb('p3D435UseHandeye', cloud_params.d435_use_handeye);
|
||
state.cloud_params_populated = true;
|
||
}
|
||
|
||
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)';
|
||
// Memorizza i parametri usati in questa acquisizione (best-effort,
|
||
// non blocca il rendering del cloud se il save fallisce).
|
||
saveCloudParamsToConfig();
|
||
// Nuova immagine acquisita: se l'overlay tag e' attivo ri-esegui la
|
||
// detection una volta sola, sul frame appena acquisito.
|
||
if ($('p3ShowFrontTags')?.checked) pollTags();
|
||
} 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 = '';
|
||
}
|
||
|
||
// ----- Front-camera tag finder -----
|
||
// Overlay sulla scena 3D dei marker ArUco visibili dal D435i frontale.
|
||
// Per ogni tag: LineLoop dei 4 corner + sprite label con id, edge_mm
|
||
// (misurato dal depth sensor, non da pnp) e distanza camera→centro.
|
||
// Stesso parent + transform del d435 cloud, cosi' l'overlay segue la
|
||
// camera in modalita' URDF link e in modalita' hand-eye.
|
||
|
||
const FRONT_TAG_COLOR = 0x4dd0e1;
|
||
const NUWA_TAG_COLOR = 0xff3b30; // rosso: tag visti dalla camera Nuwa (polso)
|
||
const TAG_COLORS = { nuwa: NUWA_TAG_COLOR, d435: FRONT_TAG_COLOR };
|
||
const FRONT_TAG_LABEL_COLOR = '#4dd0e1';
|
||
|
||
// Label compatta auto-sized: canvas largo quanto il testo, font piccolo,
|
||
// sprite proporzionato al canvas. Evita il clipping di makeLabelSprite che
|
||
// usa canvas fisso 128x64 con font 36px (i nostri "id=20 · 101.4mm · ..."
|
||
// uscivano dal box).
|
||
function makeCompactLabel(text, color = FRONT_TAG_LABEL_COLOR) {
|
||
const fontPx = 24;
|
||
const padX = 8, padY = 4;
|
||
// Misura testo su un canvas temporaneo
|
||
const probe = document.createElement('canvas').getContext('2d');
|
||
probe.font = `${fontPx}px sans-serif`;
|
||
const textW = Math.ceil(probe.measureText(text).width);
|
||
const w = textW + padX * 2;
|
||
const h = fontPx + padY * 2;
|
||
const canvas = document.createElement('canvas');
|
||
canvas.width = w; canvas.height = h;
|
||
const ctx = canvas.getContext('2d');
|
||
ctx.fillStyle = 'rgba(0,0,0,0.65)';
|
||
ctx.fillRect(0, 0, w, h);
|
||
ctx.fillStyle = color;
|
||
ctx.font = `${fontPx}px sans-serif`;
|
||
ctx.textAlign = 'center';
|
||
ctx.textBaseline = 'middle';
|
||
ctx.fillText(text, w / 2, h / 2);
|
||
const tex = new THREE.CanvasTexture(canvas);
|
||
const mat = new THREE.SpriteMaterial({ map: tex, depthTest: false });
|
||
const sprite = new THREE.Sprite(mat);
|
||
// Scala in metri: ~4 cm di altezza, larghezza proporzionale all'aspect
|
||
const targetH = 0.04;
|
||
sprite.scale.set(targetH * (w / h), targetH, 1);
|
||
return sprite;
|
||
}
|
||
|
||
function applyCloudLikeTransform(obj, camKey) {
|
||
const sv = readCloudScaleVec(camKey);
|
||
if (camKey === 'd435' && useD435Handeye()) {
|
||
const M = getD435HandeyeMatrix();
|
||
if (M) {
|
||
obj.matrixAutoUpdate = false;
|
||
const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]);
|
||
obj.matrix.copy(M).multiply(S);
|
||
obj.matrixWorldNeedsUpdate = true;
|
||
return;
|
||
}
|
||
}
|
||
obj.matrixAutoUpdate = true;
|
||
const t = getCloudTransform(camKey);
|
||
obj.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',
|
||
);
|
||
obj.quaternion.setFromEuler(e);
|
||
obj.scale.set(sv[0], sv[1], sv[2]);
|
||
}
|
||
|
||
function clearTagsGroup(camKey) {
|
||
const grp = state.tags[camKey].group;
|
||
if (!grp) return;
|
||
if (grp.parent) grp.parent.remove(grp);
|
||
grp.traverse(o => {
|
||
if (o.geometry) o.geometry.dispose();
|
||
if (o.material) {
|
||
if (Array.isArray(o.material)) o.material.forEach(m => m.dispose());
|
||
else o.material.dispose();
|
||
}
|
||
});
|
||
state.tags[camKey].group = null;
|
||
}
|
||
|
||
function addOneTag(grp, t, color) {
|
||
const c = t.corners_3d_optical_m;
|
||
if (!Array.isArray(c) || c.length !== 4) return;
|
||
if (c.some(p => !p || p.some(v => !Number.isFinite(v)))) return;
|
||
const v = new Float32Array(5 * 3);
|
||
for (let i = 0; i < 4; i++) { v[3*i]=c[i][0]; v[3*i+1]=c[i][1]; v[3*i+2]=c[i][2]; }
|
||
v[12] = c[0][0]; v[13] = c[0][1]; v[14] = c[0][2];
|
||
const geom = new THREE.BufferGeometry();
|
||
geom.setAttribute('position', new THREE.BufferAttribute(v, 3));
|
||
grp.add(new THREE.Line(geom, new THREE.LineBasicMaterial({ color })));
|
||
const edge_s = Number.isFinite(t.edge_mm) ? `${t.edge_mm.toFixed(1)}mm` : '?';
|
||
const dist_s = Number.isFinite(t.distance_m) ? `${(t.distance_m * 1000).toFixed(0)}mm` : '?';
|
||
const sprite = makeCompactLabel(`id=${t.id} · ${edge_s} · d=${dist_s}`);
|
||
const ctr = t.center_3d_optical_m;
|
||
if (ctr && ctr.every(Number.isFinite)) sprite.position.set(ctr[0], ctr[1] - 0.04, ctr[2]);
|
||
grp.add(sprite);
|
||
}
|
||
|
||
function renderTags(camKey, tags, color) {
|
||
clearTagsGroup(camKey);
|
||
if (!tags || tags.length === 0) return;
|
||
const parent = getCloudParent(camKey);
|
||
if (!parent) { console.warn(`[pose_3d] tags: parent ${camKey} non disponibile`); return; }
|
||
const grp = new THREE.Group();
|
||
grp.name = `${camKey}_tags_group`;
|
||
applyCloudLikeTransform(grp, camKey);
|
||
parent.add(grp);
|
||
state.tags[camKey].group = grp;
|
||
for (const t of tags) addOneTag(grp, t, color);
|
||
}
|
||
|
||
async function pollTags() {
|
||
const ids = ($('p3FrontTagIds')?.value || '0-49').trim();
|
||
const status = $('p3FrontTagStatus');
|
||
try {
|
||
const r = await jsonFetch(`/api/pose_3d/tags?ids=${encodeURIComponent(ids)}`);
|
||
const cams = r.cameras || {};
|
||
const parts = [];
|
||
for (const camKey of ['nuwa', 'd435']) {
|
||
const c = cams[camKey] || {};
|
||
renderTags(camKey, c.tags || [], TAG_COLORS[camKey]);
|
||
if (c.error) parts.push(`${camKey}: err`);
|
||
else parts.push(`${camKey}: ${(c.tags || []).length}`);
|
||
}
|
||
if (status) status.textContent = parts.join(' · ');
|
||
} catch (e) {
|
||
if (status) status.textContent = `errore: ${e?.message || e}`;
|
||
}
|
||
}
|
||
|
||
function bindFrontTagFinder() {
|
||
const cb = $('p3ShowFrontTags');
|
||
if (!cb) return;
|
||
cb.addEventListener('change', () => {
|
||
if (cb.checked) {
|
||
pollTags();
|
||
} else {
|
||
clearTagsGroup('nuwa');
|
||
clearTagsGroup('d435');
|
||
const s = $('p3FrontTagStatus');
|
||
if (s) s.textContent = '—';
|
||
}
|
||
});
|
||
}
|
||
|
||
|
||
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);
|
||
}
|
||
|
||
// Tab rail a destra: 3 voci (POSE/CAMERAS/MISC) che mostrano un gruppo
|
||
// alla volta. Solo presentazione: i listener interni alle sezioni nascoste
|
||
// restano agganciati, quindi switchare tab non perde stato.
|
||
function bindTabRail() {
|
||
const tabs = document.querySelectorAll('#tab-pose-3d .p3-tab');
|
||
const groups = document.querySelectorAll('#tab-pose-3d .p3-group');
|
||
if (!tabs.length || !groups.length) return;
|
||
for (const tab of tabs) {
|
||
tab.addEventListener('click', () => {
|
||
const target = tab.dataset.target;
|
||
for (const t of tabs) t.classList.toggle('active', t === tab);
|
||
for (const g of groups) g.hidden = (g.dataset.group !== target);
|
||
});
|
||
}
|
||
}
|
||
|
||
// ----- init triggered on first activation -----
|
||
let inited = false;
|
||
function ensureInit() {
|
||
if (inited) return;
|
||
if (!$('p3Canvas')) return;
|
||
inited = true;
|
||
initScene();
|
||
setupSelection();
|
||
setupTagLocator();
|
||
bindSideControls();
|
||
bindGizmoControls();
|
||
bindGizmoEvents();
|
||
bindEditorButtons();
|
||
bindModeControls();
|
||
bindPreciseControl();
|
||
bindJogAutoApply();
|
||
bindPoseButtons();
|
||
bindUnlockButton();
|
||
bindNuwaControls();
|
||
bindD435Controls();
|
||
bindJointUCSControls();
|
||
bindCloudControls();
|
||
bindReadoutInputs();
|
||
bindTabRail();
|
||
bindFrontTagFinder();
|
||
pollState();
|
||
pollTimer = setInterval(pollState, POLL_MS);
|
||
}
|
||
|
||
// Observe quando la tab diventa visibile per inizializzare
|
||
const tabObserver = new MutationObserver(() => {
|
||
if ($('tab-pose-3d')?.classList.contains('active')) ensureInit();
|
||
});
|
||
if ($('tab-pose-3d')) {
|
||
tabObserver.observe($('tab-pose-3d'), { attributes: true, attributeFilter: ['class'] });
|
||
// Se è già attivo all'avvio
|
||
if ($('tab-pose-3d').classList.contains('active')) ensureInit();
|
||
}
|