Spaces:
Running
Running
File size: 4,236 Bytes
590673f | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 | const f=Math.fround;
export function multiply(a,b){const r=Array(16).fill(0);for(let i=0;i<4;i++)for(let j=0;j<4;j++)for(let k=0;k<4;k++)r[i*4+j]+=a[i*4+k]*b[k*4+j];return r;}
const identity=()=>[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1];
export function chainPoints(config,q){let t=identity();const points=[];for(const link of config.links){t=multiply(t,link.base_matrix.flat());if(link.joint!==null){const [x,y,z]=link.axis,a=q[link.joint],c=Math.cos(a),s=Math.sin(a),v=1-c;t=multiply(t,[x*x*v+c,x*y*v-z*s,x*z*v+y*s,0,y*x*v+z*s,y*y*v+c,y*z*v-x*s,0,z*x*v-y*s,z*y*v+x*s,z*z*v+c,0,0,0,0,1]);}points.push([f(t[3]),f(t[7]),f(t[11])]);}return points;}
export const fk=(config,q)=>chainPoints(config,q).at(-1);
export const distance=(a,b)=>Math.hypot(...a.map((x,i)=>x-b[i]));
export class Actor{
constructor(buffer){const values=new Float32Array(buffer);if(values.length!==512*29+512+512*512+512+6*512+6)throw Error('Invalid actor size');let n=0;this.parts=[512*29,512,512*512,512,6*512,6].map(size=>{const a=values.slice(n,n+size);n+=size;return a;});}
predict(observation){if(observation.length!==29||!Array.from(observation).every(Number.isFinite))throw Error('Expected finite 29D observation');const [w1,b1,w2,b2,wm,bm]=this.parts;const layer=(x,w,b,rows,cols,relu)=>{const y=new Float32Array(rows);for(let i=0;i<rows;i++){let s=b[i];for(let j=0;j<cols;j++)s+=w[i*cols+j]*x[j];y[i]=relu?Math.max(0,s):Math.tanh(s);}return y;};return layer(layer(layer(observation,w1,b1,512,29,true),w2,b2,512,512,true),wm,bm,6,512,false);}
}
function solve3(a,b){a=a.map((r,i)=>[...r,b[i]]);for(let j=0;j<3;j++){let pivot=j;for(let i=j+1;i<3;i++)if(Math.abs(a[i][j])>Math.abs(a[pivot][j]))pivot=i;[a[j],a[pivot]]=[a[pivot],a[j]];let d=a[j][j];if(Math.abs(d)<1e-12)throw Error('Singular DLS matrix');for(let k=j;k<4;k++)a[j][k]/=d;for(let i=0;i<3;i++)if(i!==j){const z=a[i][j];for(let k=j;k<4;k++)a[i][k]-=z*a[j][k];}}return a.map(r=>r[3]);}
export function dls(config,o){const q=Array.from(o.slice(0,6),(v,i)=>v*config.limits[i][1]);const jac=[[],[],[]];for(let j=0;j<6;j++){const a=q.slice(),b=q.slice();a[j]+=1e-4;b[j]-=1e-4;const fa=fk(config,a),fb=fk(config,b);for(let k=0;k<3;k++)jac[k][j]=(fa[k]-fb[k])/2e-4;}const mat=Array.from({length:3},(_,i)=>Array.from({length:3},(_,j)=>jac[i].reduce((s,v,k)=>s+v*jac[j][k],0)+(i===j?.02**2:0)));const x=solve3(mat,Array.from(o.slice(11,14),(v,i)=>v-o[6+i]));return Float32Array.from({length:6},(_,i)=>Math.max(-1,Math.min(1,.8*jac.reduce((s,r,k)=>s+r[i]*x[k],0)/.05)));}
export class Environment{
constructor(config,target){this.config=config;this.target=target;this.q=Float32Array.from(config.home);this.cursor=0;this.phase=0;this.steps=0;this.stagnation=0;this.errors=[];this.path=[];this.done=false;this.success=false;this.reason='Ready';}
observation(){const p=fk(this.config,this.q),look=[];for(let i=0;i<5;i++)look.push(...this.target[Math.min(this.cursor+i,59)]);return Float32Array.from([...Array.from(this.q,(v,i)=>v/this.config.limits[i][1]),...p,this.cursor/60,this.phase,...this.target[this.cursor],...look]);}
step(action){if(this.done)return;this.steps++;for(let i=0;i<6;i++)this.q[i]=f(Math.max(this.config.limits[i][0],Math.min(this.config.limits[i][1],f(this.q[i]+f(Math.max(-1,Math.min(1,action[i]))*.05)))));const p=fk(this.config,this.q);this.path.push(p);const dist=distance(p,this.target[this.cursor]);let error=Infinity;for(let i=Math.max(0,this.cursor-3);i<Math.min(60,this.cursor+12);i++)error=Math.min(error,distance(p,this.target[i]));if(this.phase===0){if(distance(p,this.target[0])<.02){this.phase=1;this.errors=[];}}else{this.errors.push(error*1000);if(dist<.025&&this.cursor<59){this.cursor++;this.stagnation=0;}else this.stagnation++;if(this.cursor===59&&dist<.025){this.done=true;this.reason='Endpoint tolerance not met';}if(error>.06){this.done=true;this.reason='Off path';}if(this.stagnation>150){this.done=true;this.reason='Stagnation';}}if(this.steps>=800){this.done=true;this.reason='Step limit';}if(this.done&&this.phase===1&&this.cursor===59&&distance(p,this.target[59])<.025&&this.errors.every(e=>e<=60)){this.success=true;this.reason='Strict completion';}}
get rms(){return this.errors.length?Math.sqrt(this.errors.reduce((s,v)=>s+v*v,0)/this.errors.length):null;}
}
|