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