UIMenu::UIMenu() {
_options.clear();
_selected = -1;
_id = IDManager::instance()->getPickID();
fillCol(Color4D ( 0.8, 0.8, 1.0, 0.5 ) );
lineCol(Color4D ( 0,0,0,1.0) ) ;
}
bool minimizeMeritFunction(std::vector< Matrix<B_DIM> >& B, std::vector< Matrix<U_DIM> >& U, beliefPenaltyMPC_params &problem, beliefPenaltyMPC_output &output, beliefPenaltyMPC_info &info, double penalty_coeff)
{
LOG_DEBUG("Solving sqp problem with penalty parameter: %2.4f", penalty_coeff);
//Matrix<B_DIM,1> b0 = B[0];
std::vector< Matrix<B_DIM,B_DIM> > F(T-1);
std::vector< Matrix<B_DIM,U_DIM> > G(T-1);
std::vector< Matrix<B_DIM> > h(T-1);
double Beps = cfg::initial_trust_box_size;
double Ueps = cfg::initial_trust_box_size;
double Xpos_eps = cfg::initial_Xpos_trust_box_size;
double Xangle_eps = cfg::initial_Xangle_trust_box_size;
double Uvel_eps = cfg::initial_Uvel_trust_box_size;
double Uangle_eps = cfg::initial_Uangle_trust_box_size;
double optcost;
std::vector<Matrix<B_DIM> > Bopt(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;
int sqp_iter = 1, index = 0;
bool success;
Matrix<B_DIM,B_DIM> IB = identity<B_DIM>();
Matrix<B_DIM,B_DIM> minusIB = IB;
for(int i = 0; i < B_DIM; ++i) {
minusIB(i,i) = -1;
}
//Matrix<3*B_DIM+U_DIM, 3*B_DIM+U_DIM> HMat;
Matrix<B_DIM,3*B_DIM+U_DIM> CMat;
Matrix<B_DIM> eVec;
//Matrix<S_DIM,S_DIM> Hess;
// 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 = computeMerit(B, U, penalty_coeff);
LOG_DEBUG(" merit: %4.10f", merit);
// Problem linearization and definition
// fill in H, f, C, e
for (int t = 0; t < T-1; ++t)
{
Matrix<B_DIM>& bt = B[t];
Matrix<U_DIM>& ut = U[t];
linearizeBeliefDynamics(bt, ut, F[t], G[t], h[t]);
// initialize f in cost function to penalize
// belief dynamics slack variables
index = 0;
for(int i = 0; i < (B_DIM+U_DIM); ++i) { f[t][index++] = 0; }
for(int i = 0; i < 2*B_DIM; ++i) { f[t][index++] = penalty_coeff; }
CMat.reset();
eVec.reset();
CMat.insert<B_DIM,B_DIM>(0,0,F[t]);
CMat.insert<B_DIM,U_DIM>(0,B_DIM,G[t]);
CMat.insert<B_DIM,B_DIM>(0,B_DIM+U_DIM,IB);
CMat.insert<B_DIM,B_DIM>(0,2*B_DIM+U_DIM,minusIB);
fillColMajor(C[t], CMat);
if (t == 0) {
eVec.insert<B_DIM,1>(0,0,B[0]);
fillCol(e[0], eVec);
}
eVec = -h[t] + F[t]*bt + G[t]*ut;
fillCol(e[t+1], eVec);
}
// trust region size adjustment
while(true)
{
//std::cout << "PAUSED INSIDE MINIMIZEMERITFUNCTION" << std::endl;
//std::cin.ignore();
LOG_DEBUG(" trust region size: %2.6f %2.6f", Beps, Ueps);
// solve the innermost QP here
for(int t = 0; t < T-1; ++t)
{
Matrix<B_DIM>& bt = B[t];
Matrix<U_DIM>& ut = U[t];
// Fill in lb, ub
index = 0;
// x lower bound
//for(int i = 0; i < X_DIM; ++i) { lb[t][index++] = MAX(xMin[i], bt[i] - Beps); }
// car pos lower bound
for(int i = 0; i < P_DIM; ++i) { lb[t][index++] = MAX(xMin[i], bt[i] - Xpos_eps); }
// car angle lower bound
lb[t][index++] = MAX(xMin[P_DIM], bt[P_DIM] - Xangle_eps);
// landmark pos lower bound
for(int i = C_DIM; i < X_DIM; ++i) { lb[t][index++] = MAX(xMin[i], bt[i] - Xpos_eps); }
// sigma lower bound
for(int i = 0; i < S_DIM; ++i) { lb[t][index] = bt[index] - Beps; index++; }
// u lower bound
//for(int i = 0; i < U_DIM; ++i) { lb[t][index++] = MAX(uMin[i], ut[i] - Ueps); }
// 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*B_DIM; ++i) { lb[t][index++] = 0; }
index = 0;
// x upper bound
//for(int i = 0; i < X_DIM; ++i) { ub[t][index++] = MIN(xMax[i], bt[i] + Beps); }
// car pos upper bound
for(int i = 0; i < P_DIM; ++i) { ub[t][index++] = MIN(xMax[i], bt[i] + Xpos_eps); }
// car angle upper bound
ub[t][index++] = MIN(xMax[P_DIM], bt[P_DIM] + Xangle_eps);
// landmark pos upper bound
for(int i = C_DIM; i < X_DIM; ++i) { ub[t][index++] = MIN(xMax[i], bt[i] + Xpos_eps); }
// sigma upper bound
for(int i = 0; i < S_DIM; ++i) { ub[t][index] = bt[index] + Beps; index++; }
// u upper bound
//for(int i = 0; i < U_DIM; ++i) { ub[t][index++] = MIN(uMax[i], ut[i] + Ueps); }
// 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);
}
Matrix<B_DIM>& bT = B[T-1];
// Fill in lb, ub, C, e
index = 0;
double finalPosDelta = .1;
double finalAngleDelta = M_PI/4;
// xGoal lower bound
for(int i = 0; i < P_DIM; ++i) { lb[T-1][index++] = xGoal[i] - finalPosDelta; }
// loose on car angles and landmarks
lb[T-1][index++] = nearestAngleFromTo(bT[2], xGoal[2] - finalAngleDelta);
//lb[T-1][index++] = xGoal[2] - finalAngleDelta;
for(int i = C_DIM; i < X_DIM; ++i) { lb[T-1][index++] = MAX(xMin[i], bT[i] - Xpos_eps); }
// sigma lower bound
for(int i = 0; i < S_DIM; ++i) { lb[T-1][index] = bT[index] - Beps; index++;}
index = 0;
// xGoal upper bound
for(int i = 0; i < P_DIM; ++i) { ub[T-1][index++] = xGoal[i] + finalPosDelta; }
// loose on car angles and landmarks
ub[T-1][index++] = nearestAngleFromTo(bT[2], xGoal[2] + finalAngleDelta);
//ub[T-1][index++] = xGoal[2] + finalAngleDelta;
for(int i = C_DIM; i < X_DIM; ++i) { ub[T-1][index++] = MIN(xMax[i], bT[i] + Xpos_eps); }
// sigma upper bound
for(int i = 0; i < S_DIM; ++i) { ub[T-1][index] = bT[index] + Beps; index++;}
// Verify problem inputs
//if (!isValidInputs()) {
// std::cout << "Inputs are not valid!" << std::endl;
// exit(-1);
//}
int exitflag = beliefPenaltyMPC_solve(&problem, &output, &info);
if (exitflag == 1) {
for(int t = 0; t < T-1; ++t) {
Matrix<B_DIM>& bt = Bopt[t];
Matrix<U_DIM>& ut = Uopt[t];
for(int i = 0; i < B_DIM; ++i) {
bt[i] = z[t][i];
}
for(int i = 0; i < U_DIM; ++i) {
ut[i] = z[t][B_DIM+i];
}
optcost = info.pobj;
}
for(int i = 0; i < B_DIM; ++i) {
Bopt[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;
new_merit = computeMerit(Bopt, 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);
//std::cout << "PAUSED INSIDE minimizeMeritFunction" << std::endl;
//int num;
//std::cin >> num;
if (approx_merit_improve < -1e-5) {
LOG_ERROR("Approximate merit function got worse: %1.6f", approx_merit_improve);
//LOG_ERROR("Either convexification is wrong to zeroth order, or you are in numerical trouble");
//LOG_ERROR("Failure!");
return false;
} else if (approx_merit_improve < cfg::min_approx_improve) {
LOG_DEBUG("Converged: improvement small enough");
B = Bopt; U = Uopt;
return true;
} else if ((exact_merit_improve < 0) || (merit_improve_ratio < cfg::improve_ratio_threshold)) {
Beps *= 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;
LOG_DEBUG("Shrinking trust region size to: %2.6f %2.6f %2.6f %2.6f", Xpos_eps, Xangle_eps, Uvel_eps, Uangle_eps);
} else {
Beps *= 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;
B = Bopt; U = Uopt;
LOG_DEBUG("Accepted, Increasing trust region size to: %2.6f %2.6f", Beps, Ueps);
break;
}
if (Beps < cfg::min_trust_box_size && Ueps < cfg::min_trust_box_size &&
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) {
LOG_DEBUG("Converged: x tolerance");
return true;
}
} // trust region loop
sqp_iter++;
} // sqp loop
return success;
}
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;
}
