String operator==( const ExportIndex& _arg1,
const ExportIndex& _arg2
)
{
String ret;
ret << _arg1.get() << " = " << _arg2.get() << ";\n";
return ret;
}
ExportArgument ExportArgumentInternal::getAddress( const ExportIndex& rowIdx,
const ExportIndex& colIdx
) const
{
if ( rowIdx.isGiven( ) )
{
ASSERT( rowIdx.getGivenValue() < (int)getNumRows() );
}
if( colIdx.isGiven() )
{
ASSERT( colIdx.getGivenValue() < (int)getNumCols() );
}
ExportIndex ind = getTotalIdx(rowIdx, colIdx);
ExportArgument tmp(name, data, type, dataStruct, BT_FALSE, ind, prefix);
return tmp;
}
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;
}
tmp.assignNode(new ExportIndexNode(ESO_MODULO, _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;
}
returnValue ExportArithmeticStatement::exportCodeAddSubtract( std::ostream& stream,
const std::string& _sign,
const std::string& _realString,
const std::string& _intString
) const
{
if ( ( rhs1->getNumRows() != rhs2->getNumRows() ) || ( rhs1->getNumCols() != rhs2->getNumCols() ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
if (rhs1->getNumRows() != lhs->getNumRows() || rhs1->getNumCols() != lhs->getNumCols())
{
LOG( LVL_DEBUG )
<< "lhs name is " << lhs.getName() << ", size: " << lhs.getNumRows() << " x " << lhs.getNumCols() << endl
<< "rhs1 name is " << rhs1.getName() << ", size: " << rhs1.getNumRows() << " x " << rhs1.getNumCols() << endl;
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
}
//
// Rough approximation of flops needed for matrix multiplication
//
unsigned numberOfFlops = lhs->getNumRows() * lhs->getNumCols();
//
// Optimization can be performed only if both matrices are not given.
// Currently, optimizations cannot be performed on hard-coded matrices.
//
bool optimizationsAllowed = ( rhs1->isGiven() == false ) && ( rhs2->isGiven() == false );
if (numberOfFlops < 4096 || optimizationsAllowed == false)
{
for( uint i=0; i<getNumRows( ); ++i )
for( uint j=0; j<getNumCols( ); ++j )
{
if ( ( op0 != ESO_ASSIGN ) &&
( rhs1->isGiven(i,j) == true ) && ( rhs2->isGiven(i,j) == true ) )
{
// check for zero value in case of "+=" or "-="
if ( ( op1 == ESO_ADD ) && ( acadoIsZero(rhs1(i, j) + rhs2(i, j)) == true ) )
continue;
if ( ( op1 == ESO_SUBTRACT ) && ( acadoIsZero( rhs1(i, j) - rhs2(i, j)) == true ) )
continue;
}
stream << lhs.get(i, j) << " " << getAssignString();
if ( rhs1->isZero(i, j) == false )
{
stream << " " << rhs1->get(i, j);
if ( rhs2->isZero(i,j) == false )
stream << " " << _sign << " " << rhs2->get(i, j) << ";\n";
else
stream << ";" << endl;
}
else
{
if (rhs2->isZero(i, j) == false)
stream << " " << _sign << " " << rhs2->get(i, j) << ";\n";
else
stream << " 0.0;\n";
}
}
}
else if ( numberOfFlops < 32768 )
{
ExportIndex ii;
memAllocator->acquire( ii );
stream << "for (" << ii.getName() << " = 0; ";
stream << ii.getName() << " < " << getNumRows() << "; ";
stream << "++" << ii.getName() << ")\n{\n";
for(unsigned j = 0; j < getNumCols( ); ++j)
{
stream << lhs->get(ii, j) << " " << getAssignString();
stream << " " << _sign << " " << rhs2->get(ii, j) << ";\n";
}
stream << "\n{\n";
memAllocator->release( ii );
}
else
{
ExportIndex ii, jj;
memAllocator->acquire( ii );
memAllocator->acquire( jj );
stream << "for (" << ii.getName() << " = 0; "
<< ii.getName() << " < " << getNumRows() <<"; "
<< "++" << ii.getName() << ")\n{\n";
stream << "for (" << jj.getName() << " = 0; "
<< jj.getName() << " < " << getNumCols() <<"; "
<< "++" << jj.getName() << ")\n{\n";
stream << lhs->get(ii, jj) << " " << getAssignString()
<< _sign << " " << rhs2->get(ii, jj) << ";\n";
stream << "\n}\n"
<< "\n}\n";
memAllocator->release( ii );
memAllocator->release( jj );
}
return SUCCESSFUL_RETURN;
}
returnValue ExportArithmeticStatement::exportCodeAddSubtract( FILE* file,
const String& _sign,
const String& _realString,
const String& _intString,
int _precision
) const
{
// if ( ( rhs1.isNull() ) || ( rhs2.isNull() ) || ( !rhs3.isNull() ) )
// return ACADOERROR( RET_UNABLE_TO_EXPORT_STATEMENT );
if (rhs1.getDim() == 0)
return SUCCESSFUL_RETURN;
if ( ( rhs1->getNumRows() != rhs2->getNumRows() ) ||
( rhs1->getNumCols() != rhs2->getNumCols() ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
if ( !lhs.isNull() )
{
if ( ( rhs1->getNumRows() != lhs->getNumRows() ) ||
( rhs1->getNumCols() != lhs->getNumCols() ) )
{
cout << "lhs name is " << lhs.getName().getName() <<
", size: " << lhs.getNumRows() << " x " << lhs.getNumCols() << endl;
cout << "rhs1 name is " << rhs1.getName().getName() <<
", size: " << rhs1.getNumRows() << " x " << rhs1.getNumCols() << endl;
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
}
}
String assignString;
if ( getAssignString( assignString ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_UNABLE_TO_EXPORT_STATEMENT );
//
// Rough approximation of flops needed for matrix multiplication
//
unsigned numberOfFlops = lhs->getNumRows() * lhs->getNumCols();
//
// Optimization can be performed only if both matrices are not given.
// Currently, optimizations cannot be performed on hard-coded matrices.
//
int optimizationsAllowed =
( rhs1->isGiven() == BT_FALSE ) && ( rhs2->isGiven() == BT_FALSE );
if ((numberOfFlops < 4096) || (optimizationsAllowed == 0))
{
for( uint i=0; i<getNumRows( ); ++i )
for( uint j=0; j<getNumCols( ); ++j )
{
if ( ( op0 != ESO_ASSIGN ) &&
( rhs1->isGiven(i,j) == BT_TRUE ) && ( rhs2->isGiven(i,j) == BT_TRUE ) )
{
// check for zero value in case of "+=" or "-="
if ( ( op1 == ESO_ADD ) && ( acadoIsZero(rhs1(i, j) + rhs2(i, j)) == BT_TRUE ) )
continue;
if ( ( op1 == ESO_SUBTRACT ) && ( acadoIsZero( rhs1(i, j) - rhs2(i, j)) == BT_TRUE ) )
continue;
}
if ( !lhs.isNull() )
acadoFPrintf( file,"%s %s ", lhs.get(i,j).getName(),assignString.getName() );
if ( rhs1->isZero(i, j) == BT_FALSE )
{
acadoFPrintf( file,"%s", rhs1->get(i,j).getName() );
if ( rhs2->isZero(i,j) == BT_FALSE )
acadoFPrintf( file," %s %s;\n", _sign.getName(),rhs2->get(i,j).getName() );
else
acadoFPrintf( file,";\n" );
}
else
{
if ( rhs2->isZero(i,j) == BT_FALSE )
acadoFPrintf( file,"%s %s;\n", _sign.getName(),rhs2->get(i,j).getName() );
else
acadoFPrintf( file,"0.0;\n" );
}
}
}
else if ( numberOfFlops < 32768 )
{
ExportIndex ii;
memAllocator->acquire( ii );
acadoFPrintf(file, "for (%s = 0; ", ii.getName().getName());
acadoFPrintf(file, "%s < %d; ", ii.getName().getName(), getNumRows());
acadoFPrintf(file, "++%s)\n{\n", ii.getName().getName());
for(unsigned j = 0; j < getNumCols( ); ++j)
{
acadoFPrintf( file,"%s %s ", lhs->get(ii, j).getName(), assignString.getName() );
acadoFPrintf( file,"%s %s;\n", _sign.getName(), rhs2->get(ii,j).getName() );
}
acadoFPrintf(file, "\n{\n");
memAllocator->release( ii );
}
else
{
ExportIndex ii, jj;
memAllocator->acquire( ii );
memAllocator->acquire( jj );
acadoFPrintf(file, "for (%s = 0; ", ii.getName().getName());
acadoFPrintf(file, "%s < %d; ", ii.getName().getName(), getNumRows());
acadoFPrintf(file, "++%s)\n{\n", ii.getName().getName());
acadoFPrintf(file, "for (%s = 0; ", jj.getName().getName());
acadoFPrintf(file, "%s < %d; ", jj.getName().getName(), getNumRows());
acadoFPrintf(file, "++%s)\n{\n", jj.getName().getName());
acadoFPrintf( file,"%s %s ", lhs->get(ii, jj).getName(), assignString.getName() );
acadoFPrintf( file,"%s %s;\n", _sign.getName(), rhs2->get(ii,jj).getName() );
acadoFPrintf(file, "\n{\n");
acadoFPrintf(file, "\n{\n");
memAllocator->release( ii );
memAllocator->release( jj );
}
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;
}
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() * _arg2.getGivenValue(), _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() * _arg1.getGivenValue(), _arg2->getOffset() * _arg1.getGivenValue()));
return tmp;
}
tmp.assignNode(new ExportIndexNode(ESO_MULTIPLY, _arg1, _arg2));
return tmp;
}
