Presa-Mano/web/diagnostic3d.js

/* G1 3D diagnostic viewer.
 *
 * Loads the URDF, builds the scene graph with two-level groups per joint
 * (outer = <origin>, inner = motion driven by diagnostic q), renders meshes
 * tinted by per-motor temperature and colored spheres at joint origins.
 * Exposes a tiny global API consumed by app.js powerPollOnce:
 *   window.g1Diag3d.update(diagData)
 *
 * Deliberately avoids depending on any URDF library — the URDF subset we
 * need (fixed/revolute/continuous/prismatic joints, single mesh per visual,
 * relative mesh paths) is small enough to parse in ~50 lines.
 */

import * as THREE from 'three';
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
import { STLLoader } from 'three/addons/loaders/STLLoader.js';

const URDF_URL = '/static/assets/g1/g1_body29_hand14.urdf';
const MESH_BASE = '/static/assets/g1/';

const WARN_C_DEFAULT = 75;
const CRIT_C_DEFAULT = 85;

function parseOrigin(el) {
  if (!el) return { xyz: [0, 0, 0], rpy: [0, 0, 0] };
  const parse = (s) => s ? s.trim().split(/\s+/).map(Number) : [0, 0, 0];
  return {
    xyz: parse(el.getAttribute('xyz')),
    rpy: parse(el.getAttribute('rpy')),
  };
}
function parseAxis(el) {
  if (!el) return [1, 0, 0];
  const s = el.getAttribute('xyz');
  return s ? s.trim().split(/\s+/).map(Number) : [1, 0, 0];
}

async function parseURDF(xmlText) {
  const doc = new DOMParser().parseFromString(xmlText, 'application/xml');
  const links = {};
  for (const el of doc.querySelectorAll('link')) {
    const name = el.getAttribute('name');
    if (!name) continue;
    const v = el.querySelector('visual');
    let mesh = null, visualOrigin = { xyz: [0,0,0], rpy: [0,0,0] };
    if (v) {
      visualOrigin = parseOrigin(v.querySelector('origin'));
      const me = v.querySelector('geometry mesh');
      if (me) mesh = me.getAttribute('filename');
    }
    links[name] = { name, mesh, visualOrigin };
  }
  const joints = [];
  for (const el of doc.querySelectorAll('joint')) {
    const parent = el.querySelector('parent');
    const child = el.querySelector('child');
    if (!parent || !child) continue;
    joints.push({
      name: el.getAttribute('name'),
      type: el.getAttribute('type') || 'fixed',
      parent: parent.getAttribute('link'),
      child: child.getAttribute('link'),
      origin: parseOrigin(el.querySelector('origin')),
      axis: parseAxis(el.querySelector('axis')),
    });
  }
  return { links, joints };
}

function applyTransform(obj, o) {
  obj.position.set(o.xyz[0], o.xyz[1], o.xyz[2]);
  obj.rotation.order = 'ZYX';            // URDF RPY: intrinsic rotations yaw * pitch * roll
  obj.rotation.set(o.rpy[0], o.rpy[1], o.rpy[2]);
}

function tempToColor(t, warn, crit) {
  if (!Number.isFinite(t)) return 0x888888;
  if (t >= crit) return 0xe74c3c;
  if (t >= warn) return 0xf0ad4e;
  // green->yellow interpolation below warn
  const u = Math.max(0, Math.min(1, (t - 30) / (warn - 30)));
  const r = Math.round(0x3e + u * (0xf0 - 0x3e));
  const g = Math.round(0xcf - u * (0xcf - 0xad));
  const b = Math.round(0x8e - u * (0x8e - 0x4e));
  return (r << 16) | (g << 8) | b;
}

class G1Viewer {
  constructor(canvas) {
    this.canvas = canvas;
    this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
    this.renderer.setPixelRatio(window.devicePixelRatio || 1);
    this.scene = new THREE.Scene();
    this.scene.background = new THREE.Color(0x0d1014);

    // URDF is Z-up. Keep Z-up throughout for coherence with the robot base frame.
    this.camera = new THREE.PerspectiveCamera(45, canvas.clientWidth / canvas.clientHeight, 0.05, 20);
    this.camera.up.set(0, 0, 1);
    this.camera.position.set(1.8, -1.8, 1.2);
    this.camera.lookAt(0, 0, 0.4);

    this.controls = new OrbitControls(this.camera, canvas);
    this.controls.target.set(0, 0, 0.4);
    this.controls.update();

    const hemi = new THREE.HemisphereLight(0xffffff, 0x222233, 0.6);
    this.scene.add(hemi);
    const dir = new THREE.DirectionalLight(0xffffff, 0.8);
    dir.position.set(2, 2, 4);
    this.scene.add(dir);
    const dir2 = new THREE.DirectionalLight(0xffffff, 0.3);
    dir2.position.set(-2, -1, 2);
    this.scene.add(dir2);

    this.grid = new THREE.GridHelper(2, 20, 0x444444, 0x222233);
    this.grid.rotation.x = Math.PI / 2;   // grid is XY plane in Z-up
    this.scene.add(this.grid);

    this.robotRoot = new THREE.Group();
    this.scene.add(this.robotRoot);

    this.linkGroups = {};
    this.linkMeshes = {};
    this.jointGroups = {};         // static <origin> holders
    this.jointMotions = {};        // inner motion-driven holders
    this.jointIndicators = {};     // sphere at joint origin
    this.motorByJointName = {};    // joint_name -> motor dict (from /diagnostics)
    this.warnC = WARN_C_DEFAULT;
    this.critC = CRIT_C_DEFAULT;
    this.showMeshTint = true;
    this.showSpheres = true;
    this.pinnedJoint = null;

    this.raycaster = new THREE.Raycaster();
    this.mouseNDC = new THREE.Vector2();

    this._onResize = () => this.resize();
    window.addEventListener('resize', this._onResize);
    this.canvas.addEventListener('pointermove', (ev) => this._onHover(ev));
    this.canvas.addEventListener('click', (ev) => this._onClick(ev));

    this._animate = this._animate.bind(this);
    this.resize();
    this._animate();
  }

  resize() {
    const w = this.canvas.clientWidth || 720;
    const h = this.canvas.clientHeight || 560;
    this.renderer.setSize(w, h, false);
    this.camera.aspect = w / h;
    this.camera.updateProjectionMatrix();
  }

  _animate() {
    this._raf = requestAnimationFrame(this._animate);
    this.controls.update();
    this.renderer.render(this.scene, this.camera);
  }

  async load() {
    const res = await fetch(URDF_URL);
    if (!res.ok) throw new Error(`URDF fetch ${res.status}`);
    const xml = await res.text();
    const urdf = await parseURDF(xml);
    this._urdf = urdf;
    this._build(urdf);
    await this._loadMeshes(urdf);
    this._placeFloor();
  }

  _build(urdf) {
    const childSet = new Set(urdf.joints.map(j => j.child));
    const rootName = Object.keys(urdf.links).find(n => !childSet.has(n));
    const buildLink = (name) => {
      const link = urdf.links[name];
      const g = new THREE.Group();
      g.name = `link:${name}`;
      g.userData.linkName = name;
      this.linkGroups[name] = g;
      // child joints
      for (const j of urdf.joints.filter(x => x.parent === name)) {
        const origin = new THREE.Group();
        applyTransform(origin, j.origin);
        origin.userData.joint = j;
        this.jointGroups[j.name] = origin;
        const motion = new THREE.Group();
        origin.add(motion);
        this.jointMotions[j.name] = motion;
        // sphere indicator at joint origin (in origin's local frame → origin(0,0,0))
        const sphere = new THREE.Mesh(
          new THREE.SphereGeometry(0.012, 16, 12),
          new THREE.MeshBasicMaterial({ color: 0x888888, depthTest: false, transparent: true, opacity: 0.9 }),
        );
        sphere.renderOrder = 999;
        sphere.userData.jointName = j.name;
        origin.add(sphere);
        this.jointIndicators[j.name] = sphere;
        motion.add(buildLink(j.child));
        g.add(origin);
      }
      return g;
    };
    this.robotRoot.add(buildLink(rootName));
  }

  async _loadMeshes(urdf) {
    const loader = new STLLoader();
    const tasks = [];
    for (const name of Object.keys(urdf.links)) {
      const link = urdf.links[name];
      if (!link.mesh) continue;
      const url = MESH_BASE + link.mesh;
      tasks.push(new Promise((resolve) => {
        loader.load(url, (geom) => {
          geom.computeVertexNormals();
          const mat = new THREE.MeshStandardMaterial({
            color: 0x888888, metalness: 0.1, roughness: 0.6,
            flatShading: false,
          });
          const mesh = new THREE.Mesh(geom, mat);
          mesh.userData.linkName = name;
          applyTransform(mesh, link.visualOrigin);
          this.linkMeshes[name] = mesh;
          this.linkGroups[name].add(mesh);
          resolve();
        }, undefined, (err) => {
          console.warn('STL load failed:', url, err);
          resolve();
        });
      }));
    }
    await Promise.all(tasks);
  }

  _placeFloor() {
    // position grid at model's feet (min z of bounding box).
    const box = new THREE.Box3().setFromObject(this.robotRoot);
    if (isFinite(box.min.z)) this.grid.position.z = box.min.z;
  }

  setShowGrid(v) { this.grid.visible = v; }
  setShowSpheres(v) {
    this.showSpheres = v;
    for (const s of Object.values(this.jointIndicators)) s.visible = v;
  }
  setShowMeshTint(v) {
    this.showMeshTint = v;
    this._applyColors();
  }

  fitView() {
    const box = new THREE.Box3().setFromObject(this.robotRoot);
    if (!isFinite(box.min.z)) return;
    const c = new THREE.Vector3();
    box.getCenter(c);
    const s = new THREE.Vector3();
    box.getSize(s);
    const d = Math.max(s.x, s.y, s.z) * 1.8;
    this.controls.target.copy(c);
    const dir = new THREE.Vector3(1, -1, 0.4).normalize();
    this.camera.position.copy(c).addScaledVector(dir, d);
    this.controls.update();
  }
  resetView() {
    this.controls.target.set(0, 0, 0.4);
    this.camera.position.set(1.8, -1.8, 1.2);
    this.controls.update();
  }

  update(diagData) {
    if (!diagData || !Array.isArray(diagData.motors)) return;
    this.warnC = Number(diagData.warn_temp_c ?? WARN_C_DEFAULT);
    this.critC = Number(diagData.crit_temp_c ?? CRIT_C_DEFAULT);
    this.motorByJointName = {};
    for (const m of diagData.motors) {
      const jn = m.name + '_joint';
      this.motorByJointName[jn] = m;
      // Apply joint angle.
      const jg = this.jointMotions[jn];
      if (jg) {
        const joint = this.jointGroups[jn].userData.joint;
        const ax = new THREE.Vector3(...joint.axis).normalize();
        if (joint.type === 'revolute' || joint.type === 'continuous') {
          jg.quaternion.setFromAxisAngle(ax, Number(m.q) || 0);
        } else if (joint.type === 'prismatic') {
          jg.position.copy(ax.multiplyScalar(Number(m.q) || 0));
        }
      }
    }
    this._applyColors();
    if (this.pinnedJoint) this._renderPinned();
    this._renderSummary(diagData);
  }

  _applyColors() {
    for (const [jointName, motor] of Object.entries(this.motorByJointName)) {
      const joint = this.jointGroups[jointName];
      if (!joint) continue;
      const temp = motor.temperature_motor;
      const faulted = !!motor.faulted;
      const enabled = motor.enabled !== false;
      let color = tempToColor(temp, this.warnC, this.critC);
      if (!enabled) color = 0x333333;
      // sphere
      const sph = this.jointIndicators[jointName];
      if (sph) {
        sph.material.color.setHex(color);
        if (faulted) sph.material.color.setHex(0xe74c3c);
      }
      // mesh tint on child link
      const childLink = joint.userData.joint.child;
      const mesh = this.linkMeshes[childLink];
      if (mesh) {
        if (this.showMeshTint) {
          mesh.material.color.setHex(color);
          mesh.material.emissive.setHex(faulted ? 0x330000 : 0x000000);
          mesh.material.emissiveIntensity = faulted ? 0.8 : 0;
        } else {
          mesh.material.color.setHex(0x888888);
          mesh.material.emissiveIntensity = 0;
        }
      }
    }
  }

  _pickJointAt(clientX, clientY) {
    const rect = this.canvas.getBoundingClientRect();
    this.mouseNDC.x = ((clientX - rect.left) / rect.width) * 2 - 1;
    this.mouseNDC.y = -((clientY - rect.top) / rect.height) * 2 + 1;
    this.raycaster.setFromCamera(this.mouseNDC, this.camera);
    // Prefer hits on joint spheres first (small but always on top).
    const sphHits = this.raycaster.intersectObjects(Object.values(this.jointIndicators), false);
    if (sphHits.length) return sphHits[0].object.userData.jointName;
    // Fall back to mesh hits (walk up the tree to find the joint whose motion group is the ancestor).
    const meshHits = this.raycaster.intersectObjects(Object.values(this.linkMeshes), false);
    if (!meshHits.length) return null;
    const linkName = meshHits[0].object.userData.linkName;
    // Find the joint whose child == linkName.
    const joint = this._urdf.joints.find(j => j.child === linkName && this.motorByJointName[j.name]);
    return joint ? joint.name : null;
  }

  _onHover(ev) {
    const tt = document.getElementById('diag3dTooltip');
    const joint = this._pickJointAt(ev.clientX, ev.clientY);
    if (!joint) { tt.classList.add('hidden'); return; }
    const m = this.motorByJointName[joint];
    if (!m) { tt.classList.add('hidden'); return; }
    const cls = m.faulted ? 'err' : (m.temperature_motor >= this.critC ? 'err' :
                m.temperature_motor >= this.warnC ? 'warn' : '');
    tt.className = 'diag3d-tooltip ' + cls;
    tt.innerHTML = this._tooltipHTML(m);
    const rect = this.canvas.getBoundingClientRect();
    tt.style.left = (ev.clientX - rect.left + 14) + 'px';
    tt.style.top  = (ev.clientY - rect.top  + 14) + 'px';
  }

  _onClick(ev) {
    const joint = this._pickJointAt(ev.clientX, ev.clientY);
    if (!joint) return;
    this.pinnedJoint = joint;
    this._renderPinned();
  }

  _tooltipHTML(m) {
    const rows = [
      ['T motore', `${m.temperature_motor}°C`],
      ['T ctrl',   `${m.temperature_ctrl}°C`],
      ['q',        Number(m.q).toFixed(3) + ' rad'],
      ['dq',       Number(m.dq).toFixed(3) + ' rad/s'],
      ['tau_est',  Number(m.tau_est).toFixed(2) + ' Nm'],
      ['V',        Number(m.voltage).toFixed(2) + ' V'],
      ['mode',     m.mode + (m.enabled === false ? ' (OFF)' : '')],
      ['fault',    m.faulted ? ('0x' + Number(m.motorstate).toString(16)) : 'no'],
    ];
    const body = rows.map(([k, v]) => `<div class="tt-row"><span class="k">${k}</span><span>${v}</span></div>`).join('');
    return `<div class="tt-title">#${m.index} · ${m.name}</div>${body}`;
  }

  _renderPinned() {
    const host = document.getElementById('diag3dPinned');
    if (!host) return;
    if (!this.pinnedJoint) return;
    const m = this.motorByJointName[this.pinnedJoint];
    if (!m) {
      host.innerHTML = `<div class="dim">giunto ${this.pinnedJoint}: nessun motore associato</div>`;
      return;
    }
    const statusCls = m.faulted ? 'err' :
      m.temperature_motor >= this.critC ? 'err' :
      m.temperature_motor >= this.warnC ? 'warn' : '';
    const rows = [
      ['nome', m.name],
      ['indice', m.index],
      ['T motore (°C)', m.temperature_motor],
      ['T ctrl (°C)', m.temperature_ctrl],
      ['q (rad)', Number(m.q).toFixed(4)],
      ['dq (rad/s)', Number(m.dq).toFixed(4)],
      ['tau_est (Nm)', Number(m.tau_est).toFixed(3)],
      ['voltaggio (V)', Number(m.voltage).toFixed(3)],
      ['mode', m.mode],
      ['enabled', m.enabled ? 'sì' : 'no'],
      ['motorstate raw', '0x' + Number(m.motorstate).toString(16)],
      ['faulted', m.faulted ? 'SÌ' : 'no'],
      ['derating', m.derating ? 'sì' : 'no'],
    ];
    host.innerHTML =
      `<div><strong>#${m.index} · ${m.name}</strong> <span class="dim">(giunto: ${this.pinnedJoint})</span></div>` +
      `<table><tbody>${rows.map(([k,v]) =>
        `<tr class="${statusCls}"><td>${k}</td><td>${v}</td></tr>`).join('')}</tbody></table>`;
  }

  _renderSummary(d) {
    const host = document.getElementById('diag3dSummary');
    if (!host) return;
    const s = d.summary || {};
    const fmt = (arr) => (arr && arr.length) ? arr.join(', ') : '—';
    const rows = [
      ['lowstate', d.stale ? 'STALE' : (d.tick != null ? `tick ${d.tick}` : 'ok')],
      ['max temp (°C)', s.max_motor_temp_c != null ? `${s.max_motor_temp_c} @ ${s.max_motor_temp_joint}` : '—'],
      ['warn / crit (°C)', `${d.warn_temp_c} / ${d.crit_temp_c}`],
      ['derating', fmt(s.derating_joints)],
      ['critical', fmt(s.critical_joints)],
      ['faulted',  fmt(s.faulted_joints)],
      ['disabled', fmt(s.disabled_joints)],
      ['min V', s.min_voltage != null ? Number(s.min_voltage).toFixed(2) + ' V' : '—'],
    ];
    host.innerHTML = `<table><tbody>${rows.map(([k,v]) =>
      `<tr><td>${k}</td><td>${v}</td></tr>`).join('')}</tbody></table>`;
  }
}

// Expose the viewer class so other modules (Auto-Calibrazione) can
// instantiate a second canvas over the same URDF pipeline.
export { G1Viewer };


// ---- Boot and expose global hook. ----

const canvas = document.getElementById('diag3dCanvas');
const statusEl = document.getElementById('diag3dStatus');
let viewer = null;

async function boot() {
  if (!canvas) return;
  try {
    statusEl.textContent = 'caricamento URDF + mesh…';
    viewer = new G1Viewer(canvas);
    await viewer.load();
    statusEl.textContent = 'URDF caricato · in attesa di /diagnostics…';
    // Wire up toolbar controls.
    const by = (id) => document.getElementById(id);
    by('diag3dMeshTint').addEventListener('change', (e) => viewer.setShowMeshTint(e.target.checked));
    by('diag3dSpheres').addEventListener('change', (e) => viewer.setShowSpheres(e.target.checked));
    by('diag3dGrid').addEventListener('change', (e) => viewer.setShowGrid(e.target.checked));
    by('diag3dResetBtn').addEventListener('click', () => viewer.resetView());
    by('diag3dFitBtn').addEventListener('click', () => viewer.fitView());
    // Re-size when the diagnostic tab becomes active (canvas may have 0 size while hidden).
    document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
      if (btn.dataset.tab === 'diagnostic') setTimeout(() => viewer && viewer.resize(), 50);
    }));
    // Global hook for app.js to push diagnostic updates.
    window.g1Diag3d = {
      update: (d) => {
        try { viewer.update(d); statusEl.textContent =
          d.stale ? 'lowstate STALE' :
          d.available === false ? (d.reason || '/diagnostics non disponibile') :
          `motori aggiornati · tick ${d.tick ?? '?'}`; }
        catch (e) { console.warn('g1Diag3d.update:', e); }
      },
    };
  } catch (e) {
    console.error('diag3d boot failed:', e);
    statusEl.textContent = 'errore caricamento: ' + e.message;
  }
}

// Defer boot until the tab is first opened — avoids eager CDN fetch and STL load
// for users who never visit this tab.
let booted = false;
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
  if (btn.dataset.tab === 'diagnostic' && !booted) { booted = true; boot(); }
}));