/* * h o t s t a r t */ returnValue SQProblem::hotstart( SymmetricMatrix *H_new, const real_t* const g_new, Matrix *A_new, const real_t* const lb_new, const real_t* const ub_new, const real_t* const lbA_new, const real_t* const ubA_new, int_t& nWSR, real_t* const cputime, const Bounds* const guessedBounds, const Constraints* const guessedConstraints ) { if ( ( getStatus( ) == QPS_NOTINITIALISED ) || ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) ) { return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED ); } real_t starttime = 0.0; real_t auxTime = 0.0; if ( cputime != 0 ) starttime = getCPUtime( ); /* I) UPDATE QP MATRICES AND VECTORS */ if ( setupNewAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); /* II) PERFORM USUAL HOMOTOPY */ /* Allow only remaining CPU time for usual hotstart. */ if ( cputime != 0 ) { auxTime = getCPUtime( ) - starttime; *cputime -= auxTime; } returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime, guessedBounds,guessedConstraints ); if ( cputime != 0 ) *cputime += auxTime; return returnvalue; }
/* * h o t s t a r t */ returnValue SQProblem::hotstart( const real_t* const H_new, const real_t* const g_new, const real_t* const A_new, const real_t* const lb_new, const real_t* const ub_new, const real_t* const lbA_new, const real_t* const ubA_new, int& nWSR, real_t* const cputime ) { if ( ( getStatus( ) == QPS_NOTINITIALISED ) || ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) ) { return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED ); } /* start runtime measurement */ real_t starttime = 0.0; if ( cputime != 0 ) starttime = getCPUtime( ); /* I) UPDATE QP MATRICES AND VECTORS */ if ( setupAuxiliaryQP( H_new,A_new,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN ) return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED ); /* II) PERFORM USUAL HOMOTOPY */ /* Allow only remaining CPU time for usual hotstart. */ if ( cputime != 0 ) *cputime -= getCPUtime( ) - starttime; returnValue returnvalue = QProblem::hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime ); /* stop runtime measurement */ if ( cputime != 0 ) *cputime = getCPUtime( ) - starttime; return returnvalue; }
USING_NAMESPACE_QPOASES int main(int argc, char **argv) { ros::init(argc, argv, "mpc"); ros::NodeHandle node_handle("mpc"); boost::scoped_ptr<realtime_tools::RealtimePublisher<mpc::MPCState> > mpc_pub; mpc_pub.reset(new realtime_tools::RealtimePublisher<mpc::MPCState> (node_handle, "data", 1)); mpc_pub->lock(); mpc_pub->msg_.states.resize(12); mpc_pub->msg_.reference_states.resize(12); mpc_pub->msg_.inputs.resize(4); mpc_pub->unlock(); mpc::model::Model *model_ptr = new mpc::example_models::ArDrone(); mpc::optimizer::Optimizer *optimizer_ptr = new mpc::optimizer::qpOASES(node_handle); mpc::model::Simulator *simulator_ptr = new mpc::example_models::ArDroneSimulator(); mpc::ModelPredictiveControl *mpc_ptr = new mpc::STDMPC(node_handle); mpc_ptr->resetMPC(model_ptr, optimizer_ptr, simulator_ptr); // Operation state double x_operation[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // Set the initial conditions as the first linearization points in order to initiate the MPC algorithm model_ptr->setLinearizationPoints(x_operation); mpc_ptr->initMPC(); double x_ref[12] = {0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; double x_meas[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; double *u, *delta_u, *x_bar, *u_bar, *new_state; double delta_xmeas[12], delta_xref[12]; double sampling_time = 0.0083; u = new double[4]; new_state = new double[12]; x_bar = new double[12]; u_bar = new double[4]; double t_sim; timespec start_rt, end_rt; clock_gettime(CLOCK_REALTIME, &start_rt); real_t begin, end; for (int counter = 0; counter < 4000; counter++) { /*while (ros::ok())*/ t_sim = sampling_time * counter; if (t_sim < 8){ x_ref[0] = 0.25*t_sim; // referencia de posicion en X x_ref[1] = 0.; // referencia de posicion en Y x_ref[3] = 0.25; // referencia de velocidad en X (rampa ascendente) x_ref[4] = 0.; } else if (t_sim >= 8 && t_sim < 16){ x_ref[0] = 2.0; // referencia de posicion en X x_ref[1] = -0.25*t_sim + 2.0; // referencia de posicion en Y x_ref[3] = 0.; // referencia de velocidad en X x_ref[4] = -0.25; } else if (t_sim >= 16 && t_sim < 24){ x_ref[0] = -0.25*t_sim + 6.0; // referencia de posicion en X x_ref[1] = -2.0; // referencia de posicion en Y x_ref[3] = -0.25; // referencia de velocidad en X x_ref[4] = 0.; } else if (t_sim >= 24 && t_sim < 32){ x_ref[0] = 0.; // referencia de posicion en X x_ref[1] = 0.25*t_sim - 8.0; // referencia de posicion en Y x_ref[3] = 0.; // referencia de velocidad en X x_ref[4] = 0.25; } ///////////////////////////////////////////////////////////////////////////////// else { x_ref[0] = 0.; x_ref[1] = 0.; x_ref[3] = 0.; x_ref[4] = 0.; } x_bar = model_ptr->getOperationPointsStates(); for (int i = 0; i < 12; i++) { delta_xmeas[i] = x_meas[i] - x_bar[i]; delta_xref[i] = x_ref[i] - x_bar[i]; } // Solving the quadratic problem to obtain the new inputs begin = getCPUtime(); mpc_ptr->updateMPC(delta_xmeas, delta_xref); // Here we are also recalculating the system matrices A and B end = getCPUtime(); real_t duration = end - begin; //std::cout << "Optimization problem computational time:" << static_cast<double>(duration) << std::endl; delta_u = mpc_ptr->getControlSignal(); u_bar = model_ptr->getOperationPointsInputs(); for (int i = 0; i < 4; i++) u[i] = u_bar[i] + delta_u[i]; // Updating the simulator with the new inputs new_state = simulator_ptr->simulatePlant(x_meas, u, sampling_time); Eigen::Map<Eigen::VectorXd> new_(new_state, 12); //std::cout << "New state\t" << new_.transpose() << std::endl; // Setting the new operation points // model_ptr->setLinearizationPoints(new_state); if (mpc_pub->trylock()) { mpc_pub->msg_.header.stamp = ros::Time::now(); for (int j = 0; j < 12; j++) { mpc_pub->msg_.states[j] = x_meas[j]; mpc_pub->msg_.reference_states[j] = x_ref[j]; } for (int j = 0; j < 4; j++) mpc_pub->msg_.inputs[j] = u[j]; } mpc_pub->unlockAndPublish(); // Shifting the state vector for (int i = 0; i < 12; i++) { x_meas[i] = new_state[i]; } } //clock_gettime(CLOCK_REALTIME, &end_rt); //double duration = (end_rt.tv_sec - start_rt.tv_sec) + 1e-9*(end_rt.tv_nsec - start_rt.tv_nsec); clock_gettime(CLOCK_REALTIME, &end_rt); double duration = (end_rt.tv_sec - start_rt.tv_sec) + 1e-9*(end_rt.tv_nsec - start_rt.tv_nsec); ROS_INFO("The duration of computation of optimization problem is %f seg.", duration); mpc_ptr->writeToDisc(); return 0; }
/** Example for calling qpOASES with sparse matrices. */ int main( ) { long i; int nWSR; real_t err, tic, toc; real_t *x1 = new real_t[180]; real_t *y1 = new real_t[271]; real_t *x2 = new real_t[180]; real_t *y2 = new real_t[271]; /* create sparse matrices */ SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val); SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val); H->createDiagInfo(); real_t* H_full = H->full(); real_t* A_full = A->full(); SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full); DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full); /* solve with dense matrices */ nWSR = 1000; QProblem qrecipeD(180, 91); tic = getCPUtime(); qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0); toc = getCPUtime(); qrecipeD.getPrimalSolution(x1); qrecipeD.getDualSolution(y1); fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic); /* solve with sparse matrices */ nWSR = 1000; QProblem qrecipeS(180, 91); tic = getCPUtime(); qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0); toc = getCPUtime(); qrecipeS.getPrimalSolution(x2); qrecipeS.getDualSolution(y2); fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic); /* check distance of solutions */ err = 0.0; for (i = 0; i < 180; i++) if (getAbs(x1[i] - x2[i]) > err) err = getAbs(x1[i] - x2[i]); fprintf(stdFile, "Primal error: %9.2e\n", err); err = 0.0; for (i = 0; i < 271; i++) if (getAbs(y1[i] - y2[i]) > err) err = getAbs(y1[i] - y2[i]); fprintf(stdFile, "Dual error: %9.2e (might not be unique)\n", err); delete H; delete A; delete[] H_full; delete[] A_full; delete Hd; delete Ad; delete[] y2; delete[] x2; delete[] y1; delete[] x1; return 0; }
void HCOD::activeSearch( VectorXd & u ) { // if( isDebugOnce ) { sotDebugTrace::openFile(); isDebugOnce = false; } // else { if(sotDEBUGFLOW.outputbuffer.good()) sotDebugTrace::closeFile(); } //if(sotDEBUGFLOW.outputbuffer.good()) { sotDebugTrace::closeFile();sotDebugTrace::openFile(); } sotDEBUGIN(15); /* * foreach stage: stage.initCOD(Ir_init) * u = 0 * u0 = solve * do * tau,cst_ref = max( violation(stages) ) * u += (1-tau)u0 + tau*u1 * if( tau<1 ) * update(cst_ref); break; * * lambda,w = computeLambda * cst_ref,lmin = min( lambda,w ) * if lmin<0 * downdate( cst_ref ) * */ assert(VectorXi::LinSpaced(3,0,2)[0] == 0 && VectorXi::LinSpaced(3,0,2)[1] == 1 && VectorXi::LinSpaced(3,0,2)[2] == 2 && "new version of Eigen might have change the " "order of arguments in LinSpaced, please correct"); /*struct timeval t0,t1,t2;double time1,time2; gettimeofday(&t0,NULL);*/ initialize(); sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl; Y.computeExplicitly(); // TODO: this should be done automatically on Y size. sotDEBUG(5) << "Y= " << (MATLAB)Y.matrixExplicit<< std::endl; /*gettimeofday(&t1,NULL); time1 = ((t1.tv_sec-t0.tv_sec)+(t1.tv_usec-t0.tv_usec)/1.0e6);*/ int iter = 0; startTime=getCPUtime(); Index stageMinimal = 0; do { iter ++; sotDEBUG(5) << " --- *** \t" << iter << "\t***.---" << std::endl; //if( iter>1 ) { break; } if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW ); assert( testRecomposition(&std::cerr) ); damp(); computeSolution(); assert( testSolution(&std::cerr) ); double tau = computeStepAndUpdate(); if( tau<1 ) { sotDEBUG(5) << "Update done, make step <1." << std::endl; makeStep(tau); } else { sotDEBUG(5) << "No update, make step ==1." << std::endl; makeStep(); for( ;stageMinimal<=(Index)stages.size();++stageMinimal ) { sotDEBUG(5) << "--- Started to examinate stage " << stageMinimal << std::endl; computeLagrangeMultipliers(stageMinimal); if( sotDEBUG_ENABLE(15) ) show( sotDEBUGFLOW ); //assert( testLagrangeMultipliers(stageMinimal,std::cerr) ); if( searchAndDowndate(stageMinimal) ) { sotDEBUG(5) << "Lagrange<0, downdate done." << std::endl; break; } for( Index i=0;i<stageMinimal;++i ) stages[i]->freezeSlacks(false); if( stageMinimal<nbStages() ) stages[stageMinimal]->freezeSlacks(true); } } lastTime=getCPUtime()-startTime; lastNumberIterations=iter; if( lastTime>maxTime ) throw 667; if( iter>maxNumberIterations ) throw 666; } while(stageMinimal<=nbStages()); sotDEBUG(5) << "Lagrange>=0, no downdate, active search completed." << std::endl; /*gettimeofday(&t2,NULL); time2 = ((t2.tv_sec-t1.tv_sec)+(t2.tv_usec-t1.tv_usec)/1.0e6); std::ofstream fup("/tmp/haset.dat",std::ios::app); fup << time1<<"\t"<<time2<<"\t"<<iter<<"\t";*/ u=solution; sotDEBUG(5) << "uf =" << (MATLAB)u << std::endl; sotDEBUGOUT(15); }
int main( ) { USING_NAMESPACE_QPOASES long i; int nWSR; real_t tic, toc; real_t errP=0.0, errD=0.0; real_t *x1 = new real_t[180]; real_t *y1 = new real_t[271]; real_t *x2 = new real_t[180]; real_t *y2 = new real_t[271]; /* create sparse matrices */ SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val); SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val); H->createDiagInfo(); real_t* H_full = H->full(); real_t* A_full = A->full(); SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full); DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full); /* solve with dense matrices */ nWSR = 1000; QProblem qrecipeD(180, 91); tic = getCPUtime(); qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0); toc = getCPUtime(); qrecipeD.getPrimalSolution(x1); qrecipeD.getDualSolution(y1); fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic); /* Compute KKT tolerances */ real_t stat, feas, cmpl; getKKTResidual( 180,91, H_full,g,A_full,lb,ub,lbA,ubA, x1,y1, stat,feas,cmpl ); printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat,feas,cmpl ); /* solve with sparse matrices */ nWSR = 1000; QProblem qrecipeS(180, 91); tic = getCPUtime(); qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0); toc = getCPUtime(); qrecipeS.getPrimalSolution(x2); qrecipeS.getDualSolution(y2); fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic); /* Compute KKT tolerances */ real_t stat2, feas2, cmpl2; getKKTResidual( 180,91, H_full,g,A_full,lb,ub,lbA,ubA, x2,y2, stat2,feas2,cmpl2 ); printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat2,feas2,cmpl2 ); /* check distance of solutions */ for (i = 0; i < 180; i++) if (getAbs(x1[i] - x2[i]) > errP) errP = getAbs(x1[i] - x2[i]); fprintf(stdFile, "Primal error: %9.2e\n", errP); for (i = 0; i < 271; i++) if (getAbs(y1[i] - y2[i]) > errD) errD = getAbs(y1[i] - y2[i]); fprintf(stdFile, "Dual error: %9.2e (might not be unique)\n", errD); delete H; delete A; delete[] H_full; delete[] A_full; delete Hd; delete Ad; delete[] y2; delete[] x2; delete[] y1; delete[] x1; QPOASES_TEST_FOR_TRUE( stat <= 1e-15 ); QPOASES_TEST_FOR_TRUE( feas <= 1e-15 ); QPOASES_TEST_FOR_TRUE( cmpl <= 1e-15 ); QPOASES_TEST_FOR_TRUE( stat2 <= 1e-14 ); QPOASES_TEST_FOR_TRUE( feas2 <= 1e-14 ); QPOASES_TEST_FOR_TRUE( cmpl2 <= 1e-13 ); QPOASES_TEST_FOR_TRUE( errP <= 1e-13 ); return TEST_PASSED; }
int main( ) { USING_NAMESPACE_QPOASES long i; int_t nWSR; real_t errP1, errP2, errP3, errD1, errD2, errD3, tic, toc; real_t *x1 = new real_t[180]; real_t *y1 = new real_t[271]; real_t *x2 = new real_t[180]; real_t *y2 = new real_t[271]; real_t *x3 = new real_t[180]; real_t *y3 = new real_t[271]; Options options; options.setToDefault(); options.enableEqualities = BT_TRUE; /* create sparse matrices */ SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val); SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val); H->createDiagInfo(); real_t* H_full = H->full(); real_t* A_full = A->full(); SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full); DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full); /* solve with dense matrices */ nWSR = 1000; QProblem qrecipeD(180, 91); qrecipeD.setOptions(options); tic = getCPUtime(); qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0); toc = getCPUtime(); qrecipeD.getPrimalSolution(x1); qrecipeD.getDualSolution(y1); fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic); /* solve with sparse matrices (nullspace factorization) */ nWSR = 1000; QProblem qrecipeS(180, 91); qrecipeS.setOptions(options); tic = getCPUtime(); qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0); toc = getCPUtime(); qrecipeS.getPrimalSolution(x2); qrecipeS.getDualSolution(y2); fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic); /* solve with sparse matrices (Schur complement) */ #ifndef SOLVER_NONE nWSR = 1000; SQProblemSchur qrecipeSchur(180, 91); qrecipeSchur.setOptions(options); tic = getCPUtime(); qrecipeSchur.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0); toc = getCPUtime(); qrecipeSchur.getPrimalSolution(x3); qrecipeSchur.getDualSolution(y3); fprintf(stdFile, "Solved sparse problem (Schur complement approach) in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic); #endif /* SOLVER_NONE */ /* check distance of solutions */ errP1 = 0.0; errP2 = 0.0; errP3 = 0.0; #ifndef SOLVER_NONE for (i = 0; i < 180; i++) if (getAbs(x1[i] - x2[i]) > errP1) errP1 = getAbs(x1[i] - x2[i]); for (i = 0; i < 180; i++) if (getAbs(x1[i] - x3[i]) > errP2) errP2 = getAbs(x1[i] - x3[i]); for (i = 0; i < 180; i++) if (getAbs(x2[i] - x3[i]) > errP3) errP3 = getAbs(x2[i] - x3[i]); #endif /* SOLVER_NONE */ fprintf(stdFile, "Primal error (dense and sparse): %9.2e\n", errP1); fprintf(stdFile, "Primal error (dense and Schur): %9.2e\n", errP2); fprintf(stdFile, "Primal error (sparse and Schur): %9.2e\n", errP3); errD1 = 0.0; errD2 = 0.0; errD3 = 0.0; for (i = 0; i < 271; i++) if (getAbs(y1[i] - y2[i]) > errD1) errD1 = getAbs(y1[i] - y2[i]); #ifndef SOLVER_NONE for (i = 0; i < 271; i++) if (getAbs(y1[i] - y3[i]) > errD2) errD2 = getAbs(y1[i] - y3[i]); for (i = 0; i < 271; i++) if (getAbs(y2[i] - y3[i]) > errD3) errD3 = getAbs(y2[i] - y3[i]); #endif /* SOLVER_NONE */ fprintf(stdFile, "Dual error (dense and sparse): %9.2e (might not be unique)\n", errD1); fprintf(stdFile, "Dual error (dense and Schur): %9.2e (might not be unique)\n", errD2); fprintf(stdFile, "Dual error (sparse and Schur): %9.2e (might not be unique)\n", errD3); delete H; delete A; delete[] H_full; delete[] A_full; delete Hd; delete Ad; delete[] y3; delete[] x3; delete[] y2; delete[] x2; delete[] y1; delete[] x1; QPOASES_TEST_FOR_TOL( errP1,1e-13 ); QPOASES_TEST_FOR_TOL( errP2,1e-13 ); QPOASES_TEST_FOR_TOL( errP3,1e-13 ); return TEST_PASSED; }