feat(pose_3d): IK 2-bone su URDF dopo Apply/Jog/GoPose (polso modello segue il target)

web/pose_3d.js

@@ -74,6 +74,8 @@ const state = {
   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
 };
 
 function checkWorkspace(side, position_m) {
@@ -142,7 +144,10 @@ async function loadG1Urdf(scene) {
         // alziamo il robot di ~0.793m cosi' i piedi toccano la griglia Z=0.
         robot.position.z = 0.793;
         scene.add(robot);
-        console.log('[pose_3d] G1 URDF caricato:', robot.name || 'g1');
+        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);
         resolve(robot);
       },
       undefined,
@@ -151,6 +156,121 @@ async function loadG1Urdf(scene) {
   });
 }
 
+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,
+  };
+}
+
+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');
@@ -620,6 +740,7 @@ async function onApplyClick() {
     // 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();
@@ -737,6 +858,7 @@ function bindJogAutoApply() {
       } 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();
       }
@@ -798,6 +920,8 @@ async function onGoPose() {
       $('p3Status').textContent = `✓ Posa "${name}" 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);