895 lines
29 KiB
HTML
895 lines
29 KiB
HTML
<!DOCTYPE html>
|
||
<html lang="en">
|
||
<head>
|
||
<meta charset="UTF-8">
|
||
<title>Robot FK Viewer</title>
|
||
<style>
|
||
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
|
||
|
||
:root {
|
||
--bg: #0d0f13;
|
||
--panel: #161920;
|
||
--border: #2a2d35;
|
||
--text: #c8cdd8;
|
||
--accent: #4a9eff;
|
||
--muted: #555b6e;
|
||
--ok: #3ecf6b;
|
||
--warn: #f59e0b;
|
||
--err: #ff4f4f;
|
||
--panel-w: 300px;
|
||
}
|
||
|
||
body {
|
||
background: var(--bg);
|
||
color: var(--text);
|
||
font: 13px/1.5 'IBM Plex Mono', 'Cascadia Code', 'Courier New', monospace;
|
||
display: flex;
|
||
height: 100vh;
|
||
overflow: hidden;
|
||
}
|
||
|
||
/* ── sidebar ── */
|
||
#sidebar {
|
||
width: var(--panel-w);
|
||
flex-shrink: 0;
|
||
background: var(--panel);
|
||
border-right: 1px solid var(--border);
|
||
overflow-y: auto;
|
||
display: flex;
|
||
flex-direction: column;
|
||
gap: 0;
|
||
}
|
||
|
||
.section {
|
||
border-bottom: 1px solid var(--border);
|
||
padding: 12px 14px;
|
||
}
|
||
|
||
.section h3 {
|
||
font-size: 10px;
|
||
letter-spacing: 0.12em;
|
||
text-transform: uppercase;
|
||
color: var(--muted);
|
||
margin-bottom: 8px;
|
||
}
|
||
|
||
/* file inputs */
|
||
.file-row {
|
||
display: flex;
|
||
align-items: center;
|
||
gap: 8px;
|
||
margin-bottom: 6px;
|
||
}
|
||
.file-row label {
|
||
font-size: 11px;
|
||
color: var(--muted);
|
||
width: 58px;
|
||
flex-shrink: 0;
|
||
}
|
||
.file-row input[type=file] {
|
||
flex: 1;
|
||
font-size: 10px;
|
||
color: var(--text);
|
||
background: var(--bg);
|
||
border: 1px solid var(--border);
|
||
border-radius: 3px;
|
||
padding: 3px 6px;
|
||
cursor: pointer;
|
||
}
|
||
.file-row input[type=file]::file-selector-button {
|
||
background: var(--border);
|
||
color: var(--text);
|
||
border: none;
|
||
padding: 2px 8px;
|
||
cursor: pointer;
|
||
font-family: inherit;
|
||
font-size: 10px;
|
||
border-radius: 2px;
|
||
margin-right: 6px;
|
||
}
|
||
|
||
/* joint sliders */
|
||
.slider-row {
|
||
display: grid;
|
||
grid-template-columns: 22px 1fr 48px;
|
||
align-items: center;
|
||
gap: 6px;
|
||
margin-bottom: 5px;
|
||
}
|
||
.slider-row span { color: var(--accent); font-size: 12px; }
|
||
input[type=range] {
|
||
-webkit-appearance: none;
|
||
height: 3px;
|
||
background: var(--border);
|
||
border-radius: 2px;
|
||
outline: none;
|
||
cursor: pointer;
|
||
}
|
||
input[type=range]::-webkit-slider-thumb {
|
||
-webkit-appearance: none;
|
||
width: 12px; height: 12px;
|
||
background: var(--accent);
|
||
border-radius: 50%;
|
||
}
|
||
.val-box {
|
||
background: var(--bg);
|
||
border: 1px solid var(--border);
|
||
border-radius: 3px;
|
||
padding: 2px 4px;
|
||
font-size: 11px;
|
||
text-align: right;
|
||
color: var(--text);
|
||
width: 48px;
|
||
}
|
||
|
||
/* toggles */
|
||
.toggle-row {
|
||
display: flex;
|
||
align-items: center;
|
||
justify-content: space-between;
|
||
margin-bottom: 5px;
|
||
}
|
||
.toggle-label { font-size: 11px; color: var(--text); }
|
||
.toggle {
|
||
position: relative; width: 36px; height: 18px;
|
||
}
|
||
.toggle input { display: none; }
|
||
.slider-track {
|
||
position: absolute; inset: 0;
|
||
background: var(--border);
|
||
border-radius: 9px;
|
||
cursor: pointer;
|
||
transition: background .2s;
|
||
}
|
||
.slider-track::after {
|
||
content: '';
|
||
position: absolute;
|
||
left: 2px; top: 2px;
|
||
width: 14px; height: 14px;
|
||
background: var(--muted);
|
||
border-radius: 50%;
|
||
transition: transform .2s, background .2s;
|
||
}
|
||
.toggle input:checked + .slider-track { background: var(--accent); }
|
||
.toggle input:checked + .slider-track::after {
|
||
transform: translateX(18px);
|
||
background: #fff;
|
||
}
|
||
|
||
/* stats */
|
||
#stats-content {
|
||
font-size: 11px;
|
||
line-height: 1.8;
|
||
}
|
||
.stat-line { display: flex; justify-content: space-between; }
|
||
.stat-val { color: var(--accent); }
|
||
.stat-ok { color: var(--ok); }
|
||
.stat-warn { color: var(--warn); }
|
||
.stat-err { color: var(--err); }
|
||
|
||
#status-bar {
|
||
font-size: 10px;
|
||
color: var(--muted);
|
||
padding: 8px 14px;
|
||
border-top: 1px solid var(--border);
|
||
margin-top: auto;
|
||
}
|
||
|
||
/* canvas */
|
||
#canvas-wrap {
|
||
flex: 1;
|
||
position: relative;
|
||
overflow: hidden;
|
||
}
|
||
canvas { display: block; width: 100%; height: 100%; }
|
||
</style>
|
||
|
||
<script type="importmap">
|
||
{
|
||
"imports": {
|
||
"three": "https://unpkg.com/three@0.162.0/build/three.module.js",
|
||
"three/addons/": "https://unpkg.com/three@0.162.0/examples/jsm/"
|
||
}
|
||
}
|
||
</script>
|
||
</head>
|
||
|
||
<body>
|
||
<div id="sidebar">
|
||
|
||
<!-- files -->
|
||
<div class="section">
|
||
<h3>Data Files</h3>
|
||
<div class="file-row">
|
||
<label>robot.json</label>
|
||
<input type="file" id="fRobot" accept=".json">
|
||
</div>
|
||
<div class="file-row">
|
||
<label>aruco</label>
|
||
<input type="file" id="fAruco" accept=".json">
|
||
</div>
|
||
</div>
|
||
|
||
<!-- test poses -->
|
||
<div class="section" id="poseSection" style="display:none">
|
||
<h3>Test Poses</h3>
|
||
<select id="poseSelect" style="
|
||
width:100%; background:var(--bg); border:1px solid var(--border);
|
||
color:var(--text); font:inherit; font-size:11px; padding:4px 6px;
|
||
border-radius:3px; cursor:pointer; margin-bottom:4px;">
|
||
<option value="">— select pose —</option>
|
||
</select>
|
||
<div id="poseInfo" style="font-size:10px;color:var(--muted);min-height:14px;"></div>
|
||
</div>
|
||
|
||
<!-- joint sliders -->
|
||
<div class="section">
|
||
<h3>Joint Values</h3>
|
||
<div id="sliders"></div>
|
||
</div>
|
||
|
||
<!-- display toggles -->
|
||
<div class="section">
|
||
<h3>Display</h3>
|
||
<div class="toggle-row">
|
||
<span class="toggle-label">Skeleton</span>
|
||
<label class="toggle"><input type="checkbox" id="tSkeleton" checked><span class="slider-track"></span></label>
|
||
</div>
|
||
<div class="toggle-row">
|
||
<span class="toggle-label">Model markers</span>
|
||
<label class="toggle"><input type="checkbox" id="tModel" checked><span class="slider-track"></span></label>
|
||
</div>
|
||
<div class="toggle-row">
|
||
<span class="toggle-label">Model normals</span>
|
||
<label class="toggle"><input type="checkbox" id="tNormals"><span class="slider-track"></span></label>
|
||
</div>
|
||
<div class="toggle-row">
|
||
<span class="toggle-label">Observed markers</span>
|
||
<label class="toggle"><input type="checkbox" id="tObserved" checked><span class="slider-track"></span></label>
|
||
</div>
|
||
<div class="toggle-row">
|
||
<span class="toggle-label">Observed normals</span>
|
||
<label class="toggle"><input type="checkbox" id="tObsNormals"><span class="slider-track"></span></label>
|
||
</div>
|
||
<div class="toggle-row">
|
||
<span class="toggle-label">Error lines</span>
|
||
<label class="toggle"><input type="checkbox" id="tErrors" checked><span class="slider-track"></span></label>
|
||
</div>
|
||
<div class="toggle-row">
|
||
<span class="toggle-label">Board plane</span>
|
||
<label class="toggle"><input type="checkbox" id="tBoard" checked><span class="slider-track"></span></label>
|
||
</div>
|
||
</div>
|
||
|
||
<!-- stats -->
|
||
<div class="section">
|
||
<h3>Error Statistics</h3>
|
||
<div id="stats-content"><span style="color:var(--muted)">Load both files…</span></div>
|
||
</div>
|
||
|
||
<div id="status-bar">Drag to orbit · Scroll to zoom · Right-drag to pan</div>
|
||
</div>
|
||
|
||
<div id="canvas-wrap">
|
||
<canvas id="cv"></canvas>
|
||
</div>
|
||
|
||
<script type="module">
|
||
import * as THREE from 'three';
|
||
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
|
||
|
||
// ═══════════════════════════════════════════════════════════════
|
||
// FK in plain JavaScript — mirrors robot_fk.py exactly
|
||
// ═══════════════════════════════════════════════════════════════
|
||
|
||
function norm(v) {
|
||
const n = Math.sqrt(v.reduce((s, x) => s + x * x, 0));
|
||
return n < 1e-12 ? v.map(() => 0) : v.map(x => x / n);
|
||
}
|
||
|
||
function mm3(A, B) { // 3×3 row-major mat-mat
|
||
const C = new Array(9).fill(0);
|
||
for (let i = 0; i < 3; i++)
|
||
for (let j = 0; j < 3; j++)
|
||
for (let k = 0; k < 3; k++)
|
||
C[i*3+j] += A[i*3+k] * B[k*3+j];
|
||
return C;
|
||
}
|
||
|
||
function rotAA(axis, deg) { // Rodrigues
|
||
const [x, y, z] = norm(axis);
|
||
const r = deg * Math.PI / 180, c = Math.cos(r), s = Math.sin(r), t = 1 - c;
|
||
return [t*x*x+c, t*x*y-s*z, t*x*z+s*y,
|
||
t*x*y+s*z, t*y*y+c, t*y*z-s*x,
|
||
t*x*z-s*y, t*y*z+s*x, t*z*z+c];
|
||
}
|
||
|
||
function rotXYZ(rx, ry, rz) {
|
||
return mm3(mm3(rotAA([0,0,1],rz), rotAA([0,1,0],ry)), rotAA([1,0,0],rx));
|
||
}
|
||
|
||
// 4×4 row-major
|
||
function makeT(R3, tx, ty, tz) {
|
||
return [R3[0],R3[1],R3[2],tx, R3[3],R3[4],R3[5],ty,
|
||
R3[6],R3[7],R3[8],tz, 0,0,0,1];
|
||
}
|
||
function I4() { return [1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1]; }
|
||
|
||
function mm4(A, B) {
|
||
const C = new Array(16).fill(0);
|
||
for (let i = 0; i < 4; i++)
|
||
for (let j = 0; j < 4; j++)
|
||
for (let k = 0; k < 4; k++)
|
||
C[i*4+j] += A[i*4+k] * B[k*4+j];
|
||
return C;
|
||
}
|
||
|
||
function applyT(T, p) {
|
||
return [T[0]*p[0]+T[1]*p[1]+T[2]*p[2]+T[3],
|
||
T[4]*p[0]+T[5]*p[1]+T[6]*p[2]+T[7],
|
||
T[8]*p[0]+T[9]*p[1]+T[10]*p[2]+T[11]];
|
||
}
|
||
|
||
function buildTopoOrder(links) {
|
||
const parent = Object.fromEntries(Object.entries(links).map(([n,d]) => [n, d.parent||null]));
|
||
const visited = new Set(), order = [];
|
||
const visit = n => {
|
||
if (visited.has(n)) return;
|
||
visited.add(n);
|
||
if (parent[n]) visit(parent[n]);
|
||
order.push(n);
|
||
};
|
||
Object.keys(links).forEach(visit);
|
||
return order;
|
||
}
|
||
|
||
function computeFK(robotData, joints) {
|
||
const links = robotData.links || {};
|
||
const state = {x:0,y:0,z:0,a:0,b:0,c:0,e:0, ...joints};
|
||
const T = {};
|
||
|
||
for (const lname of buildTopoOrder(links)) {
|
||
const ld = links[lname];
|
||
const par = ld.parent;
|
||
const Tp = T[par] || I4();
|
||
|
||
const mp = ld.mountPosition || [0,0,0];
|
||
const mr = ld.mountRotation || [0,0,0];
|
||
const Tm = makeT(rotXYZ(...mr), ...mp);
|
||
|
||
const jt = ld.jointToParent || {};
|
||
const jo = jt.origin || [0,0,0];
|
||
const jr = jt.rotation || [0,0,0];
|
||
const Tj = makeT(rotXYZ(...jr), ...jo);
|
||
|
||
const type = (jt.type||'').toLowerCase();
|
||
const axis = jt.axis || [1,0,0];
|
||
const v = state[jt.variable] || 0;
|
||
|
||
let Tmot = I4();
|
||
if (type === 'revolute') {
|
||
Tmot = makeT(rotAA(axis, v), 0, 0, 0);
|
||
} else if (type === 'linear') {
|
||
const [ax, ay, az] = norm(axis);
|
||
Tmot = makeT([1,0,0,0,1,0,0,0,1], ax*v, ay*v, az*v);
|
||
}
|
||
|
||
T[lname] = mm4(mm4(mm4(Tp, Tm), Tj), Tmot);
|
||
}
|
||
return T;
|
||
}
|
||
|
||
function markerWorld(T, link, local) {
|
||
return applyT(T[link] || I4(), local);
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════════
|
||
// Coordinate conversion robot-mm → Three.js scene (metres)
|
||
// robot: x=right, y=backward, z=up
|
||
// Three.js: x=right, y=up, z=toward viewer
|
||
// ═══════════════════════════════════════════════════════════════
|
||
const S = 1 / 1000;
|
||
function r2v(rx, ry, rz) { return new THREE.Vector3(rx*S, rz*S, -ry*S); }
|
||
function r2vArr(p) { return r2v(p[0], p[1], p[2]); }
|
||
|
||
// ═══════════════════════════════════════════════════════════════
|
||
// Three.js scene
|
||
// ═══════════════════════════════════════════════════════════════
|
||
|
||
const canvas = document.getElementById('cv');
|
||
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||
renderer.setPixelRatio(devicePixelRatio);
|
||
renderer.shadowMap.enabled = true;
|
||
renderer.toneMapping = THREE.ACESFilmicToneMapping;
|
||
renderer.toneMappingExposure = 0.9;
|
||
|
||
const scene = new THREE.Scene();
|
||
scene.background = new THREE.Color(0x0d0f13);
|
||
scene.fog = new THREE.FogExp2(0x0d0f13, 0.35);
|
||
|
||
const camera = new THREE.PerspectiveCamera(45, 1, 0.001, 20);
|
||
camera.position.set(0.35, 0.55, 1.1);
|
||
camera.lookAt(0.2, 0.05, 0);
|
||
|
||
const controls = new OrbitControls(camera, canvas);
|
||
controls.target.set(0.2, 0.05, 0);
|
||
controls.enableDamping = true;
|
||
controls.dampingFactor = 0.08;
|
||
|
||
// Lighting
|
||
scene.add(new THREE.AmbientLight(0xffffff, 0.55));
|
||
const sun = new THREE.DirectionalLight(0xfff4e0, 1.4);
|
||
sun.position.set(-0.8, 1.2, 0.9);
|
||
sun.castShadow = true;
|
||
scene.add(sun);
|
||
const fill = new THREE.DirectionalLight(0xc0d8ff, 0.4);
|
||
fill.position.set(0.6, 0.3, -0.5);
|
||
scene.add(fill);
|
||
|
||
// Grid
|
||
const grid = new THREE.GridHelper(3, 30, 0x1e2230, 0x1a1e28);
|
||
grid.position.y = -0.028;
|
||
scene.add(grid);
|
||
|
||
// Axes helper (world origin)
|
||
const axes = new THREE.AxesHelper(0.1);
|
||
axes.position.set(0, 0, 0);
|
||
scene.add(axes);
|
||
|
||
// ─── geometry helpers ─────────────────────────────────────────
|
||
|
||
function makeCylinder(p1, p2, radius, color, opacity=1) {
|
||
const dir = p2.clone().sub(p1);
|
||
const len = dir.length();
|
||
if (len < 0.0001) return null;
|
||
const geo = new THREE.CylinderGeometry(radius, radius, len, 8, 1);
|
||
const mat = new THREE.MeshPhongMaterial({
|
||
color, transparent: opacity < 1, opacity,
|
||
shininess: 60
|
||
});
|
||
const m = new THREE.Mesh(geo, mat);
|
||
m.castShadow = true;
|
||
m.position.copy(p1.clone().add(p2).multiplyScalar(0.5));
|
||
m.quaternion.setFromUnitVectors(new THREE.Vector3(0,1,0), dir.normalize());
|
||
return m;
|
||
}
|
||
|
||
function makeSphere(pos, radius, color) {
|
||
const geo = new THREE.SphereGeometry(radius, 12, 8);
|
||
const mat = new THREE.MeshPhongMaterial({ color, shininess: 80 });
|
||
const m = new THREE.Mesh(geo, mat);
|
||
m.position.copy(pos);
|
||
m.castShadow = true;
|
||
return m;
|
||
}
|
||
|
||
function transformDirByT(T, dir) {
|
||
return [
|
||
T[0]*dir[0] + T[1]*dir[1] + T[2]*dir[2],
|
||
T[4]*dir[0] + T[5]*dir[1] + T[6]*dir[2],
|
||
T[8]*dir[0] + T[9]*dir[1] + T[10]*dir[2],
|
||
];
|
||
}
|
||
|
||
function makeMarkerSquare(pos, normal, size, color) {
|
||
const geo = new THREE.BoxGeometry(size, size, size * 0.1);
|
||
const mat = new THREE.MeshPhongMaterial({
|
||
color,
|
||
shininess: 40
|
||
});
|
||
|
||
const m = new THREE.Mesh(geo, mat);
|
||
m.position.copy(pos);
|
||
|
||
// Fallback falls keine gültige Normale vorhanden
|
||
let nx = 0, ny = 0, nz = 1;
|
||
|
||
if (Array.isArray(normal) && normal.length >= 3) {
|
||
nx = Number(normal[0]) || 0;
|
||
ny = Number(normal[1]) || 0;
|
||
nz = Number(normal[2]) || 1;
|
||
} else if (normal instanceof THREE.Vector3) {
|
||
nx = normal.x;
|
||
ny = normal.y;
|
||
nz = normal.z;
|
||
}
|
||
|
||
const n = new THREE.Vector3(nx, ny, nz);
|
||
|
||
if (n.lengthSq() > 1e-12) {
|
||
n.normalize();
|
||
m.quaternion.setFromUnitVectors(
|
||
new THREE.Vector3(0, 0, 1),
|
||
n
|
||
);
|
||
}
|
||
|
||
return m;
|
||
}
|
||
|
||
function makeLine(p1, p2, color) {
|
||
const geo = new THREE.BufferGeometry().setFromPoints([p1, p2]);
|
||
const mat = new THREE.LineBasicMaterial({ color });
|
||
return new THREE.Line(geo, mat);
|
||
}
|
||
|
||
// ─── link colours ─────────────────────────────────────────────
|
||
const LINK_COLORS = {
|
||
Board: 0x8b6528,
|
||
Base: 0xc8c8c8,
|
||
Arm1: 0x3355cc,
|
||
Ellbow: 0xcccccc,
|
||
Arm2: 0xbbbbbb,
|
||
Hand: 0xaaaaaa,
|
||
Palm: 0x999999,
|
||
FingerA: 0xdddddd,
|
||
FingerB: 0xdddddd,
|
||
};
|
||
function linkColor(name) { return LINK_COLORS[name] ?? 0x888888; }
|
||
|
||
// ─── scene groups ─────────────────────────────────────────────
|
||
const gSkeleton = new THREE.Group();
|
||
const gModel = new THREE.Group();
|
||
const gNormals = new THREE.Group();
|
||
const gObserved = new THREE.Group();
|
||
const gObsNormals= new THREE.Group();
|
||
const gErrors = new THREE.Group();
|
||
const gBoard = new THREE.Group();
|
||
scene.add(gSkeleton, gModel, gNormals, gObserved, gObsNormals, gErrors, gBoard);
|
||
|
||
// ─── toggle wiring ────────────────────────────────────────────
|
||
const toggles = {
|
||
tSkeleton: gSkeleton,
|
||
tModel: gModel,
|
||
tNormals: gNormals,
|
||
tObserved: gObserved,
|
||
tObsNormals:gObsNormals,
|
||
tErrors: gErrors,
|
||
tBoard: gBoard,
|
||
};
|
||
for (const [id, grp] of Object.entries(toggles)) {
|
||
document.getElementById(id).addEventListener('change', e => {
|
||
grp.visible = e.target.checked;
|
||
});
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════════
|
||
// State
|
||
// ═══════════════════════════════════════════════════════════════
|
||
|
||
let robotData = null;
|
||
let arucoData = null;
|
||
const joints = { x:0, y:0, z:0, a:0, b:0, c:0, e:0 };
|
||
|
||
// ─── slider setup ─────────────────────────────────────────────
|
||
const JOINT_DEFS = [
|
||
{ key:'x', label:'x', min:-50, max:600, step:1, unit:'mm' },
|
||
{ key:'y', label:'y', min:-180, max:180, step:0.5,unit:'°' },
|
||
{ key:'z', label:'z', min:-180, max:180, step:0.5,unit:'°' },
|
||
{ key:'a', label:'a', min:-180, max:180, step:0.5,unit:'°' },
|
||
{ key:'b', label:'b', min:-180, max:180, step:0.5,unit:'°' },
|
||
{ key:'c', label:'c', min:-180, max:180, step:0.5,unit:'°' },
|
||
{ key:'e', label:'e', min:0, max:60, step:0.5,unit:'mm' },
|
||
];
|
||
|
||
const sliderEls = {}, valEls = {};
|
||
const slidersDiv = document.getElementById('sliders');
|
||
|
||
for (const d of JOINT_DEFS) {
|
||
const row = document.createElement('div');
|
||
row.className = 'slider-row';
|
||
|
||
const lbl = document.createElement('span');
|
||
lbl.textContent = d.key;
|
||
|
||
const sl = document.createElement('input');
|
||
sl.type = 'range';
|
||
sl.min = d.min; sl.max = d.max; sl.step = d.step; sl.value = 0;
|
||
|
||
const vb = document.createElement('input');
|
||
vb.type = 'text'; vb.className = 'val-box'; vb.value = '0';
|
||
vb.style.width = '58px';
|
||
|
||
sl.addEventListener('input', () => {
|
||
joints[d.key] = parseFloat(sl.value);
|
||
vb.value = parseFloat(sl.value).toFixed(d.step < 1 ? 1 : 0);
|
||
rebuild();
|
||
});
|
||
vb.addEventListener('change', () => {
|
||
const v = parseFloat(vb.value);
|
||
if (!isNaN(v)) {
|
||
joints[d.key] = v;
|
||
sl.value = v;
|
||
rebuild();
|
||
}
|
||
});
|
||
|
||
row.append(lbl, sl, vb);
|
||
slidersDiv.appendChild(row);
|
||
sliderEls[d.key] = sl;
|
||
valEls[d.key] = vb;
|
||
}
|
||
|
||
function setSlider(key, val) {
|
||
joints[key] = val;
|
||
sliderEls[key].value = val;
|
||
valEls[key].value = val.toFixed(JOINT_DEFS.find(d=>d.key===key)?.step < 1 ? 1 : 0);
|
||
}
|
||
|
||
// ─── file loading ──────────────────────────────────────────────
|
||
function readJSON(file) {
|
||
return new Promise((res, rej) => {
|
||
const r = new FileReader();
|
||
r.onload = e => { try { res(JSON.parse(e.target.result)); } catch(err) { rej(err); } };
|
||
r.onerror = rej;
|
||
r.readAsText(file);
|
||
});
|
||
}
|
||
|
||
document.getElementById('fRobot').addEventListener('change', async e => {
|
||
if (!e.target.files[0]) return;
|
||
robotData = await readJSON(e.target.files[0]);
|
||
// init sliders from defaultPosition if present
|
||
const dp = robotData.defaultPosition || {};
|
||
for (const k of Object.keys(joints)) {
|
||
if (dp[k] != null) setSlider(k, dp[k]);
|
||
}
|
||
populatePoses(robotData);
|
||
setStatus('robot.json loaded');
|
||
rebuild();
|
||
});
|
||
|
||
document.getElementById('fAruco').addEventListener('change', async e => {
|
||
if (!e.target.files[0]) return;
|
||
arucoData = await readJSON(e.target.files[0]);
|
||
setStatus('aruco/markers.json loaded');
|
||
rebuild();
|
||
});
|
||
|
||
// ═══════════════════════════════════════════════════════════════
|
||
// Test-pose dropdown
|
||
// ═══════════════════════════════════════════════════════════════
|
||
function populatePoses(data) {
|
||
const poses = data.robot_test_poses || {};
|
||
const keys = Object.keys(poses);
|
||
const section = document.getElementById('poseSection');
|
||
const sel = document.getElementById('poseSelect');
|
||
const info = document.getElementById('poseInfo');
|
||
|
||
if (keys.length === 0) { section.style.display = 'none'; return; }
|
||
section.style.display = '';
|
||
|
||
// Rebuild options
|
||
sel.innerHTML = '<option value="">— select pose —</option>';
|
||
keys.forEach(k => {
|
||
const o = document.createElement('option');
|
||
o.value = k; o.textContent = k;
|
||
sel.appendChild(o);
|
||
});
|
||
|
||
// Remove old listener by replacing element
|
||
const newSel = sel.cloneNode(true);
|
||
sel.parentNode.replaceChild(newSel, sel);
|
||
|
||
newSel.addEventListener('change', () => {
|
||
const k = newSel.value;
|
||
if (!k) { info.textContent = ''; return; }
|
||
const p = poses[k];
|
||
info.textContent = Object.entries(p).map(([a,b])=>`${a}:${b}`).join(' ');
|
||
for (const [key, val] of Object.entries(p)) {
|
||
if (key in joints) setSlider(key, Number(val));
|
||
}
|
||
rebuild();
|
||
});
|
||
}
|
||
|
||
// ─── clear groups ──────────────────────────────────────────────
|
||
function clearGroup(g) {
|
||
while (g.children.length) {
|
||
const c = g.children[0];
|
||
c.geometry?.dispose?.();
|
||
if (Array.isArray(c.material)) c.material.forEach(m => m.dispose?.());
|
||
else c.material?.dispose?.();
|
||
g.remove(c);
|
||
}
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════════
|
||
// Normal-arrow helper
|
||
// ═══════════════════════════════════════════════════════════════
|
||
function makeNormalArrow(posThreeJS, normalRobot, length, hexColor) {
|
||
// normalRobot is in robot coords → convert to Three.js direction
|
||
const dir = new THREE.Vector3(normalRobot[0], normalRobot[2], -normalRobot[1]).normalize();
|
||
const arrow = new THREE.ArrowHelper(
|
||
dir,
|
||
posThreeJS,
|
||
length,
|
||
hexColor,
|
||
length * 0.40, // cone head length
|
||
length * 0.20 // cone head width
|
||
);
|
||
return arrow;
|
||
}
|
||
|
||
// ─── rebuild scene ─────────────────────────────────────────────
|
||
function r2vDir(rx, ry, rz) {
|
||
return new THREE.Vector3(rx, rz, -ry).normalize();
|
||
}
|
||
|
||
function rebuild() {
|
||
clearGroup(gSkeleton);
|
||
clearGroup(gModel);
|
||
clearGroup(gNormals);
|
||
clearGroup(gObserved);
|
||
clearGroup(gObsNormals);
|
||
clearGroup(gErrors);
|
||
clearGroup(gBoard);
|
||
|
||
if (!robotData) return;
|
||
|
||
const T = computeFK(robotData, joints);
|
||
|
||
// ── board plane ──
|
||
const boardSize = robotData.links?.Board?.size || [1000, 200, 25];
|
||
{
|
||
const w = boardSize[0]*S, d = boardSize[1]*S, h = boardSize[2]*S;
|
||
const geo = new THREE.BoxGeometry(w, h, d);
|
||
const mat = new THREE.MeshPhongMaterial({ color:0x7a5520, transparent:true, opacity:0.5 });
|
||
const m = new THREE.Mesh(geo, mat);
|
||
m.position.set(w/2, -h/2, -d/2);
|
||
m.receiveShadow = true;
|
||
gBoard.add(m);
|
||
}
|
||
|
||
// ── skeleton ──
|
||
const links = robotData.links || {};
|
||
for (const [lname, ld] of Object.entries(links)) {
|
||
const sk = ld.skeleton;
|
||
if (!sk) continue;
|
||
const from = markerWorld(T, lname, sk.from || [0,0,0]);
|
||
const to = markerWorld(T, lname, sk.to || [0,0,0]);
|
||
const r = (sk.radius || 4) * S * 0.8;
|
||
const col = sk.color ? new THREE.Color(...sk.color) : new THREE.Color(linkColor(lname));
|
||
const cyl = makeCylinder(r2vArr(from), r2vArr(to), r, col);
|
||
if (cyl) gSkeleton.add(cyl);
|
||
}
|
||
|
||
// ── model markers + normals ──
|
||
const modelPositions = {};
|
||
const modelNormals = {};
|
||
for (const [lname, ld] of Object.entries(links)) {
|
||
const col = linkColor(lname);
|
||
for (const m of (ld.markers||[])) {
|
||
if (!m.position) continue;
|
||
const mid = m.id;
|
||
const wp = markerWorld(T, lname, m.position);
|
||
const nLocal = m.normal || [0,0,1];
|
||
const nWorld = transformDirByT(T[lname] || I4(), nLocal);
|
||
|
||
modelPositions[mid] = wp;
|
||
modelNormals[mid] = nWorld;
|
||
|
||
const sq = makeMarkerSquare(r2vArr(wp), r2vDir(...nWorld), 0.022, col);
|
||
gModel.add(sq);
|
||
|
||
// normal arrow (length = half a marker size = ~12.5mm → 0.0125m)
|
||
const arr = makeNormalArrow(r2vArr(wp), nWorld, 0.018, col);
|
||
gNormals.add(arr);
|
||
}
|
||
}
|
||
|
||
// ── observed markers + normals + error lines ──
|
||
const obs = {};
|
||
if (arucoData) {
|
||
for (const m of (arucoData.markers||[])) {
|
||
const mid = m.marker_id ?? m.id;
|
||
if (mid == null) continue;
|
||
let pos, nor = null;
|
||
if (m.position_mm) pos = m.position_mm;
|
||
else if (m.position_m) pos = m.position_m.map(v=>v*1000);
|
||
else continue;
|
||
// optional orientation from step-3 output or render markers.json
|
||
if (m.normal_world) nor = m.normal_world;
|
||
else if (m.normal) nor = m.normal;
|
||
obs[mid] = { pos, nor };
|
||
}
|
||
}
|
||
|
||
const errors = [];
|
||
for (const [midStr, {pos: opos, nor: oNor}] of Object.entries(obs)) {
|
||
const mid = parseInt(midStr);
|
||
const mpos = modelPositions[mid];
|
||
const op = r2vArr(opos);
|
||
|
||
const errMm = mpos
|
||
? Math.sqrt(opos.reduce((s,v,i) => s+(v-mpos[i])**2, 0))
|
||
: null;
|
||
const col = errMm == null ? 0x888888
|
||
: errMm < 2 ? 0x3ecf6b
|
||
: errMm < 5 ? 0xf59e0b
|
||
: 0xff4f4f;
|
||
|
||
gObserved.add(makeSphere(op, 0.006, col));
|
||
|
||
// observed normal arrow — orange if available
|
||
if (oNor) {
|
||
try {
|
||
const obsArrow = makeNormalArrow(op, oNor, 0.018, 0xffaa00);
|
||
gObsNormals.add(obsArrow);
|
||
} catch (e) {
|
||
console.warn('Invalid normal for marker', mid, oNor, e);
|
||
}
|
||
} else if (modelNormals[mid]) {
|
||
// fallback: show model normal at observed position (grey = no obs normal data)
|
||
const obsArrow = makeNormalArrow(op, modelNormals[mid], 0.014, 0x666666);
|
||
gObsNormals.add(obsArrow);
|
||
}
|
||
|
||
if (mpos) {
|
||
errors.push(errMm);
|
||
const mp = r2vArr(mpos);
|
||
gErrors.add(makeLine(mp, op, col));
|
||
}
|
||
}
|
||
|
||
// ── stats ──
|
||
updateStats(errors, Object.keys(obs).length, Object.keys(modelPositions).length);
|
||
}
|
||
|
||
// ─── statistics panel ──────────────────────────────────────────
|
||
function updateStats(errArr, nObs, nModel) {
|
||
const el = document.getElementById('stats-content');
|
||
if (errArr.length === 0) {
|
||
el.innerHTML = `<div class="stat-line"><span>Observed:</span><span class="stat-val">${nObs}</span></div>
|
||
<div class="stat-line"><span>Model:</span><span class="stat-val">${nModel}</span></div>
|
||
<div class="stat-line"><span style="color:var(--muted)">No matches found</span></div>`;
|
||
return;
|
||
}
|
||
const sorted = [...errArr].sort((a,b)=>a-b);
|
||
const mean = errArr.reduce((s,v)=>s+v,0) / errArr.length;
|
||
const rms = Math.sqrt(errArr.reduce((s,v)=>s+v*v,0) / errArr.length);
|
||
const p50 = sorted[Math.floor(sorted.length*0.5)];
|
||
const p90 = sorted[Math.floor(sorted.length*0.9)];
|
||
const max = sorted[sorted.length-1];
|
||
|
||
const cls = v => v < 2 ? 'stat-ok' : v < 5 ? 'stat-warn' : 'stat-err';
|
||
|
||
el.innerHTML = `
|
||
<div class="stat-line"><span>Observed:</span><span class="stat-val">${nObs}</span></div>
|
||
<div class="stat-line"><span>Matched:</span><span class="stat-val">${errArr.length}</span></div>
|
||
<div class="stat-line"><span>Mean error:</span><span class="${cls(mean)}">${mean.toFixed(1)} mm</span></div>
|
||
<div class="stat-line"><span>RMS error:</span><span class="${cls(rms)}">${rms.toFixed(1)} mm</span></div>
|
||
<div class="stat-line"><span>Median:</span><span class="${cls(p50)}">${p50.toFixed(1)} mm</span></div>
|
||
<div class="stat-line"><span>p90:</span><span class="${cls(p90)}">${p90.toFixed(1)} mm</span></div>
|
||
<div class="stat-line"><span>Max:</span><span class="${cls(max)}">${max.toFixed(1)} mm</span></div>
|
||
<div style="margin-top:6px;font-size:10px;color:var(--muted)">
|
||
🟢 <2mm 🟡 2–5mm 🔴 >5mm</div>`;
|
||
}
|
||
|
||
function setStatus(msg) {
|
||
document.getElementById('status-bar').textContent = msg + ' — Drag to orbit · Scroll to zoom';
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════════
|
||
// Render loop
|
||
// ═══════════════════════════════════════════════════════════════
|
||
function onResize() {
|
||
const w = canvas.parentElement.clientWidth;
|
||
const h = canvas.parentElement.clientHeight;
|
||
renderer.setSize(w, h);
|
||
camera.aspect = w / h;
|
||
camera.updateProjectionMatrix();
|
||
}
|
||
window.addEventListener('resize', onResize);
|
||
onResize();
|
||
|
||
function animate() {
|
||
requestAnimationFrame(animate);
|
||
controls.update();
|
||
renderer.render(scene, camera);
|
||
}
|
||
animate();
|
||
</script>
|
||
</body>
|
||
</html>
|