virtual double fs(arr& g, arr& H, const arr& x) { double f=exp(-sumOfSqr(x)); if(&g) g=2.*f*x; if(&H) { H.setDiag(2.*f, x.N); H -= 4.*f*(x^x); } f = 1.-f; return f; }
real SocSystem_Analytical::getCosts(arr& R,arr& r,uint t,const arr& qt){ uint N=x.N; R.resize(N,N); R.setZero(); r.resize(N); r.setZero(); real C=0.; #ifndef USE_TRUNCATION //potentials for collision cost arr J(1,qt.N),phiHatQ(1); J.setZero(); phiHatQ.setZero(); for(uint i=0;i<obstacles.d0;i++){ real margin = .1; real d = (1.-norm(x-obstacles[i])/margin); if(d<0) continue; phiHatQ(0) += d*d; J += ((real)2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]); } J.reshape(1,J.N); arr tJ,target(1); target=(real)0.; transpose(tJ,J); real colprec = (real)5e2; C += colprec*sqrDistance(target,phiHatQ); R += colprec*tJ*J; r += colprec*tJ*(target - phiHatQ + J*qt); #endif if(t!=T-1) return C; R.setDiag(1.); r = x1; R *= prec; r *= prec; C += prec*sqrDistance(x1,x); return C; }
//initialization methods void initKinematic(uint dim,uint trajectory_length, real w, real endPrec){ x0.resize(dim); x0.setZero(); x1=x0; x1(0)=1.; x=x0; W.setDiag(w,x.N); prec=endPrec; T=trajectory_length; obstacles.resize(2,x.N); obstacles(0,0)=.3; obstacles(0,1)=.05; obstacles(1,0)=.7; obstacles(1,1)=-.05; dynamic=false; //os = &cout; }
virtual double fs(arr& g, arr& H, const arr& x) { if(&g) g=2.*x; if(&H) H.setDiag(2., x.N); return sumOfSqr(x); }
void SocSystem_Analytical::getProcess(arr& A,arr& a,arr& B){ uint N=x.N; A.setDiag(1.,N); B.setDiag(1.,N); a.resize(N); a.setZero(); }