returnValue CondensingExport::setup( )
{
String fileName( "condensing.c" );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Preparing to export %s... ",fileName.getName() );
Q.setDataStruct( ACADO_VARIABLES );
R.setDataStruct( ACADO_VARIABLES );
QF.setDataStruct( ACADO_VARIABLES );
QS.setDataStruct( ACADO_VARIABLES );
QS2.setDataStruct( ACADO_VARIABLES );
setupQQF( );
// set up in this order to ensure correct initialization!
setupMultiplicationRoutines( );
setupCondensing( );
setupEvaluation( );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
returnValue OptionsList::printOptionsList( ) const
{
OptionsItem* current = first;
acadoPrintf( "\nThis class provides the following %d user options:\n",number );
while ( current != 0 )
{
if ( current->getType( ) == OIT_INT )
{
int curVal;
current->getValue( curVal );
acadoPrintf( " --> set( \"%-22d\", <int> ); current value: %d\n",current->getName(),curVal );
}
else
{
double curVal;
current->getValue( curVal );
acadoPrintf( " --> set( \"%-22d\", <double> ); current value: %e\n",current->getName(),curVal );
}
current = current->getNext( );
}
acadoPrintf( "\n" );
return SUCCESSFUL_RETURN;
}
returnValue FunctionEvaluationTree::evaluate( double *x, double *result, PrintLevel printL ){
int run1;
if( printL == MEDIUM || printL == HIGH ){
acadoPrintf("symbolic expression evaluation:\n");
}
for( run1 = 0; run1 < n; run1++ ){
sub[run1]->evaluate( 0, x, &x[ indexList->index(VT_INTERMEDIATE_STATE,
lhs_comp[run1] ) ] );
if( printL == HIGH ){
acadoPrintf("sub[%d] = %.16e \n", lhs_comp[run1],
x[ indexList->index(VT_INTERMEDIATE_STATE, lhs_comp[run1]) ] );
}
}
for( run1 = 0; run1 < dim; run1++ ){
f[run1]->evaluate( 0, x, &result[run1] );
if( printL == HIGH ){
acadoPrintf("f[%d] = %.16e \n", run1, result[run1] );
}
if( printL == MEDIUM ){
acadoPrintf("f[%d] = %.16e \n", run1, result[run1] );
}
}
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::init( VariablesGrid* x_init ,
VariablesGrid* xa_init,
VariablesGrid* p_init ,
VariablesGrid* u_init ,
VariablesGrid* w_init )
{
int printC;
get( PRINT_COPYRIGHT, printC );
// PRINT THE HEADER:
// -----------------
if( printC == BT_TRUE )
acadoPrintCopyrightNotice( "SCPmethod -- A Sequential Quadratic Programming Algorithm." );
iter.init( x_init, xa_init, p_init, u_init, w_init );
// iter.print(); // already here different!!
if ( setup( ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_INIT_FAILED );
// COMPUTATION OF DERIVATIVES:
// ---------------------------
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Computing initial linearization of NLP system ...\n" );
// iter.print();
ACADO_TRY( eval->evaluateSensitivities( iter,bandedCP ) ).changeType( RET_NLP_INIT_FAILED );
// iter.print();
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Computing initial linearization of NLP system done.\n" );
int useRealtimeIterations;
get( USE_REALTIME_ITERATIONS,useRealtimeIterations );
if ( (BooleanType)useRealtimeIterations == BT_TRUE )
{
if ( bandedCPsolver->prepareSolve( bandedCP ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
}
// freeze condensing in case OCP is QP -- isCP is a bit misleading...
if ( ( isCP == BT_TRUE ) && ( eval->hasLSQobjective( ) == BT_TRUE ) )
{
// bandedCPsolver->freezeCondensing( );
// eval->freezeSensitivities( );
}
status = BS_READY;
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::shiftVariables( double timeShift,
Vector lastX,
Vector lastXA,
Vector lastP,
Vector lastU,
Vector lastW
)
{
#ifdef SIM_DEBUG
acadoPrintf( "SCPmethod::shiftVariables\n" );
#endif
if ( acadoIsNegative( timeShift ) == BT_TRUE )
return ACADOERROR( RET_INVALID_ARGUMENTS );
// Matrix tmp;
// for( uint i=1; i<iter.getNumPoints()-1; ++i )
// {
// bandedCP.lambdaDynamic.getSubBlock( i,0, tmp );
// bandedCP.lambdaDynamic.setDense( i-1,0, tmp );
// }
// printf("shifted!\n");
needToReevaluate = BT_TRUE;
return iter.shift( timeShift, lastX, lastXA, lastP, lastU, lastW );
}
returnValue SCPmethod::checkForConvergence( )
{
double tol;
get( KKT_TOLERANCE,tol );
// NEEDS TO BE CHECKED CARFULLY !!!
double KKTmultiplierRegularisation;
get( KKT_TOLERANCE_SAFEGUARD,KKTmultiplierRegularisation );
if( eval->getKKTtolerance( iter,bandedCP,KKTmultiplierRegularisation ) <= tol )
{
int discretizationMode;
get( DISCRETIZATION_TYPE, discretizationMode );
if( (StateDiscretizationType)discretizationMode == SINGLE_SHOOTING )
{
eval->clearDynamicDiscretization( );
if ( eval->evaluate( iter,bandedCP ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
}
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= MEDIUM )
{
acadoPrintf("\nconvergence achieved. \n\n");
}
return CONVERGENCE_ACHIEVED;
}
return CONVERGENCE_NOT_YET_ACHIEVED;
}
returnValue VectorspaceElement::print( const char* const name,
PrintScheme printScheme
) const
{
char* string = 0;
printToString( &string, name,printScheme );
acadoPrintf( "%s",string );
if ( string != 0 )
delete[] string;
return SUCCESSFUL_RETURN;
}
returnValue VectorspaceElement::print( const char* const name,
const char* const startString,
const char* const endString,
uint width,
uint precision,
const char* const colSeparator,
const char* const rowSeparator
) const
{
char* string = 0;
printToString( &string, name,startString,endString,width,precision,colSeparator,rowSeparator );
acadoPrintf( "%s",string );
if ( string != 0 )
delete[] string;
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::getFirstControl( Vector& u0_ ) const
{
#ifdef SIM_DEBUG
acadoPrintf( "SCPmethod::getFirstControl\n" );
#endif
if( iter.u == 0 )
return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
u0_ = iter.u->getVector( 0 );
if ( hasPerformedStep == BT_FALSE )
{
Vector deltaU0( getNU() );
bandedCPsolver->getFirstControl( deltaU0 );
u0_ += deltaU0;
}
return SUCCESSFUL_RETURN;
}
returnValue EvaluationPoint::print() const {
uint run1;
acadoPrintf( "time = %.16e \n", z[idx[0][0]] );
for( run1 = 0; run1 < nx; run1++ )
acadoPrintf( "x [%d] = %.16e \n", run1,z[idx[1][run1]] );
for( run1 = 0; run1 < na; run1++ )
acadoPrintf( "xa[%d] = %.16e \n", run1,z[idx[2][run1]] );
for( run1 = 0; run1 < np; run1++ )
acadoPrintf( "p [%d] = %.16e \n", run1,z[idx[3][run1]] );
for( run1 = 0; run1 < nu; run1++ )
acadoPrintf( "u [%d] = %.16e \n", run1,z[idx[4][run1]] );
for( run1 = 0; run1 < nw; run1++ )
acadoPrintf( "w [%d] = %.16e \n", run1,z[idx[5][run1]] );
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::performCurrentStep( )
{
returnValue returnvalue;
if ( isInRealTimeMode == BT_TRUE )
{
if ( bandedCPsolver->finalizeSolve( bandedCP ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
// bandedCP.deltaX.print();
}
// acadoPrintf("bandedCP.dynResiduum = \n");
// bandedCP.dynResiduum.print();
// acadoPrintf("bandedCP.lambdaDynamic = \n");
// bandedCP.lambdaDynamic.print();
oldIter = iter;
// Perform a globalized step:
// --------------------------
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Perform globalized SQP step ...\n" );
clock.reset( );
clock.start( );
#ifdef SIM_DEBUG
/* printf("performing the current step...: old iterate \n");
(iter.x->getVector(0)).print("iter.x(0)");
(iter.u->getVector(0)).print("iter.u(0)");
(iter.x->getVector(1)).print("iter.x(1)");
(iter.u->getVector(1)).print("iter.u(1)");*/
#endif
returnvalue = scpStep->performStep( iter,bandedCP,eval );
if( returnvalue != SUCCESSFUL_RETURN )
ACADOERROR( RET_NLP_STEP_FAILED );
hasPerformedStep = BT_TRUE;
clock.stop( );
setLast( LOG_TIME_GLOBALIZATION,clock.getTime() );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Perform globalized SQP step done.\n" );
printIteration( );
// Check convergence criterion if no real-time iterations are performed
int terminateAtConvergence = 0;
get( TERMINATE_AT_CONVERGENCE,terminateAtConvergence );
if ( (BooleanType)terminateAtConvergence == BT_TRUE )
{
if ( checkForConvergence( ) == CONVERGENCE_ACHIEVED )
{
stopClockAndPrintRuntimeProfile( );
return CONVERGENCE_ACHIEVED;
}
}
if ( numberOfSteps >= 0 )
set( KKT_TOLERANCE_SAFEGUARD,0.0 );
return CONVERGENCE_NOT_YET_ACHIEVED;
}
returnValue SCPmethod::feedbackStep( const Vector& x0_,
const Vector& p_
)
{
#ifdef SIM_DEBUG
printf("START OF THE FEEDBACK STEP \n");
x0_.print("x0");
#endif
returnValue returnvalue;
if ( ( status != BS_READY ) && ( status != BS_RUNNING ) )
return ACADOERROR( RET_INITIALIZE_FIRST );
clockTotalTime.reset( );
clockTotalTime.start( );
status = BS_RUNNING;
hasPerformedStep = BT_FALSE;
if ( checkForRealTimeMode( x0_,p_ ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
int hessianMode;
get( HESSIAN_APPROXIMATION,hessianMode );
if ( ( isInRealTimeMode == BT_FALSE ) && ( (HessianApproximationMode)hessianMode == EXACT_HESSIAN ) )
{
returnvalue = initializeHessianProjection();
if( returnvalue != SUCCESSFUL_RETURN )
return ACADOERROR( returnvalue );
}
//bandedCP.objectiveGradient.print();
//x0_.print("x0");
if ( isInRealTimeMode == BT_TRUE )
{
if ( setupRealTimeParameters( x0_,p_ ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
}
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Solving banded QP ...\n" );
// iter.print();
if ( bandedCPsolver->solve( bandedCP ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
// bandedCP.deltaX.print();
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Solving banded QP done.\n" );
++numberOfSteps;
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::setup( )
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
ExportStruct structWspace;
structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
// non equidistant integration grids not implemented for NARX integrators
if( !equidistant ) return ACADOERROR( RET_INVALID_OPTION );
String fileName( "integrator.c" );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Preparing to export %s... ",fileName.getName() );
ExportIndex run( "run" );
ExportIndex i( "i" );
ExportIndex j( "j" );
ExportIndex k( "k" );
ExportIndex tmp_index("tmp_index");
uint diffsDim = NX*(NX+NU);
uint inputDim = NX*(NX+NU+1) + NU + NP;
// setup INTEGRATE function
rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, BT_TRUE );
rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
if( equidistantControlGrid() ) {
integrate = ExportFunction( "integrate", rk_eta, reset_int );
}
else {
integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
}
integrate.setReturnValue( error_code );
integrate.addIndex( run );
integrate.addIndex( i );
integrate.addIndex( j );
integrate.addIndex( k );
integrate.addIndex( tmp_index );
rhs_in = ExportVariable( "x", inputDim-diffsDim, 1, REAL, ACADO_LOCAL );
rhs_out = ExportVariable( "f", NX, 1, REAL, ACADO_LOCAL );
fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
rk_xxx = ExportVariable( "rk_xxx", 1, inputDim-diffsDim, REAL, structWspace );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", NX1, NX1+NU, REAL, structWspace );
rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", NX2, NX1+NX2+NU, REAL, structWspace );
}
rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2, NX1+NX2+NU, REAL, structWspace );
ExportVariable numInt( "numInts", 1, 1, INT );
if( !equidistantControlGrid() ) {
ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
integrate.addStatement( String( "int " ) << numInt.getName() << " = " << numStepsV.getName() << "[" << rk_index.getName() << "];\n" );
}
integrate.addStatement( rk_xxx.getCols( NX,inputDim-diffsDim ) == rk_eta.getCols( NX+diffsDim,inputDim ) );
integrate.addLinebreak( );
// integrator loop:
ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
ExportStatementBlock *loop;
if( equidistantControlGrid() ) {
loop = &tmpLoop;
}
else {
loop = &integrate;
loop->addStatement( String("for(") << run.getName() << " = 0; " << run.getName() << " < " << numInt.getName() << "; " << run.getName() << "++ ) {\n" );
}
loop->addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
// Set rk_diffsPrev:
loop->addStatement( String("if( run > 0 ) {\n") );
if( NX1 > 0 ) {
ExportForLoop loopTemp1( i,0,NX1 );
loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX+NXA,i*NX+NX+NXA+NX1 ) );
if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX1,NX1+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1),i*NU+(NX+NXA)*(NX+1)+NU ) );
loop->addStatement( loopTemp1 );
}
if( NX2 > 0 ) {
ExportForLoop loopTemp2( i,0,NX2 );
loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,NX1+NX2 ) == rk_eta.getCols( i*NX+NX+NXA+NX1*NX,i*NX+NX+NXA+NX1*NX+NX1+NX2 ) );
if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,NX1+NX2,NX1+NX2+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+NX1*NU,i*NU+(NX+NXA)*(NX+1)+NX1*NU+NU ) );
loop->addStatement( loopTemp2 );
}
loop->addStatement( String("}\n") );
}
// evaluate states:
if( NX1 > 0 ) {
loop->addFunctionCall( lin_input.getName(), rk_xxx, rk_eta.getAddress(0,0) );
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameRHS(), rk_xxx, rk_eta.getAddress(0,NX1) );
}
// evaluate sensitivities:
if( NX1 > 0 ) {
for( uint i1 = 0; i1 < NX1; i1++ ) {
for( uint i2 = 0; i2 < NX1; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,i2,i2+1) == A11(i1,i2) );
}
for( uint i2 = 0; i2 < NU; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,NX1+i2,NX1+i2+1) == B11(i1,i2) );
}
}
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameDiffsRHS(), rk_xxx, rk_diffsNew2.getAddress(0,0) );
}
// computation of the sensitivities using chain rule:
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "if( run == 0 ) {\n" ) );
}
// PART 1
updateInputSystem(loop, i, j, tmp_index);
// PART 2
updateImplicitSystem(loop, i, j, tmp_index);
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "}\n" ) );
loop->addStatement( String( "else {\n" ) );
// PART 1
propagateInputSystem(loop, i, j, k, tmp_index);
// PART 2
propagateImplicitSystem(loop, i, j, k, tmp_index);
loop->addStatement( String( "}\n" ) );
}
// end of the integrator loop.
if( !equidistantControlGrid() ) {
loop->addStatement( "}\n" );
}
else {
integrate.addStatement( *loop );
}
// PART 1
if( NX1 > 0 ) {
Matrix zeroR = zeros(1, NX2);
ExportForLoop loop1( i,0,NX1 );
loop1.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1,i*NX+NX+NXA+NX ) == zeroR );
integrate.addStatement( loop1 );
}
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
returnValue AuxiliaryFunctionsExport::setup( )
{
String fileName( "auxiliary_functions.c" );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Preparing to export %s... ",fileName.getName() );
ExportVariable x ( "x", N+1,NX, REAL,ACADO_VARIABLES );
ExportVariable u ( "u", N,NU, REAL,ACADO_VARIABLES );
ExportVariable p ( "p", 1,NP, REAL,ACADO_VARIABLES );
ExportVariable xRef( "xRef", N+1,NX, REAL,ACADO_VARIABLES );
ExportVariable uRef( "uRef", N,NU, REAL,ACADO_VARIABLES );
getAcadoVariablesX.setup( "getAcadoVariablesX" );
getAcadoVariablesX.setReturnValue( x,BT_TRUE );
getAcadoVariablesU.setup( "getAcadoVariablesU" );
getAcadoVariablesU.setReturnValue( u,BT_TRUE );
getAcadoVariablesXRef.setup( "getAcadoVariablesXRef" );
getAcadoVariablesXRef.setReturnValue( xRef,BT_TRUE );
getAcadoVariablesURef.setup( "getAcadoVariablesURef" );
getAcadoVariablesURef.setReturnValue( uRef,BT_TRUE );
printStates.setup( "printStates" );
printStates.addStatement( ExportPrintf(x.getTranspose()) );
printControls.setup( "printControls" );
printControls.addStatement( ExportPrintf(u.getTranspose()) );
int operatingSystem;
get( OPERATING_SYSTEM,operatingSystem );
getTime.setup( "getTime" );
ExportVariable currentTime( "current_time",(Matrix)0.0 );
currentTime.callByValue();
if ( (OperatingSystem)operatingSystem == OS_WINDOWS )
{
getTime.addStatement( "LARGE_INTEGER counter, frequency;\n" );
getTime.addStatement( "QueryPerformanceFrequency(&frequency);\n" );
getTime.addStatement( "QueryPerformanceCounter(&counter);\n" );
getTime.addStatement( "current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);\n" );
}
else
{
// OS_UNIX
getTime.addStatement( "struct timeval theclock;\n" );
getTime.addStatement( "gettimeofday( &theclock,0 );\n" );
getTime.addStatement( "current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;\n" );
}
getTime.setReturnValue( currentTime );
printHeader.setup( "printHeader" );
printHeader.addStatement( " printf(\"\\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\\nCopyright (C) 2008-2011 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.\\nDeveloped within the Optimization in Engineering Center (OPTEC) under\\nsupervision of Moritz Diehl. All rights reserved.\\n\\nACADO Toolkit is distributed under the terms of the GNU Lesser\\nGeneral Public License 3 in the hope that it will be useful,\\nbut WITHOUT ANY WARRANTY; without even the implied warranty of\\nMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\\nGNU Lesser General Public License for more details.\\n\\n\" );\n" );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
returnValue String::print() const {
acadoPrintf( name );
acadoPrintf( "\n" );
return SUCCESSFUL_RETURN;
}
returnValue ConjugateGradientMethod::solve( double *b ){
// CONSISTENCY CHECKS:
// -------------------
if( dim <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
if( nDense <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
if( A == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
int run1, run2;
double *aux = new double[dim];
double auxR, auxR2, norm1;
double alpha, beta;
// INITIALISE X:
// --------------
for( run1 = 0; run1 < dim; run1++ )
x[run1] = 0.0;
// RESET THE COUNTER IF TOO LARGE:
// -------------------------------
if( pCounter > dim ) pCounter = dim-1;
// APPLY THE PRECONDITIONER ON b:
// ------------------------------
applyPreconditioner( b );
// COMPUTE WARM START INITIALIZATION FOR X BASED ON PREVIOUS
// DECOMPOSITIONS:
// ----------------------------------------------------------
for( run1 = 0; run1 < pCounter; run1++ ){
alpha = scalarProduct( p[run1], r )/norm2[run1];
for( run2 = 0; run2 < dim; run2++ ){
x[run2] += alpha*p[run1][run2];
}
}
// COMPUTE INITIAL RESIDUUM:
// -------------------------
multiply( x, aux );
for( run1 = 0; run1 < dim; run1++ )
r[run1] -= aux[run1];
for( run1 = 0; run1 < dim; run1++ )
p[pCounter][run1] = r[run1];
while( pCounter <= 2*dim-1 ){
norm1 = 0.0;
for( run1 = 0; run1 < dim; run1++ ){
if( r[run1] >= 0 && norm1 < r[run1] ) norm1 = r[run1];
if( r[run1] <= 0 && norm1 < -r[run1] ) norm1 = -r[run1];
}
if( printLevel == HIGH )
acadoPrintf("STEP NUMBER %d, WEIGHTED NORM = %.16e \n", pCounter, norm1 );
if( norm1 < TOL ) break;
auxR = scalarProduct( r,r );
multiply( p[pCounter], aux );
norm2[pCounter] = scalarProduct( p[pCounter], aux );
alpha = auxR/norm2[pCounter];
for( run1 = 0; run1 < dim; run1++ ){
x [run1] += alpha*p[pCounter][run1];
r [run1] -= alpha*aux[run1];
}
auxR2 = scalarProduct( r,r );
beta = auxR2/auxR;
for( run1 = 0; run1 < dim; run1++ )
p[pCounter+1][run1] = r[run1] + beta*p[pCounter][run1];
pCounter++;
}
delete[] aux ;
if( pCounter >= 2*dim ){
if( printLevel == MEDIUM || printLevel == HIGH )
return ACADOWARNING( RET_LINEAR_SYSTEM_NUMERICALLY_SINGULAR );
else return RET_LINEAR_SYSTEM_NUMERICALLY_SINGULAR;
}
return SUCCESSFUL_RETURN;
}
returnValue MultiObjectiveAlgorithm::solve( ){
int run1,run2;
returnValue returnvalue;
ASSERT( ocp != 0 );
ASSERT( m >= 2 );
if( N == 0 ) get( PARETO_FRONT_DISCRETIZATION, N );
int paretoGeneration;
get( PARETO_FRONT_GENERATION, paretoGeneration );
int hotstart;
get( PARETO_FRONT_HOTSTART, hotstart );
Expression **arg = 0;
arg = new Expression*[m];
for( run1 = 0; run1 < m; run1++ )
ocp->getObjective( run1, &arg[run1] );
Constraint tmp_con;
double *idx = new double[m];
WeightGeneration generator;
Matrix Weights;
Vector formers;
Vector lb(m);
Vector ub(m);
lb.setZero();
ub.setAll(1.0);
generator.getWeights( m, N, lb, ub, Weights, formers );
result.init( Weights.getNumCols(), m );
count = 0;
if( xResults == 0 ) xResults = new VariablesGrid[Weights.getNumCols()];
if( xaResults == 0 ) xaResults = new VariablesGrid[Weights.getNumCols()];
if( pResults == 0 ) pResults = new VariablesGrid[Weights.getNumCols()];
if( uResults == 0 ) uResults = new VariablesGrid[Weights.getNumCols()];
if( wResults == 0 ) wResults = new VariablesGrid[Weights.getNumCols()];
totalNumberOfSQPiterations = 0;
totalCPUtime = -acadoGetTime();
run1 = 0;
while( run1 < (int) Weights.getNumCols() ){
// PRINT THE ITERATION NUMBER:
// ---------------------------
acadoPrintf("\n\n Multi-objective point: %d out of %d \n\n",run1+1, (int) Weights.getNumCols() );
ocp->getConstraint( tmp_con );
for( run2 = 0; run2 < (int) Weights.getNumRows(); run2++ )
idx[run2] = Weights( run2, run1 );
// THIS PART OF THE CODE WILL NOT RUN YET FOR GENERAL WEIGHTS
int vertex = -1;
for( run2 = 0; run2 < m; run2++ ){
if( fabs( idx[run2]-1.0 ) < 100.0*EPS )
vertex = run2;
}
// ----------------------------------------------------------
if( vertex == -1 || paretoGeneration == PFG_WEIGHTED_SUM ){
formulateOCP( idx, ocp, arg );
setStatus( BS_NOT_INITIALIZED );
returnvalue = OptimizationAlgorithm::solve();
if( nlpSolver != 0 )
totalNumberOfSQPiterations += nlpSolver->getNumberOfSteps();
ocp->setConstraint( tmp_con );
set( PRINT_COPYRIGHT, BT_FALSE );
if( returnvalue != SUCCESSFUL_RETURN ){
ACADOERROR(returnvalue);
}
else{
getDifferentialStates( xResults[run1] );
getAlgebraicStates ( xaResults[run1] );
getParameters ( pResults[run1] );
getControls ( uResults[run1] );
getDisturbances ( wResults[run1] );
if( hotstart == BT_TRUE ){
getDifferentialStates( *userInit.x );
getAlgebraicStates ( *userInit.xa );
getParameters ( *userInit.p );
getControls ( *userInit.u );
getDisturbances ( *userInit.w );
evaluateObjectives( *userInit.x, *userInit.xa, *userInit.p, *userInit.u, *userInit.w, arg );
}
else{
VariablesGrid xd_tmp, xa_tmp, p_tmp, u_tmp, w_tmp;
getDifferentialStates( xd_tmp );
getAlgebraicStates ( xa_tmp );
getParameters ( p_tmp );
getControls ( u_tmp );
getDisturbances ( w_tmp );
evaluateObjectives( xd_tmp, xa_tmp, p_tmp, u_tmp, w_tmp, arg );
}
}
}
else{
acadoPrintf(" Result from single objective optimization is adopted. \n\n" );
for( run2 = 0; run2 < m; run2++ ){
result(count,run2) = vertices(vertex,run2);
}
count++;
}
run1++;
}
totalCPUtime += acadoGetTime();
for( run1 = 0; run1 < m; run1++ )
delete arg[run1];
delete[] arg;
delete[] idx;
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::setup( )
{
// CONSISTENCY CHECKS:
// -------------------
if ( isCP == BT_TRUE )
{
int hessianApproximation;
get( HESSIAN_APPROXIMATION,hessianApproximation );
// Gauss-Newton is exact for linear-quadratic systems
if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN )
set( HESSIAN_APPROXIMATION,GAUSS_NEWTON );
}
// PREPARE THE SQP ALGORITHM:
// --------------------------
if ( eval == 0 )
return ACADOERROR( RET_UNKNOWN_BUG );
if ( eval->init( iter ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_INIT_FAILED );
// PREPARE THE DATA FOR THE SQP ALGORITHM:
// ---------------------------------------
if ( bandedCPsolver != 0 )
delete bandedCPsolver;
int sparseQPsolution;
get( SPARSE_QP_SOLUTION,sparseQPsolution );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Initializing banded QP solver ...\n" );
if ( (SparseQPsolutionMethods)sparseQPsolution == CONDENSING )
{
bandedCP.lambdaConstraint.init( eval->getNumConstraintBlocks(), 1 );
bandedCP.lambdaDynamic.init( getNumPoints()-1, 1 );
bandedCPsolver = new CondensingBasedCPsolver( userInteraction,eval->getNumConstraints(),eval->getConstraintBlockDims() );
bandedCPsolver->init( iter );
}
else
{
return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
}
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Initializing banded QP solver done.\n" );
// INITIALIZE GLOBALIZATION STRATEGY (SCPstep):
// --------------------------------------------
if ( scpStep != 0 )
delete scpStep;
int globalizationStrategy;
get( GLOBALIZATION_STRATEGY,globalizationStrategy );
switch( (GlobalizationStrategy)globalizationStrategy )
{
case GS_FULLSTEP:
scpStep = new SCPstepFullstep( userInteraction );
break;
case GS_LINESEARCH:
scpStep = new SCPstepLinesearch( userInteraction );
break;
default:
return ACADOERROR( RET_UNKNOWN_BUG );
}
// EVALUATION OF THE NLP FUNCTIONS:
// --------------------------------
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Initial integration of dynamic system ...\n" );
ACADO_TRY( eval->evaluate(iter,bandedCP) ).changeType( RET_NLP_INIT_FAILED );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Initial integration of dynamic system done.\n" );
// INITIALIZE HESSIAN MATRIX:
// --------------------------
int hessianMode;
get( HESSIAN_APPROXIMATION,hessianMode );
if( ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON ) || ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON_WITH_BLOCK_BFGS ) )
{
if( eval->hasLSQobjective( ) == BT_FALSE ){
// ACADOWARNING( RET_GAUSS_NEWTON_APPROXIMATION_NOT_SUPPORTED );
// set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
// get( HESSIAN_APPROXIMATION, hessianMode );
}
}
if ( derivativeApproximation != 0 )
delete derivativeApproximation;
switch( (HessianApproximationMode)hessianMode )
{
case EXACT_HESSIAN:
derivativeApproximation = new ExactHessian( userInteraction );
break;
case CONSTANT_HESSIAN:
derivativeApproximation = new ConstantHessian( userInteraction );
break;
case FULL_BFGS_UPDATE:
derivativeApproximation = new BFGSupdate( userInteraction );
break;
case BLOCK_BFGS_UPDATE:
derivativeApproximation = new BFGSupdate( userInteraction,getNumPoints() );
break;
case GAUSS_NEWTON:
derivativeApproximation = new GaussNewtonApproximation( userInteraction );
break;
case GAUSS_NEWTON_WITH_BLOCK_BFGS:
derivativeApproximation = new GaussNewtonApproximationWithBFGS( userInteraction,getNumPoints() );
break;
default:
return ACADOERROR( RET_UNKNOWN_BUG );
}
bandedCP.hessian.init( 5*getNumPoints(), 5*getNumPoints() );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Initializing Hessian computations ...\n" );
ACADO_TRY( derivativeApproximation->initHessian( bandedCP.hessian,getNumPoints(),iter ) );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Initializing Hessian computations done.\n" );
// SWITCH BETWEEN SINGLE- AND MULTIPLE SHOOTING:
// ---------------------------------------------
int discretizationMode;
get( DISCRETIZATION_TYPE, discretizationMode );
if( (StateDiscretizationType)discretizationMode != SINGLE_SHOOTING ){
if( iter.x != 0 ) iter.x ->disableAutoInit();
if( iter.xa != 0 ) iter.xa->disableAutoInit();
}
return SUCCESSFUL_RETURN;
}
returnValue SIMexport::exportCode( const String& dirName,
const String& _realString,
const String& _intString,
int _precision
)
{
if (!modelDimensionsSet()) return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
set( QP_SOLVER, QP_NONE );
if ( setup( ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
int printLevel;
get( PRINTLEVEL,printLevel );
// export mandatory source code files
if ( exportAcadoHeader( dirName,commonHeaderName,_realString,_intString,_precision ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
if( integrator != 0 )
{
String fileName( dirName );
fileName << "/integrator.c";
ExportFile integratorFile( fileName,commonHeaderName,_realString,_intString,_precision );
integrator->getCode( integratorFile );
if ( integratorFile.exportCode( ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
int measGrid;
get( MEASUREMENT_GRID, measGrid );
int generateMatlabInterface;
get( GENERATE_MATLAB_INTERFACE, generateMatlabInterface );
int debugMode;
get( INTEGRATOR_DEBUG_MODE, debugMode );
if ( (BooleanType)generateMatlabInterface == BT_TRUE ) {
String integrateInterface( dirName );
integrateInterface << "/integrate.c";
ExportMatlabIntegrator exportMexFun( INTEGRATOR_MEX_TEMPLATE, integrateInterface, commonHeaderName,_realString,_intString,_precision );
exportMexFun.configure((MeasurementGrid)measGrid == ONLINE_GRID, (BooleanType)debugMode, timingCalls, ((RungeKuttaExport*)integrator)->getNumStages());
exportMexFun.exportCode();
String rhsInterface( dirName );
rhsInterface << "/rhs.c";
ExportMatlabRhs exportMexFun2( RHS_MEX_TEMPLATE, rhsInterface, commonHeaderName,_realString,_intString,_precision );
exportMexFun2.configure(integrator->getNameFullRHS());
exportMexFun2.exportCode();
}
}
// export template for main file, if desired
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Exporting remaining files... " );
// export a basic Makefile, if desired
int generateMakeFile;
get( GENERATE_MAKE_FILE,generateMakeFile );
if ( (BooleanType)generateMakeFile == BT_TRUE )
if ( exportMakefile( dirName,"Makefile",_realString,_intString,_precision ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
// export the evaluation file
int exportTest;
get( GENERATE_TEST_FILE, exportTest );
if ( exportTest && exportEvaluation( dirName, String( "compare.c" ) ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
if ( (PrintLevel)printLevel > NONE )
ACADOINFO( RET_CODE_EXPORT_SUCCESSFUL );
return SUCCESSFUL_RETURN;
}
USING_NAMESPACE_ACADO
int main( ){
// Define variables, functions and constants:
// ----------------------------------------------------------
DifferentialState dT1;
DifferentialState dT2;
DifferentialState dT3;
DifferentialState dT4;
DifferentialState T1;
DifferentialState T2;
DifferentialState T3;
DifferentialState T4;
DifferentialState W1;
DifferentialState W2;
DifferentialState W3;
DifferentialState W4;
DifferentialState q1;
DifferentialState q2;
DifferentialState q3;
DifferentialState q4;
DifferentialState Omega1;
DifferentialState Omega2;
DifferentialState Omega3;
DifferentialState V1;
DifferentialState V2;
DifferentialState V3;
DifferentialState P1; // x
DifferentialState P2; // y
DifferentialState P3; // z
DifferentialState IP1;
DifferentialState IP2;
DifferentialState IP3;
Control U1;
Control U2;
Control U3;
Control U4;
DifferentialEquation f1, f2;
const double rho = 1.23;
const double A = 0.1;
const double Cl = 0.25;
const double Cd = 0.3*Cl;
const double m = 10;
const double g = 9.81;
const double L = 0.5;
const double Jp = 1e-2;
const double xi = 1e-2;
const double J1 = 0.25;
const double J2 = 0.25;
const double J3 = 1;
const double gain = 1e-4;
const double alpha = 0.0;
// Define the quadcopter ODE model in fully nonlinear form:
// ----------------------------------------------------------
f1 << U1*gain;
f1 << U2*gain;
f1 << U3*gain;
f1 << U4*gain;
f1 << dT1;
f1 << dT2;
f1 << dT3;
f1 << dT4;
f1 << (T1 - W1*xi)/Jp;
f1 << (T2 - W2*xi)/Jp;
f1 << (T3 - W3*xi)/Jp;
f1 << (T4 - W4*xi)/Jp;
f1 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
f1 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
f1 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
f1 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f1 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f1 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
f1 << V1;
f1 << V2;
f1 << V3;
f1 << P1;
f1 << P2;
f1 << P3;
// Define the quadcopter ODE model in 3-stage format:
// ----------------------------------------------------------
// LINEAR INPUT SYSTEM (STAGE 1):
Matrix M1, A1, B1;
M1 = eye(12);
A1 = zeros(12,12);
B1 = zeros(12,4);
A1(4,0) = 1.0;
A1(5,1) = 1.0;
A1(6,2) = 1.0;
A1(7,3) = 1.0;
A1(8,4) = 1.0/Jp; A1(8,8) = -xi/Jp;
A1(9,5) = 1.0/Jp; A1(9,9) = -xi/Jp;
A1(10,6) = 1.0/Jp; A1(10,10) = -xi/Jp;
A1(11,7) = 1.0/Jp; A1(11,11) = -xi/Jp;
B1(0,0) = gain;
B1(1,1) = gain;
B1(2,2) = gain;
B1(3,3) = gain;
// NONLINEAR SYSTEM (STAGE 2):
f2 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
f2 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
f2 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
f2 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f2 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f2 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
// LINEAR OUTPUT SYSTEM (STAGE 3):
Matrix M3, A3;
M3 = eye(6);
A3 = zeros(6,6);
A3(3,0) = 1.0;
A3(4,1) = 1.0;
A3(5,2) = 1.0;
OutputFcn f3;
f3 << V1;
f3 << V2;
f3 << V3;
f3 << 0.0;
f3 << 0.0;
f3 << 0.0;
// ----------------------------------------------------------
// ----------------------------------------------------------
SIMexport sim1( 10, 1.0 );
sim1.setModel( f1 );
sim1.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
sim1.set( NUM_INTEGRATOR_STEPS, 50 );
sim1.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------------------------\n Using a QuadCopter ODE model in fully nonlinear form:\n-----------------------------------------------------------\n" );
sim1.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
// ----------------------------------------------------------
// ----------------------------------------------------------
SIMexport sim2( 10, 1.0 );
sim2.setLinearInput( M1, A1, B1 );
sim2.setModel( f2 );
sim2.setLinearOutput( M3, A3, f3 );
sim2.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
sim2.set( NUM_INTEGRATOR_STEPS, 50 );
sim2.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------------------------\n Using a QuadCopter ODE model in 3-stage format:\n-----------------------------------------------------------\n" );
sim2.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
return 0;
}
returnValue SCPmethod::prepareNextStep( )
{
returnValue returnvalue;
RealClock clockLG;
BlockMatrix oldLagrangeGradient;
BlockMatrix newLagrangeGradient;
// acadoPrintf("bandedCP.dynResiduum (possibly shifted) = \n");
// bandedCP.dynResiduum.print();
// acadoPrintf("bandedCP.lambdaDynamic (possibly shifted) = \n");
// bandedCP.lambdaDynamic.print();
// Coumpute the "old" Lagrange Gradient with the latest multipliers:
// -----------------------------------------------------------------
clockLG.reset( );
clockLG.start( );
returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),oldIter,bandedCP, oldLagrangeGradient );
if( returnvalue != SUCCESSFUL_RETURN )
ACADOERROR( RET_NLP_STEP_FAILED );
clockLG.stop( );
// Linearize the NLP system at the new point:
// ------------------------------------------
int printLevel;
get( PRINTLEVEL,printLevel );
#ifdef SIM_DEBUG
/* printf("preparation step \n");
(iter.x->getVector(0)).print("iter.x(0)");
(iter.u->getVector(0)).print("iter.u(0)");
(iter.x->getVector(1)).print("iter.x(1)");
(iter.u->getVector(1)).print("iter.u(1)");*/
#endif
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Computing new linearization of NLP system ...\n" );
clock.reset( );
clock.start( );
if ( needToReevaluate == BT_TRUE )
{
// needs to re-evaluate due to eval->evaluate call within merit function evaluation!
eval->clearDynamicDiscretization( );
if ( eval->evaluate( iter,bandedCP ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
needToReevaluate = BT_FALSE;
}
returnvalue = eval->evaluateSensitivities( iter,bandedCP );
if( returnvalue != SUCCESSFUL_RETURN )
ACADOERROR( RET_NLP_STEP_FAILED );
clock.stop( );
setLast( LOG_TIME_SENSITIVITIES,clock.getTime() );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Computing new linearization of NLP system done.\n" );
//bandedCP.objectiveGradient.print();
// Coumpute the "new" Lagrange Gradient with the latest multipliers:
// -----------------------------------------------------------------
clockLG.start( );
returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),iter,bandedCP, newLagrangeGradient );
if( returnvalue != SUCCESSFUL_RETURN )
ACADOERROR( RET_NLP_STEP_FAILED );
clockLG.stop( );
setLast( LOG_TIME_LAGRANGE_GRADIENT,clockLG.getTime() );
// Compute the next Hessian:
// -------------------------
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Computing or approximating Hessian matrix ...\n" );
clock.reset( );
clock.start( );
returnvalue = computeHessianMatrix( oldLagrangeGradient,newLagrangeGradient );
if( returnvalue != SUCCESSFUL_RETURN )
ACADOERROR( RET_NLP_STEP_FAILED );
clock.stop( );
setLast( LOG_TIME_HESSIAN_COMPUTATION,clock.getTime() );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Computing or approximating Hessian matrix done.\n" );
// CONDENSE THE KKT-SYSTEM:
// ------------------------
if ( isInRealTimeMode == BT_TRUE )
{
if ( bandedCPsolver->prepareSolve( bandedCP ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
}
stopClockAndPrintRuntimeProfile( );
return CONVERGENCE_NOT_YET_ACHIEVED;
}
int main() {
USING_NAMESPACE_ACADO
//
// DEFINE THE VARIABLES:
//
DifferentialState xT; // the trolley position
DifferentialState vT; // the trolley velocity
IntermediateState aT; // the trolley acceleration
DifferentialState xL; // the cable length
DifferentialState vL; // the cable velocity
IntermediateState aL; // the cable acceleration
DifferentialState phi; // the excitation angle
DifferentialState omega; // the angular velocity
DifferentialState uT; // trolley velocity control
DifferentialState uL; // cable velocity control
Control duT;
Control duL;
//
// DEFINE THE PARAMETERS:
//
const double tau1 = 0.012790605943772;
const double a1 = 0.047418203070092;
const double tau2 = 0.024695192379264;
const double a2 = 0.034087337273386;
const double g = 9.81;
const double c = 0.0;
const double m = 1318.0;
//
// DEFINE THE MODEL EQUATIONS:
//
DifferentialEquation f;
aT = -1.0 / tau1 * vT + a1 / tau1 * uT;
aL = -1.0 / tau2 * vL + a2 / tau2 * uL;
f << 0 == dot( xT ) - vT;
f << 0 == dot( vT ) - aT;
f << 0 == dot( xL ) - vL;
f << 0 == dot( vL ) - aL;
f << 0 == dot( phi ) - omega;
f << 0 == dot( omega ) - 1.0/xL*(-g*sin(phi)-aT*cos(phi)
-2*vL*omega-c*omega/(m*xL));
f << 0 == dot( uT ) - duT;
f << 0 == dot( uL ) - duL;
//
// DEFINE THE OUTPUT MODEL:
//
OutputFcn h;
h << aT;
h << aL;
//
// SET UP THE SIMULATION EXPORT MODULE:
//
acadoPrintf( "-----------------------------------------\n Using an equidistant grid:\n-----------------------------------------\n" );
SIMexport sim( 1, 0.1 );
sim.setModel( f );
sim.addOutput( h, 5 );
sim.set( INTEGRATOR_TYPE, INT_IRK_RIIA5 );
sim.set( NUM_INTEGRATOR_STEPS, 5 );
sim.setTimingSteps( 10000 );
sim.exportAndRun( "crane_export", "init_crane.txt", "controls_crane.txt" );
acadoPrintf( "-----------------------------------------\n Using a provided grid:\n-----------------------------------------\n" );
Vector Meas(5);
Meas(0) = 0.0;
Meas(1) = 0.2;
Meas(2) = 0.4;
Meas(3) = 0.6;
Meas(4) = 0.8;
SIMexport sim2( 1, 0.1 );
sim2.setModel( f );
sim2.addOutput( h, Meas );
sim2.set( INTEGRATOR_TYPE, INT_IRK_RIIA5 );
sim2.set( NUM_INTEGRATOR_STEPS, 5 );
sim2.setTimingSteps( 10000 );
sim2.exportAndRun( "crane_export", "init_crane.txt", "controls_crane.txt" );
return 0;
}
int main( ){
USING_NAMESPACE_ACADO;
// VARIABLES:
// ------------------------
DifferentialState x; // Position of the trolley
DifferentialState v; // Velocity of the trolley
DifferentialState phi; // excitation angle
DifferentialState dphi; // rotational velocity
Control ax; // trolley accelaration
Disturbance W; // disturbance
double L = 1.0 ; // length
double m = 1.0 ; // mass
double g = 9.81; // gravitational constant
double b = 0.2 ; // friction coefficient
// DIFFERENTIAL EQUATION:
// ------------------------
DifferentialEquation f, fSim; // The model equations
f << dot(x) == v;
f << dot(v) == ax;
f << dot(phi ) == dphi;
f << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;
L = 1.2; // introduce model plant mismatch
fSim << dot(x) == v;
fSim << dot(v) == ax + W;
fSim << dot(phi ) == dphi;
fSim << dot(dphi) == -g/L*sin(phi) -ax/L*cos(phi) - b/(m*L*L)*dphi;
// DEFINE LEAST SQUARE FUNCTION:
// -----------------------------
Function h;
h << x;
h << v;
h << phi;
h << dphi;
Matrix Q(4,4); // LSQ coefficient matrix
Q.setIdentity();
Vector r(4); // Reference
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0;
const double t_end = 5.0;
OCP ocp( t_start, t_end, 25 );
ocp.minimizeLSQ( Q, h, r );
ocp.subjectTo( f );
ocp.subjectTo( -5.0 <= ax <= 5.0 );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( fSim,identity );
Process process( dynamicSystem,INT_RK45 );
VariablesGrid disturbance = readFromFile( "dist.txt" );
if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// SETTING UP THE MPC CONTROLLER:
// ------------------------------
double samplingTime = 0.1;
RealTimeAlgorithm alg( ocp, samplingTime );
// alg.set( USE_REALTIME_ITERATIONS,NO );
// alg.set( MAX_NUM_ITERATIONS,20 );
StaticReferenceTrajectory zeroReference;
Controller controller( alg, zeroReference );
Vector x0(4);
x0.setZero();
x0(3) = 1.0;
double startTime = 0.0;
double endTime = 20.0;
Vector uCon;
VariablesGrid ySim;
if (controller.init( startTime,x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
controller.getU( uCon );
if (process.init( startTime,x0,uCon ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
process.getY( ySim );
// hand-coding call to
// sim.run( )
int nSteps = 0;
double currentTime = startTime;
while ( currentTime <= endTime )
{
acadoPrintf( "\n*** Simulation Loop No. %d (starting at time %.3f) ***\n",nSteps,currentTime );
if (controller.step( currentTime,ySim.getLastVector() ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
controller.getU( uCon );
if (process.step( currentTime,currentTime+samplingTime,uCon ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
process.getY( ySim );
++nSteps;
currentTime = (double)nSteps * samplingTime;
}
return EXIT_SUCCESS;
}
int main() {
USING_NAMESPACE_ACADO
// ----------------------------------------------------------
DifferentialState x;
DifferentialState y;
DifferentialState alpha;
DifferentialState dx;
DifferentialState dy;
DifferentialState dalpha;
AlgebraicState ddx;
AlgebraicState ddy;
AlgebraicState ddalpha;
AlgebraicState Fx;
AlgebraicState Fy;
Control u;
DifferentialEquation f;
OutputFcn h;
h << Fx;
h << Fy;
const double m = 2;
const double M = 3.5;
const double I = 0.1;
const double g = 9.81;
f << 0 == dot( x ) - dx;
f << 0 == dot( y ) - dy;
f << 0 == dot( alpha ) - dalpha;
f << 0 == dot( dx ) - ddx ;
f << 0 == dot( dy ) - ddy;
f << 0 == dot( dalpha ) - ddalpha;
f << 0 == m*ddx - (Fx+u);
f << 0 == m*ddy + m*g - (Fy+u);
f << 0 == I*ddalpha - M - (Fx+u)*y + (Fy+u)*x;
f << 0 == ddx + dy*dalpha + y*ddalpha;
f << 0 == ddy - dx*dalpha - x*ddalpha;
// ----------------------------------------------------------
Vector Meas(1);
Meas(0) = 5;
SIMexport sim( 1, 0.1 );
sim.set( INTEGRATOR_TYPE, INT_IRK_RIIA3 );
sim.set( NUM_INTEGRATOR_STEPS, 4 );
sim.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );
sim.setModel( f );
sim.addOutput( h );
sim.setMeasurements( Meas );
sim.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------\n Using a Pendulum DAE model in ACADO syntax:\n-----------------------------------------\n" );
sim.exportAndRun( "externModel_export", "init_externModel.txt", "controls_externModel.txt" );
SIMexport sim2( 1, 0.1 );
sim2.set( INTEGRATOR_TYPE, INT_IRK_RIIA3 );
sim2.set( NUM_INTEGRATOR_STEPS, 4 );
sim2.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );
sim2.setModel( "model", "rhs", "rhs_jac" );
sim2.setDimensions( 6, 6, 5, 1 );
sim2.addOutput( "out", "out_jac", 2 );
sim2.setMeasurements( Meas );
sim2.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------\n Using an externally defined Pendulum DAE model:\n-----------------------------------------\n" );
sim2.exportAndRun( "externModel_export", "init_externModel.txt", "controls_externModel.txt" );
return 0;
}
returnValue SimulationEnvironment::step( )
{
/* Consistency check. */
if ( getStatus( ) != BS_READY )
return ACADOERROR( RET_BLOCK_NOT_READY );
++nSteps;
acadoPrintf( "\n*** Simulation Loop No. %d (starting at time %.3f) ***\n",nSteps,simulationClock.getTime( ) );
/* Perform one single simulation loop */
Vector u, p;
Vector uPrevious, pPrevious;
if ( getNU( ) > 0 )
feedbackControl.evaluate( simulationClock.getTime( ),uPrevious );
if ( getNP( ) > 0 )
feedbackParameter.evaluate( simulationClock.getTime( ),pPrevious );
VariablesGrid y;
Vector yPrevious;
if ( getNY( ) > 0 )
processOutput.evaluate( simulationClock.getTime( ),yPrevious );
// step controller
// yPrevious.print("controller input y");
if ( controller->step( simulationClock.getTime( ),yPrevious ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
double compDelay = determineComputationalDelay( controller->getPreviousRealRuntime( ) );
double nextSamplingInstant = controller->getNextSamplingInstant( simulationClock.getTime( ) );
nextSamplingInstant = round( nextSamplingInstant * 1.0e6 ) / 1.0e6;
// obtain new controls and parameters
if ( controller->getU( u ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// u.print("controller output u");
if ( controller->getP( p ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
if ( acadoIsEqual( simulationClock.getTime( ),endTime ) == BT_TRUE )
{
simulationClock.init( nextSamplingInstant );
return SUCCESSFUL_RETURN;
}
if ( fabs( compDelay ) < 100.0*EPS )
{
// step process without computational delay
if ( process->step( simulationClock.getTime( ),nextSamplingInstant,u,p ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// Obtain current process output
if ( process->getY( y ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// y.print("process output y");
// update history
if ( getNU( ) > 0 )
feedbackControl. add( simulationClock.getTime( ),nextSamplingInstant,u );
if ( getNP( ) > 0 )
feedbackParameter.add( simulationClock.getTime( ),nextSamplingInstant,p );
if ( getNY( ) > 0 )
processOutput. add( y,IM_LINEAR );
}
else
{
// step process WITH computational delay
if ( simulationClock.getTime( )+compDelay > nextSamplingInstant )
return ACADOERROR( RET_COMPUTATIONAL_DELAY_TOO_BIG );
Grid delayGrid( 3 );
delayGrid.setTime( simulationClock.getTime( ) );
delayGrid.setTime( simulationClock.getTime( )+compDelay );
delayGrid.setTime( nextSamplingInstant );
VariablesGrid uDelayed( u.getDim( ),delayGrid,VT_CONTROL );
uDelayed.setVector( 0,uPrevious );
uDelayed.setVector( 1,u );
uDelayed.setVector( 2,u );
VariablesGrid pDelayed( p.getDim( ),delayGrid,VT_PARAMETER );
pDelayed.setVector( 0,pPrevious );
pDelayed.setVector( 1,p );
pDelayed.setVector( 2,p );
if ( process->step( uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// Obtain current process output
if ( process->getY( y ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
// update history
if ( getNU( ) > 0 )
feedbackControl. add( uDelayed,IM_CONSTANT );
if ( getNP( ) > 0 )
feedbackParameter.add( pDelayed,IM_CONSTANT );
if ( getNY( ) > 0 )
processOutput. add( y,IM_LINEAR );
}
// update simulation clock
simulationClock.init( nextSamplingInstant );
return SUCCESSFUL_RETURN;
}
returnValue MultiObjectiveAlgorithm::solveSingleObjective( const int &number_ ){
int run1 ;
returnValue returnvalue;
ASSERT( ocp != 0 );
ASSERT( number_ < m );
if( N == 0 ) get( PARETO_FRONT_DISCRETIZATION, N );
int N_pow_m_minus_1 = 1;
int i;
for(i=0; i<m-1; i++)
N_pow_m_minus_1 *= N;
Expression **arg = 0;
arg = new Expression*[m];
for( run1 = 0; run1 < m; run1++ )
ocp->getObjective( run1, &arg[run1] );
Grid tmp_grid;
ocp->getGrid( tmp_grid );
Objective tmp(tmp_grid);
tmp.addMayerTerm(*arg[number_]);
ocp->setObjective( tmp );
setStatus( BS_NOT_INITIALIZED );
acadoPrintf("\n\n Optimization of individual objective %d out of %d \n\n",number_ +1, m );
totalCPUtime = -acadoGetTime();
returnvalue = OptimizationAlgorithm::solve();
totalCPUtime += acadoGetTime();
int index;
Matrix Weights = getWeights();
for( run1 = 0; run1 < (int) Weights.getNumCols(); run1++ ){
if( fabs( Weights( number_, run1 ) - 1.0 ) < 1000.0*EPS )
index = run1;
}
if( xResults == 0 ) xResults = new VariablesGrid[Weights.getNumCols()];
if( xaResults == 0 ) xaResults = new VariablesGrid[Weights.getNumCols()];
if( pResults == 0 ) pResults = new VariablesGrid[Weights.getNumCols()];
if( uResults == 0 ) uResults = new VariablesGrid[Weights.getNumCols()];
if( wResults == 0 ) wResults = new VariablesGrid[Weights.getNumCols()];
getDifferentialStates( xResults[index] );
getAlgebraicStates ( xaResults[index] );
getParameters ( pResults[index] );
getControls ( uResults[index] );
getDisturbances ( wResults[index] );
if( returnvalue != SUCCESSFUL_RETURN )
return ACADOERROR(returnvalue);
if( nlpSolver != 0 )
totalNumberOfSQPiterations = nlpSolver->getNumberOfSteps();
int hotstart;
get( PARETO_FRONT_HOTSTART, hotstart );
int *indices = new int[m];
for( run1 = 0; run1 < m; run1++ )
indices[run1] = number_;
VariablesGrid xd_tmp, xa_tmp, p_tmp, u_tmp, w_tmp;
if( hotstart == BT_TRUE ){
getDifferentialStates( *userInit.x );
getAlgebraicStates ( *userInit.xa );
getParameters ( *userInit.p );
getControls ( *userInit.u );
getDisturbances ( *userInit.w );
xd_tmp = *userInit.x;
xa_tmp = *userInit.xa;
p_tmp = *userInit.p;
u_tmp = *userInit.u;
w_tmp = *userInit.w;
}
else{
getDifferentialStates( xd_tmp );
getAlgebraicStates ( xa_tmp );
getParameters ( p_tmp );
getControls ( u_tmp );
getDisturbances ( w_tmp );
}
VariablesGrid *_xd = 0;
VariablesGrid *_xa = 0;
VariablesGrid *_p = 0;
VariablesGrid *_u = 0;
VariablesGrid *_w = 0;
if( xd_tmp.isEmpty() == BT_FALSE ) _xd = new VariablesGrid(xd_tmp);
if( xa_tmp.isEmpty() == BT_FALSE ) _xa = new VariablesGrid(xa_tmp);
if( p_tmp.isEmpty() == BT_FALSE ) _p = new VariablesGrid(p_tmp );
if( u_tmp.isEmpty() == BT_FALSE ) _u = new VariablesGrid(u_tmp );
if( w_tmp.isEmpty() == BT_FALSE ) _w = new VariablesGrid(w_tmp );
if( vertices.getDim() == 0 ){
vertices.init(m,m);
vertices.setAll(-INFTY);
}
Objective **obj = new Objective*[m];
for( run1 = 0; run1 < m; run1++ ){
obj[run1] = new Objective( tmp_grid );
obj[run1]->addMayerTerm(*arg[run1]);
OCPiterate xx( _xd, _xa, _p, _u, _w );
obj[run1]->evaluate( xx );
obj[run1]->getObjectiveValue( vertices(number_,run1) );
}
if( _xd != 0 ) delete _xd;
if( _xa != 0 ) delete _xa;
if( _p != 0 ) delete _p ;
if( _u != 0 ) delete _u ;
if( _w != 0 ) delete _w ;
delete[] indices;
for( run1 = 0; run1 < m; run1++ )
delete arg[run1];
delete[] arg;
for( run1 = 0; run1 < m; run1++ )
delete obj[run1];
delete[] obj;
return SUCCESSFUL_RETURN;
}
