bool sRprop::step(arr& w, const arr& grad, uint *singleI) { if(!stepSize.N) { //initialize stepSize.resize(w.N); lastGrad.resize(w.N); lastGrad.setZero(); stepSize = delta0; } CHECK(grad.N==stepSize.N, "Rprop: gradient dimensionality changed!"); CHECK(w.N==stepSize.N , "Rprop: parameter dimensionality changed!"); uint i=0, I=w.N; if(singleI) { i=*(singleI); I=i+1; } for(; i<I; i++) { if(grad.elem(i) * lastGrad(i) > 0) { //same direction as last time if(rMax) dMax=fabs(rMax*w.elem(i)); stepSize(i) = _mymin(dMax, incr * stepSize(i)); //increase step size w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction lastGrad(i) = grad.elem(i); //memorize gradient } else if(grad.elem(i) * lastGrad(i) < 0) { //change of direction stepSize(i) = _mymax(dMin, decr * stepSize(i)); //decrease step size w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction (undo half the step) lastGrad(i) = 0; //memorize to continue below next time } else { //after change of direcion w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction lastGrad(i) = grad.elem(i); //memorize gradient } } return stepSize.max() < incr*dMin; }
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; }
virtual double fs(arr& g, arr& H, const arr& x) { double A=.5, f=A*x.N; for(uint i=0; i<x.N; i++) f += x(i)*x(i) - A*::cos(10.*x(i)); if(&g) { g.resize(x.N); for(uint i=0; i<x.N; i++) g(i) = 2*x(i) + 10.*A*::sin(10.*x(i)); } if(&H) { H.resize(x.N,x.N); H.setZero(); for(uint i=0; i<x.N; i++) H(i,i) = 2 + 100.*A*::cos(10.*x(i)); } return f; }
//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; }
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 }
virtual double fs(arr& g, arr& H, const arr& x) { //initialize on first call if(which==none){ which = (Which) MT::getParameter<int>("fctChoice"); } if(!condition.N!=x.N){ condition.resize(x.N); double cond = MT::getParameter<double>("condition"); for(uint i=0; i<x.N; i++) condition(i) = pow(cond,0.5*i/(x.N-1)); } arr y = x; y *= condition; //elem-wise product double f; switch(which) { case sum: f = SumFunction.fs(g, H, y); break; case square: f = SquareFunction.fs(g, H, y); break; case hole: f = HoleFunction.fs(g, H, y); break; case rosenbrock: f = RosenbrockFunction.fs(g, H, y); break; case rastrigin: f = RastriginFunction.fs(g, H, y); break; default: NIY; } if(&g) g *= condition; //elem-wise product if(&H) H = condition%H%condition; return f; }
void getPhi(arr& phiq_i,uint i){ NIY; if(i==1){ phiq_i.resize(1); for(uint i=0;i<obstacles.N;i++){ phiq_i(0) += norm(x-obstacles[i]); } } }
void getTriNormals(const Mesh& m, arr& Tn) { uint i; ors::Vector a, b, c; Tn.resize(m.T.d0, 3); //tri normals for(i=0; i<m.T.d0; i++) { a.set(&m.V(m.T(i, 0), 0)); b.set(&m.V(m.T(i, 1), 0)); c.set(&m.V(m.T(i, 2), 0)); b-=a; c-=a; a=b^c; a.normalize(); Tn(i, 0)=a.x; Tn(i, 1)=a.y; Tn(i, 2)=a.z; } }
void generateConditionedRandomProjection(arr& M, uint n, double condition) { uint i,j; //let M be a ortho-normal matrix (=random rotation matrix) M.resize(n,n); rndUniform(M,-1.,1.,false); //orthogonalize for(i=0; i<n; i++) { for(j=0; j<i; j++) M[i]()-=scalarProduct(M[i],M[j])*M[j]; M[i]()/=length(M[i]); } //we condition each column of M with powers of the condition for(i=0; i<n; i++) M[i]() *= pow(condition, double(i) / (2.*double(n - 1))); }
void soc::SocSystem_Ors::getMF(arr& M,arr& F){ if(!WS->pseudoDynamic){ if(WS->Qlin.N) NIY; ors->clearForces(); ors->gravityToForces(); //ors->frictionToForces(1.1); ors->equationOfMotion(M,F,WS->v_act); //M.setId(); F = .1; //Minv *= .2;//1e-1; }else{ uint n=qDim(); M.setId(n); F.resize(n); F.setZero(); } }
virtual double fs(arr& g, arr& H, const arr& x) { if(&g) { g.resize(x.N); g=1.; } if(&H) { H.resize(x.N,x.N); H.setZero(); } return sum(x); }
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.; } } } }
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(); }