From 9d30073d575e86a6611402992c08e7803cb107d5 Mon Sep 17 00:00:00 2001
From: Adriano Dal Pastro <adrianodalpastro@tielogic.com>
Date: Thu, 21 May 2026 18:06:38 +0200
Subject: [PATCH] fix(pose_3d): zero scena al centro robot (pelvi), griglia
 sotto i piedi a Z=-0.793

---
 web/pose_3d.js | 24 ++++++++++++++----------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/web/pose_3d.js b/web/pose_3d.js
index 5b3b157..60ae7c6 100644
--- a/web/pose_3d.js
+++ b/web/pose_3d.js
@@ -100,13 +100,15 @@ const TORSO_POS = new THREE.Vector3(0, 0, 0.40);
 // 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, 1.05),
-  left:  new THREE.Vector3(0.0,  0.18, 1.05),
+  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.45),  // braccia giu', polso ~45cm sopra il suolo
-  left:  new THREE.Vector3(0.0,  0.18, 0.45),
+  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)
 
@@ -140,9 +142,8 @@ async function loadG1Urdf(scene) {
     loader.load(
       '/static/vendor/g1/g1.urdf',
       (robot) => {
-        // G1 URDF uses Z-up (Unitree convention). Pelvi e' all'origine,
-        // alziamo il robot di ~0.793m cosi' i piedi toccano la griglia Z=0.
-        robot.position.z = 0.793;
+        // 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);
@@ -292,9 +293,12 @@ function initScene() {
   dir.position.set(2, 2, 3);
   scene.add(dir);
 
-  // Grid + base UCS
+  // 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;  // piano XY (z=0)
+  grid.rotation.x = Math.PI / 2;
+  grid.position.z = -0.793;
   scene.add(grid);
   const baseUCS = makeUCS(0.2);
   scene.add(baseUCS);
@@ -303,7 +307,7 @@ function initScene() {
   // 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.01);
+  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);