From d5bb9cebd395bf0cd673b41d5a2b731ffc34d33e Mon Sep 17 00:00:00 2001
From: Adriano Dal Pastro <adrianodalpastro@tielogic.com>
Date: Thu, 21 May 2026 17:21:09 +0200
Subject: [PATCH] feat(pose_3d): workspace check client-side (ghost
 verde/rosso) + SHOULDER da URDF

---
 web/index.html  |  1 +
 web/pose_3d.css |  9 +++++++
 web/pose_3d.js  | 65 +++++++++++++++++++++++++++++++++++++++++++++++--
 3 files changed, 73 insertions(+), 2 deletions(-)

diff --git a/web/index.html b/web/index.html
index 8426b49..8a9e58a 100644
--- a/web/index.html
+++ b/web/index.html
@@ -507,6 +507,7 @@
           <label>P <input id="p3P" type="number" step="1" /> °</label>
           <label>W <input id="p3W" type="number" step="1" /> °</label>
         </div>
+        <div id="p3Feasibility" class="p3-feasibility"></div>
         <div class="p3-status" id="p3Status">— polso fermo</div>
         <div class="p3-row" id="p3EditorActions">
           <button id="p3ApplyBtn">Applica (Editor)</button>
diff --git a/web/pose_3d.css b/web/pose_3d.css
index 4fef9ef..4ab2948 100644
--- a/web/pose_3d.css
+++ b/web/pose_3d.css
@@ -105,6 +105,15 @@
 #tab-pose-3d .p3-status.ok { color: #3ecf8e; }
 #tab-pose-3d .p3-status.error { color: #e74c3c; }
 
+#tab-pose-3d .p3-feasibility {
+  font-size: 12px;
+  margin-bottom: 6px;
+  font-family: monospace;
+  min-height: 1em;
+}
+#tab-pose-3d .p3-feasibility.ok { color: #3ecf8e; }
+#tab-pose-3d .p3-feasibility.error { color: #e74c3c; }
+
 #tab-pose-3d button {
   background: #2a3038;
   border: 1px solid #3a4048;
diff --git a/web/pose_3d.js b/web/pose_3d.js
index c4ef614..70527d4 100644
--- a/web/pose_3d.js
+++ b/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 -----