SuperHQ Motion experiment: locomotion and action checkpoints, runtime, clips, evaluation
6adbbb1 verified | /** Bounded runtime corrections for action poses. | |
| * Canonical poses only: host translation and collision state are never filtered. | |
| * Transition offsets use a critically damped decay; toe planting uses two-bone | |
| * IK with the predicted knee plane and ankle orientation preserved. | |
| */ | |
| export const add=(a,b)=>a.map((v,i)=>v+b[i]); | |
| export const sub=(a,b)=>a.map((v,i)=>v-b[i]); | |
| const scale=(a,s)=>a.map(v=>v*s),dot=(a,b)=>a.reduce((s,v,i)=>s+v*b[i],0); | |
| const cross=(a,b)=>[a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0]]; | |
| const length=a=>Math.hypot(...a),unit=a=>scale(a,1/Math.max(length(a),1e-12)); | |
| const bounded=(v,maximum)=>scale(v,Math.min(1,maximum/Math.max(length(v),1e-12))); | |
| const clamp=(v,a,b)=>Math.max(a,Math.min(b,v)); | |
| export const multiply=(a,b)=>[ | |
| a[3]*b[0]+a[0]*b[3]+a[1]*b[2]-a[2]*b[1], | |
| a[3]*b[1]-a[0]*b[2]+a[1]*b[3]+a[2]*b[0], | |
| a[3]*b[2]+a[0]*b[1]-a[1]*b[0]+a[2]*b[3], | |
| a[3]*b[3]-dot(a.slice(0,3),b.slice(0,3))]; | |
| const inverse=q=>[-q[0],-q[1],-q[2],q[3]]; | |
| const rotate=(v,q)=>multiply(multiply(q,[...v,0]),inverse(q)).slice(0,3); | |
| const log=q=>{if(q[3]<0)q=q.map(v=>-v);const n=length(q.slice(0,3));return scale(q.slice(0,3),n<1e-10?2:2*Math.atan2(n,q[3])/n);}; | |
| const exp=v=>{const a=length(v);return [...scale(v,a<1e-10?.5:Math.sin(a/2)/a),Math.cos(a/2)];}; | |
| const delta=(a,b)=>log(multiply(a,inverse(b))); | |
| const between=(a,b)=>{a=unit(a);b=unit(b);const d=clamp(dot(a,b),-1,1);if(d<-.999999){const basis=Math.abs(a[0])<.8?[1,0,0]:[0,1,0];return exp(scale(unit(cross(a,basis)),Math.PI));}const q=[...cross(a,b),1+d];return scale(q,1/Math.max(length(q),1e-12));}; | |
| const copy=p=>({...p,rootPosition:p.rootPosition.slice(),localQuaternions:p.localQuaternions.map(q=>q.slice())}); | |
| const DT=1/60; | |
| export function forward(pose,rig) { | |
| const positions=[],rotations=[]; | |
| for(let i=0;i<rig.names.length;i++){ | |
| const p=rig.parents[i],q=pose.localQuaternions[i]; | |
| positions.push(p<0?pose.rootPosition.slice():add(positions[p],rotate(rig.offsets[i],rotations[p]))); | |
| rotations.push(p<0?q:multiply(rotations[p],q)); | |
| } | |
| return {positions,rotations}; | |
| } | |
| export class ActionStabilizer { | |
| constructor(){this.previous=null;this.velocity=null;this.offset=null;this.offsetVelocity=null;this.age=0;} | |
| apply(raw,next,changed) { | |
| const p=copy(raw),rootLimit=.08,angleLimit=Math.PI/4; | |
| let saturated=false; | |
| if(changed&&this.previous){ | |
| const incomingRoot=scale(sub(next.rootPosition,raw.rootPosition),1/DT); | |
| const previousVelocity=this.velocity||{root:[0,0,0],rot:raw.localQuaternions.map(()=>[0,0,0])}; | |
| const requestedRoot=sub(add(this.previous.rootPosition,scale(previousVelocity.root,DT)),raw.rootPosition); | |
| const requestedRot=raw.localQuaternions.map((q,i)=>delta(multiply(exp(scale(previousVelocity.rot[i],DT)),this.previous.localQuaternions[i]),q)); | |
| saturated=length(requestedRoot)>rootLimit||requestedRot.some(v=>length(v)>angleLimit); | |
| this.offset={root:bounded(requestedRoot,rootLimit),rot:requestedRot.map(v=>bounded(v,angleLimit))}; | |
| this.offsetVelocity={root:sub(previousVelocity.root,incomingRoot),rot:raw.localQuaternions.map((q,i)=>sub(previousVelocity.rot[i],scale(delta(next.localQuaternions[i],q),1/DT)))}; | |
| this.age=0; | |
| } | |
| if(this.offset){ | |
| const omega=36,t=this.age,decay=Math.exp(-omega*t); | |
| const evolve=(v,velocity,limit)=>{ | |
| const requested=v.map((x,i)=>(x+(velocity[i]+omega*x)*t)*decay); | |
| saturated ||= length(requested)>limit+1e-10; | |
| return bounded(requested,limit); | |
| }; | |
| const root=evolve(this.offset.root,this.offsetVelocity.root,rootLimit); | |
| p.rootPosition=add(raw.rootPosition,root); | |
| p.localQuaternions=raw.localQuaternions.map((q,i)=>multiply(exp(evolve(this.offset.rot[i],this.offsetVelocity.rot[i],angleLimit)),q)); | |
| this.age+=DT;if(this.age>.5)this.offset=null; | |
| } | |
| if(this.previous)this.velocity={root:scale(sub(p.rootPosition,this.previous.rootPosition),1/DT),rot:p.localQuaternions.map((q,i)=>scale(delta(q,this.previous.localQuaternions[i]),1/DT))}; | |
| this.previous=copy(p); | |
| const correction={root_mm:length(sub(p.rootPosition,raw.rootPosition))*1000,maximum_joint_degrees:Math.max(...p.localQuaternions.map((q,i)=>length(delta(q,raw.localQuaternions[i]))*180/Math.PI)),saturated}; | |
| return {pose:p,correction}; | |
| } | |
| } | |
| /** Stationary-only plant. Limits apply to actual displacement and each joint. */ | |
| export function plantToes(raw,rig,anchors,weight=1,{maximum=.015,degrees=5}={}) { | |
| const p=copy(raw),details=[]; | |
| for(const [side,chain] of [[18,19,20,22],[23,24,25,27]].entries()){ | |
| const [hip,knee,ankle,toe]=chain,{positions:w,rotations:g}=forward(p,rig); | |
| const requested=sub(anchors[side],w[toe]),offset=scale(bounded(requested,maximum),weight); | |
| const a=w[hip],b=w[knee],c=w[ankle],target=add(c,offset),direction=unit(sub(target,a)); | |
| const l1=length(sub(b,a)),l2=length(sub(c,b)),distance=clamp(length(sub(target,a)),Math.abs(l1-l2)+1e-6,l1+l2-1e-6); | |
| let pole=sub(sub(b,a),scale(direction,dot(sub(b,a),direction))); | |
| if(length(pole)<1e-8)pole=cross(direction,rotate([1,0,0],g[knee])); | |
| if(length(pole)<1e-8)pole=cross(direction,Math.abs(direction[0])<.8?[1,0,0]:[0,1,0]); | |
| const along=(l1*l1+distance*distance-l2*l2)/(2*distance); | |
| const kneeTarget=add(add(a,scale(direction,along)),scale(unit(pole),Math.sqrt(Math.max(0,l1*l1-along*along)))); | |
| const original=copy(p),limit=degrees*Math.PI/180; | |
| p.localQuaternions[hip]=multiply(inverse(g[rig.parents[hip]]),multiply(between(sub(b,a),sub(kneeTarget,a)),g[hip])); | |
| let f=forward(p,rig); | |
| p.localQuaternions[knee]=multiply(inverse(f.rotations[hip]),multiply(between(sub(f.positions[ankle],f.positions[knee]),sub(add(a,scale(direction,distance)),f.positions[knee])),f.rotations[knee])); | |
| f=forward(p,rig);p.localQuaternions[ankle]=multiply(inverse(f.rotations[knee]),g[ankle]); | |
| for(const j of [hip,knee,ankle])p.localQuaternions[j]=multiply(exp(bounded(delta(p.localQuaternions[j],original.localQuaternions[j]),limit)),original.localQuaternions[j]); | |
| // Scaling the rotation deltas enforces the endpoint bound even near extension. | |
| const candidate=copy(p);let low=0,high=1; | |
| if(length(sub(forward(p,rig).positions[toe],w[toe]))>maximum+1e-7){ | |
| for(let n=0;n<14;n++){const fraction=(low+high)/2;for(const j of [hip,knee,ankle])p.localQuaternions[j]=multiply(exp(scale(delta(candidate.localQuaternions[j],original.localQuaternions[j]),fraction)),original.localQuaternions[j]);if(length(sub(forward(p,rig).positions[toe],w[toe]))<=maximum)low=fraction;else high=fraction;} | |
| for(const j of [hip,knee,ankle])p.localQuaternions[j]=multiply(exp(scale(delta(candidate.localQuaternions[j],original.localQuaternions[j]),low)),original.localQuaternions[j]); | |
| } | |
| const actual=forward(p,rig).positions[toe]; | |
| details.push({requested_mm:length(requested)*1000,actual_mm:length(sub(actual,w[toe]))*1000,residual_mm:length(sub(actual,anchors[side]))*1000,maximum_joint_degrees:Math.max(...[hip,knee,ankle].map(j=>length(delta(p.localQuaternions[j],original.localQuaternions[j]))*180/Math.PI)),saturated:length(requested)>maximum}); | |
| } | |
| return {pose:p,details}; | |
| } | |