feat(pose_3d): IK 2-bone su URDF dopo Apply/Jog/GoPose (polso modello segue il target)
This commit is contained in:
Adriano Dal Pastro committed
1 parent
7f60238be9
commit
38848ad53c
1 file changed
+125
-1
+125
-1
@@ -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);
|
||||
|
||||
Reference in new issue
Block a user