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