ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Claude: Rotation-Fix

Browse Source
This commit is contained in:
chk
2026-06-16 05:48:34 +02:00
parent a79729ca1f
commit 2582b2adc9
2 changed files with 35 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
public/boardViewer.html
View File

@@ -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;
}

2
scripts/robot_1781069752019.json
View File

@@ -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": [