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.; } } } }