void SocSystem_Analytical::getConstraints(arr& cdir,arr& coff,uint t,const arr& qt){
cdir.clear();
coff.clear();
#ifndef USE_TRUNCATION
return;
#endif
uint i,M=obstacles.d0;
arr d;
#if 0 //direct and clean way to do it -- but depends simple scenario
cdir.resize(M,x.N);
coff.resize(M);
for(i=0;i<M;i++){
cdir[i] = qt-obstacles[i];
coff(i) = scalarProduct(cdir[i],obstacles[i]);
}
#elif 1 //assume that the task vector is a list of scalars, each constrained >0
arr J,y;
for(i=0;i<M;i++){
real haty = norm(x-obstacles[i]);
if(haty>.5) continue; //that's good enough -> don't add the constraint
J = (x-obstacles[i])/norm(x-obstacles[i]);
coff.append(-haty + scalarProduct(J,x));
cdir.append(J);
}
cdir.reshape(coff.N,x.N);
coff.reshape(coff.N);
#else //messy: try to combine all constraints into a single scalar, doesn't really work...
//first compute squared collision meassure...
arr J(1,qt.N),phiHatQ(1);
J.setZero();
phiHatQ.setZero();
for(i=0;i<obstacles.d0;i++){
real margin = .25;
real d = 1.-norm(x-obstacles[i])/margin;
//if(d<0) continue;
//phiHatQ(0) += d*d;
//J += (2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
phiHatQ(0) += d;
J += (1./margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
}
//...then add a single constraint
if(phiHatQ(0)>0.){ //potential violation, else discard
cdir.append(-J);
coff.append(phiHatQ-scalarProduct(J,x)-1.);
cdir.reshape(1,x.N);
coff.reshape(1);
}
#endif
}
void ParticleAroundWalls::phi_t(arr& phi, arr& J, uint t, const arr& x_bar){
uint T=get_T(), n=dim_x(), k=get_k();
//assert some dimensions
CHECK(x_bar.d0==k+1,"");
CHECK(x_bar.d1==n,"");
CHECK(t<=T,"");
//-- transition costs: append to phi
if(k==1) phi = x_bar[1]-x_bar[0]; //penalize velocity
if(k==2) phi = x_bar[2]-2.*x_bar[1]+x_bar[0]; //penalize acceleration
if(k==3) phi = x_bar[3]-3.*x_bar[2]+3.*x_bar[1]-x_bar[0]; //penalize jerk
//-- walls: append to phi
//Note: here we append to phi ONLY in certain time slices: the dimensionality of phi may very with time slices; see dim_phi(uint t)
double eps=.1, power=2.;
if(!hardConstrained){
//-- wall costs
for(uint i=0;i<n;i++){ //add barrier costs to each dimension
if(t==T/4) phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power)); //middle factor: ``greater than i''
if(t==T/2) phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power)); //last factor: ``lower than -i''
if(t==3*T/4) phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power)); //middle factor: ``greater than i''
if(t==T) phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power)); //last factor: ``lower than -i''
}
}else{
//-- wall constraints
for(uint i=0;i<n;i++){ //add barrier costs to each dimension
if(t==T/4) phi.append((i+1.-x_bar(k,i))); //middle factor: ``greater than i''
if(t==T/2) phi.append((x_bar(k,i)+i+1.)); //last factor: ``lower than -i''
if(t==3*T/4) phi.append((i+1.-x_bar(k,i))); //middle factor: ``greater than i''
if(t==T) phi.append((x_bar(k,i)+i+1.)); //last factor: ``lower than -i''
}
}
uint m=phi.N;
CHECK(m==dim_phi(t),"");
if(&J){ //we also need to return the Jacobian
J.resize(m,k+1,n).setZero();
//-- transition costs
for(uint i=0;i<n;i++){
if(k==1){ J(i,1,i) = 1.; J(i,0,i) = -1.; }
if(k==2){ J(i,2,i) = 1.; J(i,1,i) = -2.; J(i,0,i) = 1.; }
if(k==3){ J(i,3,i) = 1.; J(i,2,i) = -3.; J(i,1,i) = +3.; J(i,0,i) = -1.; }
}
//-- walls
if(!hardConstrained){
for(uint i=0;i<n;i++){
if(t==T/4) J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
if(t==T/2) J(n+i,k,i) = MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
if(t==3*T/4) J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
if(t==T) J(n+i,k,i) = MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
}
}else{
for(uint i=0;i<n;i++){
if(t==T/4) J(n+i,k,i) = -1.;
if(t==T/2) J(n+i,k,i) = +1.;
if(t==3*T/4) J(n+i,k,i) = -1.;
if(t==T) J(n+i,k,i) = +1.;
}
}
}
}
