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