282 lines
9.2 KiB
JavaScript
282 lines
9.2 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';
|
|
|
|
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 makeWristMarker(color) {
|
|
const group = new THREE.Group();
|
|
const sphere = new THREE.Mesh(
|
|
new THREE.SphereGeometry(0.025, 16, 12),
|
|
new THREE.MeshStandardMaterial({ color, metalness: 0.2, roughness: 0.5 }),
|
|
);
|
|
group.add(sphere);
|
|
group.add(makeUCS(0.07));
|
|
group.userData.sphere = sphere;
|
|
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,
|
|
};
|
|
|
|
// Posizioni torso e spalle hardcoded (riferimento visivo, non IK)
|
|
const TORSO_POS = new THREE.Vector3(0, 0, 0.40);
|
|
const SHOULDER = {
|
|
right: new THREE.Vector3(0, -0.20, 0.60),
|
|
left: new THREE.Vector3(0, 0.20, 0.60),
|
|
};
|
|
|
|
// ----- 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);
|
|
|
|
// Torso
|
|
const torso = new THREE.Mesh(
|
|
new THREE.BoxGeometry(0.20, 0.40, 0.50),
|
|
new THREE.MeshStandardMaterial({ color: 0x4a5560, transparent: true, opacity: 0.7 }),
|
|
);
|
|
torso.position.copy(TORSO_POS);
|
|
scene.add(torso);
|
|
|
|
// Arms
|
|
for (const side of ['right', 'left']) {
|
|
const wrist = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
|
|
wrist.position.copy(SHOULDER[side]); // posizione di partenza, sarà aggiornata dal polling
|
|
wrist.userData.side = side;
|
|
scene.add(wrist);
|
|
state.arms[side] = wrist;
|
|
|
|
// 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');
|
|
// Disable orbit while dragging gizmo
|
|
transform.addEventListener('dragging-changed', (e) => { orbit.enabled = !e.value; });
|
|
scene.add(transform);
|
|
|
|
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];
|
|
}
|
|
|
|
// ----- 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 updateReadout() {
|
|
const side = state.active_side;
|
|
const pose = state.target[side] || state.current[side];
|
|
if (!pose) {
|
|
$('p3X').textContent = '—';
|
|
return;
|
|
}
|
|
const p = pose.position, q = pose.quaternion;
|
|
$('p3X').textContent = (p[0] * 1000).toFixed(1);
|
|
$('p3Y').textContent = (p[1] * 1000).toFixed(1);
|
|
$('p3Z').textContent = (p[2] * 1000).toFixed(1);
|
|
const rpy = quatToRPYdeg(q);
|
|
$('p3R').textContent = rpy[0].toFixed(1);
|
|
$('p3P').textContent = rpy[1].toFixed(1);
|
|
$('p3W').textContent = rpy[2].toFixed(1);
|
|
$('p3Status').textContent = state.target[side] ? '● modificata' : '— polso fermo';
|
|
$('p3Status').className = 'p3-status' + (state.target[side] ? ' modified' : '');
|
|
}
|
|
|
|
// ----- 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();
|
|
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;
|
|
}
|
|
|
|
// ----- side change listener -----
|
|
function bindSideControls() {
|
|
for (const r of document.querySelectorAll('input[name=p3Side]')) {
|
|
r.addEventListener('change', () => {
|
|
state.active_side = r.value;
|
|
updateReadout();
|
|
});
|
|
}
|
|
}
|
|
|
|
// ----- init triggered on first activation -----
|
|
let inited = false;
|
|
function ensureInit() {
|
|
if (inited) return;
|
|
if (!$('p3Canvas')) return;
|
|
inited = true;
|
|
initScene();
|
|
bindSideControls();
|
|
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();
|
|
}
|