void Hess_R (int *pnPar, double *pdX, double *pdMu, double *pdHess)
{
double *pdTempP1 = new double [pnPar[0]], *pdTempP2 = new double [pnPar[0]] ;
Hess (pnPar[0], pnPar[1], pdX, pdMu, pdHess, pdTempP1, pdTempP2) ;
delete pdTempP1 ;
delete pdTempP2 ;
}
bool minimizeMeritFunction(std::vector< Matrix<C_DIM> >& X, std::vector< Matrix<U_DIM> >& U, stateMPC_params& problem, stateMPC_output& output, stateMPC_info& info, double penalty_coeff)
{
LOG_DEBUG("Solving sqp problem with penalty parameter: %2.4f", penalty_coeff);
std::vector< Matrix<C_DIM,C_DIM> > F(T-1);
std::vector< Matrix<C_DIM,U_DIM> > G(T-1);
std::vector< Matrix<C_DIM> > h(T-1);
double Xeps = cfg::initial_trust_box_size;
double Ueps = cfg::initial_trust_box_size;
double Xpos_eps = cfg::initial_trust_box_factor * cfg::initial_Xpos_trust_box_size;
double Xangle_eps = cfg::initial_trust_box_factor * cfg::initial_Xangle_trust_box_size;
double Uvel_eps = cfg::initial_trust_box_factor * cfg::initial_Uvel_trust_box_size;
double Uangle_eps = cfg::initial_trust_box_factor * cfg::initial_Uangle_trust_box_size;
double landmark_eps = cfg::initial_trust_box_factor * cfg::initial_landmark_trust_box_size;
double optcost;
std::vector<Matrix<C_DIM> > Xopt(T);
std::vector<Matrix<U_DIM> > Uopt(T-1);
double merit, model_merit, new_merit;
double approx_merit_improve, exact_merit_improve, merit_improve_ratio;
double constant_cost, hessian_constant, jac_constant;
int sqp_iter = 1, index = 0, shrink_iters = 0;
bool success;
Matrix<C_DIM+U_DIM, C_DIM+U_DIM> HMat;
Matrix<C_DIM,C_DIM> HfMat;
Matrix<C_DIM> eVec;
Matrix<C_DIM,3*C_DIM+U_DIM> CMat;
Matrix<C_DIM,C_DIM> IX = identity<C_DIM>();
Matrix<C_DIM,C_DIM> minusIX = IX;
for(int i = 0; i < C_DIM; ++i) {
minusIX(i,i) = -1;
}
Matrix<C_DIM+U_DIM> zbar;
// full Hessian from current timstep
Matrix<CU_DIM,CU_DIM>Hess = identity<CU_DIM>();
Matrix<CU_DIM> Grad, Gradopt;
double cost;
int idx = 0;
// sqp loop
while(true)
{
// In this loop, we repeatedly construct a linear approximation to the nonlinear belief dynamics constraint
LOG_DEBUG(" sqp iter: %d", sqp_iter);
merit = casadiComputeMerit(X, U, penalty_coeff);
LOG_DEBUG(" merit: %4.10f", merit);
// Compute gradients
casadiComputeCostGrad(X, U, cost, Grad);
// Problem linearization and definition
// fill in H, f
hessian_constant = 0;
jac_constant = 0;
idx = 0;
for (int t = 0; t < T-1; ++t)
{
Matrix<C_DIM>& xt = X[t];
Matrix<U_DIM>& ut = U[t];
idx = t*(C_DIM+U_DIM);
// fill in gradients and Hessians
HMat.reset();
for(int i = 0; i < (C_DIM+U_DIM); ++i) {
double val = Hess(idx+i,idx+i);
HMat(i,i) = (val < 0) ? 0 : val;
}
// since diagonal, fill directly
for(int i = 0; i < (C_DIM+U_DIM); ++i) { H[t][i] = HMat(i,i); }
// TODO: why does this work???
for(int i = 0; i < (2*C_DIM); ++i) { H[t][i + (C_DIM+U_DIM)] = 5e2; } //1e3
zbar.insert(0,0,xt);
zbar.insert(C_DIM,0,ut);
for(int i = 0; i < (C_DIM+U_DIM); ++i) {
hessian_constant += HMat(i,i)*zbar[i]*zbar[i];
jac_constant -= Grad[idx+i]*zbar[i];
f[t][i] = Grad[idx+i] - HMat(i,i)*zbar[i];
}
// penalize dynamics slack variables
for(int i = C_DIM+U_DIM; i < 3*C_DIM+U_DIM; ++i) { f[t][i] = penalty_coeff; }
// fill in linearizations
linearizeCarDynamics(xt, ut, F[t], G[t], h[t]);
CMat.reset();
eVec.reset();
CMat.insert<C_DIM,C_DIM>(0,0,F[t]);
CMat.insert<C_DIM,U_DIM>(0,C_DIM,G[t]);
CMat.insert<C_DIM,C_DIM>(0,C_DIM+U_DIM,IX);
CMat.insert<C_DIM,C_DIM>(0,2*C_DIM+U_DIM,minusIX);
fillColMajor(C[t], CMat);
if (t == 0) {
eVec.insert<C_DIM,1>(0,0,X[0]);
fillCol(e[0], eVec);
}
eVec = -h[t] + F[t]*xt + G[t]*ut;
fillCol(e[t+1], eVec);
}
// For last stage, fill in H, f
Matrix<C_DIM>& xT = X[T-1];
idx = (T-1)*(C_DIM+U_DIM);
HfMat.reset();
for(int i = 0; i < C_DIM; ++i) {
double val = Hess(idx+i,idx+i);
HfMat(i,i) = (val < 0) ? 0 : val;
}
// since diagonal, fill directly
for(int i = 0; i < C_DIM; ++i) { H[T-1][i] = HfMat(i,i); }
for(int i = 0; i < C_DIM; ++i) {
hessian_constant += HfMat(i,i)*xT[i]*xT[i];
jac_constant -= Grad[idx+i]*xT[i];
f[T-1][i] = Grad[idx+i] - HfMat(i,i)*xT[i];
}
constant_cost = 0.5*hessian_constant + jac_constant + cost;
LOG_DEBUG(" hessian cost: %4.10f", 0.5*hessian_constant);
LOG_DEBUG(" jacobian cost: %4.10f", jac_constant);
LOG_DEBUG(" constant cost: %4.10f", constant_cost);
// trust region size adjustment
while(true)
{
LOG_DEBUG(" trust region size: %2.6f %2.6f", Xeps, Ueps);
// solve the innermost QP here
for(int t = 0; t < T-1; ++t)
{
Matrix<C_DIM>& xt = X[t];
Matrix<U_DIM>& ut = U[t];
// Fill in lb, ub
index = 0;
// car pos lower bound
for(int i = 0; i < P_DIM; ++i) { lb[t][index++] = MAX(xMin[i], xt[i] - Xpos_eps); }
// car angle lower bound
lb[t][index++] = MAX(xMin[P_DIM], xt[P_DIM] - Xangle_eps);
// u velocity lower bound
lb[t][index++] = MAX(uMin[0], ut[0] - Uvel_eps);
// u angle lower bound
lb[t][index++] = MAX(uMin[1], ut[1] - Uangle_eps);
// for lower bound on L1 slacks
for(int i = 0; i < 2*C_DIM; ++i) { lb[t][index++] = 0; }
index = 0;
// car pos upper bound
for(int i = 0; i < P_DIM; ++i) { ub[t][index++] = MIN(xMax[i], xt[i] + Xpos_eps); }
// car angle upper bound
ub[t][index++] = MIN(xMax[P_DIM], xt[P_DIM] + Xangle_eps);
// u velocity upper bound
ub[t][index++] = MIN(uMax[0], ut[0] + Uvel_eps);
// u angle upper bound
ub[t][index++] = MIN(uMax[1], ut[1] + Uangle_eps);
}
// Fill in lb, ub
double finalPosDelta = .1;
double finalAngleDelta = M_PI/4;
Matrix<C_DIM>& xT_MPC = X[T_MPC-1];
index = 0;
// xMidpoint lower bound
for(int i = 0; i < P_DIM; ++i) { lb[T_MPC-1][index++] = xMidpoint[i] - finalPosDelta; }
// loose on car angle and landmarks
lb[T_MPC-1][index++] = nearestAngleFromTo(xT_MPC[2], xMidpoint[2] - finalAngleDelta);
index = 0;
// xMidpoint upper bound
for(int i = 0; i < P_DIM; ++i) { ub[T_MPC-1][index++] = xMidpoint[i] + finalPosDelta; }
// loose on car angle and landmarks
ub[T_MPC-1][index++] = nearestAngleFromTo(xT_MPC[2], xMidpoint[2] + finalAngleDelta);
Matrix<C_DIM>& xT = X[T-1];
index = 0;
// xGoal lower bound
for(int i = 0; i < P_DIM; ++i) { lb[T-1][index++] = xGoal[i] - finalPosDelta; }
// loose on car angle and landmarks
//lb[T_MPC-1][index++] = xGoal[2] - finalAngleDelta;
lb[T-1][index++] = nearestAngleFromTo(xT[2], xGoal[2] - finalAngleDelta);
index = 0;
// xGoal upper bound
for(int i = 0; i < P_DIM; ++i) { ub[T-1][index++] = xGoal[i] + finalPosDelta; }
// loose on car angle and landmarks
//ub[T_MPC-1][index++] = xGoal[2] + finalAngleDelta;
ub[T-1][index++] = nearestAngleFromTo(xT[2], xGoal[2] + finalAngleDelta);
// Verify problem inputs
if (!isValidInputs()) {
LOG_ERROR("Inputs are not valid!");
exit(-1);
}
int exitflag = stateMPC_solve(&problem, &output, &info);
if (exitflag == 1) {
for(int t = 0; t < T-1; ++t) {
Matrix<C_DIM>& xt = Xopt[t];
Matrix<U_DIM>& ut = Uopt[t];
for(int i = 0; i < C_DIM; ++i) {
xt[i] = z[t][i];
}
for(int i = 0; i < U_DIM; ++i) {
ut[i] = z[t][C_DIM+i];
}
optcost = info.pobj;
}
for(int i = 0; i < C_DIM; ++i) {
Xopt[T-1][i] = z[T-1][i];
}
}
else {
LOG_ERROR("Some problem in solver");
throw forces_exception();
}
LOG_DEBUG(" Optimized cost: %4.10f", optcost);
model_merit = optcost + constant_cost;
new_merit = casadiComputeMerit(Xopt, Uopt, penalty_coeff);
LOG_DEBUG(" merit: %4.10f", merit);
LOG_DEBUG(" model_merit: %4.10f", model_merit);
LOG_DEBUG(" new_merit: %4.10f", new_merit);
approx_merit_improve = merit - model_merit;
exact_merit_improve = merit - new_merit;
merit_improve_ratio = exact_merit_improve / approx_merit_improve;
LOG_DEBUG(" approx_merit_improve: %1.6f", approx_merit_improve);
LOG_DEBUG(" exact_merit_improve: %1.6f", exact_merit_improve);
LOG_DEBUG(" merit_improve_ratio: %1.6f", merit_improve_ratio);
counter_inner_loop++;
//std::cout << "PAUSED INSIDE minimizeMeritFunction AFTER OPTIMIZATION" << std::endl;
//std::cin.ignore();
if (approx_merit_improve < -1e-5) {
LOG_ERROR("Approximate merit function got worse: %1.6f", approx_merit_improve);
std::cout << "penalty coeff: " << penalty_coeff << "\n";
//LOG_ERROR("Either convexification is wrong to zeroth order, or you are in numerical trouble");
//LOG_ERROR("Failure!");
counter_approx_hess_neg++;
return false;
} else if (approx_merit_improve < cfg::min_approx_improve) {
LOG_DEBUG("Converged: improvement small enough");
X = Xopt; U = Uopt;
return true;
} else if ((exact_merit_improve < 0) || (merit_improve_ratio < cfg::improve_ratio_threshold)) {
//Xeps *= cfg::trust_shrink_ratio;
//Ueps *= cfg::trust_shrink_ratio;
Xpos_eps *= cfg::trust_shrink_ratio;
Xangle_eps *= cfg::trust_shrink_ratio;
Uvel_eps *= cfg::trust_shrink_ratio;
Uangle_eps *= cfg::trust_shrink_ratio;
landmark_eps *= cfg::trust_shrink_ratio;
shrink_iters++;
LOG_DEBUG("Shrinking trust region size to: %2.6f %2.6f %2.6f %2.6f", Xpos_eps, Xangle_eps, Uvel_eps, Uangle_eps);
} else {
//Xeps *= cfg::trust_expand_ratio;
//Ueps *= cfg::trust_expand_ratio;
Xpos_eps *= cfg::trust_expand_ratio;
Xangle_eps *= cfg::trust_expand_ratio;
Uvel_eps *= cfg::trust_expand_ratio;
Uangle_eps *= cfg::trust_expand_ratio;
landmark_eps *= cfg::trust_expand_ratio;
shrink_iters--;
casadiComputeCostGrad(Xopt, Uopt, cost, Gradopt);
Matrix<CU_DIM> s, y;
idx = 0;
for(int t = 0; t < T-1; ++t) {
for(int i=0; i < C_DIM; ++i) {
s[idx+i] = Xopt[t][i] - X[t][i];
y[idx+i] = Gradopt[idx+i] - Grad[idx+i];
}
idx += C_DIM;
for(int i=0; i < U_DIM; ++i) {
s[idx+i] = Uopt[t][i] - U[t][i];
y[idx+i] = Gradopt[idx+i] - Grad[idx+i];
}
idx += U_DIM;
}
for(int i=0; i < C_DIM; ++i) {
s[idx+i] = Xopt[T-1][i] - X[T-1][i];
y[idx+i] = Gradopt[idx+i] - Grad[idx+i];
}
double theta;
Matrix<CU_DIM> Bs = Hess*s;
bool decision = ((~s*y)[0] >= .2*(~s*Bs)[0]);
if (decision) {
theta = 1;
} else {
theta = (.8*(~s*Bs)[0])/((~s*Bs-~s*y)[0]);
}
//std::cout << "theta: " << theta << std::endl;
Matrix<CU_DIM> r = theta*y + (1-theta)*Bs;
//Matrix<XU_DIM> rBs = theta*(y -Bs);
// SR1 update
//B = B + (rBs*~rBs)/((~rBs*s)[0]);
// L-BFGS update
Hess = Hess - (Bs*~Bs)/((~s*Bs)[0]) + (r*~r)/((~s*r)[0]);
// take in diagonal of B
// find minimum value among diagonal elements
// negate and add to other vals
double minValue = INFTY;
double maxValue = -INFTY;
for(int i=0; i < CU_DIM; ++i) {
minValue = MIN(minValue, Hess(i,i));
maxValue = MAX(maxValue, Hess(i,i));
}
if (minValue < 0) {
std::cout << "negative minValue, press enter\n";
std::cin.ignore();
Hess = Hess + fabs(minValue)*identity<CU_DIM>();
}
// Do not update B
//B = identity<XU_DIM>();
X = Xopt; U = Uopt;
LOG_DEBUG("Accepted, Increasing trust region size to: %2.6f %2.6f %2.6f %2.6f", Xpos_eps, Xangle_eps, Uvel_eps, Uangle_eps);
break;
}
if (Xpos_eps < cfg::min_trust_box_size && Xangle_eps < cfg::min_trust_box_size &&
Uvel_eps < cfg::min_trust_box_size && Uangle_eps < cfg::min_trust_box_size && landmark_eps < cfg::min_trust_box_size) {
//if (shrink_iters > cfg::min_trust_shrink_iters) {
LOG_DEBUG("Converged: x tolerance");
return true;
}
//std::cout << "U" << std::endl;
//for(int t=0; t < T-1; ++t) {
// std::cout << ~U[t];
//}
//std::cout << std::endl << std::endl;
//pythonDisplayTrajectory(U, T, true);
//pythonDisplayTrajectory(X, T, true);
} // trust region loop
sqp_iter++;
} // sqp loop
return success;
}
/**
* Description not yet available.
* \param
*/
void function_minimizer::trust_region_update(int nvar,int _crit,
independent_variables& x,const dvector& _g,const double& _f)
{
double & f= (double&)_f;
dvector & g= (dvector&)_g;
fmm fmc(nvar);
if (random_effects_flag)
{
initial_params::set_active_only_random_effects();
//int unvar=initial_params::nvarcalc(); // get the number of active
initial_params::restore_start_phase();
initial_params::set_inactive_random_effects();
int nvar1=initial_params::nvarcalc(); // get the number of active
if (nvar1 != nvar)
{
cerr << "failed sanity check in "
"void function_minimizer::quasi_newton_block" << endl;
ad_exit(1);
}
}
uistream uis("admodel.hes");
if (!uis)
{
cerr << "Error trying to open file admodel.hes" << endl;
ad_exit(1);
}
int hnvar = 0;
uis >> hnvar;
dmatrix Hess(1,hnvar,1,hnvar);
uis >> Hess;
if (!uis)
{
cerr << "Error trying to read Hessian from admodel.hes" << endl;
ad_exit(1);
}
dmatrix tester(1,hnvar,1,hnvar);
tester=Hess;
dvector e=sort(eigenvalues(Hess));
double lambda=-e(1)+100.;
for (int i=1;i<=hnvar;i++)
{
tester(i,i)+=lambda;
}
dvector step = x-solve(tester,g);
{
// calculate the number of random effects unvar
// this turns on random effects variables and turns off
// everything else
//cout << nvar << endl;
initial_params::set_active_only_random_effects();
//cout << nvar << endl;
int unvar=initial_params::nvarcalc(); // get the number of active
//df1b2_gradlist::set_no_derivatives();
if (lapprox)
{
delete lapprox;
lapprox=0;
df1b2variable::pool->deallocate();
for (int i=0;i<df1b2variable::adpool_counter;i++)
{
delete df1b2variable::adpool_vector[i];
df1b2variable::adpool_vector[i]=0;
df1b2variable::nvar_vector[i]=0;
df1b2variable::adpool_counter=0;
}
}
lapprox=new laplace_approximation_calculator(nvar,unvar,1,nvar+unvar,
this);
initial_df1b2params::current_phase=initial_params::current_phase;
initial_df1b2params::save_varsptr();
allocate();
initial_df1b2params::restore_varsptr();
df1b2_gradlist::set_no_derivatives();
int ynvar=initial_params::nvarcalc_all();
dvector y(1,ynvar);
initial_params::xinit_all(y);
initial_df1b2params::reset_all(y);
g=(*lapprox)(step,f,this);
}
} // end block for quasi newton minimization
// Use analytical nuclear gradient to compute the hessian via finite
// difference
Matrix GetFiniteDifferenceHessian(Vector Original_coords, int imon) {
int Natoms, istart;
if (imon == 0) {
Natoms = Cluster::cluster().GetTotalNumberOfAtoms();
istart = 0;
}
else {
Natoms = Cluster::cluster().GetNumberOfAtoms(imon);
istart = 0;
for (int q=1;q<imon;q++) {
istart += 3*Cluster::cluster().GetNumberOfAtoms(q);
}
}
printf("Natoms = %d\n",Natoms);
//printf("istart = %d\n",istart);
Matrix Hess(3*Natoms,3*Natoms);
Hess.Set();
// Do the finite difference
double delta = 0.001; // step size
for (int i=istart;i<istart+3*Natoms;i++) {
printf("Shifting coord %d by -%f\n",i,delta);
Vector Coords = Original_coords;
Coords[i] -= delta;
// Update the coordinates & get the gradient
Cluster::cluster().SetNewCoordinates(Coords);
Cluster::cluster().RunJobsAndComputeEnergy();
Vector Gplus = Cluster::cluster().GetHMBIGradient();
Gplus.PrintGradient("Gplus:");
printf("Shifting coord %d by +%f\n",i,delta);
Coords[i] += 2.0*delta;
// Update the coordinates & get the gradient
Cluster::cluster().SetNewCoordinates(Coords);
Cluster::cluster().RunJobsAndComputeEnergy();
Vector Gminus = Cluster::cluster().GetHMBIGradient();
Gminus.PrintGradient("Gminus:");
// Use the two gradients to determine a column of the Hessian
// H(i,*) = (Gminus - Gplus)/(2*delta)
Gminus -= Gplus;
Gminus.Scale(1.0/(2*delta*AngToBohr));
if (imon == 0)
Hess.SetColumnVector(Gminus,i);
else {
for (int j=0;j<3*Natoms;j++) {
Hess(j,i-istart) = Gminus[j+istart];
}
}
}
// Form the actual hessian in atomic units, hartree/bohr^2
Hess.Print("Finite Difference HMBI Hessian");
return Hess;
}
