returnValue ExportGaussNewtonForces::setupEvaluation( )
{
////////////////////////////////////////////////////////////////////////////
//
// Setup preparation phase
//
////////////////////////////////////////////////////////////////////////////
preparation.setup("preparationStep");
preparation.doc( "Preparation step of the RTI scheme." );
ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
preparation.setReturnValue(retSim, false);
preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
preparation.addFunctionCall( evaluateObjective );
preparation.addFunctionCall( evaluateConstraints );
////////////////////////////////////////////////////////////////////////////
//
// Setup feedback phase
//
////////////////////////////////////////////////////////////////////////////
ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
feedback.setup("feedbackStep" );
feedback.doc( "Feedback/estimation step of the RTI scheme." );
feedback.setReturnValue( returnValueFeedbackPhase );
feedback.addStatement(
// cond[ 0 ].getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose()
cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose()
);
feedback.addLinebreak();
// Calculate objective residuals
feedback << (Dy -= y) << (DyN -= yN);
feedback.addLinebreak();
for (unsigned i = 0; i < N; ++i)
feedback.addFunctionCall(setStagef, objGradients[ i ], ExportIndex( i ));
feedback.addStatement( objGradients[ N ] == QN2 * DyN );
feedback.addLinebreak();
//
// Configure output variables
//
std::vector< ExportVariable > vecQPVars;
vecQPVars.clear();
vecQPVars.resize(N + 1);
for (unsigned i = 0; i < N; ++i)
vecQPVars[ i ].setup(string("out") + toString(i + 1), NX + NU, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
vecQPVars[ N ].setup(string("out") + toString(N + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
//
// In case warm starting is enabled, give an initial guess, based on the old solution
//
int hotstartQP;
get(HOTSTART_QP, hotstartQP);
if ( hotstartQP )
{
std::vector< ExportVariable > zInit;
zInit.clear();
zInit.resize(N + 1);
for (unsigned i = 0; i < N; ++i)
{
string name = "z_init_";
name = name + (i < 10 ? "0" : "") + toString( i );
zInit[ i ].setup(name, NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
}
string name = "z_init_";
name = name + (N < 10 ? "0" : "") + toString( N );
zInit[ N ].setup(name, NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
// TODO This should be further investigated.
//
// 1) Just use the old solution
//
// for (unsigned blk = 0; blk < N + 1; blk++)
// feedback.addStatement(zInit[ blk ] == vecQPVars[ blk ] );
//
// 2) Initialization by shifting
//
// for (unsigned blk = 0; blk < N - 1; blk++)
// feedback.addStatement( zInit[ blk ] == vecQPVars[blk + 1] );
// for (unsigned el = 0; el < NX; el++)
// feedback.addStatement( zInit[N - 1].getElement(el, 0) == vecQPVars[ N ].getElement(el, 0) );
}
//
// Call a QP solver
// NOTE: we need two prefixes:
// 1. module prefix
// 2. structure instance prefix
//
ExportFunction solveQP;
solveQP.setup("solve");
feedback
<< returnValueFeedbackPhase.getFullName() << " = "
<< qpModuleName << "_" << solveQP.getName() << "( "
<< "&" << qpObjPrefix << "_" << "params" << ", "
<< "&" << qpObjPrefix << "_" << "output" << ", "
<< "&" << qpObjPrefix << "_" << "info" << " );\n";
feedback.addLinebreak();
//
// Here we have to add the differences....
//
ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL);
ExportIndex index( "index" );
acc.setup("accumulate", stageOut, index);
acc.addStatement( x.getRow( index ) += stageOut.getCols(0, NX) );
acc.addLinebreak();
acc.addStatement( u.getRow( index ) += stageOut.getCols(NX, NX + NU) );
acc.addLinebreak();
for (unsigned i = 0; i < N; ++i)
feedback.addFunctionCall(acc, vecQPVars[ i ], ExportIndex( i ));
feedback.addLinebreak();
feedback.addStatement( x.getRow( N ) += vecQPVars[ N ].getTranspose() );
feedback.addLinebreak();
////////////////////////////////////////////////////////////////////////////
//
// TODO Setup evaluation of KKT
//
////////////////////////////////////////////////////////////////////////////
ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
getKKT.setup( "getKKT" );
getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
// kkt.setDoc( "The KKT tolerance value." );
kkt.setDoc( "1e-15." );
getKKT.setReturnValue( kkt );
getKKT.addStatement( kkt == 1e-15 );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonBlockForces::setupEvaluation( )
{
////////////////////////////////////////////////////////////////////////////
//
// Preparation phase
//
////////////////////////////////////////////////////////////////////////////
preparation.setup( "preparationStep" );
preparation.doc( "Preparation step of the RTI scheme." );
ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
preparation.setReturnValue(retSim, false);
ExportIndex index("index");
preparation.addIndex( index );
preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";
preparation.addFunctionCall( evaluateObjective );
if( regularizeHessian.isDefined() ) preparation.addFunctionCall( regularizeHessian );
preparation.addFunctionCall( evaluateConstraints );
preparation.addLinebreak();
preparation << (Dy -= y) << (DyN -= yN);
preparation.addLinebreak();
ExportForLoop condensePrepLoop( index, 0, getNumberOfBlocks() );
condensePrepLoop.addFunctionCall( condensePrep, index );
preparation.addStatement( condensePrepLoop );
preparation.addFunctionCall( evaluateAffineConstraints );
preparation.addStatement( objHessians[getNumberOfBlocks()] == QN1 );
DMatrix mReg = eye<double>( getNX() );
mReg *= levenbergMarquardt;
preparation.addStatement( objHessians[getNumberOfBlocks()] += mReg );
preparation.addLinebreak();
preparation.addStatement( objGradients[ getNumberOfBlocks() ] == QN2 * DyN );
int variableObjS;
get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
ExportVariable SlxCall =
objSlx.isGiven() == true || variableObjS == false ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX);
preparation.addStatement( objGradients[ getNumberOfBlocks() ] += SlxCall );
preparation.addLinebreak();
////////////////////////////////////////////////////////////////////////////
//
// Feedback phase
//
////////////////////////////////////////////////////////////////////////////
ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
tmp.setDoc( "Status code of the qpOASES QP solver." );
feedback.setup("feedbackStep");
feedback.doc( "Feedback/estimation step of the RTI scheme." );
feedback.setReturnValue( tmp );
feedback.addIndex( index );
if (initialStateFixed() == true)
{
feedback.addStatement( cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose() );
}
feedback.addLinebreak();
//
// Configure output variables
//
std::vector< ExportVariable > vecQPVars;
vecQPVars.clear();
vecQPVars.resize(getNumberOfBlocks() + 1);
for (unsigned i = 0; i < getNumberOfBlocks(); ++i)
vecQPVars[ i ].setup(string("out") + toString(i + 1), getNumBlockVariables(), 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
vecQPVars[ getNumberOfBlocks() ].setup(string("out") + toString(getNumberOfBlocks() + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
//
// In case warm starting is enabled, give an initial guess, based on the old solution
//
int hotstartQP;
get(HOTSTART_QP, hotstartQP);
if ( hotstartQP )
{
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
//
// Call a QP solver
// NOTE: we need two prefixes:
// 1. module prefix
// 2. structure instance prefix
//
ExportFunction solveQP;
solveQP.setup("solve");
solveQP.setName( "solve" );
feedback
<< tmp.getFullName() << " = "
<< qpModuleName << "_" << solveQP.getName() << "( "
<< "&" << qpObjPrefix << "_" << "params" << ", "
<< "&" << qpObjPrefix << "_" << "output" << ", "
<< "&" << qpObjPrefix << "_" << "info" << " , NULL);\n";
feedback.addLinebreak();
for (unsigned i = 0; i < getNumberOfBlocks(); ++i) {
feedback.addFunctionCall( expand, vecQPVars[i], ExportIndex(i) );
}
feedback.addStatement( x.getRow( N ) += vecQPVars[ getNumberOfBlocks() ].getTranspose() );
feedback.addLinebreak();
////////////////////////////////////////////////////////////////////////////
//
// Setup evaluation of the KKT tolerance
//
////////////////////////////////////////////////////////////////////////////
ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);
getKKT.setup( "getKKT" );
getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
// kkt.setDoc( "The KKT tolerance value." );
kkt.setDoc( "1e-15." );
getKKT.setReturnValue( kkt );
getKKT.addStatement( kkt == 1e-15 );
return SUCCESSFUL_RETURN;
}
