diff --git a/packages/posecode-eval/src/probe.ts b/packages/posecode-eval/src/probe.ts index f7b1e84..a98e33f 100644 --- a/packages/posecode-eval/src/probe.ts +++ b/packages/posecode-eval/src/probe.ts @@ -260,7 +260,7 @@ export function probeMovement(source: string): ProbeResult { ...info.reaches.map((r) => ({ effector: r.effector, anchor: r.target })), ])); const propPush: Vec3 = [m.root.position.x - prePush.x, 0, m.root.position.z - prePush.z]; - alignFloorPalms(m, info.reaches, info.pins); + alignFloorPalms(m, info.reaches, info.pins, info.groundLock); // Plantigrade correction (viewer parity): flatten planted soles. This lifts // the foot mesh a little, so it must run BEFORE the floor clamp reconciles. levelPlantedFeet(m, info.groundLock); diff --git a/packages/posecode-render/src/contacts.ts b/packages/posecode-render/src/contacts.ts index 964a794..e8f206d 100644 --- a/packages/posecode-render/src/contacts.ts +++ b/packages/posecode-render/src/contacts.ts @@ -6,11 +6,18 @@ import type { Mannequin } from "./mannequin.js"; const DOWN = new THREE.Vector3(0, -1, 0); const DEG = Math.PI / 180; -/** Rotate contacting wrists so the palm face normal points into the floor. */ +/** + * Rotate contacting wrists so the palm face normal points into the floor. + * Applies to hands pressed to the floor via `reach`/`pin: hands floor` AND via + * `ground-lock: hands` (a high plank / push-up / mountain-climber): those bear + * weight flat on the ground and must be palm-down, not left in their raw FK + * (pronated) orientation. + */ export function alignFloorPalms( m: Mannequin, reaches: readonly ReachTarget[], pins: readonly PinTarget[], + groundLock: readonly string[] = [], ): void { const sides = new Set<"left" | "right">(); const collect = (effector: string, target: string) => { @@ -20,6 +27,10 @@ export function alignFloorPalms( }; reaches.forEach((r) => collect(r.effector, r.target)); pins.forEach((p) => collect(p.effector, p.anchor)); + if (groundLock.includes("hands")) { + sides.add("left"); + sides.add("right"); + } for (const side of sides) { const wrist = m.bones.get(`wrist_${side}`); diff --git a/packages/posecode-render/src/groundlock.ts b/packages/posecode-render/src/groundlock.ts index fe803f6..c90e3a3 100644 --- a/packages/posecode-render/src/groundlock.ts +++ b/packages/posecode-render/src/groundlock.ts @@ -100,6 +100,15 @@ export function applyGroundLock( const upperSupports = forearms.length > 0 ? forearms : hands; if (upperSupports.length > 0 && feet.length > 0) { + // Plant the feet FIRST: drop the body so the foot mesh rests on the floor, + // so the pivot the body then rotates about is itself at floor level. The + // rotation keeps the pivot fixed, so grounding the feet up front is what + // lets BOTH ends land — without it the pivot sits at whatever height the + // toes happened to reach and only the forearms plant while the feet float + // (the "plank feet off the ground" bug). A straight, correctly-authored + // plank has its forearms/toes near-coplanar, so the follow-up rotation is + // small; a piked pose still ends with the toes planted. + dropFeetToFloor(m, feet); const pivot = avgWorld(m, feet); // Newton iterations: rotate about the toes until the authored upper-body // support reaches the floor (palms for high plank, elbows for forearm @@ -132,21 +141,31 @@ export function applyGroundLock( // Ground the FOOT MESH's lowest point, not the ankle bone's origin: the // bone sits ~0.04m above the sole (foot box + capsule radius), so // anchoring the bone itself left the visible foot sunk into the floor. - let minY = Infinity; - for (const id of feet) { - const node = m.bones.get(id); - if (!node) continue; - const box = new THREE.Box3().setFromObject(node); - if (Number.isFinite(box.min.y)) minY = Math.min(minY, box.min.y); - } - if (Number.isFinite(minY)) { - m.root.position.y -= minY; - m.root.updateMatrixWorld(true); - } + dropFeetToFloor(m, feet); if (anchors) plantFeetHorizontally(m, feet, anchors); } } +/** + * Drop the whole body vertically so the lowest FOOT-mesh point rests on the + * floor. Grounds the visible sole (bounding box), not the ankle bone, which + * sits ~0.04m above it. Shared by the feet-only path and the plank/push-up path + * (which grounds the feet before pivoting the body onto its hands). + */ +function dropFeetToFloor(m: Mannequin, feet: string[]): void { + let minY = Infinity; + for (const id of feet) { + const node = m.bones.get(id); + if (!node) continue; + const box = new THREE.Box3().setFromObject(node); + if (Number.isFinite(box.min.y)) minY = Math.min(minY, box.min.y); + } + if (Number.isFinite(minY)) { + m.root.position.y -= minY; + m.root.updateMatrixWorld(true); + } +} + /** * Translate the root in X/Z so grounded feet return to their anchors (see * module doc). Runs after vertical grounding so "near the floor" is judged in diff --git a/packages/posecode-render/src/index.ts b/packages/posecode-render/src/index.ts index 61829d3..be62925 100644 --- a/packages/posecode-render/src/index.ts +++ b/packages/posecode-render/src/index.ts @@ -698,7 +698,7 @@ export function createViewer( // bug. Ground-lock and pins have already fixed the root placement that // floor/landmark targets resolve against. applyReaches(info.reaches); - alignFloorPalms(mannequin, info.reaches, info.pins); + alignFloorPalms(mannequin, info.reaches, info.pins, info.groundLock); // Plantigrade correction: keep planted soles flat to the floor so grounded // lower-body poses (squat, lunge, deadlift) don't balance on the toes. // Runs before the floor clamp so the leveled sole is what rests on y=0. @@ -710,7 +710,7 @@ export function createViewer( mannequin, gripSidesOf(info.grips), authoredFingers, - floorHandSidesOf(info.reaches, info.pins), + floorHandSidesOf(info.reaches, info.pins, info.groundLock), ); // L4.3 aliveness: turn the head toward the active contact (bar / floor reach). applyLookAt(info); @@ -843,7 +843,7 @@ export function createViewer( mannequin, gripSidesOf(ir.phases[0]?.grips ?? []), authoredFingers, - floorHandSidesOf(ir.phases[0]?.reaches ?? [], ir.phases[0]?.pins ?? []), + floorHandSidesOf(ir.phases[0]?.reaches ?? [], ir.phases[0]?.pins ?? [], ir.phases[0]?.groundLock ?? []), ); applyLookAt({ grips: ir.phases[0]?.grips ?? [], reaches: ir.phases[0]?.reaches ?? [] }); captureGroundTargets(); @@ -1080,13 +1080,17 @@ function gripSidesOf(grips: readonly { effector: string }[]): Set<"left" | "righ } /** - * Hand sides pressed onto the floor this phase (a `reach`/`pin: hands floor`). - * Their fingers rest flat instead of taking the idle inward hook, so a plank or - * push-up hand lies on the ground rather than clawing into it. + * Hand sides pressed onto the floor this phase — via `reach`/`pin: hands floor` + * OR `ground-lock: hands` (a high plank / push-up / mountain-climber, where the + * hands bear weight flat on the ground). Their fingers rest flat instead of + * taking the idle inward hook, so the palm lies on the floor rather than + * clawing into it. Ground-locked hands never carry a `floor` reach/pin, so + * without the ground-lock check they were mis-read as free and hooked up. */ function floorHandSidesOf( reaches: readonly { effector: string; target: string }[], pins: readonly { effector: string; anchor: string }[], + groundLock: readonly string[] = [], ): Set<"left" | "right"> { const sides = new Set<"left" | "right">(); const add = (effector: string): void => { @@ -1095,6 +1099,7 @@ function floorHandSidesOf( }; for (const r of reaches) if (r.target === "floor") add(r.effector); for (const p of pins) if (p.anchor === "floor") add(p.effector); + if (groundLock.includes("hands")) add("hands"); return sides; } diff --git a/packages/posecode-render/src/timeline.ts b/packages/posecode-render/src/timeline.ts index e1c1588..852e729 100644 --- a/packages/posecode-render/src/timeline.ts +++ b/packages/posecode-render/src/timeline.ts @@ -21,6 +21,16 @@ interface Keyframe { name: string; cue?: string; easing: TimingMode; + /** + * The figure is at rest here (zero boundary velocity), so the spline uses this + * keyframe's own value as its control (no velocity carried across it). True for + * settle/snap phases AND for the two structural anchors — the start pose and + * the loop-reset — which represent the figure standing still at the base pose. + * Those anchors have no real predecessor/successor, so deriving a squad tangent + * from a clamped neighbor yields a backward-biased control that overshoots + * (the "snap to fully-curled" biceps bug); a rest tangent slerps cleanly. + */ + rest: boolean; quats: Map; groundLock: string[]; reaches: ReachTarget[]; @@ -115,6 +125,7 @@ export function buildTimeline(ir: PosecodeIR): BuiltTimeline { time: 0, name: ir.startPose ?? "start", easing: "flow", + rest: true, quats: snapshot(curr), groundLock: [], reaches: [], @@ -138,6 +149,7 @@ export function buildTimeline(ir: PosecodeIR): BuiltTimeline { name: phase.name, ...(phase.cue ? { cue: phase.cue } : {}), easing: phase.easing, + rest: REST_MODE[phase.easing], quats: snapshot(curr), groundLock: phase.groundLock, reaches: phase.reaches, @@ -160,6 +172,7 @@ export function buildTimeline(ir: PosecodeIR): BuiltTimeline { time: t, name: "reset", easing: "flow", + rest: true, quats: snapshot(new Map(baseJoints)), groundLock: [], reaches: [], @@ -227,10 +240,10 @@ export function buildTimeline(ir: PosecodeIR): BuiltTimeline { // A rest-point keyframe uses its own value as the control (zero tangent // → the spline comes to / leaves from rest there); otherwise the // Shoemake control from the neighboring keyframe carries velocity. - const s0 = REST_MODE[a.easing] + const s0 = a.rest ? q0.clone() : squadControl(kPrev.quats.get(bone)!, q0, q1); - const s1 = REST_MODE[b.easing] + const s1 = b.rest ? q1.clone() : squadControl(q0, q1, kNext.quats.get(bone)!); squad(q0, s0, s1, q1, eased, node.quaternion); diff --git a/packages/posecode-render/test/render.test.ts b/packages/posecode-render/test/render.test.ts index 95ae7bd..b9b45a5 100644 --- a/packages/posecode-render/test/render.test.ts +++ b/packages/posecode-render/test/render.test.ts @@ -74,6 +74,47 @@ describe("timeline", () => { expect(start.angleTo(lowered)).toBeGreaterThan(1.0); // ~90deg in radians }); + + // Regression: a large rest-to-rest move (biceps curl: elbow flex 135 + + // supinate 80, near-antipodal endpoints) must sweep MONOTONICALLY. The squad + // spline used to derive a backward-biased control at the clamped start + // keyframe, flinging the forearm past +50deg then snapping ~135deg in a single + // step — the "curl happens suddenly" bug. The start/reset anchors are now rest + // points, so their segments slerp cleanly. + it("curls the forearm monotonically from rest (no squad overshoot snap)", () => { + const CURL = [ + 'posecode exercise "Curl"', + " rig humanoid", + " pose start = standing", + ' step "Curl" 1.1s settle:', + " elbows: flex 135", + " elbows: supinate 80", + ' step "Lower" 1.4s drive:', + " elbows: flex 15", + " elbows: supinate 80", + " repeat 10", + ].join("\n"); + const { ir } = parse(CURL); + const tl = buildTimeline(ir!); + const m = buildMannequin(); + const forearm = new THREE.Vector3(0, -1, 0); // wrist sits at elbow-local -Y + const wq = new THREE.Quaternion(); + const dir = new THREE.Vector3(); + let prev: THREE.Vector3 | null = null; + // Walk the 1.1s Curl segment in even steps. The forearm swings ~135deg + // total, so at this resolution each step is small; the overshoot bug instead + // parked the forearm near rest then jumped ~135deg in a single step. Assert + // no adjacent step exceeds 0.9rad (~50deg): the fix keeps every step under + // ~0.5rad, while the snap produced a ~2.3rad jump. + for (let t = 0; t <= 1.1 + 1e-9; t += 1.1 / 22) { + tl.sample(t, m.bones); + m.root.updateMatrixWorld(true); + m.bones.get("elbow_left")!.getWorldQuaternion(wq); + dir.copy(forearm).applyQuaternion(wq).normalize(); + if (prev) expect(dir.angleTo(prev)).toBeLessThan(0.9); + prev = dir.clone(); + } + }); }); describe("hip-hinge coupling", () => {