Anthropic Claude
This commit is contained in:
chk
894
run/robot_viewer.html
Normal file
894
run/robot_viewer.html
Normal file
@@ -0,0 +1,894 @@
|
||||
<!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 < 5 ? 0x3ecf6b
|
||||
: errMm < 15 ? 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 < 5 ? 'stat-ok' : v < 15 ? '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)">
|
||||
🟢 <5mm 🟡 5–15mm 🔴 >15mm</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>
|
||||
@@ -95,6 +95,6 @@ echo [STEP 4] Robotics Pose calculation of angles and joint positions
|
||||
|
||||
|
||||
python3 "..\pipeline\4_robotState_estimation_v6.py" ^
|
||||
"%OUT_DIR%\aruco_positions_optimized.json" ^
|
||||
"%OUT_DIR%\aruco_positions_initial.json" ^
|
||||
-robot "%ROBOT_JSON%" ^
|
||||
-out "%OUT_DIR%\robot_state.json"
|
||||
|
||||
100
run/run_pipeline_v8.bat
Normal file
100
run/run_pipeline_v8.bat
Normal file
@@ -0,0 +1,100 @@
|
||||
@echo off
|
||||
setlocal EnableDelayedExpansion
|
||||
REM 3_pipeline_multiview.bat
|
||||
REM Multi-camera ArUco detection and pose estimation pipeline
|
||||
REM Parametr e.g.
|
||||
REM run_pipeline.bat ../data/simulation/Scene4
|
||||
|
||||
|
||||
|
||||
|
||||
REM Wenn kein Argument übergeben wurde
|
||||
if "%1"=="" (
|
||||
echo.
|
||||
echo [32m[INFO] Aufruf fehlt![0m
|
||||
echo [32mBeispiel: .\run_pipeline.bat ../data/simulation/Scene4[0m
|
||||
echo.
|
||||
exit /b
|
||||
)
|
||||
|
||||
|
||||
set "IMAGES=%~1"
|
||||
|
||||
REM trailing slash entfernen
|
||||
if "%IMAGES:~-1%"=="\" set "IMAGES=%IMAGES:~0,-1%"
|
||||
if "%IMAGES:~-1%"=="/" set "IMAGES=%IMAGES:~0,-1%"
|
||||
|
||||
set ROBOT_JSON=..\data\robot\robot.json
|
||||
set BASE_OUT_DIR=C:\Users\kech\SynologyDrive\2026-AppServer-AppRobot\appRobotRendering\data\evaluations
|
||||
|
||||
REM ✅ richtigen Namen extrahieren
|
||||
for %%I in ("%IMAGES%") do set "SCENE_NAME=%%~nxI"
|
||||
|
||||
set OUT_DIR=%BASE_OUT_DIR%\%SCENE_NAME%
|
||||
|
||||
|
||||
|
||||
echo.
|
||||
echo [STEP 1] Detect ArUco markers from all cameras in the folder %IMAGES%
|
||||
|
||||
for %%F in ("%IMAGES%\*.png" "%IMAGES%\*.PNG" "%IMAGES%\*.jpg" "%IMAGES%\*.jpeg" "%IMAGES%\*.JPG" "%IMAGES%\*.JPEG") do (
|
||||
|
||||
REM Dateiname ohne Pfad und ohne .png
|
||||
set "NAME=%%~nF"
|
||||
echo Bearbeite: !NAME!
|
||||
|
||||
REM Split bei "_" → nehme 2. Teil (die ID)
|
||||
for /f "tokens=2 delims=_" %%A in ("%%~nF") do (
|
||||
set "CAMID=%%A"
|
||||
|
||||
REM Takes files and detected arucos output to render_c_aruco_detection.json
|
||||
python3 ../pipeline/1_detect_aruco_observations.py ^
|
||||
-i "%%F" ^
|
||||
-npz "%IMAGES%\render_a.npz" ^
|
||||
-outDir %OUT_DIR% ^
|
||||
-robot %ROBOT_JSON% ^
|
||||
-cameraId !CAMID!
|
||||
)
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [STEP 2] Estimate camera poses from detections
|
||||
|
||||
for %%F in ("%OUT_DIR%\*_aruco_detection.json") do (
|
||||
echo Bearbeite: %%F
|
||||
|
||||
python3 ../pipeline/2_estimate_camera_from_observations.py ^
|
||||
-i "%%F" ^
|
||||
-robot %ROBOT_JSON% ^
|
||||
-outDir %OUT_DIR%
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [STEP 3] Triangulate marker positions from multi-view observations
|
||||
|
||||
|
||||
REM Alle detection files sammeln
|
||||
for %%F in ("%OUT_DIR%\*_aruco_detection.json") do (
|
||||
set DET_ARGS=!DET_ARGS! -det "%%F"
|
||||
)
|
||||
|
||||
REM Alle pose files sammeln
|
||||
for %%F in ("%OUT_DIR%\*_camera_pose.json") do (
|
||||
set POSE_ARGS=!POSE_ARGS! -pose "%%F"
|
||||
)
|
||||
|
||||
python3 "..\pipeline\3_multiview_bundle_adjustment_v4.py" ^
|
||||
-robot "%ROBOT_JSON%" ^
|
||||
-lambdaWeight 100.0 ^
|
||||
!DET_ARGS! ^
|
||||
!POSE_ARGS!
|
||||
|
||||
|
||||
echo.
|
||||
echo [STEP 4] Robotics Pose calculation of angles and joint positions
|
||||
|
||||
|
||||
python3 "..\pipeline\4_v8_4a_base_slider.py" ^
|
||||
"%OUT_DIR%\aruco_positions_initial.json" ^
|
||||
-robot "%ROBOT_JSON%" ^
|
||||
-out "%OUT_DIR%\robot_state.json"
|
||||
Reference in New Issue
Block a user