returnValue MemoryAllocator::add(const ExportIndex& _obj)
{
if (indices.add( _obj ) == false)
{
LOG( LVL_WARNING ) << "Object '" << _obj.getFullName().getName() << "' already exists in an object pool" << endl;
return ACADOERROR( RET_INVALID_ARGUMENTS );
}
return SUCCESSFUL_RETURN;
}
returnValue MemoryAllocator::release(const ExportIndex& _obj)
{
if (indices.release( _obj ) == false)
{
LOG( LVL_ERROR ) << "Object '" << _obj.getFullName().getName() << "' is not found in an object pool" << endl;
return ACADOERROR( RET_INVALID_ARGUMENTS );
}
return SUCCESSFUL_RETURN;
}
ExportIndex operator+( const ExportIndex& _arg1,
const ExportIndex& _arg2
)
{
ExportIndex tmp;
if (_arg1.isGiven() == BT_TRUE && _arg2.isGiven() == BT_TRUE)
{
tmp.assignNode(new ExportIndexNode(_arg1.getGivenValue() + _arg2.getGivenValue()));
return tmp;
}
if (_arg1.isVariable() == BT_TRUE && _arg2.isGiven() == BT_TRUE)
{
tmp.assignNode(new ExportIndexNode(_arg1.getName(), _arg1.getPrefix(), _arg1->getFactor(), _arg1->getOffset() + _arg2.getGivenValue()));
return tmp;
}
if (_arg1.isGiven() == BT_TRUE && _arg2.isVariable() == BT_TRUE)
{
tmp.assignNode(new ExportIndexNode(_arg2.getName(), _arg2.getPrefix(), _arg2->getFactor(), _arg2->getOffset() + _arg1.getGivenValue()));
return tmp;
}
if(_arg1.isVariable() == BT_TRUE && _arg2.isVariable() == BT_TRUE && _arg1.getFullName() == _arg2.getFullName())
{
if ((_arg1->getFactor() + _arg2->getFactor()) == 0)
tmp.assignNode(new ExportIndexNode(_arg1->getOffset() + _arg2->getOffset()));
else
tmp.assignNode(new ExportIndexNode(_arg1.getName(), _arg1.getPrefix(), _arg1->getFactor() + _arg2->getFactor(), _arg1->getOffset() + _arg2->getOffset()));
}
else
{
tmp.assignNode(new ExportIndexNode(ESO_ADD, _arg1, _arg2));
}
return tmp;
}
returnValue ExportNLPSolver::setupSimulation( void )
{
// \todo Implement free parameters and support for DAEs
//
// By default, here will be defined model simulation suitable for sparse QP solver.
// Condensing based QP solvers should redefine/extend model simulation
//
// \todo Move to something like: setupInitialization
ExportVariable retInit("ret", 1, 1, INT, ACADO_LOCAL);
retInit.setDoc("=0: OK, otherwise an error code of a QP solver.");
initialize.setup( "initializeSolver" );
initialize.doc( "Solver initialization. Must be called once before any other function call." );
initialize.setReturnValue(retInit);
initialize << retInit.getFullName() << String(" = 0;\n");
initialize.addComment( "This is a function which must be called once before any other function call!" );
initialize.addLinebreak( 2 );
modelSimulation.setup( "modelSimulation" );
ExportIndex run;
modelSimulation.acquire( run );
ExportForLoop loop(run, 0, getN());
int useOMP;
get(CG_USE_OPENMP, useOMP);
x.setup("x", (getN() + 1), getNX(), REAL, ACADO_VARIABLES);
x.setDoc( (String)"Matrix containing " << (getN() + 1) << " differential variable vectors." );
z.setup("z", getN(), getNXA(), REAL, ACADO_VARIABLES);
z.setDoc( (String)"Matrix containing " << N << " algebraic variable vectors." );
u.setup("u", getN(), getNU(), REAL, ACADO_VARIABLES);
u.setDoc( (String)"Matrix containing " << N << " control variable vectors." );
p.setup("p", 1, getNP(), REAL, ACADO_VARIABLES);
p.setDoc( (String)"Vector of parameters." );
if (performsSingleShooting() == BT_FALSE)
{
d.setup("d", getN() * getNX(), 1, REAL, ACADO_WORKSPACE);
}
evGx.setup("evGx", N * NX, NX, REAL, ACADO_WORKSPACE);
evGu.setup("evGu", N * NX, NU, REAL, ACADO_WORKSPACE);
ExportStruct dataStructWspace;
dataStructWspace = (useOMP && performsSingleShooting() == BT_FALSE) ? ACADO_LOCAL : ACADO_WORKSPACE;
state.setup("state", 1, (getNX() + getNXA()) * (getNX() + getNU() + 1) + getNU() + getNP(), REAL, dataStructWspace);
unsigned indexZ = NX + NXA;
unsigned indexGxx = indexZ + NX * NX;
unsigned indexGzx = indexGxx + NXA * NX;
unsigned indexGxu = indexGzx + NX * NU;
unsigned indexGzu = indexGxu + NXA * NU;
unsigned indexU = indexGzu + NU;
unsigned indexP = indexU + NP;
////////////////////////////////////////////////////////////////////////////
//
// Code for model simulation
//
////////////////////////////////////////////////////////////////////////////
if (performsSingleShooting() == BT_TRUE)
{
modelSimulation.addStatement( state.getCols(0, NX) == x.getRow( 0 ) );
modelSimulation.addStatement( state.getCols(NX, NX + NXA) == z.getRow( 0 ) );
modelSimulation.addStatement( state.getCols(indexGzu, indexU) == u.getRow( 0 ) );
modelSimulation.addStatement( state.getCols(indexU, indexP) == p );
modelSimulation.addLinebreak( );
}
if ( useOMP )
{
stringstream s;
s << "#pragma omp parallel for private(" << run.getName().getName() << ", " << state.getFullName().getName()
<< ") shared("
<< evGx.getDataStructString().getName() << ", "
<< x.getDataStructString().getName()
<< ")" << endl;
modelSimulation.addStatement( s.str().c_str() );
}
if (performsSingleShooting() == BT_FALSE)
{
loop.addStatement( state.getCols(0, NX) == x.getRow( run ) );
loop.addStatement( state.getCols(NX, NX + NXA) == z.getRow( run ) );
}
loop.addLinebreak( );
// Fill in the input vector
loop.addStatement( state.getCols(indexGzu, indexU) == u.getRow( run ) );
loop.addStatement( state.getCols(indexU, indexP) == p );
loop.addLinebreak( );
// Integrate the model
// TODO make that function calls can accept constant defined scalars
if ( integrator->equidistantControlGrid() )
{
if (performsSingleShooting() == BT_FALSE)
loop.addStatement( (String)"integrate"
<< "(" << state.getFullName() << ", 1);\n" );
else
loop.addStatement( (String)"integrate"
<< "(" << state.getFullName() << ", "
<< run.getFullName() << " == 0"
<< ");\n" );
}
else
{
if (performsSingleShooting() == BT_FALSE)
loop.addStatement( (String)"integrate"
<< "(" << state.getFullName() << ", 1, " << run.getFullName() << ");\n" );
else
loop.addStatement( (String)"integrate"
<< "(" << state.getFullName() << ", "
<< run.getFullName() << " == 0"
<< ", " << run.getFullName() << ");\n" );
}
loop.addLinebreak( );
if ( performsSingleShooting() == BT_TRUE )
{
// Single shooting case: prepare for the next iteration
loop.addStatement( x.getRow(run + 1) == state.getCols(0, NX) );
loop.addLinebreak( );
}
else
{
// Multiple shootin', compute residuum
loop.addStatement( d.getTranspose().getCols(run * NX, (run + 1) * NX) == state.getCols( 0,getNX() ) - x.getRow( run+1 ) );
loop.addLinebreak( );
}
loop.addStatement( z.getRow( run ) == state.getCols(NX, NX + NXA) );
// Stack sensitivities
// \todo Upgrade this code later to stack Z sens
loop.addStatement(
evGx.makeRowVector().getCols(run * NX * NX, (run + 1) * NX * NX) == state.getCols(indexZ, indexGxx)
);
loop.addLinebreak();
loop.addStatement(
evGu.makeRowVector().getCols(run * NX * NU, (run + 1) * NX * NU) == state.getCols(indexGzx, indexGxu)
);
modelSimulation.addStatement( loop );
// XXX This should be revisited at some point
// modelSimulation.release( run );
return SUCCESSFUL_RETURN;
}
