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

boardViewer

Browse Source
This commit is contained in:
chk
2026-06-19 06:43:06 +02:00
parent d36ef6189d
commit aa78116837
8 changed files with 593 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
public/boardViewer.html
View File

@@ -367,20 +367,24 @@ function buildSkeletonFK(robot, angles) {
const markerSizeM = (m.size ?? 25) * S;
const [nx, ny, nz] = m.normal ?? [0, 0, 1];
// 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
// Marker-Orientierung ZUERST im lokalen ROBOT-Frame bauen (rohe Normale:
// Minimal-Rotation [0,0,1]→Normale + Spin um die Normale), DANN über qView
// in three.js-Achsen und mit qFrame in die Welt drehen. Würde man die
// Normale wie früher schon VOR der Minimal-Rotation nach three.js drehen
// (nx,nz,-ny), verdreht eine schräg liegende Normale (z.B. [-1,0,1]) das
// Quadrat zusätzlich um ihren Azimut (~45°) um die eigene Achse; der Spin
// kann das nicht kompensieren. Der Link-Roll um die Normale bleibt
// erhalten, weil qFrame zuletzt wirkt (z.B. Marker 197: normal [-1,0,0] ∥
// Arm1-Achse). Gegen triangulierte Ecken geprüft (Capture 20260616_133151,
// Marker 146): diese Reihenfolge 0.8°, die alte 45.5°.
const nRobot = new THREE.Vector3(nx, ny, nz).normalize(); // rohe robot-Normale
const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
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 qNormalLoc = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nRobot);
const qSpinLoc = new THREE.Quaternion().setFromAxisAngle(nRobot, spinRad);
const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2); // robot→three.js
const qFrame = new THREE.Quaternion().setFromRotationMatrix(childFrame);
const qMarkerW = qFrame.clone().multiply(qMarkerLoc); // in Welt drehen
const normalW = nLocal.clone().applyQuaternion(qFrame).normalize();
const qMarkerW = qFrame.clone().multiply(qView).multiply(qSpinLoc.multiply(qNormalLoc)); // lokal → three.js → Welt
const normalW = new THREE.Vector3(nx, nz, -ny).applyQuaternion(qFrame).normalize();
// P1: orientiertes Quadrat (Normale + Roll + Spin in einem Quaternion).
// PlaneGeometry hat nativ die +Z-Normale, qMarkerW dreht +Z auf die

67
public/sceneViewer.html
View File

@@ -483,39 +483,23 @@ function transformDirByT(T, dir) {
];
}
function makeMarkerSquare(pos, normal, size, color) {
// Marker-Quadrat mit vorab berechneter Orientierung (Quaternion). Die
// BoxGeometry ist dünn in lokal-Z, ihre Normale ist also lokal +Z — quat
// dreht +Z auf die Marker-Normale inkl. Link-Rotation und Spin.
//
// Die Orientierung MUSS im lokalen Link-Frame gebaut und erst danach in die
// Szene gedreht werden (siehe Aufrufer). Würde man wie früher
// setFromUnitVectors([0,0,1], welt_normale) NACH dem robot→three.js-
// Achsentausch anwenden, verdreht eine schräg liegende Normale (z.B.
// [-1,0,1]) das Quadrat zusätzlich um ihren Azimut (~45°) um die eigene
// Achse, und der Spin fehlt ganz. Gegen triangulierte Ecken geprüft
// (Capture 20260616_133151, Marker 146): lokale Variante 0.8°, alte 45.5°.
function makeMarkerSquareQuat(pos, quat, size, color) {
const geo = new THREE.BoxGeometry(size, size, size * 0.1);
const mat = new THREE.MeshPhongMaterial({
color,
shininess: 40
});
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
);
}
m.quaternion.copy(quat);
return m;
}
@@ -837,8 +821,19 @@ function rebuild() {
// ── model markers + normals ──
const modelPositions = {};
const modelNormals = {};
// robot→three.js view rotation (x,y,z)->(x,z,-y) == Rot_x(-90°). Applied LAST,
// so the marker orientation can be built in the robot/link frame first.
const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2);
for (const [lname, ld] of Object.entries(links)) {
const col = linkColor(lname);
// link rotation in the robot frame (from FK), as a quaternion
const Tl = T[lname] || I4();
const qLink = new THREE.Quaternion().setFromRotationMatrix(new THREE.Matrix4().set(
Tl[0], Tl[1], Tl[2], 0,
Tl[4], Tl[5], Tl[6], 0,
Tl[8], Tl[9], Tl[10], 0,
0, 0, 0, 1
));
for (const m of (ld.markers||[])) {
if (!m.position) continue;
const mid = m.id;
@@ -849,8 +844,16 @@ function rebuild() {
modelPositions[mid] = wp;
modelNormals[mid] = nWorld;
const sq = makeMarkerSquare(r2vArr(wp), r2vDir(...nWorld), 0.022, col);
gModel.add(sq);
// Orientierung ZUERST im lokalen Link-Frame (robot): Minimal-Rotation
// [0,0,1]→Normale, dann Spin um diese Normale; DANN qLink (Link-Drehung)
// und qView (in die Szene). So bleibt der Roll des Links um die Normale
// erhalten und der Spin-Azimut-Twist entfällt (siehe makeMarkerSquareQuat).
const nLR = new THREE.Vector3(nLocal[0], nLocal[1], nLocal[2]).normalize();
const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
const qNormal = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLR);
const qSpin = new THREE.Quaternion().setFromAxisAngle(nLR, spinRad);
const qMarker = qView.clone().multiply(qLink).multiply(qSpin.multiply(qNormal));
gModel.add(makeMarkerSquareQuat(r2vArr(wp), qMarker, 0.022, col));
// normal arrow (length = half a marker size = ~12.5mm → 0.0125m)
const arr = makeNormalArrow(r2vArr(wp), nWorld, 0.018, col);