diff --git a/web/pose_3d.js b/web/pose_3d.js index 98930f8..5b3b157 100644 --- a/web/pose_3d.js +++ b/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);