@konitif/viewer-3d 0.285.5 → 0.285.6

package/dist/renderer/Viewer3DRenderer.js

@@ -5,6 +5,7 @@ import { resolvePhysicsSourceString, resolvePhysicsSourceStringListMap, resolveP
 import { createClockGraph, createInvalidationController, createTemporalTransport } from '@konitif/temporal';
 import { createViewerTemporalScheduler } from './ViewerTemporalScheduler.js';
 import { copyViewerMetadata } from './ViewerMetadata.js';
+import { IncarnationMaterialProjector, normalizeIncarnationMaterialProjection } from './IncarnationMaterialProjection.js';
 import { resolveInteractionInputActions, resolveInteractionInputBindings } from '@konitif/tools/input';
 import { AxesHelper, Box3, BoxGeometry, BufferGeometry, CanvasTexture, Color, ConeGeometry, CylinderGeometry, DirectionalLight, DoubleSide, Float32BufferAttribute, FogExp2, GridHelper, Group, HalfFloatType, HemisphereLight, Line, LineBasicMaterial, LineSegments, Matrix4, MathUtils, Mesh, MeshBasicMaterial, MeshDepthMaterial, NoToneMapping, Object3D, OrthographicCamera, Plane, PlaneGeometry, PerspectiveCamera, VSMShadowMap, Quaternion, Raycaster, Scene, CircleGeometry, SphereGeometry, Sprite, SpriteMaterial, SRGBColorSpace, Vector2, Vector3, WebGLRenderTarget, WebGLRenderer } from 'three';
 import { ArcballControls } from 'three/examples/jsm/controls/ArcballControls.js';
@@ -30,6 +31,8 @@ import { resolveViewerIncarnationHealth } from './ViewerIncarnationHealth.js';
 import { PerformanceMonitor } from './PerformanceMonitor.js';
 import { ViewerTemporalProjectionBuffer } from './TemporalProjectionModel.js';
 import { ObservedRobotPresentationBuffer } from './ObservedRobotPresentationBuffer.js';
+import { ObservedSoleAnchors, projectBoundedSoleCorrection } from './ObservedSoleStabilization.js';
+import { ConvexHull } from 'three/examples/jsm/math/ConvexHull.js';
 import { resolveViewerFrameSignalIntervalMs, VIEWER_FRAME_SIGNAL_CATALOG } from './ViewerFrameSignals.js';
 import { DEFAULT_DISPLAY_FRAME_RATE_HZ, normalizeConfiguredDisplayFrameRate, observeDisplayFrameRateCeiling, resolveDisplayFrameRateTarget } from './displayFrameRate.js';
 import { ViewerTemporalProjectionExperimentAdapter } from './ViewerTemporalProjectionExperimentAdapter.js';
@@ -219,6 +222,8 @@ export class Viewer3D {
     poseControlTarget = 'simulated';
     observedPoseLiveControlEnabled = false;
     robotMaterialBaselines = new Map();
+    incarnationMaterialProjector = new IncarnationMaterialProjector();
+    incarnationMaterialProjections = {};
     observedRobotGhostMaterialBaselines = new Map();
     observedRobotGhostRenderOrderBaselines = new Map();
     observedRobotGhostShadowMaterials = new Map();
@@ -229,6 +234,11 @@ export class Viewer3D {
     observedRobotGhostBaseQuaternion = new Quaternion();
     observedRobotGhostBaseScale = new Vector3(1, 1, 1);
     observedRobotTemporalSupportAnchorState = null;
+    observedSoleAnchors = new ObservedSoleAnchors();
+    observedSoleConnectionId = '';
+    observedSoleStrength = 0;
+    observedSoleBaseline = new Map();
+    observedSoleSurfaceVertices = new WeakMap();
     observedRobotGrounding = resolveObservedRobotGroundingProjection({
         enabled: false,
         floorY: ROBOT_VIEWER_SUPPORT_FLOOR_Y,
@@ -1051,11 +1061,17 @@ export class Viewer3D {
             sampleId: input.frameId,
             connectionId: input.connectionId,
             sourceObservedAtSeconds: input.observedAtSeconds,
+            sourceClock: input.sourceClock,
             receivedAtMs,
             jointValues: input.jointValues
         });
         if (!accepted)
             return false;
+        if (this.observedSoleConnectionId !== input.connectionId) {
+            this.restoreObservedSoleBaseline();
+            this.observedSoleAnchors.reset();
+            this.observedSoleConnectionId = input.connectionId;
+        }
         const frame = this.observedRobotPresentation.project(receivedAtMs);
         if (frame)
             this.applyObservedRobotGhostJointValues(frame.jointValues);
@@ -1063,14 +1079,35 @@ export class Viewer3D {
         return true;
     }
     resetObservedRobotGhostJointObservation() {
+        this.restoreObservedSoleBaseline();
+        this.observedSoleAnchors.reset();
         this.observedRobotPresentation.reset('discontinuity');
     }
+    setObservedRobotSoleStabilizationStrength(value) {
+        this.observedSoleAnchors ??= new ObservedSoleAnchors();
+        const strength = Number.isFinite(value) ? Math.max(0, Math.min(1, value)) : 0;
+        if (strength === this.observedSoleStrength)
+            return;
+        this.restoreObservedSoleBaseline();
+        this.observedSoleAnchors.reset();
+        this.observedSoleStrength = strength;
+        this.applyObservedRobotGhostGrounding();
+        this.requestRender();
+    }
+    restoreObservedSoleBaseline() {
+        this.observedSoleBaseline ??= new Map();
+        for (const [joint, value] of this.observedSoleBaseline)
+            joint.setJointValue(value);
+        this.observedSoleBaseline.clear();
+    }
     getObservedRobotPresentationStatus() {
         return this.observedRobotPresentation.getStatus();
     }
     applyObservedRobotGhostJointValues(values) {
         if (!this.observedRobotGhost)
             return false;
+        const hadSoleCorrection = (this.observedSoleBaseline?.size ?? 0) > 0;
+        this.restoreObservedSoleBaseline();
         const joints = this.observedRobotGhost.joints ?? {};
         let changed = false;
         for (const [target, value] of Object.entries(values)) {
@@ -1082,7 +1119,7 @@ export class Viewer3D {
                 continue;
             changed = joint.setJointValue(value) !== false || changed;
         }
-        if (changed) {
+        if (changed || hadSoleCorrection) {
             this.applyObservedRobotGhostGrounding();
             if (this.poseControlTarget === 'observed') {
                 this.refreshPoseHandleProjection();
@@ -1145,10 +1182,28 @@ export class Viewer3D {
         // invalidate that anchor.
         if (spatialLayoutChanged) {
             this.observedRobotTemporalSupportAnchorState = null;
+            this.observedSoleAnchors?.reset();
         }
         this.applyRobotComparisonAppearance();
         this.requestRender();
     }
+    /** Tune one visual incarnation independently; source meshes and telemetry remain unchanged. */
+    setIncarnationMaterialProjection(target, projection) {
+        if (target !== 'simulated' && target !== 'observed')
+            throw new Error('Invalid material incarnation target');
+        this.incarnationMaterialProjections[target] = normalizeIncarnationMaterialProjection(projection);
+        this.applyIncarnationMaterialProjections();
+        this.requestRender();
+    }
+    applyIncarnationMaterialProjections() {
+        for (const target of ['simulated', 'observed']) {
+            const projection = this.incarnationMaterialProjections[target];
+            if (!projection)
+                continue;
+            const materials = target === 'simulated' ? this.robotMaterialBaselines : this.observedRobotGhostMaterialBaselines;
+            this.incarnationMaterialProjector.apply(materials.keys(), projection);
+        }
+    }
     async setPhysicsEngine(physicsEngine, physicsEnabled = physicsEngine !== 'none') {
         if (this.destroyed)
             return;
@@ -5150,6 +5205,10 @@ export class Viewer3D {
                 return;
             const ghostMaterials = sourceMaterials.map((source) => {
                 const material = source.clone();
+                // Three material cloning does not retain consumer-owned shader hooks.
+                // Preserve the observed model's own hooks, never the simulated state.
+                material.onBeforeCompile = source.onBeforeCompile;
+                material.customProgramCacheKey = source.customProgramCacheKey;
                 this.captureMaterialBaseline(material, this.observedRobotGhostMaterialBaselines);
                 return material;
             });
@@ -5189,6 +5248,8 @@ export class Viewer3D {
         }
     }
     clearObservedRobotGhost() {
+        this.restoreObservedSoleBaseline();
+        this.observedSoleAnchors?.reset();
         this.cancelRobotComparisonVisibilityTransition();
         this.observedRobotPresentation.reset('discontinuity');
         this.simulatedRobotPresentationRoot.position.x = 0;
@@ -5232,6 +5293,7 @@ export class Viewer3D {
         this.applyBalanceDebugPresentationOffset();
     }
     applyRobotComparisonAppearance() {
+        this.applyIncarnationMaterialProjections();
         this.applyMaterialAppearance(this.robotMaterialBaselines, this.robotComparisonAppearance.simulated);
         this.applyMaterialAppearance(this.observedRobotGhostMaterialBaselines, this.robotComparisonAppearance.observed);
         this.applyObservedRobotGhostRenderOrder(this.robotComparisonAppearance.observed.opacity < 0.999);
@@ -5268,6 +5330,7 @@ export class Viewer3D {
         }
     }
     applyObservedRobotGhostGrounding() {
+        this.restoreObservedSoleBaseline();
         if (this.comparisonProfileId) {
             this.projectComparisonSimulation();
             return;
@@ -5295,6 +5358,12 @@ export class Viewer3D {
         robot.updateMatrixWorld(true);
         const supportContacts = this.resolveObservedRobotSupportContacts(robot);
         const supportPoints = supportContacts.supportPoints;
+        const lowestRawSupport = Math.min(...supportPoints.map(point => point.y));
+        const soleEligible = new Map();
+        for (const group of new Set(supportContacts.supportGroupIds)) {
+            const minimum = Math.min(...supportPoints.filter((_, index) => supportContacts.supportGroupIds[index] === group).map(point => point.y));
+            soleEligible.set(group, minimum - lowestRawSupport <= 0.008);
+        }
         const activeSupport = resolveObservedRobotActiveSupportProjection({
             supportPoints,
             contactIds: supportContacts.contactIds,
@@ -5343,18 +5412,134 @@ export class Viewer3D {
         robot.position.x += temporalSupportAnchor.offsetX;
         robot.position.z += temporalSupportAnchor.offsetZ;
         robot.updateMatrixWorld(true);
+        const stabilizedSurfaceMinimumY = this.applyObservedSoleStabilization(robot, soleEligible, footSupportEligibility.eligible);
         const observedSupport = this.resolveObservedRobotSupport(robot, activeSupport.activeContactCount);
         this.observedRobotGrounding = resolveObservedRobotGroundingProjection({
             enabled: this.options.groundObservedRobotGhost,
             floorY: ROBOT_VIEWER_SUPPORT_FLOOR_Y,
-            supportMinimumY: observedSupport.supportMinimumY,
+            supportMinimumY: stabilizedSurfaceMinimumY ?? observedSupport.supportMinimumY,
             supportContactCount: observedSupport.supportContactCount,
-            source: observedSupport.source
+            source: stabilizedSurfaceMinimumY === null || stabilizedSurfaceMinimumY === undefined ? observedSupport.source : 'foot-geometry'
         });
         robot.position.y += this.observedRobotGrounding.offsetY;
         robot.updateMatrixWorld(true);
         this.applyNavigationPreviewPresentationTransform();
     }
+    applyObservedSoleStabilization(robot, eligible, supportEligible) {
+        if (!(this.observedSoleStrength > 0) || !supportEligible)
+            return null;
+        const status = this.observedRobotPresentation?.getStatus();
+        if (status?.mode === 'raw' || status?.fallbackReason === 'sample_gap') {
+            this.observedSoleAnchors.reset();
+            return null;
+        }
+        const chains = this.options.supportDefinition?.soleStabilizationChains ?? [];
+        const joints = robot.joints ?? {};
+        const pose = (groupId, foot) => {
+            const contacts = this.resolveObservedRobotSupportContacts(robot);
+            const points = contacts.supportPoints.filter((_, index) => contacts.supportGroupIds[index] === groupId);
+            const center = points.reduce((sum, point) => sum.add(point), new Vector3()).multiplyScalar(1 / Math.max(1, points.length));
+            const rotation = foot.getWorldQuaternion(new Quaternion());
+            const supportMinimumY = this.resolvePreciseObservedSoleMinimumY(foot) ?? Math.min(...points.map(point => point.y));
+            return { position: [center.x, center.y, center.z], orientation: [rotation.x, rotation.y, rotation.z, rotation.w], supportMinimumY };
+        };
+        const candidates = chains.flatMap(chain => {
+            const selected = chain.jointNames.map(name => joints[name]);
+            const foot = selected[selected.length - 1];
+            if (!foot || typeof foot.getWorldQuaternion !== 'function' || selected.some(joint => !joint || joint.mimicJoint))
+                return [];
+            const baseline = selected.map(joint => this.readJointValue(joint));
+            if (baseline.some(value => typeof value !== 'number' || !Number.isFinite(value)))
+                return [];
+            return [{ chain, selected: selected, foot, baseline: baseline, raw: pose(chain.groupId, foot) }];
+        });
+        const targets = this.observedSoleAnchors.resolve(this.observedSoleConnectionId, candidates.map(candidate => ({
+            id: candidate.chain.groupId, position: candidate.raw.position,
+            eligible: this.options.groundObservedRobotGhost && supportEligible && eligible.get(candidate.chain.groupId) === true
+        })), this.observedSoleStrength);
+        // Both eligible soles share a surface before the final root grounding.
+        // Preserve lifted/released feet rather than treating every chain as contact.
+        const supportPlaneY = Math.min(...candidates.filter(candidate => targets.some(target => target.id === candidate.chain.groupId)).map(candidate => candidate.raw.supportMinimumY));
+        for (const target of targets) {
+            const candidate = candidates.find(entry => entry.chain.groupId === target.id);
+            const verticalOffset = supportPlaneY - candidate.raw.supportMinimumY;
+            const targetPosition = [target.target[0], candidate.raw.position[1] + verticalOffset, target.target[2]];
+            if (Math.hypot(...targetPosition.map((value, index) => value - candidate.raw.position[index])) < 0.00001)
+                continue;
+            const apply = (values) => {
+                candidate.selected.forEach((joint, index) => joint.setJointValue(values[index]));
+                robot.updateMatrixWorld(true);
+            };
+            try {
+                const correction = projectBoundedSoleCorrection({
+                    values: candidate.baseline,
+                    limits: candidate.selected.map(joint => [joint.limit?.lower ?? -Math.PI, joint.limit?.upper ?? Math.PI]),
+                    targetPosition, targetOrientation: candidate.raw.orientation,
+                    targetSupportMinimumY: supportPlaneY,
+                    evaluate: values => { apply(values); return pose(target.id, candidate.foot); }
+                });
+                apply(correction.values);
+                candidate.selected.forEach((joint, index) => this.observedSoleBaseline.set(joint, candidate.baseline[index]));
+            }
+            catch {
+                // Invalid/unreachable authored chains must leave the observed copy intact.
+                apply(candidate.baseline);
+            }
+        }
+        if (!targets.length)
+            return null;
+        // An unanchored foot must still not be pushed through the floor by root grounding.
+        const visibleMinima = candidates.map(candidate => this.resolvePreciseObservedSoleMinimumY(candidate.foot))
+            .filter((value) => value !== null);
+        const minimum = Math.min(...visibleMinima);
+        return Number.isFinite(minimum) ? minimum : null;
+    }
+    /** Exact directional support of rigid visible meshes, with cached convex vertices. */
+    resolvePreciseObservedSoleMinimumY(foot) {
+        this.observedSoleSurfaceVertices ??= new WeakMap();
+        let minimum = Infinity;
+        foot.traverse(node => {
+            const mesh = node;
+            if (!mesh.isMesh || mesh.isURDFCollider || !mesh.geometry)
+                return;
+            const attribute = mesh.geometry.getAttribute('position');
+            if (!attribute)
+                return;
+            const version = 'version' in attribute ? Number(attribute.version) : attribute.data.version;
+            let cached = this.observedSoleSurfaceVertices.get(mesh.geometry);
+            if (!cached || cached.attribute !== attribute || cached.version !== version) {
+                const points = [];
+                for (let index = 0; index < attribute.count; index++) {
+                    const point = new Vector3().fromBufferAttribute(attribute, index);
+                    if ([point.x, point.y, point.z].every(Number.isFinite))
+                        points.push(point);
+                }
+                let vertices = points;
+                if (points.length > 4) {
+                    try {
+                        const hull = new ConvexHull().setFromPoints(points);
+                        const unique = new Set();
+                        for (const face of hull.faces) {
+                            let edge = face.edge;
+                            do {
+                                unique.add(edge.vertex.point);
+                                edge = edge.next;
+                            } while (edge !== face.edge);
+                        }
+                        if (unique.size)
+                            vertices = [...unique];
+                    }
+                    catch { /* Degenerate meshes retain their exact original vertices. */ }
+                }
+                cached = { attribute, version, points: vertices };
+                this.observedSoleSurfaceVertices.set(mesh.geometry, cached);
+            }
+            const matrix = mesh.matrixWorld.elements;
+            for (const point of cached.points)
+                minimum = Math.min(minimum, matrix[1] * point.x + matrix[5] * point.y + matrix[9] * point.z + matrix[13]);
+        });
+        return Number.isFinite(minimum) ? minimum : null;
+    }
     resolveObservedRobotSupportContacts(robot) {
         const supportPoints = [];
         const contactIds = [];