From 2582b2adc973a5518b2052e422ddc62c714a0e39 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Tue, 16 Jun 2026 05:48:34 +0200 Subject: [PATCH] Claude: Rotation-Fix --- public/boardViewer.html | 50 ++++++++++++++++++++++---------- scripts/robot_1781069752019.json | 2 +- 2 files changed, 35 insertions(+), 17 deletions(-) diff --git a/public/boardViewer.html b/public/boardViewer.html index 403e9a6..8c0fa52 100644 --- a/public/boardViewer.html +++ b/public/boardViewer.html @@ -322,7 +322,16 @@ function buildSkeletonFK(robot, angles) { const skel = link.skeleton; if (skel?.from && skel?.to) { const [fx, fy, fz] = skel.from; - const [tx, ty, tz] = skel.to; + // Endpunkt folgt dem (kalibrierten) jointToParent.origin des einzigen + // Kind-Links, statt der statischen skeleton.to-Koordinate. Dadurch wandert + // z.B. die Basis-Linie mit, wenn der Arm1-Pivot in Y/Z kalibriert wurde. + let toCoord = skel.to; + const childNames = order.filter(cn => links[cn]?.parent === linkName); + if (childNames.length === 1) { + const childOrigin = links[childNames[0]]?.jointToParent?.origin; + if (Array.isArray(childOrigin) && childOrigin.length >= 3) toCoord = childOrigin; + } + const [tx, ty, tz] = toCoord; const fromW = new THREE.Vector3(fx * S, fz * S, -fy * S).applyMatrix4(childFrame); const toW = new THREE.Vector3(tx * S, tz * S, -ty * S).applyMatrix4(childFrame); @@ -347,22 +356,31 @@ function buildSkeletonFK(robot, angles) { const posWorld = new THREE.Vector3(lx * S, lz * S, -ly * S).applyMatrix4(childFrame); const markerSizeM = (m.size ?? 25) * S; const [nx, ny, nz] = m.normal ?? [0, 0, 1]; - const normalW = new THREE.Vector3(nx, nz, -ny).transformDirection(childFrame).normalize(); - // P1: Quadrat mit spin-Rotation (um die Marker-Normale in Welt-Koordinaten) - const markerMesh = makeMarkerSquareOriented(posWorld, normalW, markerSizeM, col); + // Marker-Orientierung ZUERST im lokalen Link-Frame bauen, DANN die volle + // childFrame-Rotation anwenden. So wird der Roll (Drehung des Markers um + // seine eigene Normale) korrekt mitgeführt — auch wenn die Link-Drehachse + // parallel zur Marker-Normale liegt (z.B. Marker 197: normal [-1,0,0] ∥ + // Arm1-Achse [-1,0,0]). Eine reine Welt-Normalen-Rekonstruktion würde + // genau diesen Anteil verlieren. + const nLocal = new THREE.Vector3(nx, nz, -ny).normalize(); // robot→three.js const spinRad = ((m.spin ?? 0) * Math.PI) / 180; - if (Math.abs(spinRad) > 1e-6) { - markerMesh.quaternion.premultiply( - new THREE.Quaternion().setFromAxisAngle(normalW, spinRad) - ); - } + const qNormalLoc = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLocal); + const qSpinLoc = new THREE.Quaternion().setFromAxisAngle(nLocal, spinRad); + const qMarkerLoc = qSpinLoc.multiply(qNormalLoc); // Q_spin ∘ Q_normal (lokal) + const qFrame = new THREE.Quaternion().setFromRotationMatrix(childFrame); + const qMarkerW = qFrame.clone().multiply(qMarkerLoc); // in Welt drehen + const normalW = nLocal.clone().applyQuaternion(qFrame).normalize(); + + // P1: orientiertes Quadrat (Normale + Roll + Spin in einem Quaternion). + // PlaneGeometry hat nativ die +Z-Normale, qMarkerW dreht +Z auf die + // Marker-Normale inkl. Link-Roll und Spin. + const markerMesh = makeMarkerSquareQuat(posWorld, qMarkerW, markerSizeM, col); gArmMarkers.add(markerMesh); gArmMarkers.add(makeSphere(posWorld, 0.0006, col)); // P3b (Modell-Seite): Orientierungszeiger zur Ecke 0 (top-left bei spin=0) - // markerMesh.quaternion kodiert bereits Q_normal ∘ Q_spin - const ptrDir = new THREE.Vector3(1, 1, 0).normalize().applyQuaternion(markerMesh.quaternion); + const ptrDir = new THREE.Vector3(1, 1, 0).normalize().applyQuaternion(qMarkerW); const corner0W = posWorld.clone().add(ptrDir.multiplyScalar(markerSizeM * Math.SQRT1_2)); gArmMarkers.add(makeLine(posWorld, corner0W, col, 0.9)); gArmMarkers.add(makeSphere(corner0W, 0.0008, col)); @@ -454,15 +472,15 @@ function makeMarkerSquare(pos, size, color) { return m; } -function makeMarkerSquareOriented(pos, normalVec, size, color) { +// Quadrat mit voll vorgegebener Orientierung (Quaternion). PlaneGeometry hat +// nativ die +Z-Normale; das Quaternion dreht den Marker komplett (Normale, Roll, +// Spin) – nötig, damit der Roll bei achs-paralleler Normale nicht verloren geht. +function makeMarkerSquareQuat(pos, quat, size, color) { const geo = new THREE.PlaneGeometry(size, size); const mat = new THREE.MeshPhongMaterial({ color, side: THREE.DoubleSide, transparent: true, opacity: 0.85 }); const mesh = new THREE.Mesh(geo, mat); mesh.position.copy(pos); - const n = normalVec.clone().normalize(); - if (n.lengthSq() > 1e-9) { - mesh.quaternion.setFromUnitVectors(new THREE.Vector3(0, 0, 1), n); - } + mesh.quaternion.copy(quat); return mesh; } diff --git a/scripts/robot_1781069752019.json b/scripts/robot_1781069752019.json index d359c5b..70f37bd 100644 --- a/scripts/robot_1781069752019.json +++ b/scripts/robot_1781069752019.json @@ -295,7 +295,7 @@ {"id": 198, "name": "aruco_198", "position": [0, -160, 35], "normal": [0, 0, 1], "size": 25, "spin": 90}, {"id": 229, "name": "aruco_229", "position": [0, -250, 35], "normal": [0, 0, 1], "size": 25, "spin": 90}, {"id": 242, "name": "aruco_242", "position": [0, -250, -35], "normal": [0, 0, -1], "size": 25, "spin": 0}, - {"id": 243, "name": "aruco_243", "position": [0, -285, 0], "normal": [0, -1, 0], "size": 25, "spin": 90}, + {"id": 243, "name": "aruco_243", "position": [0, -285, 0], "normal": [0, -1, 0], "size": 25, "spin": 180}, {"id": 197, "name": "aruco_197", "position": [-35, -250, 0], "normal": [-1, 0, 0], "size": 25, "spin": 0} ], "model": [