997 lines
35 KiB
JavaScript
997 lines
35 KiB
JavaScript
// Tab "Pose 3D" — viewport Three.js, gizmo translate/rotate, drag dei polsi.
|
|
// Vanilla JS, ES modules.
|
|
|
|
import * as THREE from 'three';
|
|
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
|
|
import { TransformControls } from 'three/addons/controls/TransformControls.js';
|
|
import { STLLoader } from 'three/addons/loaders/STLLoader.js';
|
|
import URDFLoader from 'three/addons/loaders/URDFLoader.js';
|
|
|
|
const $ = (id) => document.getElementById(id);
|
|
const POLL_MS = 1000;
|
|
|
|
// ----- helpers geometria -----
|
|
function makeUCS(size = 0.1) {
|
|
// 3 linee X(rosso) Y(verde) Z(blu) dalla origine
|
|
const g = new THREE.Group();
|
|
const mat = (color) => new THREE.LineBasicMaterial({ color, linewidth: 2 });
|
|
const make = (dir, color) => {
|
|
const geom = new THREE.BufferGeometry().setFromPoints([
|
|
new THREE.Vector3(0, 0, 0),
|
|
new THREE.Vector3(dir.x * size, dir.y * size, dir.z * size),
|
|
]);
|
|
return new THREE.Line(geom, mat(color));
|
|
};
|
|
g.add(make(new THREE.Vector3(1, 0, 0), 0xe74c3c));
|
|
g.add(make(new THREE.Vector3(0, 1, 0), 0x3ecf8e));
|
|
g.add(make(new THREE.Vector3(0, 0, 1), 0x4da3ff));
|
|
return g;
|
|
}
|
|
|
|
function makeLabelSprite(text, color = '#ffffff') {
|
|
const canvas = document.createElement('canvas');
|
|
canvas.width = 128;
|
|
canvas.height = 64;
|
|
const ctx = canvas.getContext('2d');
|
|
ctx.fillStyle = 'rgba(0, 0, 0, 0.6)';
|
|
ctx.fillRect(0, 0, canvas.width, canvas.height);
|
|
ctx.fillStyle = color;
|
|
ctx.font = 'bold 36px sans-serif';
|
|
ctx.textAlign = 'center';
|
|
ctx.textBaseline = 'middle';
|
|
ctx.fillText(text, canvas.width / 2, canvas.height / 2);
|
|
const texture = new THREE.CanvasTexture(canvas);
|
|
const material = new THREE.SpriteMaterial({ map: texture, depthTest: false });
|
|
const sprite = new THREE.Sprite(material);
|
|
sprite.scale.set(0.10, 0.05, 1); // ~10cm wide x 5cm tall in scene units
|
|
return sprite;
|
|
}
|
|
|
|
function makeWristMarker(color) {
|
|
const group = new THREE.Group();
|
|
const sphere = new THREE.Mesh(
|
|
new THREE.SphereGeometry(0.04, 16, 12),
|
|
new THREE.MeshStandardMaterial({ color, metalness: 0.2, roughness: 0.5 }),
|
|
);
|
|
group.add(sphere);
|
|
group.add(makeUCS(0.07));
|
|
group.userData.sphere = sphere;
|
|
group.userData.sphere.userData.kind = 'wrist';
|
|
return group;
|
|
}
|
|
|
|
// ----- state -----
|
|
const state = {
|
|
three: null, // {scene, camera, renderer, orbit, transform}
|
|
arms: { right: null, left: null }, // wrist groups (real)
|
|
ghosts: { right: null, left: null }, // wrist groups (target preview)
|
|
links: { right: null, left: null }, // line shoulder→wrist
|
|
current: { right: null, left: null }, // {position, quaternion}
|
|
target: { right: null, left: null }, // null = nessuna modifica locale
|
|
active_side: 'right',
|
|
mode: 'editor',
|
|
gizmo_mode: 'translate',
|
|
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) {
|
|
// 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);
|
|
// Posizioni di riferimento del G1 in base frame (feet on floor convention,
|
|
// 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).
|
|
const SHOULDER = {
|
|
right: new THREE.Vector3(0.0, -0.18, 1.05),
|
|
left: new THREE.Vector3(0.0, 0.18, 1.05),
|
|
};
|
|
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),
|
|
};
|
|
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
|
|
// /static/vendor/g1/. Le mesh STL referenziate nel URDF (path relativi tipo
|
|
// "meshes/pelvis.STL") vengono risolti contro lo stesso prefisso.
|
|
async function loadG1Urdf(scene) {
|
|
const loader = new URDFLoader();
|
|
loader.loadMeshCb = function (path, manager, onComplete) {
|
|
// URDFLoader risolve gia' i path relativi rispetto alla URL della URDF,
|
|
// quindi `path` arriva come "/static/vendor/g1/meshes/X.STL". Non
|
|
// ri-prefissare.
|
|
const url = path;
|
|
const stl = new STLLoader(manager);
|
|
stl.load(
|
|
url,
|
|
(geometry) => {
|
|
const mat = new THREE.MeshStandardMaterial({
|
|
color: 0x9099a3, metalness: 0.4, roughness: 0.6,
|
|
transparent: true, opacity: 0.85,
|
|
});
|
|
const mesh = new THREE.Mesh(geometry, mat);
|
|
onComplete(mesh);
|
|
},
|
|
undefined,
|
|
(err) => onComplete(null, err),
|
|
);
|
|
};
|
|
return new Promise((resolve, reject) => {
|
|
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;
|
|
scene.add(robot);
|
|
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,
|
|
(err) => reject(err),
|
|
);
|
|
});
|
|
}
|
|
|
|
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');
|
|
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
|
const rect = canvas.parentElement.getBoundingClientRect();
|
|
renderer.setSize(rect.width, rect.height, false);
|
|
renderer.setPixelRatio(window.devicePixelRatio || 1);
|
|
|
|
const scene = new THREE.Scene();
|
|
scene.background = new THREE.Color(0x0d1014);
|
|
|
|
const camera = new THREE.PerspectiveCamera(45, rect.width / rect.height, 0.05, 20);
|
|
camera.position.set(1, -1, 0.8);
|
|
camera.up.set(0, 0, 1); // Z up (coerente col base frame del robot)
|
|
camera.lookAt(0, 0, 0.3);
|
|
|
|
scene.add(new THREE.HemisphereLight(0xffffff, 0x202030, 0.6));
|
|
const dir = new THREE.DirectionalLight(0xffffff, 0.8);
|
|
dir.position.set(2, 2, 3);
|
|
scene.add(dir);
|
|
|
|
// Grid + base UCS
|
|
const grid = new THREE.GridHelper(2, 20, 0x444444, 0x222222);
|
|
grid.rotation.x = Math.PI / 2; // piano XY (z=0)
|
|
scene.add(grid);
|
|
const baseUCS = makeUCS(0.2);
|
|
scene.add(baseUCS);
|
|
|
|
// "FRONT" label sul piano del pavimento (Z=0.01, sotto tutto) per indicare
|
|
// 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.scale.set(0.18, 0.045, 1); // più largo ma sottile, "etichetta a terra"
|
|
scene.add(frontArrow);
|
|
|
|
// Modello G1 reale via URDF (best-effort: se il loader fallisce, niente
|
|
// body — la tab funziona comunque coi soli polsi/UCS).
|
|
loadG1Urdf(scene).catch((e) => {
|
|
console.warn('[pose_3d] URDF G1 non caricato:', e.message);
|
|
});
|
|
|
|
// Arms
|
|
for (const side of ['right', 'left']) {
|
|
const wrist = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
|
|
wrist.position.copy(WRIST_ZERO_POSE[side]); // posizione fallback "braccia giù"
|
|
wrist.userData.side = side;
|
|
scene.add(wrist);
|
|
state.arms[side] = wrist;
|
|
|
|
// Etichetta DX / SX sopra la sfera
|
|
const label = makeLabelSprite(side === 'right' ? 'DX' : 'SX',
|
|
side === 'right' ? '#4da3ff' : '#ff5e7e');
|
|
label.position.set(0, 0, 0.08); // 8cm sopra la sfera (locale al gruppo)
|
|
wrist.add(label);
|
|
|
|
// Linea shoulder → wrist
|
|
const lineGeom = new THREE.BufferGeometry().setFromPoints([
|
|
SHOULDER[side].clone(),
|
|
wrist.position.clone(),
|
|
]);
|
|
const line = new THREE.Line(lineGeom, new THREE.LineBasicMaterial({ color: 0xaaaaaa }));
|
|
scene.add(line);
|
|
state.links[side] = line;
|
|
|
|
// Ghost (preview target), inizialmente nascosto
|
|
const ghost = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
|
|
ghost.userData.sphere.material = ghost.userData.sphere.material.clone();
|
|
ghost.userData.sphere.material.transparent = true;
|
|
ghost.userData.sphere.material.opacity = 0.4;
|
|
ghost.visible = false;
|
|
scene.add(ghost);
|
|
state.ghosts[side] = ghost;
|
|
}
|
|
|
|
const orbit = new OrbitControls(camera, renderer.domElement);
|
|
orbit.target.set(0, 0, 0.3);
|
|
orbit.update();
|
|
|
|
const transform = new TransformControls(camera, renderer.domElement);
|
|
transform.setMode('translate');
|
|
transform.size = 0.6;
|
|
// Disable orbit while dragging gizmo
|
|
transform.addEventListener('dragging-changed', (e) => { orbit.enabled = !e.value; });
|
|
// In r160 the visual helper is exposed via getHelper(); fall back to the
|
|
// transform object itself for older versions.
|
|
const transformHelper = typeof transform.getHelper === 'function'
|
|
? transform.getHelper()
|
|
: transform;
|
|
scene.add(transformHelper);
|
|
|
|
state.three = { scene, camera, renderer, orbit, transform };
|
|
|
|
// Resize handler
|
|
const onResize = () => {
|
|
const r = canvas.parentElement.getBoundingClientRect();
|
|
renderer.setSize(r.width, r.height, false);
|
|
camera.aspect = r.width / r.height;
|
|
camera.updateProjectionMatrix();
|
|
};
|
|
window.addEventListener('resize', onResize);
|
|
|
|
// Render loop
|
|
function animate() {
|
|
requestAnimationFrame(animate);
|
|
renderer.render(scene, camera);
|
|
}
|
|
animate();
|
|
}
|
|
|
|
// ----- fetch helpers -----
|
|
async function jsonFetch(url, opts) {
|
|
const r = await fetch(url, opts);
|
|
const body = await r.json().catch(() => ({}));
|
|
if (!r.ok) {
|
|
const err = new Error(body.detail || `HTTP ${r.status}`);
|
|
err.status = r.status;
|
|
throw err;
|
|
}
|
|
return body;
|
|
}
|
|
|
|
// ----- quaternion / RPY conversion (xyzw, deg) -----
|
|
function quatToRPYdeg(q) {
|
|
const [x, y, z, w] = q;
|
|
// ZYX intrinsic = RPY (roll=x, pitch=y, yaw=z)
|
|
const sinr_cosp = 2 * (w * x + y * z);
|
|
const cosr_cosp = 1 - 2 * (x * x + y * y);
|
|
const roll = Math.atan2(sinr_cosp, cosr_cosp);
|
|
const sinp = 2 * (w * y - z * x);
|
|
const pitch = Math.abs(sinp) >= 1 ? Math.sign(sinp) * Math.PI / 2 : Math.asin(sinp);
|
|
const siny_cosp = 2 * (w * z + x * y);
|
|
const cosy_cosp = 1 - 2 * (y * y + z * z);
|
|
const yaw = Math.atan2(siny_cosp, cosy_cosp);
|
|
return [roll * 180 / Math.PI, pitch * 180 / Math.PI, yaw * 180 / Math.PI];
|
|
}
|
|
|
|
function rpyDegToQuat(roll, pitch, yaw) {
|
|
const r = roll * Math.PI / 180;
|
|
const p = pitch * Math.PI / 180;
|
|
const y = yaw * Math.PI / 180;
|
|
const cr = Math.cos(r / 2), sr = Math.sin(r / 2);
|
|
const cp = Math.cos(p / 2), sp = Math.sin(p / 2);
|
|
const cy = Math.cos(y / 2), sy = Math.sin(y / 2);
|
|
// ZYX intrinsic (matches quatToRPYdeg)
|
|
const qx = sr * cp * cy - cr * sp * sy;
|
|
const qy = cr * sp * cy + sr * cp * sy;
|
|
const qz = cr * cp * sy - sr * sp * cy;
|
|
const qw = cr * cp * cy + sr * sp * sy;
|
|
return [qx, qy, qz, qw];
|
|
}
|
|
|
|
// ----- state apply to scene -----
|
|
function updateRealWrists() {
|
|
for (const side of ['right', 'left']) {
|
|
const pose = state.current[side];
|
|
if (!pose) continue;
|
|
const arm = state.arms[side];
|
|
arm.position.set(pose.position[0], pose.position[1], pose.position[2]);
|
|
arm.quaternion.set(pose.quaternion[0], pose.quaternion[1],
|
|
pose.quaternion[2], pose.quaternion[3]);
|
|
// aggiorna line shoulder→wrist
|
|
const line = state.links[side];
|
|
const pts = [SHOULDER[side].clone(), arm.position.clone()];
|
|
line.geometry.setFromPoints(pts);
|
|
line.geometry.attributes.position.needsUpdate = true;
|
|
}
|
|
}
|
|
|
|
function syncGhostsToTargetOrReal() {
|
|
for (const side of ['right', 'left']) {
|
|
const ghost = state.ghosts[side];
|
|
if (!ghost) continue;
|
|
if (state.target[side]) {
|
|
// ghost segue il target locale (mosso dal gizmo)
|
|
ghost.visible = true;
|
|
} else if (side === state.active_side) {
|
|
// ghost allineato al reale (preparato per drag).
|
|
// Fallback se /state e' offline: copia dall'arm visibile.
|
|
const real = state.current[side];
|
|
const arm = state.arms[side];
|
|
if (real) {
|
|
ghost.position.set(real.position[0], real.position[1], real.position[2]);
|
|
ghost.quaternion.set(real.quaternion[0], real.quaternion[1],
|
|
real.quaternion[2], real.quaternion[3]);
|
|
} else if (arm) {
|
|
ghost.position.copy(arm.position);
|
|
ghost.quaternion.copy(arm.quaternion);
|
|
}
|
|
} else {
|
|
ghost.visible = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
function setInputIfNotFocused(id, value) {
|
|
const el = $(id);
|
|
if (!el) return;
|
|
if (document.activeElement === el) return; // user is typing, don't stomp
|
|
el.value = value;
|
|
}
|
|
|
|
function updateReadout() {
|
|
const side = state.active_side;
|
|
const pose = state.target[side] || state.current[side];
|
|
if (!pose) {
|
|
setInputIfNotFocused('p3X', '');
|
|
setInputIfNotFocused('p3Y', '');
|
|
setInputIfNotFocused('p3Z', '');
|
|
setInputIfNotFocused('p3R', '');
|
|
setInputIfNotFocused('p3P', '');
|
|
setInputIfNotFocused('p3W', '');
|
|
return;
|
|
}
|
|
const p = pose.position, q = pose.quaternion;
|
|
setInputIfNotFocused('p3X', (p[0] * 1000).toFixed(1));
|
|
setInputIfNotFocused('p3Y', (p[1] * 1000).toFixed(1));
|
|
setInputIfNotFocused('p3Z', (p[2] * 1000).toFixed(1));
|
|
const rpy = quatToRPYdeg(q);
|
|
setInputIfNotFocused('p3R', rpy[0].toFixed(1));
|
|
setInputIfNotFocused('p3P', rpy[1].toFixed(1));
|
|
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 -----
|
|
let pollTimer = null;
|
|
async function pollState() {
|
|
try {
|
|
const s = await jsonFetch('/api/pose_3d/state');
|
|
state.current.right = s.wrist.right;
|
|
state.current.left = s.wrist.left;
|
|
state.saved_poses = s.poses || {};
|
|
state.live_max_step_m = s.live_max_step_m || 0.15;
|
|
updateRealWrists();
|
|
syncGhostsToTargetOrReal();
|
|
updateReadout();
|
|
// Aggiorna dropdown pose salvate
|
|
refreshPoseSelect();
|
|
} catch (e) {
|
|
// soft fail: log + segna lo stato
|
|
console.warn('pose_3d /state fail:', e.message);
|
|
}
|
|
}
|
|
|
|
function refreshPoseSelect() {
|
|
const sel = $('p3PoseSelect');
|
|
if (!sel) return;
|
|
const cur = sel.value;
|
|
sel.innerHTML = '<option value="">(seleziona)</option>';
|
|
for (const name of Object.keys(state.saved_poses).sort()) {
|
|
const opt = document.createElement('option');
|
|
opt.value = name; opt.textContent = name;
|
|
sel.appendChild(opt);
|
|
}
|
|
sel.value = cur;
|
|
}
|
|
|
|
// ----- selezione polso via click -----
|
|
function setupSelection() {
|
|
const { renderer, camera, transform } = state.three;
|
|
const raycaster = new THREE.Raycaster();
|
|
const mouse = new THREE.Vector2();
|
|
const canvas = renderer.domElement;
|
|
|
|
canvas.addEventListener('pointerdown', (e) => {
|
|
// Se sta interagendo con il gizmo, non fare nulla
|
|
if (transform.dragging) return;
|
|
const rect = canvas.getBoundingClientRect();
|
|
mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1;
|
|
mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1;
|
|
raycaster.setFromCamera(mouse, camera);
|
|
// Verifica intersezione con sphere destra o sinistra
|
|
const candidates = [state.arms.right.userData.sphere,
|
|
state.arms.left.userData.sphere];
|
|
const hits = raycaster.intersectObjects(candidates, false);
|
|
console.log('[pose_3d] click on canvas, raycast hits:', hits.length);
|
|
if (hits.length > 0) {
|
|
const sphere = hits[0].object;
|
|
const side = sphere.parent.userData.side;
|
|
console.log('[pose_3d] activating side:', side);
|
|
activateSide(side);
|
|
}
|
|
});
|
|
}
|
|
|
|
function activateSide(side) {
|
|
state.active_side = side;
|
|
// sync radio buttons
|
|
for (const r of document.querySelectorAll('input[name=p3Side]')) {
|
|
r.checked = (r.value === side);
|
|
}
|
|
// Attacca gizmo al ghost (lo creiamo se non esiste, posizionandolo sul reale)
|
|
ensureGhostInit(side);
|
|
const { transform } = state.three;
|
|
transform.attach(state.ghosts[side]);
|
|
console.log('[pose_3d] gizmo attached to ghost', side, 'at', state.ghosts[side].position);
|
|
state.ghosts[side].visible = true;
|
|
updateReadout();
|
|
}
|
|
|
|
function ensureGhostInit(side) {
|
|
// Se non c'è target locale, allinea ghost al reale (preparato per drag).
|
|
// Se /state non e' disponibile (G1 offline), fallback alla posizione
|
|
// dell'arm visibile (che usa SHOULDER come default).
|
|
if (state.target[side]) return;
|
|
const real = state.current[side];
|
|
const arm = state.arms[side];
|
|
if (real) {
|
|
state.ghosts[side].position.set(real.position[0], real.position[1], real.position[2]);
|
|
state.ghosts[side].quaternion.set(real.quaternion[0], real.quaternion[1],
|
|
real.quaternion[2], real.quaternion[3]);
|
|
} else if (arm) {
|
|
state.ghosts[side].position.copy(arm.position);
|
|
state.ghosts[side].quaternion.copy(arm.quaternion);
|
|
}
|
|
}
|
|
|
|
// ----- gizmo mode toggle -----
|
|
function bindGizmoControls() {
|
|
for (const r of document.querySelectorAll('input[name=p3Gizmo]')) {
|
|
r.addEventListener('change', () => {
|
|
state.gizmo_mode = r.value;
|
|
state.three.transform.setMode(r.value);
|
|
});
|
|
}
|
|
// tasti t/r
|
|
window.addEventListener('keydown', (e) => {
|
|
if (!$('tab-pose-3d')?.classList.contains('active')) return;
|
|
if (e.target.tagName === 'INPUT' || e.target.tagName === 'TEXTAREA') return;
|
|
if (e.key === 't') { setGizmoMode('translate'); }
|
|
if (e.key === 'r') { setGizmoMode('rotate'); }
|
|
});
|
|
}
|
|
|
|
function setGizmoMode(mode) {
|
|
state.gizmo_mode = mode;
|
|
state.three.transform.setMode(mode);
|
|
for (const r of document.querySelectorAll('input[name=p3Gizmo]')) {
|
|
r.checked = (r.value === mode);
|
|
}
|
|
}
|
|
|
|
// ----- target tracking durante drag gizmo -----
|
|
function bindGizmoEvents() {
|
|
const { transform } = state.three;
|
|
transform.addEventListener('change', () => {
|
|
const side = state.active_side;
|
|
const ghost = state.ghosts[side];
|
|
if (!ghost) return;
|
|
state.target[side] = {
|
|
position: [ghost.position.x, ghost.position.y, ghost.position.z],
|
|
quaternion: [ghost.quaternion.x, ghost.quaternion.y,
|
|
ghost.quaternion.z, ghost.quaternion.w],
|
|
};
|
|
updateReadout();
|
|
});
|
|
}
|
|
|
|
function readReadoutInputs() {
|
|
const num = (id, dflt) => {
|
|
const v = parseFloat($(id).value);
|
|
return Number.isFinite(v) ? v : dflt;
|
|
};
|
|
const side = state.active_side;
|
|
if (!side) return;
|
|
// fallback values: current pose if any, else SHOULDER position via arm
|
|
const fallback = state.current[side] || (state.arms[side] ? {
|
|
position: [state.arms[side].position.x, state.arms[side].position.y, state.arms[side].position.z],
|
|
quaternion: [0, 0, 0, 1],
|
|
} : { position: [0, 0, 0], quaternion: [0, 0, 0, 1] });
|
|
const x_mm = num('p3X', fallback.position[0] * 1000);
|
|
const y_mm = num('p3Y', fallback.position[1] * 1000);
|
|
const z_mm = num('p3Z', fallback.position[2] * 1000);
|
|
const r_deg = num('p3R', 0);
|
|
const p_deg = num('p3P', 0);
|
|
const w_deg = num('p3W', 0);
|
|
const position = [x_mm / 1000, y_mm / 1000, z_mm / 1000];
|
|
const quaternion = rpyDegToQuat(r_deg, p_deg, w_deg);
|
|
state.target[side] = { position, quaternion };
|
|
// Aggiorna ghost in scena
|
|
const ghost = state.ghosts[side];
|
|
if (ghost) {
|
|
ghost.position.set(position[0], position[1], position[2]);
|
|
ghost.quaternion.set(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
|
|
ghost.visible = true;
|
|
}
|
|
// Status
|
|
$('p3Status').textContent = '● modificata (da input)';
|
|
$('p3Status').className = 'p3-status modified';
|
|
}
|
|
|
|
function bindReadoutInputs() {
|
|
for (const id of ['p3X', 'p3Y', 'p3Z', 'p3R', 'p3P', 'p3W']) {
|
|
const el = $(id);
|
|
if (!el) continue;
|
|
el.addEventListener('change', readReadoutInputs); // fire on blur / Enter
|
|
}
|
|
}
|
|
|
|
// ----- side change listener -----
|
|
function bindSideControls() {
|
|
for (const r of document.querySelectorAll('input[name=p3Side]')) {
|
|
r.addEventListener('change', () => {
|
|
state.active_side = r.value;
|
|
updateReadout();
|
|
});
|
|
}
|
|
}
|
|
|
|
// ----- Editor mode actions -----
|
|
async function onApplyClick() {
|
|
if (state.mode !== 'editor') return; // in Jog non c'è Apply
|
|
const side = state.active_side;
|
|
const tgt = state.target[side];
|
|
if (!tgt) return;
|
|
try {
|
|
await jsonFetch('/api/pose_3d/apply', {
|
|
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
|
body: JSON.stringify({
|
|
side, position: tgt.position, quaternion: tgt.quaternion,
|
|
dry_run: true, precise: state.precise,
|
|
}),
|
|
});
|
|
// 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();
|
|
$('p3Status').textContent = '✓ Editor: target committato (simulato)';
|
|
$('p3Status').className = 'p3-status ok';
|
|
} catch (e) {
|
|
showError(e.message);
|
|
}
|
|
}
|
|
|
|
function onCancelClick() {
|
|
const side = state.active_side;
|
|
state.target[side] = null;
|
|
syncGhostsToTargetOrReal();
|
|
updateReadout();
|
|
}
|
|
|
|
async function onSyncWithRobot() {
|
|
// Force-refresh dallo stato del robot e ricopia la posa attuale nel target
|
|
// (cosi' gli input riflettono il robot, e l'utente puo' partire da li').
|
|
clearError();
|
|
try {
|
|
await pollState(); // refresha state.current dal backend
|
|
} catch (e) {
|
|
// soft fail; pollState gia' logga internamente
|
|
}
|
|
const side = state.active_side;
|
|
if (!side) return;
|
|
state.target[side] = null;
|
|
// Forza il ghost (e quindi gli input) ad allinearsi sul reale (con fallback arm)
|
|
syncGhostsToTargetOrReal();
|
|
updateReadout();
|
|
$('p3Status').textContent = '✓ Sincronizzato con posa robot';
|
|
$('p3Status').className = 'p3-status ok';
|
|
}
|
|
|
|
function showError(msg) {
|
|
$('p3ErrorBox').style.display = 'block';
|
|
$('p3ErrorText').textContent = msg;
|
|
$('p3Status').textContent = '✗ errore';
|
|
$('p3Status').className = 'p3-status error';
|
|
}
|
|
|
|
function clearError() {
|
|
$('p3ErrorBox').style.display = 'none';
|
|
$('p3ErrorText').textContent = '';
|
|
}
|
|
|
|
function bindEditorButtons() {
|
|
$('p3ApplyBtn').addEventListener('click', onApplyClick);
|
|
$('p3CancelBtn').addEventListener('click', onCancelClick);
|
|
$('p3SyncBtn').addEventListener('click', onSyncWithRobot);
|
|
}
|
|
|
|
// ----- mode toggle -----
|
|
function bindModeControls() {
|
|
for (const r of document.querySelectorAll('input[name=p3Mode]')) {
|
|
r.addEventListener('change', () => {
|
|
state.mode = r.value;
|
|
const badge = $('p3ModeBadge');
|
|
if (state.mode === 'jog') {
|
|
badge.textContent = '🔴 JOG (live)';
|
|
badge.classList.add('jog');
|
|
$('p3EditorActions').style.display = 'none';
|
|
} else {
|
|
badge.textContent = 'Editor';
|
|
badge.classList.remove('jog');
|
|
$('p3EditorActions').style.display = '';
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|
|
function bindPreciseControl() {
|
|
$('p3Precise').addEventListener('change', (e) => {
|
|
state.precise = e.target.checked;
|
|
});
|
|
}
|
|
|
|
// ----- jog auto-apply al mouseup gizmo -----
|
|
function bindJogAutoApply() {
|
|
const { transform } = state.three;
|
|
transform.addEventListener('dragging-changed', async (e) => {
|
|
if (e.value) return; // inizio drag, niente
|
|
if (state.mode !== 'jog') return;
|
|
const side = state.active_side;
|
|
const tgt = state.target[side];
|
|
if (!tgt) return;
|
|
const cur = state.current[side];
|
|
if (!cur) return;
|
|
const dx = tgt.position[0] - cur.position[0];
|
|
const dy = tgt.position[1] - cur.position[1];
|
|
const dz = tgt.position[2] - cur.position[2];
|
|
const delta = Math.sqrt(dx*dx + dy*dy + dz*dz);
|
|
if (delta > state.live_max_step_m) {
|
|
const ok = confirm(
|
|
`Movimento richiesto ${(delta*1000).toFixed(0)} mm > soglia ${(state.live_max_step_m*1000).toFixed(0)} mm.\n` +
|
|
`Procedere comunque?`,
|
|
);
|
|
if (!ok) return;
|
|
}
|
|
clearError();
|
|
try {
|
|
const res = await jsonFetch('/api/pose_3d/apply', {
|
|
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
|
body: JSON.stringify({
|
|
side, position: tgt.position, quaternion: tgt.quaternion,
|
|
dry_run: false, precise: state.precise,
|
|
}),
|
|
});
|
|
const r = res.result || {};
|
|
if (r.reached_target === false || r.stalled === true) {
|
|
showError(`Move non completato: residual ${(r.position_error_mm||0).toFixed(1)} mm. ` +
|
|
`Sblocca braccia e riprova, oppure disattiva Precise.`);
|
|
} 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();
|
|
}
|
|
} catch (e) {
|
|
showError(e.message);
|
|
}
|
|
});
|
|
}
|
|
|
|
// ----- named poses -----
|
|
async function onSavePose() {
|
|
const name = ($('p3PoseName').value || '').trim();
|
|
if (!name) { alert('Inserisci un nome'); return; }
|
|
if (!/^[A-Za-z0-9_-]{1,40}$/.test(name)) {
|
|
alert('Nome: solo [A-Za-z0-9_-], 1-40 caratteri');
|
|
return;
|
|
}
|
|
const side = state.active_side;
|
|
const pose = state.target[side] || state.current[side];
|
|
if (!pose) { alert('Nessuna posa da salvare'); return; }
|
|
if (state.saved_poses[name]) {
|
|
if (!confirm(`Posa "${name}" esiste già — sovrascrivere?`)) return;
|
|
}
|
|
try {
|
|
await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
|
|
method: 'PUT', headers: { 'Content-Type': 'application/json' },
|
|
body: JSON.stringify({
|
|
side, position: pose.position, quaternion: pose.quaternion,
|
|
}),
|
|
});
|
|
$('p3PoseName').value = '';
|
|
await pollState(); // refresh dropdown
|
|
$('p3PoseSelect').value = name;
|
|
} catch (e) {
|
|
showError(`Salvataggio fallito: ${e.message}`);
|
|
}
|
|
}
|
|
|
|
async function onGoPose() {
|
|
const name = $('p3PoseSelect').value;
|
|
if (!name) { alert('Seleziona una posa dal dropdown'); return; }
|
|
if (state.mode === 'jog') {
|
|
if (!confirm(`Eseguire posa "${name}" SUL ROBOT REALE? (Jog mode)`)) return;
|
|
}
|
|
const dry_run = (state.mode === 'editor');
|
|
clearError();
|
|
try {
|
|
const res = await jsonFetch(
|
|
`/api/pose_3d/poses/${encodeURIComponent(name)}/go`,
|
|
{
|
|
method: 'POST', headers: { 'Content-Type': 'application/json' },
|
|
body: JSON.stringify({ dry_run, precise: state.precise }),
|
|
},
|
|
);
|
|
const r = res.result || {};
|
|
if (r.reached_target === false || r.stalled === true) {
|
|
showError(`Vai a "${name}": residual ${(r.position_error_mm||0).toFixed(1)} mm`);
|
|
} else {
|
|
$('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);
|
|
}
|
|
}
|
|
|
|
async function onDeletePose() {
|
|
const name = $('p3PoseSelect').value;
|
|
if (!name) { alert('Seleziona una posa dal dropdown'); return; }
|
|
if (!confirm(`Cancellare definitivamente la posa "${name}"?`)) return;
|
|
try {
|
|
await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
|
|
method: 'DELETE',
|
|
});
|
|
await pollState();
|
|
} catch (e) {
|
|
showError(e.message);
|
|
}
|
|
}
|
|
|
|
function bindPoseButtons() {
|
|
$('p3PoseSaveBtn').addEventListener('click', onSavePose);
|
|
$('p3PoseGoBtn').addEventListener('click', onGoPose);
|
|
$('p3PoseDelBtn').addEventListener('click', onDeletePose);
|
|
}
|
|
|
|
async function onUnlock() {
|
|
clearError();
|
|
try {
|
|
await jsonFetch('/api/pose_3d/unlock', { method: 'POST' });
|
|
$('p3Status').textContent = '✓ Braccia sbloccate';
|
|
$('p3Status').className = 'p3-status ok';
|
|
} catch (e) {
|
|
showError(`Unlock fallito: ${e.message}`);
|
|
}
|
|
}
|
|
|
|
function bindUnlockButton() {
|
|
$('p3UnlockBtn').addEventListener('click', onUnlock);
|
|
}
|
|
|
|
// ----- init triggered on first activation -----
|
|
let inited = false;
|
|
function ensureInit() {
|
|
if (inited) return;
|
|
if (!$('p3Canvas')) return;
|
|
inited = true;
|
|
initScene();
|
|
setupSelection();
|
|
bindSideControls();
|
|
bindGizmoControls();
|
|
bindGizmoEvents();
|
|
bindEditorButtons();
|
|
bindModeControls();
|
|
bindPreciseControl();
|
|
bindJogAutoApply();
|
|
bindPoseButtons();
|
|
bindUnlockButton();
|
|
bindReadoutInputs();
|
|
pollState();
|
|
pollTimer = setInterval(pollState, POLL_MS);
|
|
}
|
|
|
|
// Observe quando la tab diventa visibile per inizializzare
|
|
const tabObserver = new MutationObserver(() => {
|
|
if ($('tab-pose-3d')?.classList.contains('active')) ensureInit();
|
|
});
|
|
if ($('tab-pose-3d')) {
|
|
tabObserver.observe($('tab-pose-3d'), { attributes: true, attributeFilter: ['class'] });
|
|
// Se è già attivo all'avvio
|
|
if ($('tab-pose-3d').classList.contains('active')) ensureInit();
|
|
}
|