Adriano Dal PastroandClaude Opus 4.7
0c211630cc
Elimina il flusso di acquisizione griglia automatica: tab GUI, modulo autocal3d.js, AutoRunner/auto_job, e gli endpoint /auto/* + /auto_grid. La calibrazione resta operabile via sample manuali + /solve. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
480 lines
18 KiB
JavaScript
480 lines
18 KiB
JavaScript
/* 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>`;
|
|
}
|
|
}
|
|
|
|
// ---- 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(); }
|
|
}));
|