// 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 = '';
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 (xxmax)xmax=x;
if (yymax)ymax=y;
if (zzmax)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();
}