feat(pose_3d): workspace check client-side (ghost verde/rosso) + SHOULDER da URDF

web/pose_3d.js

@@ -76,12 +76,32 @@ const state = {
   live_max_step_m: 0.15,
 };
 
+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);
+// Posizione approssimata delle spalle del G1 in base frame (zero-joint pose
+// dell'URDF). Usato come fallback visivo quando /state e' offline e per il
+// workspace check (raggiungibilita').
 const SHOULDER = {
-  right: new THREE.Vector3(0, -0.20, 0.60),
-  left:  new THREE.Vector3(0,  0.20, 0.60),
+  right: new THREE.Vector3(0.0, -0.18, 1.05),
+  left:  new THREE.Vector3(0.0,  0.18, 1.05),
 };
+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
@@ -346,6 +366,47 @@ function updateReadout() {
   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 -----