const std::array<int, 3> GridDims::getIJK(size_t globalIndex) const {
std::array<int, 3> r = { { 0, 0, 0 } };
int k = globalIndex / (getNX() * getNY());
globalIndex -= k * (getNX() * getNY());
int j = globalIndex / getNX();
globalIndex -= j * getNX();
int i = globalIndex;
r[0] = i;
r[1] = j;
r[2] = k;
return r;
}
uint CondensingExport::getNumQPvars( ) const
{
if ( performsFullCondensing() == BT_TRUE )
return getNU()*getN();
else
return getNX() + getNU()*getN();
}
returnValue FeedforwardLaw::step( double currentTime,
const Vector& _x,
const Vector& _p,
const VariablesGrid& _yRef
)
{
if ( getStatus( ) != BS_READY )
return ACADOERROR( RET_BLOCK_NOT_READY );
if ( _x.getDim( ) != getNX( ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
// use reference control as control
Vector uEv;
uRef.evaluate(currentTime,uEv);
//printf("FeedforwardLaw controllor outputs %d #.\n",uRef.getDim());
//printf(" -- yref: %d samples of size (%d,%d) -- \n",_yRef.getNumPoints(),_yRef.getNumRows(),_yRef.getNumCols());
u = uEv;
p = _p;
//printf(" -- u: [%.3e,%.3e,%.3e] -- \n",u(0),u(1),u(2));
return SUCCESSFUL_RETURN;
}
returnValue ModelData::setNARXmodel( const uint _delay, const Matrix& _parms ) {
if( rhs_name.isEmpty() && NX2 == 0 && NX3 == 0 ) {
NX2 = _parms.getNumRows();
delay = _delay;
uint numParms = 1;
uint n = delay*getNX();
if( delay >= 1 ) {
numParms = numParms + n;
}
for( uint i = 1; i < delay; i++ ) {
numParms = numParms + (n+1)*(uint)pow((double)n,(int)i)/2;
}
if( _parms.getNumCols() == numParms ) {
parms = _parms;
}
else {
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
}
model_dimensions_set = BT_TRUE;
export_rhs = BT_TRUE;
}
else {
return ACADOERROR( RET_INVALID_OPTION );
}
return SUCCESSFUL_RETURN;
}
uint ModelData::getNDX( ) const
{
if( NDX > 0 ) {
return getNX();
}
return NDX;
}
Vector CondensingExport::getUpperBoundsX0( ) const
{
if ( xBounds.isEmpty() == BT_FALSE )
return xBounds.getUpperBounds(0);
else
{
Vector tmp( getNX() );
tmp.setAll( INFTY );
return tmp;
}
}
void createEclipseWriter(const char *deckString)
{
Opm::ParseContext parseContext;
Opm::ParserConstPtr parser(new Opm::Parser());
deck = parser->parseString(deckString, parseContext);
Opm::parameter::ParameterGroup params;
params.insertParameter("deck_filename", "foo.data");
eclipseState.reset(new Opm::EclipseState(deck , parseContext));
auto eclGrid = eclipseState->getEclipseGrid();
BOOST_CHECK(eclGrid->getNX() == 3);
BOOST_CHECK(eclGrid->getNY() == 3);
BOOST_CHECK(eclGrid->getNZ() == 3);
BOOST_CHECK(eclGrid->getCartesianSize() == 3*3*3);
simTimer.reset(new Opm::SimulatorTimer());
simTimer->init(eclipseState->getSchedule()->getTimeMap());
// also create an UnstructuredGrid (required to create a BlackoilState)
Opm::EclipseGridConstPtr constEclGrid(eclGrid);
ourFineGridManagerPtr.reset(new Opm::GridManager(constEclGrid));
const UnstructuredGrid &ourFinerUnstructuredGrid = *ourFineGridManagerPtr->c_grid();
BOOST_CHECK(ourFinerUnstructuredGrid.cartdims[0] == 3);
BOOST_CHECK(ourFinerUnstructuredGrid.cartdims[1] == 3);
BOOST_CHECK(ourFinerUnstructuredGrid.cartdims[2] == 3);
BOOST_CHECK(ourFinerUnstructuredGrid.number_of_cells == 3*3*3);
Opm::PhaseUsage phaseUsage = Opm::phaseUsageFromDeck(deck);
eclWriter.reset(new Opm::EclipseWriter(params,
eclipseState,
phaseUsage,
ourFinerUnstructuredGrid.number_of_cells,
0));
// this check is disabled so far, because UnstructuredGrid uses some weird definition
// of the term "face". For this grid, "number_of_faces" is 108 which is
// 2*6*numCells...
//BOOST_CHECK(ourFinerUnstructuredGrid.number_of_faces == 4*4*4);
int numCells = ourFinerUnstructuredGrid.number_of_cells;
for (int cellIdx = 0; cellIdx < numCells; ++cellIdx)
BOOST_CHECK(ourFinerUnstructuredGrid.global_cell[cellIdx] == cellIdx);
}
std::vector<Matrix> ModelData::getOutputDependencies( ) const {
std::vector<Matrix> outputDependencies;
if( hasCompressedStorage() ) {
for( uint i = 0; i < outputNames.size(); i++ ) {
Vector colIndV = colInd_outputs[i];
Vector rowPtrV = rowPtr_outputs[i];
Matrix dependencyMat = zeros( dim_outputs[i],getNX()+NXA+NU+NDX );
int index = 1;
for( uint j = 0; j < dim_outputs[i]; j++ ) {
uint upper = (uint)rowPtrV(j+1);
for( uint k = (uint)rowPtrV(j); k < upper; k++ ) {
dependencyMat(j,(uint)colIndV(k-1)-1) = index++;
}
}
outputDependencies.push_back( dependencyMat );
}
}
return outputDependencies;
}
returnValue SIMexport::exportAcadoHeader( const String& _dirName,
const String& _fileName,
const String& _realString,
const String& _intString,
int _precision
) const
{
int qpSolver;
get( QP_SOLVER,qpSolver );
int operatingSystem;
get( OPERATING_SYSTEM,operatingSystem );
int useSinglePrecision;
get( USE_SINGLE_PRECISION,useSinglePrecision );
int fixInitialState;
get( FIX_INITIAL_STATE,fixInitialState );
String fileName( _dirName );
fileName << "/" << _fileName;
ExportFile acadoHeader( fileName,"", _realString,_intString,_precision );
acadoHeader.addStatement( "#include <stdio.h>\n" );
acadoHeader.addStatement( "#include <math.h>\n" );
if ( (OperatingSystem)operatingSystem == OS_WINDOWS )
{
acadoHeader.addStatement( "#include <windows.h>\n" );
}
else
{
// OS_UNIX
acadoHeader.addStatement( "#include <time.h>\n" );
acadoHeader.addStatement( "#include <sys/stat.h>\n" );
acadoHeader.addStatement( "#include <sys/time.h>\n" );
}
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "#ifndef ACADO_H\n" );
acadoHeader.addStatement( "#define ACADO_H\n" );
acadoHeader.addLinebreak( );
switch ( (QPSolverName)qpSolver )
{
case QP_CVXGEN:
acadoHeader.addStatement( "#define USE_CVXGEN\n" );
acadoHeader.addStatement( "#include \"cvxgen/solver.h\"\n" );
acadoHeader.addLinebreak( 2 );
if ( (BooleanType)useSinglePrecision == BT_TRUE )
acadoHeader.addStatement( "typedef float real_t;\n" );
else
acadoHeader.addStatement( "typedef double real_t;\n" );
acadoHeader.addLinebreak( 2 );
break;
case QP_QPOASES:
acadoHeader.addStatement( "#ifndef __MATLAB__\n" );
acadoHeader.addStatement( "#ifdef __cplusplus\n" );
acadoHeader.addStatement( "extern \"C\"\n" );
acadoHeader.addStatement( "{\n" );
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addStatement( "#include \"qpoases/solver.hpp\"\n" );
acadoHeader.addLinebreak( 2 );
break;
case QP_QPOASES3:
acadoHeader.addStatement( "#include \"qpoases3/solver.h\"\n" );
acadoHeader.addLinebreak( 2 );
break;
case QP_NONE:
if ( (BooleanType)useSinglePrecision == BT_TRUE )
acadoHeader.addStatement( "typedef float real_t;\n" );
else
acadoHeader.addStatement( "typedef double real_t;\n" );
acadoHeader.addLinebreak( 2 );
break;
default:
return ACADOERROR( RET_INVALID_OPTION );
}
Vector nMeasV = getNumMeas();
Vector nOutV = getDimOutputs();
if( nMeasV.getDim() != nOutV.getDim() ) return ACADOERROR( RET_INVALID_OPTION );
//
// Some common defines
//
acadoHeader.addComment( "COMMON DEFINITIONS: " );
acadoHeader.addComment( "--------------------------------" );
acadoHeader.addLinebreak( 2 );
if( (uint)nOutV.getDim() > 0 ) {
acadoHeader.addComment( "Dimension of the output functions" );
acadoHeader.addDeclaration( ExportVariable( "NOUT",nOutV,STATIC_CONST_INT ) );
acadoHeader.addComment( "Measurements of the output functions per shooting interval" );
acadoHeader.addDeclaration( ExportVariable( "NMEAS",nMeasV,STATIC_CONST_INT ) );
}
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "Number of control intervals" );
acadoHeader.addStatement( (String)"#define ACADO_N " << getN() << "\n");
acadoHeader.addComment( "Number of differential states" );
acadoHeader.addStatement( (String)"#define ACADO_NX " << getNX() << "\n" );
acadoHeader.addComment( "Number of differential state derivatives" );
acadoHeader.addStatement( (String)"#define ACADO_NDX " << getNDX() << "\n" );
acadoHeader.addComment( "Number of algebraic states" );
acadoHeader.addStatement( (String)"#define ACADO_NXA " << getNXA() << "\n" );
acadoHeader.addComment( "Number of controls" );
acadoHeader.addStatement( (String)"#define ACADO_NU " << getNU() << "\n" );
acadoHeader.addComment( "Number of parameters" );
acadoHeader.addStatement( (String)"#define ACADO_NP " << getNP() << "\n" );
acadoHeader.addComment( "Number of output functions" );
acadoHeader.addStatement( (String)"#define NUM_OUTPUTS " << (uint)nOutV.getDim() << "\n" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "GLOBAL VARIABLES: " );
acadoHeader.addComment( "--------------------------------" );
ExportStatementBlock tempHeader;
if ( collectDataDeclarations( tempHeader,ACADO_VARIABLES ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
acadoHeader.addStatement( "typedef struct ACADOvariables_ {\n" );
acadoHeader.addStatement( tempHeader );
#ifdef WIN32
if( tempHeader.getNumStatements() == 0 ) {
acadoHeader.addStatement( "int dummy; \n" );
}
#endif
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "} ACADOvariables;\n" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "GLOBAL WORKSPACE: " );
acadoHeader.addComment( "--------------------------------" );
acadoHeader.addStatement( "typedef struct ACADOworkspace_ {\n" );
if ( collectDataDeclarations( acadoHeader,ACADO_WORKSPACE ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "} ACADOworkspace;\n" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "GLOBAL FORWARD DECLARATIONS: " );
acadoHeader.addComment( "-------------------------------------" );
if ( collectFunctionDeclarations( acadoHeader ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
acadoHeader.addComment( "-------------------------------------" );
acadoHeader.addLinebreak( 2 );
acadoHeader.addComment( "EXTERN DECLARATIONS: " );
acadoHeader.addComment( "-------------------------------------" );
acadoHeader.addStatement( "extern ACADOworkspace acadoWorkspace;\n" );
acadoHeader.addStatement( "extern ACADOvariables acadoVariables;\n" );
acadoHeader.addComment( "-------------------------------------" );
switch ( (QPSolverName) qpSolver )
{
case QP_CVXGEN:
break;
case QP_QPOASES:
acadoHeader.addStatement( "#ifndef __MATLAB__\n");
acadoHeader.addStatement( "#ifdef __cplusplus\n" );
acadoHeader.addLinebreak( );
acadoHeader.addStatement( "} /* extern \"C\" */\n" );
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addStatement( "#endif\n" );
break;
case QP_QPOASES3:
break;
case QP_NONE:
break;
default:
return ACADOERROR( RET_INVALID_OPTION );
}
acadoHeader.addStatement( "#endif\n" );
acadoHeader.addLinebreak( );
acadoHeader.addComment( "END OF FILE." );
acadoHeader.addLinebreak( );
return acadoHeader.exportCode( );
}
uint FeedforwardLaw::getNY( ) const
{
return getNX( );
}
returnValue FunctionEvaluationTree::exportCode( FILE *file,
const char *fcnName,
const char *realString,
int precision,
uint _numX,
uint _numXA,
uint _numU
) const{
int run1;
int nni = 0;
for( run1 = 0; run1 < n; run1++ )
if( lhs_comp[run1]+1 > nni )
nni = lhs_comp[run1]+1;
uint numX;
if( _numX > 0 ) {
numX = _numX;
}
else {
numX = getNX();
}
uint numXA;
if( _numXA > 0 ) {
numXA = _numXA;
}
else {
numXA = getNXA();
}
uint numU;
if( _numU > 0 ) {
numU = _numU;
}
else {
numU = getNU();
}
acadoFPrintf(file,"void %s( const %s* acado_x, %s* acado_f ){\n", fcnName,realString,realString );
if( numX > 0 ){
acadoFPrintf(file,"const %s* acado_xd = acado_x;\n", realString );
}
if( numXA > 0 ){
acadoFPrintf(file,"const %s* acado_xa = acado_x + %d;\n", realString,numX );
}
if( getNU() > 0 ){
acadoFPrintf(file,"const %s* acado_u = acado_x + %d;\n", realString,numX+numXA );
}
if( getNUI() > 0 ){
acadoFPrintf(file,"const %s* acado_v = acado_x + %d;\n", realString,numX+numXA+numU );
}
if( getNP() > 0 ){
acadoFPrintf(file,"const %s* acado_p = acado_x + %d;\n", realString,numX+numXA+numU+getNUI() );
}
if( getNPI() > 0 ){
acadoFPrintf(file,"const %s* acado_q = acado_x + %d;\n", realString,numX+numXA+numU+getNUI()+getNP() );
}
if( getNW() > 0 ){
acadoFPrintf(file,"const %s* acado_w = acado_x + %d;\n", realString,numX+numXA+numU+getNUI()+getNP()+getNPI() );
}
if( getNDX() > 0 ){
acadoFPrintf(file,"const %s* acado_dx = acado_x + %d;\n", realString,numX+numXA+numU+getNUI()+getNP()+getNPI()+getNW() );
}
acadoFPrintf(file,"\n");
acadoFPrintf(file,"/* COMPUTE INTERMEDIATE STATES: */\n");
acadoFPrintf(file,"/* ---------------------------- */\n");
//
// This is a (not so quick) hack to have a flexible name for interm.
// quantities, but helps. It general case it should be done with
// ExportVariable -- to be able to set a working structure, too. So:
//
// TODO: setGlobalExportvariable should set the full name once....
//
String auxVarName;
if ( auxVariableStructName.isEmpty() )
{
auxVarName = auxVariableName;
}
else
{
auxVarName = auxVariableStructName;
auxVarName += ".";
auxVarName += auxVariableName;
}
Stream *auxVarIndividualNames = new Stream[nni];
for( run1 = 0; run1 < n; run1++ )
auxVarIndividualNames[lhs_comp[run1]] << auxVarName << "[" << run1 << "]";
// Export intermediate quantities
for( run1 = 0; run1 < n; run1++ )
{
// Convert the name for intermediate variables for subexpressions
sub[ run1 ]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames );
acadoFPrintf(file,"%s[%d] = ", auxVarName.getName(), run1 );
file << *sub[run1];
acadoFPrintf(file,";\n");
}
acadoFPrintf(file,"\n");
acadoFPrintf(file,"/* COMPUTE OUTPUT: */\n");
acadoFPrintf(file,"/* --------------- */\n");
// Export output quantities
for( run1 = 0; run1 < dim; run1++ )
{
// Convert names for interm. quantities for output expressions
f[ run1 ]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames );
acadoFPrintf(file,"acado_f[%d] = ", run1 );
file << *f[run1];
acadoFPrintf(file,";\n");
}
acadoFPrintf(file,"}\n\n");
delete [] auxVarIndividualNames;
return SUCCESSFUL_RETURN;
}
returnValue CondensingExport::setupEvaluation( )
{
x.setup( "x", (getN()+1), getNX(), REAL,ACADO_VARIABLES );
u.setup( "u", getN(), getNU(), REAL,ACADO_VARIABLES );
p.setup( "p", 1, getNP(), REAL,ACADO_VARIABLES );
state.setup ( "state", 1,getNX()*(getNX()+getNU()+1) + getNU() +getNP(), REAL,ACADO_WORKSPACE );
if ( isInitialStateFixed( ) == BT_FALSE )
{
x0Ref.setup ( "x0Ref", 1, getNX(), REAL,ACADO_VARIABLES );
x0Ref2.setup( "x0Ref2", 1, getNX(), REAL,ACADO_VARIABLES );
}
E.setup ( "E", getN()*getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
lbA.setup( "lbA", getNumStateBounds(), 1, REAL,ACADO_WORKSPACE );
ubA.setup( "ubA", getNumStateBounds(), 1, REAL,ACADO_WORKSPACE );
Matrix zeroXU = zeros( getNX(),getNU() );
Matrix idX = eye( getNX() );
//
// setupQP
//
setupQP.setup( "setupQP" );
if ( performsSingleShooting() == BT_TRUE )
setupQP.addStatement( state.getCols( 0,getNX() ) == x.getRow(0) );
// TODO: this part should be preinitialized. More specifically, E & QE matrices should be preinitializes to 0.
uint run1, run2;
for( run1 = 0; run1 < getN()-1; run1++ )
for( run2 = 1+run1; run2 < getN(); run2++ )
setupQP.addStatement( E.getSubMatrix( run1*getNX(),(run1+1)*getNX(), run2*getNU(),(run2+1)*getNU() ) == zeroXU );
// Write state bounds to the file
// TODO: Since the bounds are fixed in this case, they should be preinitialized
if( getNumStateBounds( ) > 0 )
{
Vector xLowerBounds(nxBounds), xUpperBounds(nxBounds);
for( run1 = 0; run1 < nxBounds; run1++ )
{
xLowerBounds(run1) = xBounds.getLowerBound( xBoundsIdx[run1]/getNX(),xBoundsIdx[run1]%getNX() );
xUpperBounds(run1) = xBounds.getUpperBound( xBoundsIdx[run1]/getNX(),xBoundsIdx[run1]%getNX() );
}
setupQP.addStatement( lbA == xLowerBounds );
setupQP.addStatement( ubA == xUpperBounds );
}
setupQP.addLinebreak( );
if ( isInitialStateFixed( ) == BT_FALSE )
{
setupQP.addStatement( Dx0 == x.getRow(0) - x0Ref );
setupQP.addStatement( Dx0b == x.getRow(0) - x0Ref2 );
setupQP.addLinebreak( );
}
// compute QQF if necessary
if ( QQF.isGiven( ) == BT_FALSE )
setupQP.addStatement( QQF == Q + QF );
ExportIndex reset( String("reset") );
setupQP.addIndex( reset );
setupQP.addStatement( reset == 1 );
ExportIndex run( String("run1") );
ExportForLoop loop( run, 0,getN() );
setupQP.addIndex( run );
if ( performsSingleShooting() == BT_FALSE ) {
loop.addStatement( reset == 1 );
loop.addStatement( state.getCols( 0,getNX() ) == x.getRow( run ) );
}
// no free parameters implemented yet!
uint uIdx = getNX() * ( 1+getNX() );
uint pIdx = getNX() * ( 1+getNX()+getNU() );
uIdx = pIdx;
pIdx = pIdx + getNU();
loop.addStatement( state.getCols( uIdx,pIdx ) == u.getRow( run ) );
loop.addStatement( state.getCols( pIdx,pIdx+getNP() ) == p );
loop.addLinebreak( );
if ( integrator->equidistantControlGrid() )
{
loop.addFunctionCall( "integrate", state, reset.makeArgument() );
}
else
{
loop.addFunctionCall( "integrate", state, reset.makeArgument(), run.makeArgument() );
}
if( performsSingleShooting() ) loop.addStatement( reset == 0 );
if ( performsSingleShooting() == BT_TRUE )
{
loop.addStatement( x.getRow( run+1 ) == state.getCols( 0,getNX() ) );
}
else
{
//
// Multiple shooting
// TODO: Check the sign here
//
loop.addStatement( residuum.getRow( run ) == state.getCols( 0,getNX() ) - x.getRow( run+1 ) );
}
loop.addLinebreak( );
loop.addFunctionCall( condense1, run.makeArgument(),state );
setupQP.addStatement( loop );
setupQP.addLinebreak( );
setupQP.addFunctionCall( condense2 );
////////////////////////////////////////////////////////////////////////////
//
// Get objective value
//
////////////////////////////////////////////////////////////////////////////
ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, BT_TRUE);
ExportVariable tmpDx("tmpDx", 1, getNX(), REAL, ACADO_LOCAL );
ExportVariable tmpDu("tmpDu", 1, getNU(), REAL, ACADO_LOCAL );
getObjectiveValue.setup( "getObjectiveValue" );
getObjectiveValue.setReturnValue( tmp );
getObjectiveValue.addVariable( tmpDx );
getObjectiveValue.addVariable( tmpDu );
getObjectiveValue.addStatement( tmp == 0.0 );
getObjectiveValue.addLinebreak( );
for (unsigned i = 0; i < getN(); ++i)
{
getObjectiveValue.addStatement( tmpDx == Dx.getRow( i ) * Q );
getObjectiveValue.addStatement( tmp += Dx.getRow( i ) * tmpDx.getTranspose() );
}
getObjectiveValue.addLinebreak( );
for (unsigned i = 0; i < getN(); ++i)
{
getObjectiveValue.addStatement( tmpDu == Du.getRow( i ) * R );
getObjectiveValue.addStatement( tmp += Du.getRow( i ) * tmpDu.getTranspose() );
}
getObjectiveValue.addLinebreak( );
getObjectiveValue.addStatement( tmp == tmp * Matrix( 0.5 ) );
return SUCCESSFUL_RETURN;
}
uint PIDcontroller::getNY( ) const
{
return getNX( );
}
returnValue CondensingExport::setupCondensing( )
{
uint run1;
x.setup( "x", getN()+1, getNX(), REAL,ACADO_VARIABLES );
xRef.setup( "xRef", getN(), getNX(), REAL,ACADO_VARIABLES );
uRef.setup( "uRef", getN(), getNU(), REAL,ACADO_VARIABLES );
Gx.setup( "Gx", getNX(), getNX(), REAL,ACADO_WORKSPACE );
Gu.setup( "Gu", getNX(), getNU(), REAL,ACADO_WORKSPACE );
ExportVariable yy( "yy", 1, getNX()*(getNX()+getNU()+1)+getNU() );
ExportVariable Q0 = QS;
ExportVariable Q0b = QS2;
if ( performsSingleShooting() == BT_FALSE )
{
residuum.setup( "residuum", getN(), getNX(), REAL,ACADO_WORKSPACE );
d.setup( "d", getN(), getNX(), REAL,ACADO_WORKSPACE );
}
deltaX0.setup( "deltaX0", getNX(), 1, REAL,ACADO_WORKSPACE );
C.setup ( "C", getN()*getNX(), getNX(), REAL,ACADO_WORKSPACE );
QC.setup ( "QC", getN()*getNX(), getNX(), REAL,ACADO_WORKSPACE );
E.setup ( "E", getN()*getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
QE.setup ( "QE", getN()*getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
if ( isInitialStateFixed( ) == BT_FALSE )
{
Dx0.setup ( "Dx0", 1, getNX(), REAL,ACADO_WORKSPACE );
Dx0b.setup( "Dx0b", 1, getNX(), REAL,ACADO_WORKSPACE );
}
Dx.setup ( "Dx", getN(), getNX(), REAL,ACADO_WORKSPACE );
QDx.setup ( "QDx", getN()*getNX(), 1, REAL,ACADO_WORKSPACE );
Du.setup ( "Du", getN(), getNU(), REAL,ACADO_WORKSPACE );
RDu.setup ( "RDu", getN(), getNU(), REAL,ACADO_WORKSPACE );
if ( performsFullCondensing() == BT_FALSE )
{
g0.setup ( "g0", getNX() , 1, REAL,ACADO_WORKSPACE );
H00.setup ( "H00", getNX() , getNX(), REAL,ACADO_WORKSPACE );
}
g1.setup ( "g1", getN()*getNU(), 1, REAL,ACADO_WORKSPACE );
H01.setup ( "H01", getNX(), getN()*getNU(), REAL,ACADO_WORKSPACE );
H11.setup ( "H11", getN()*getNU(), getN()*getNU(), REAL,ACADO_WORKSPACE );
deltaU.setup( "vars.x", getNumQPvars(),1 );
ExportIndex index( String("index") );
////////////////////////////////////////////////////////////////////////////
//
// First condensing routine
//
////////////////////////////////////////////////////////////////////////////
condense1.setup( "condense1", index.makeArgument(),yy );
// TO BE TESTED
if ( performsSingleShooting() == BT_TRUE )
condense1.addStatement( Dx.getRow(index) == yy.getCols( 0,getNX() ) - xRef.getRow(index) );
else
condense1.addStatement( Dx.getRow(index) == x.getRow( index + 1 ) - xRef.getRow(index) );
uint uIdx = getNX()*(getNX()+getNU()+1);
condense1.addStatement( Du.getRow(index) == yy.getCols( uIdx,uIdx+getNU() ) - uRef.getRow(index) );
condense1.addStatement( Gx.makeRowVector() == yy.getCols( getNX(),getNX()+getNX()*getNX() ) );
uIdx = getNX()*(getNX()+1);
condense1.addStatement( Gu.makeRowVector() == yy.getCols( uIdx,uIdx+getNX()*getNU() ) );
condense1.addStatement( "if( index != 0 ){\n" );
if ( performsSingleShooting() == BT_FALSE )
{
condense1.addFunctionCall( multiplyCD1, Gx, d.getAddress(index - 1), d.getAddress(index) );
// TODO: check this once again
// condense1.addStatement( d.getRow( index ) += residuum.getRow( index-1 ) );
condense1.addStatement( d.getRow( index ) += residuum.getRow( index ) );
}
condense1.addFunctionCall( multiplyC, Gx,C.getAddress((index-1)*getNX(),0), C.getAddress(index*getNX(),0) );
condense1.addFunctionCall( multiplyE, Gx,E.getAddress((index-1)*getNX(),0), E.getAddress(index*getNX(),0) );
condense1.addStatement( "}\nelse{\n" );
if ( performsSingleShooting() == BT_FALSE )
{
condense1.addStatement( d.getRow( 0 ) == residuum.getRow( 0 ) );
}
condense1.addStatement( C.getRows( 0,getNX() ) == Gx );
condense1.addStatement( "}\n" );
condense1.addStatement( E.getSubMatrix( index*getNX(),(index+1)*getNX(), index*getNU(),(index+1)*getNU() ) == Gu );
////////////////////////////////////////////////////////////////////////////
//
// Second condensing routine
//
////////////////////////////////////////////////////////////////////////////
condense2.setup( "condense2" );
//
// Create matrices QC and QE, vectors QDx, RDu
//
for( run1 = 0; run1 < getN()-1; run1++ )
{
condense2.addFunctionCall( multiplyQC1, Q, C.getAddress(run1*getNX(),0), QC.getAddress(run1*getNX(),0) );
condense2.addFunctionCall( multiplyQE1, Q, E.getAddress(run1*getNX(),0), QE.getAddress(run1*getNX(),0) );
condense2.addFunctionCall( multiplyQDX1, Q, Dx.getAddress(run1), QDx.getAddress(run1*getNX()) );
condense2.addFunctionCall( multiplyRDU1, R, Du.getAddress(run1), RDu.getAddress(run1) );
}
condense2.addFunctionCall( multiplyQC2, QQF, C.getAddress((getN()-1)*getNX(),0), QC.getAddress((getN()-1)*getNX(),0) );
condense2.addFunctionCall( multiplyQE2, QQF, E.getAddress((getN()-1)*getNX(),0), QE.getAddress((getN()-1)*getNX(),0) );
condense2.addFunctionCall( multiplyQDX2, QQF, Dx.getAddress(getN()-1), QDx.getAddress((getN()-1)*getNX()) );
condense2.addFunctionCall( multiplyRDU1, R, Du.getAddress(getN()-1), RDu.getAddress(getN()-1) );
//
// Condense gradient
//
if ( performsFullCondensing() == BT_FALSE )
{
condense2.addFunctionCall( multiplyG0, C, QDx, g0 );
if ( isInitialStateFixed( ) == BT_FALSE )
condense2.addStatement( g0 += Q0 * Dx0.makeColVector() );
if ( isInitialStateFixed( ) == BT_FALSE )
condense2.addStatement( g0 += Q0b * Dx0b.makeColVector() );
}
if ( performsSingleShooting() == BT_TRUE )
{
condense2.addFunctionCall( multiplyG1, E, QDx, g1 );
}
else
{
condense2.addStatement( Dx == Dx + d );
condense2.addFunctionCall(multiplyG1, QE, Dx, g1 );
}
condense2.addStatement( g1 += RDu.makeColVector() );
//
// Condense Hessian
//
if ( performsFullCondensing() == BT_FALSE )
{
condense2.addFunctionCall( multiplyH00, C,QC,H00 );
Matrix regH = eye( getNX() );
regH *= levenbergMarquardt;
if ( isInitialStateFixed( ) == BT_FALSE )
{
condense2.addStatement( H00 += Q0 + regH );
condense2.addStatement( H00 += Q0b );
}
else
condense2.addStatement( H00 += regH );
}
condense2.addFunctionCall( multiplyH01, C, QE, H01 );
condense2.addFunctionCall( multiplyH11, E, QE, H11 );
Matrix regH = eye( getNU() );
regH *= levenbergMarquardt;
for( run1 = 0; run1 < getN(); run1++ )
condense2.addStatement( H11.getSubMatrix( run1*getNU(),(run1+1)*getNU(), run1*getNU(),(run1+1)*getNU() ) += R + regH );
////////////////////////////////////////////////////////////////////////////
//
// Expanding routine
// TODO: create multiplication routines maybe...
//
////////////////////////////////////////////////////////////////////////////
if ( performsSingleShooting() == BT_FALSE )
{
expand.setup( "expand" );
if ( performsFullCondensing() == BT_TRUE )
{
// d += C * deltaX0
expand.addStatement( d.makeColVector() += C * deltaX0 );
// d += E * deltaU
expand.addStatement( d.makeColVector() += E * deltaU );
}
else
{
// d += C * deltaX0
expand.addStatement( d.makeColVector() += C * deltaU.getRows(0, getNX()) );
// d += E * deltaU
expand.addStatement( d.makeColVector() += E * deltaU.getRows(getNX(), getNumQPvars()) );
}
// x(:, 1: N + 1) += d
expand.addStatement( x.getRows(1, getN() + 1) += d );
}
return SUCCESSFUL_RETURN;
}
returnValue CondensingExport::setupMultiplicationRoutines( )
{
ExportVariable QQ = deepcopy( Q );
QQ.setDataStruct( ACADO_LOCAL );
ExportVariable QC1( "QC1", getNX(),getNX() );
ExportVariable QE1( "QE1", getNX(),getNU()*getN() );
ExportVariable Dx1( "Dx1", getNX(),1 );
ExportVariable QDx1("QDx1", getNX(),1 );
ExportVariable Du1( "Du1", getNU(),1 );
ExportVariable RDu1("RDu1", getNU(),1 );
ExportVariable Gx ( "Gx", getNX(),getNX() );
ExportVariable Gu ( "Gu", getNX(),getNU() );
ExportVariable C1 ( "C1", getNX(),getNX() );
ExportVariable E1 ( "E1", getNX(),getNU()*getN() );
ExportVariable d1 ( "d1", getNX(),1 );
ExportVariable u1 ( "u1", getNU(),1 );
ExportVariable C ( "C", getNX()*getN(),getNX() );
ExportVariable QC ( "QC", getNX()*getN(),getNX() );
ExportVariable E ( "E", getNX()*getN(),getNU()*getN() );
ExportVariable QE ( "QE", getNX()*getN(),getNU()*getN() );
ExportVariable QDx( "QDx", getNX()*getN(),1 );
ExportVariable g0 ( "g0", getNX(),1 );
ExportVariable g1 ( "g1", getNU()*getN(),1 );
ExportVariable H00( "H00", getNX(),getNX() );
ExportVariable H01( "H01", getNX(),getNU()*getN() );
ExportVariable H11( "H11", getNU()*getN(),getNU()*getN() );
multiplyQC1.setup( "multiplyQC1", QQ,C1,QC1 );
multiplyQC1.addStatement( QC1 == QQ*C1 );
multiplyQE1.setup( "multiplyQE1", QQ,E1,QE1 );
multiplyQE1.addStatement( QE1 == QQ*E1 );
multiplyQDX1.setup( "multiplyQDX1", QQ,Dx1,QDx1 );
multiplyQDX1.addStatement( QDx1 == QQ*Dx1 );
multiplyRDU1.setup( "multiplyRDU1", R,Du1,RDu1 );
multiplyRDU1.addStatement( RDu1 == R*Du1 );
multiplyQC2.setup( "multiplyQC2", QQF,C1,QC1 );
multiplyQC2.addStatement( QC1 == QQF*C1 );
multiplyQE2.setup( "multiplyQE2", QQF,E1,QE1 );
multiplyQE2.addStatement( QE1 == QQF*E1 );
multiplyQDX2.setup( "multiplyQDX2", QQF,Dx1,QDx1 );
multiplyQDX2.addStatement( QDx1 == QQF*Dx1 );
multiplyC.setup( "multiplyC", Gx,C1,C1("C1_new") );
multiplyC.addStatement( C1("C1_new") == Gx*C1 );
multiplyE.setup( "multiplyE", Gx,E1,E1("E1_new") );
multiplyE.addStatement( E1("E1_new") == Gx*E1 );
if ( performsSingleShooting() == BT_FALSE )
{
multiplyCD1.setup( "multiplyCD1", Gx,d1,d1("d1_new") );
multiplyCD1.addStatement( d1("d1_new") == Gx*d1 );
multiplyEU1.setup( "multiplyEU1", Gu,u1,d1("d1_new") );
multiplyEU1.addStatement( d1("d1_new") += Gu*u1 );
}
if ( performsFullCondensing() == BT_FALSE )
{
multiplyG0.setup( "multiplyG0", C,QDx,g0 );
multiplyG0.addStatement( g0 == (C^QDx) );
}
multiplyG1.setup( "multiplyG1", E,QDx,g1 );
multiplyG1.addStatement( g1 == (E^QDx) );
if ( performsFullCondensing() == BT_FALSE )
{
multiplyH00.setup( "multiplyH00", C,QC,H00 );
multiplyH00.addStatement( H00 == (C^QC) );
}
multiplyH01.setup( "multiplyH01", C,QE,H01 );
multiplyH01.addStatement( H01 == (C^QE) );
multiplyH11.setup( "multiplyH11", E,QE,H11 );
multiplyH11.addStatement( (H11 == (E^QE)) );
return SUCCESSFUL_RETURN;
}
returnValue FunctionEvaluationTree::exportCode( FILE *file,
const char *fcnName,
const char *realString,
int precision,
uint _numX,
uint _numXA,
uint _numU,
uint _numP,
uint _numDX
) const{
int run1;
int nni = 0;
for (run1 = 0; run1 < n; run1++)
if (lhs_comp[run1] + 1 > nni)
nni = lhs_comp[run1] + 1;
unsigned numX = _numX > 0 ? _numX : getNX();
unsigned numXA = _numXA > 0 ? _numXA : getNXA();
unsigned numU = _numU > 0 ? _numU : getNU();
unsigned numP = _numP > 0 ? _numP : getNP();
unsigned numDX = _numDX > 0 ? _numDX : getNDX();
acadoFPrintf(file, "void %s(const %s* in, %s* out)\n{\n", fcnName,
realString, realString);
if( numX > 0 ){
acadoFPrintf(file,"const %s* xd = in;\n", realString );
}
if( numXA > 0 ){
acadoFPrintf(file,"const %s* xa = in + %d;\n", realString,numX );
}
if( getNU() > 0 ){
acadoFPrintf(file,"const %s* u = in + %d;\n", realString,numX+numXA );
}
if( getNUI() > 0 ){
acadoFPrintf(file,"const %s* v = in + %d;\n", realString,numX+numXA+numU );
}
if( numP > 0 ){
acadoFPrintf(file,"const %s* p = in + %d;\n", realString,numX+numXA+numU+getNUI() );
}
if( getNPI() > 0 ){
acadoFPrintf(file,"const %s* q = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP );
}
if( getNW() > 0 ){
acadoFPrintf(file,"const %s* w = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP+getNPI() );
}
if( numDX > 0 ){
acadoFPrintf(file,"const %s* dx = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP+getNPI()+getNW() );
}
if( getNT() > 0 ){
acadoFPrintf(file,"const %s* t = in + %d;\n", realString,numX+numXA+numU+getNUI()+numP+getNPI()+getNW()+numDX );
}
if (n > 0)
{
acadoFPrintf(file, "/* Vector of auxiliary variables; number of elements: %d. */\n", n);
acadoFPrintf(file, "%s* a = %s;\n", realString, globalExportVariableName.getName() );
acadoFPrintf(file, "\n/* Compute intermediate quantities; */\n");
}
Stream *auxVarIndividualNames = new Stream[nni];
for( run1 = 0; run1 < n; run1++ )
auxVarIndividualNames[lhs_comp[run1]] << "a" << "[" << run1 << "]";
// Export intermediate quantities
for (run1 = 0; run1 < n; run1++)
{
// Convert the name for intermediate variables for subexpressions
sub[run1]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames);
acadoFPrintf(file, "%s[%d] = ", "a", run1);
file << *sub[run1];
acadoFPrintf(file, ";\n");
}
acadoFPrintf(file,"\n/* Compute outputs: */\n");
// Export output quantities
for (run1 = 0; run1 < dim; run1++)
{
// Convert names for interm. quantities for output expressions
f[run1]->setVariableExportName(VT_INTERMEDIATE_STATE, auxVarIndividualNames);
acadoFPrintf(file, "out[%d] = ", run1);
file << *f[run1];
acadoFPrintf(file, ";\n");
}
acadoFPrintf(file,"}\n\n");
delete [] auxVarIndividualNames;
return SUCCESSFUL_RETURN;
}
size_t GridDims::getGlobalIndex(size_t i, size_t j, size_t k) const {
return (i + j * getNX() + k * getNX() * getNY());
}
void GridDims::assertIJK(size_t i, size_t j, size_t k) const {
if (i >= getNX() || j >= getNY() || k >= getNZ())
throw std::invalid_argument("input index above valid range");
}
