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();
}
