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