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; }