returnValue ExportGaussElim::setupSolveReuseComplete( ExportFunction& _solveReuse, ExportVariable& _bPerm ) {

	ExportIndex run1( "i" );
	ExportIndex run2( "j" );
	ExportIndex tmp_index1( "index1" );
	ExportIndex tmp_index2( "index2" );
	ExportVariable tmp( "tmp_var", 1, 1, _bPerm.getType(), ACADO_LOCAL, true );
	_solveReuse.addIndex( run1 );
	_solveReuse.addIndex( run2 );
	_solveReuse.addIndex( tmp_index1 );
	_solveReuse.addIndex( tmp_index2 );
	_solveReuse.addDeclaration(tmp);
	uint run3;

	if (nRightHandSides <= 0)
		return ACADOERROR(RET_INVALID_OPTION);

	ExportForLoop loop1( run1, 0, dim );
	loop1 << run2.getName() << " = " << rk_perm.getFullName() << "[" << run1.getName() << "]*" << toString(nRightHandSides) << ";\n";
	for( run3 = 0; run3 < nRightHandSides; run3++ ) {
		loop1 << _bPerm.get( run1,run3 ) << " = b[" << run2.getName() << "+" << toString(run3) << "];\n";
	}
	_solveReuse.addStatement( loop1 );

	ExportForLoop loop2( run2, 1, dim );	// row run2
	loop2.addStatement( tmp_index1 == run2*nRightHandSides );
	ExportForLoop loop3( run1, 0, run2 );	// column run1
	loop3.addStatement( tmp_index2 == run1*nRightHandSides );
	loop3.addStatement( tmp == A.getElement(run2,run1) );
	for( run3 = 0; run3 < nRightHandSides; run3++ ) {
//		loop3.addStatement( _bPerm.getElement( run2,run3 ) += tmp * _bPerm.getElement( run1,run3 ) );
		loop3 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] += " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index2.getName() << "+" << toString(run3) << "];\n";
	}
	loop2.addStatement( loop3 );
	_solveReuse.addStatement( loop2 );


	// Solve the upper triangular system of equations:
	ExportForLoop loop4( run1, dim-1, -1, -1 );
	loop4.addStatement( tmp_index1 == run1*nRightHandSides );
	ExportForLoop loop5( run2, dim-1, run1, -1 );
	loop5.addStatement( tmp_index2 == run2*nRightHandSides );
	loop5.addStatement( tmp == A.getElement( run1,run2 ) );
	for( run3 = 0; run3 < nRightHandSides; run3++ ) {
//		loop5.addStatement( _bPerm.getElement( run1,run3 ) -= tmp * _bPerm.getElement( run2,run3 ) );
		loop5 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] -= " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index2.getName() << "+" << toString(run3) << "];\n";
	}
	loop4.addStatement( loop5 );
	loop4 << tmp.getName() << " = 1.0/A[" << run1.getName() << "*" << toString(dim+1) << "];\n";
	for( run3 = 0; run3 < nRightHandSides; run3++ ) {
//		loop4 << _bPerm.get( run1,run3 ) << " = " << _bPerm.get( run1,run3 ) << "*" << tmp.getName() << ";\n";
		loop4 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] = " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "];\n";
	}
	_solveReuse.addStatement( loop4 );

	_solveReuse.addStatement( b == _bPerm );

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupSolveReuse( ExportFunction& _solveReuse, ExportFunction& _solveTriangular, ExportVariable& _bPerm ) {

	uint run1, run2;

	if (nRightHandSides > 0)
		return ACADOERROR(RET_INVALID_OPTION);

	for( run1 = 0; run1 < dim; run1++ ) {
		_solveReuse << _bPerm.get( run1,0 ) << " = b[" << rk_perm.getFullName() << "[" << toString( run1 ) << "]];\n";
	}

	for( run2 = 1; run2 < dim; run2++ ) { 		// row run2
		for( run1 = 0; run1 < run2; run1++ ) { 	// column run1
			_solveReuse << _bPerm.get( run2,0 ) << " += A[" << toString( run2*dim+run1 ) << "]*" << _bPerm.getFullName() << "[" << toString( run1 ) << "];\n";
		}
		_solveReuse.addLinebreak();
	}
	_solveReuse.addLinebreak();

	_solveReuse.addFunctionCall( _solveTriangular, A, _bPerm );
	_solveReuse.addStatement( b == _bPerm );

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupSolveUpperTriangular( ExportFunction& _solveTriangular ) {

	if (nRightHandSides > 0)
		return ACADOERROR(RET_INVALID_OPTION);

	uint run1, run2;
	// Solve the upper triangular system of equations:
	for( run1 = dim; run1 > 0; run1--) {
		for( run2 = dim-1; run2 > (run1-1); run2--) {
			_solveTriangular.addStatement( b.getRow( (run1-1) ) -= A.getSubMatrix( (run1-1),(run1-1)+1,run2,run2+1 ) * b.getRow( run2 ) );
		}
		_solveTriangular << "b[" << toString( (run1-1) ) << "] = b[" << toString( (run1-1) ) << "]/A[" << toString( (run1-1)*dim+(run1-1) ) << "];\n";
	}

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupSolveReuseTranspose( ExportFunction& _solveReuse, ExportVariable& _bPerm ) {

	ExportIndex run1( "i" );
	ExportIndex run2( "j" );
	ExportVariable tmp( "tmp_var", 1, 1, _bPerm.getType(), ACADO_LOCAL, true );
	_solveReuse.addIndex( run1 );
	_solveReuse.addIndex( run2 );
	_solveReuse.addDeclaration(tmp);

	_solveReuse.addStatement( _bPerm == b_trans );

	ExportForLoop loop2( run2, 0, dim );	// row run2
	ExportForLoop loop3( run1, 0, run2 );	// column run1
	loop3.addStatement( _bPerm.getRow(run2) -= A.getElement(run1,run2)*_bPerm.getRow(run1) );
	loop2.addStatement( loop3 );
	loop2 << tmp.getName() << " = 1.0/A[" << run2.getName() << "*" << toString(dim+1) << "];\n";
	loop2.addStatement( _bPerm.getRow(run2) == _bPerm.getRow(run2)*tmp );
	_solveReuse.addStatement( loop2 );


	// Solve the upper triangular system of equations:
	ExportForLoop loop4( run1, dim-1, -1, -1 );
	ExportForLoop loop5( run2, dim-1, run1, -1 );
	loop5.addStatement( _bPerm.getRow(run1) += A.getElement(run2,run1)*_bPerm.getRow(run2) );
	loop4.addStatement( loop5 );
	_solveReuse.addStatement( loop4 );


	// The permutation now happens HERE!
	ExportForLoop loop1( run1, 0, dim );
	loop1 << run2.getName() << " = " << rk_perm.getFullName() << "[" << run1.getName() << "];\n";
	loop1.addStatement( b_trans.getRow(run2) == _bPerm.getRow(run1) );
	_solveReuse.addStatement( loop1 );

	return SUCCESSFUL_RETURN;
}
Esempio n. 5
0
returnValue ExportGaussNewtonCN2::setupConstraintsEvaluation( void )
{
	ExportVariable tmp("tmp", 1, 1, REAL, ACADO_LOCAL, true);

	int hardcodeConstraintValues;
	get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);

	////////////////////////////////////////////////////////////////////////////
	//
	// Setup the bounds on control variables
	//
	////////////////////////////////////////////////////////////////////////////

	unsigned numBounds = initialStateFixed( ) == true ? N * NU : NX + N * NU;
	unsigned offsetBounds = initialStateFixed( ) == true ? 0 : NX;

	DVector lbValuesMatrix( numBounds );
	DVector ubValuesMatrix( numBounds );

	if (initialStateFixed( ) == false)
		for(unsigned run1 = 0; run1 < NX; ++run1)
		{
			lbValuesMatrix( run1 )= xBounds.getLowerBound(0, run1);
			ubValuesMatrix( run1) = xBounds.getUpperBound(0, run1);
		}

	for(unsigned run1 = 0; run1 < N; ++run1)
		for(unsigned run2 = 0; run2 < NU; ++run2)
		{
			lbValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getLowerBound(run1, run2);
			ubValuesMatrix(offsetBounds + run1 * getNU() + run2) = uBounds.getUpperBound(run1, run2);
		}


	if (hardcodeConstraintValues == YES)
	{
		lbValues.setup("lbValues", lbValuesMatrix, REAL, ACADO_VARIABLES);
		ubValues.setup("ubValues", ubValuesMatrix, REAL, ACADO_VARIABLES);
	}
	else if (isFinite( lbValuesMatrix ) || isFinite( ubValuesMatrix ))
	{
		lbValues.setup("lbValues", numBounds, 1, REAL, ACADO_VARIABLES);
		lbValues.setDoc( "Lower bounds values." );
		ubValues.setup("ubValues", numBounds, 1, REAL, ACADO_VARIABLES);
		ubValues.setDoc( "Upper bounds values." );

		initialize.addStatement(lbValues == lbValuesMatrix);
		initialize.addStatement(ubValues == ubValuesMatrix);
	}

	ExportFunction* boundSetFcn = hardcodeConstraintValues == YES ? &condensePrep : &condenseFdb;

	if (performFullCondensing() == true)
	{
		boundSetFcn->addStatement( lb.getRows(0, getNumQPvars()) == lbValues - u.makeColVector() );
		boundSetFcn->addStatement( ub.getRows(0, getNumQPvars()) == ubValues - u.makeColVector() );
	}
	else
	{
		if ( initialStateFixed( ) == true )
		{
			condenseFdb.addStatement( lb.getRows(0, NX) == Dx0 );
			condenseFdb.addStatement( ub.getRows(0, NX) == Dx0 );

			boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues - u.makeColVector() );
			boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues - u.makeColVector() );
		}
		else
		{
			boundSetFcn->addStatement( lb.getRows(0, NX) == lbValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
			boundSetFcn->addStatement( lb.getRows(NX, getNumQPvars()) == lbValues.getRows(NX, getNumQPvars()) - u.makeColVector() );

			boundSetFcn->addStatement( ub.getRows(0, NX) == ubValues.getRows(0, NX) - x.getRow( 0 ).getTranspose() );
			boundSetFcn->addStatement( ub.getRows(NX, getNumQPvars()) == ubValues.getRows(NX, getNumQPvars()) - u.makeColVector() );
		}
	}
	condensePrep.addLinebreak( );
	condenseFdb.addLinebreak( );

	////////////////////////////////////////////////////////////////////////////
	//
	// Setup the bounds on state variables
	//
	////////////////////////////////////////////////////////////////////////////

	if( getNumStateBounds() )
	{
		condenseFdb.addVariable( tmp );

		DVector xLowerBounds( getNumStateBounds( )), xUpperBounds( getNumStateBounds( ) );
		for(unsigned i = 0; i < xBoundsIdx.size(); ++i)
		{
			xLowerBounds( i ) = xBounds.getLowerBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
			xUpperBounds( i ) = xBounds.getUpperBound( xBoundsIdx[ i ] / NX, xBoundsIdx[ i ] % NX );
		}

		unsigned numOps = getNumStateBounds() * N * (N + 1) / 2 * NU;

		if (numOps < 1024)
		{
			for(unsigned row = 0; row < getNumStateBounds( ); ++row)
			{
				unsigned conIdx = xBoundsIdx[ row ] - NX;

				unsigned blk = conIdx / NX + 1;
				for (unsigned col = 0; col < blk; ++col)
				{
					unsigned blkRow = (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX;

					condensePrep.addStatement(
							A.getSubMatrix(row, row + 1, col * NU, (col + 1) * NU ) == E.getRow( blkRow ) );
				}

				condensePrep.addLinebreak();
			}
		}
		else
		{
			unsigned nXBounds = getNumStateBounds( );

			DMatrix vXBoundIndices(1, nXBounds);
			for (unsigned i = 0; i < nXBounds; ++i)
				vXBoundIndices(0, i) = xBoundsIdx[ i ];
			ExportVariable evXBounds("xBoundIndices", vXBoundIndices, STATIC_CONST_INT, ACADO_LOCAL, false);

			condensePrep.addVariable( evXBounds );

			ExportIndex row, col, conIdx, blk, blkRow;

			condensePrep.acquire( row ).acquire( col ).acquire( conIdx ).acquire( blk ).acquire( blkRow );

			ExportForLoop lRow(row, 0, nXBounds);

			lRow << conIdx.getFullName() << " = " << evXBounds.getFullName() << "[ " << row.getFullName() << " ] - " << toString(NX) << ";\n";
			lRow.addStatement( blk == conIdx / NX + 1 );

			ExportForLoop lCol(col, 0, blk);

			lCol.addStatement( blkRow == (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX );
			lCol.addStatement(
					A.getSubMatrix(row, row + 1, col * NU, (col + 1) * NU ) == E.getRow( blkRow ) );

			lRow.addStatement( lCol );
			condensePrep.addStatement( lRow );

			condensePrep.release( row ).release( col ).release( conIdx ).release( blk ).release( blkRow );
		}
		condensePrep.addLinebreak( );


		unsigned nXBounds = getNumStateBounds( );
		if (hardcodeConstraintValues == YES)
		{
			lbAValues.setup("lbAValues", xLowerBounds, REAL, ACADO_VARIABLES);
			ubAValues.setup("ubAValues", xUpperBounds, REAL, ACADO_VARIABLES);
		}
		else
		{
			lbAValues.setup("lbAValues", nXBounds, 1, REAL, ACADO_VARIABLES);
			lbAValues.setDoc( "Lower bounds values for affine constraints." );
			ubAValues.setup("ubAValues", nXBounds, 1, REAL, ACADO_VARIABLES);
			ubAValues.setDoc( "Upper bounds values for affine constraints." );

			initialize.addStatement(lbAValues == xLowerBounds);
			initialize.addStatement(ubAValues == xUpperBounds);
		}

		// Shift constraint bounds by first interval
		for(unsigned boundIndex = 0; boundIndex < getNumStateBounds( ); ++boundIndex)
		{
			unsigned row = xBoundsIdx[boundIndex];

			condenseFdb.addStatement( tmp == sbar.getRow( row ) + x.makeRowVector().getCol( row ) );
			condenseFdb.addStatement( lbA.getRow( boundIndex ) == lbAValues.getRow( boundIndex ) - tmp );
			condenseFdb.addStatement( ubA.getRow( boundIndex ) == ubAValues.getRow( boundIndex ) - tmp );
		}
		condenseFdb.addLinebreak( );
	}

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonForces::setupEvaluation( )
{
	////////////////////////////////////////////////////////////////////////////
	//
	// Setup preparation phase
	//
	////////////////////////////////////////////////////////////////////////////
	preparation.setup("preparationStep");
	preparation.doc( "Preparation step of the RTI scheme." );

	ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
	retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
	preparation.setReturnValue(retSim, false);

	preparation	<< retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";

	preparation.addFunctionCall( evaluateObjective );
	preparation.addFunctionCall( evaluateConstraints );

	////////////////////////////////////////////////////////////////////////////
	//
	// Setup feedback phase
	//
	////////////////////////////////////////////////////////////////////////////
	ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL);
	ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true);
	returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." );
	feedback.setup("feedbackStep" );
	feedback.doc( "Feedback/estimation step of the RTI scheme." );
	feedback.setReturnValue( returnValueFeedbackPhase );

	feedback.addStatement(
			//			cond[ 0 ].getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose()
			cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose()
	);
	feedback.addLinebreak();

	// Calculate objective residuals
	feedback << (Dy -= y) << (DyN -= yN);
	feedback.addLinebreak();

	for (unsigned i = 0; i < N; ++i)
		feedback.addFunctionCall(setStagef, objGradients[ i ], ExportIndex( i ));
	feedback.addStatement( objGradients[ N ] == QN2 * DyN );
	feedback.addLinebreak();

	//
	// Configure output variables
	//
	std::vector< ExportVariable > vecQPVars;

	vecQPVars.clear();
	vecQPVars.resize(N + 1);
	for (unsigned i = 0; i < N; ++i)
		vecQPVars[ i ].setup(string("out") + toString(i + 1), NX + NU, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
	vecQPVars[ N ].setup(string("out") + toString(N + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);

	//
	// In case warm starting is enabled, give an initial guess, based on the old solution
	//
	int hotstartQP;
	get(HOTSTART_QP, hotstartQP);

	if ( hotstartQP )
	{
		std::vector< ExportVariable > zInit;

		zInit.clear();
		zInit.resize(N + 1);
		for (unsigned i = 0; i < N; ++i)
		{
			string name = "z_init_";
			name = name + (i < 10 ? "0" : "") + toString( i );
			zInit[ i ].setup(name, NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
		}
		string name = "z_init_";
		name = name + (N < 10 ? "0" : "") + toString( N );
		zInit[ N ].setup(name, NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);

		// TODO This should be further investigated.

		//
		// 1) Just use the old solution
		//
		//		for (unsigned blk = 0; blk < N + 1; blk++)
		//			feedback.addStatement(zInit[ blk ] == vecQPVars[ blk ] );

		//
		// 2) Initialization by shifting
		//

		//		for (unsigned blk = 0; blk < N - 1; blk++)
		//			feedback.addStatement( zInit[ blk ] == vecQPVars[blk + 1] );
		//		for (unsigned el = 0; el < NX; el++)
		//			feedback.addStatement( zInit[N - 1].getElement(el, 0) == vecQPVars[ N ].getElement(el, 0) );
	}

	//
	// Call a QP solver
	// NOTE: we need two prefixes:
	// 1. module prefix
	// 2. structure instance prefix
	//
	ExportFunction solveQP;
	solveQP.setup("solve");

	feedback
	<< returnValueFeedbackPhase.getFullName() << " = "
	<< qpModuleName << "_" << solveQP.getName() << "( "
	<< "&" << qpObjPrefix << "_" << "params" << ", "
	<< "&" << qpObjPrefix << "_" << "output" << ", "
	<< "&" << qpObjPrefix << "_" << "info" << " );\n";
	feedback.addLinebreak();

	//
	// Here we have to add the differences....
	//

	ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL);
	ExportIndex index( "index" );
	acc.setup("accumulate", stageOut, index);

	acc.addStatement( x.getRow( index ) += stageOut.getCols(0, NX) );
	acc.addLinebreak();
	acc.addStatement( u.getRow( index ) += stageOut.getCols(NX, NX + NU) );
	acc.addLinebreak();

	for (unsigned i = 0; i < N; ++i)
		feedback.addFunctionCall(acc, vecQPVars[ i ], ExportIndex( i ));
	feedback.addLinebreak();

	feedback.addStatement( x.getRow( N ) += vecQPVars[ N ].getTranspose() );
	feedback.addLinebreak();

	////////////////////////////////////////////////////////////////////////////
	//
	// TODO Setup evaluation of KKT
	//
	////////////////////////////////////////////////////////////////////////////
	ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);

	getKKT.setup( "getKKT" );
	getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
	//	kkt.setDoc( "The KKT tolerance value." );
	kkt.setDoc( "1e-15." );
	getKKT.setReturnValue( kkt );

	getKKT.addStatement( kkt == 1e-15 );

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonBlockForces::setupEvaluation( )
{
	////////////////////////////////////////////////////////////////////////////
	//
	// Preparation phase
	//
	////////////////////////////////////////////////////////////////////////////

	preparation.setup( "preparationStep" );
	preparation.doc( "Preparation step of the RTI scheme." );
	ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true);
	retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code.");
	preparation.setReturnValue(retSim, false);
	ExportIndex index("index");
	preparation.addIndex( index );

	preparation	<< retSim.getFullName() << " = " << modelSimulation.getName() << "();\n";

	preparation.addFunctionCall( evaluateObjective );
	if( regularizeHessian.isDefined() ) preparation.addFunctionCall( regularizeHessian );
	preparation.addFunctionCall( evaluateConstraints );

	preparation.addLinebreak();
	preparation << (Dy -= y) << (DyN -= yN);
	preparation.addLinebreak();

	ExportForLoop condensePrepLoop( index, 0, getNumberOfBlocks() );
	condensePrepLoop.addFunctionCall( condensePrep, index );
	preparation.addStatement( condensePrepLoop );

	preparation.addFunctionCall( evaluateAffineConstraints );

	preparation.addStatement( objHessians[getNumberOfBlocks()] == QN1 );
	DMatrix mReg = eye<double>( getNX() );
	mReg *= levenbergMarquardt;
	preparation.addStatement( objHessians[getNumberOfBlocks()] += mReg );
	preparation.addLinebreak();

	preparation.addStatement( objGradients[ getNumberOfBlocks() ] == QN2 * DyN );
	int variableObjS;
	get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
	ExportVariable SlxCall =
				objSlx.isGiven() == true || variableObjS == false ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX);
	preparation.addStatement( objGradients[ getNumberOfBlocks() ] += SlxCall );
	preparation.addLinebreak();

	////////////////////////////////////////////////////////////////////////////
	//
	// Feedback phase
	//
	////////////////////////////////////////////////////////////////////////////

	ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true);
	tmp.setDoc( "Status code of the qpOASES QP solver." );

	feedback.setup("feedbackStep");
	feedback.doc( "Feedback/estimation step of the RTI scheme." );
	feedback.setReturnValue( tmp );
	feedback.addIndex( index );

	if (initialStateFixed() == true)
	{
		feedback.addStatement( cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose() );
	}
	feedback.addLinebreak();

	//
	// Configure output variables
	//
	std::vector< ExportVariable > vecQPVars;

	vecQPVars.clear();
	vecQPVars.resize(getNumberOfBlocks() + 1);
	for (unsigned i = 0; i < getNumberOfBlocks(); ++i)
		vecQPVars[ i ].setup(string("out") + toString(i + 1), getNumBlockVariables(), 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);
	vecQPVars[ getNumberOfBlocks() ].setup(string("out") + toString(getNumberOfBlocks() + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix);

	//
	// In case warm starting is enabled, give an initial guess, based on the old solution
	//
	int hotstartQP;
	get(HOTSTART_QP, hotstartQP);

	if ( hotstartQP )
	{
		return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
	}

	//
	// Call a QP solver
	// NOTE: we need two prefixes:
	// 1. module prefix
	// 2. structure instance prefix
	//
	ExportFunction solveQP;
	solveQP.setup("solve");
	solveQP.setName( "solve" );

	feedback
	<< tmp.getFullName() << " = "
	<< qpModuleName << "_" << solveQP.getName() << "( "
	<< "&" << qpObjPrefix << "_" << "params" << ", "
	<< "&" << qpObjPrefix << "_" << "output" << ", "
	<< "&" << qpObjPrefix << "_" << "info" << " , NULL);\n";
	feedback.addLinebreak();

	for (unsigned i = 0; i < getNumberOfBlocks(); ++i) {
		feedback.addFunctionCall( expand, vecQPVars[i], ExportIndex(i) );
	}

	feedback.addStatement( x.getRow( N ) += vecQPVars[ getNumberOfBlocks() ].getTranspose() );
	feedback.addLinebreak();

	////////////////////////////////////////////////////////////////////////////
	//
	// Setup evaluation of the KKT tolerance
	//
	////////////////////////////////////////////////////////////////////////////

	ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true);

	getKKT.setup( "getKKT" );
	getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." );
	//	kkt.setDoc( "The KKT tolerance value." );
	kkt.setDoc( "1e-15." );
	getKKT.setReturnValue( kkt );

	getKKT.addStatement( kkt == 1e-15 );

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupFactorization( ExportFunction& _solve, ExportVariable& _swap, ExportVariable& _determinant, const string& absF ) {

	uint run1, run2, run3;
	ExportIndex i( "i" );
	_solve.addIndex( i );
	ExportIndex j( "j" );
	ExportIndex k( "k" );
	ExportVariable indexMax( "indexMax", 1, 1, INT, ACADO_LOCAL, true );
	ExportVariable intSwap( "intSwap", 1, 1, INT, ACADO_LOCAL, true );
	ExportVariable valueMax( "valueMax", 1, 1, REAL, ACADO_LOCAL, true );
	ExportVariable temp( "temp", 1, 1, REAL, ACADO_LOCAL, true );
	if( !UNROLLING ) {
		_solve.addIndex( j );
		_solve.addIndex( k );
		_solve.addDeclaration( indexMax );
		if( REUSE ) _solve.addDeclaration( intSwap );
		_solve.addDeclaration( valueMax );
		_solve.addDeclaration( temp );
	}

	// initialise rk_perm (the permutation vector)
	if( REUSE ) {
		ExportForLoop loop1( i,0,dim );
		loop1 << rk_perm.get( 0,i ) << " = " << i.getName() << ";\n";
		_solve.addStatement( loop1 );
	}

	_solve.addStatement( _determinant == 1 );
	if( UNROLLING || dim <= 5 ) {
		// Start the factorization:
		for( run1 = 0; run1 < (dim-1); run1++ ) {
			// Search for pivot in column run1:
			for( run2 = run1+1; run2 < dim; run2++ ) {
				// add the test (if or else if):
				stringstream test;
				if( run2 == (run1+1) ) {
					test << "if(";
				} else {
					test << "else if(";
				}
				test << absF << "(A[" << toString( run2*dim+run1 ) << "]) > " << absF << "(A[" << toString( run1*dim+run1 ) << "])";
				for( run3 = run1+1; run3 < dim; run3++ ) {
					if( run3 != run2) {
						test << " && " << absF << "(A[" << toString( run2*dim+run1 ) << "]) > " << absF << "(A[" << toString( run3*dim+run1 ) << "])";
					}
				}
				test << ") {\n";
				_solve.addStatement( test.str() );

				// do the row swaps:
				// for A:
				for( run3 = 0; run3 < dim; run3++ ) {
					_solve.addStatement( _swap == A.getSubMatrix( run1,run1+1,run3,run3+1 ) );
					_solve.addStatement( A.getSubMatrix( run1,run1+1,run3,run3+1 ) == A.getSubMatrix( run2,run2+1,run3,run3+1 ) );
					_solve.addStatement( A.getSubMatrix( run2,run2+1,run3,run3+1 ) == _swap );
				}

				if( REUSE ) { // rk_perm also needs to be updated if it needs to be possible to reuse the factorization
					_solve.addStatement( intSwap == rk_perm.getCol( run1 ) );
					_solve.addStatement( rk_perm.getCol( run1 ) == rk_perm.getCol( run2 ) );
					_solve.addStatement( rk_perm.getCol( run2 ) == intSwap );
				}

				_solve.addStatement( "}\n" );
			}
			// potentially needed row swaps are done
			_solve.addLinebreak();
			// update of the next rows:
			for( run2 = run1+1; run2 < dim; run2++ ) {
				_solve << "A[" << toString( run2*dim+run1 ) << "] = -A[" << toString( run2*dim+run1 ) << "]/A[" << toString( run1*dim+run1 ) << "];\n";
				_solve.addStatement( A.getSubMatrix( run2,run2+1,run1+1,dim ) += A.getSubMatrix( run2,run2+1,run1,run1+1 ) * A.getSubMatrix( run1,run1+1,run1+1,dim ) );
				_solve.addLinebreak();
			}
			_solve.addStatement( _determinant == _determinant*A.getSubMatrix(run1,run1+1,run1,run1+1) );
			_solve.addLinebreak();
		}
		_solve.addStatement( _determinant == _determinant*A.getSubMatrix(dim-1,dim,dim-1,dim) );
		_solve.addLinebreak();
	}
	else { // without UNROLLING:
		_solve << "for( i=0; i < (" << toString( dim-1 ) << "); i++ ) {\n";
		_solve << "	indexMax = i;\n";
		_solve << "	valueMax = " << absF << "(A[i*" << toString( dim ) << "+i]);\n";
		_solve << "	for( j=(i+1); j < " << toString( dim ) << "; j++ ) {\n";
		_solve << "		temp = " << absF << "(A[j*" << toString( dim ) << "+i]);\n";
		_solve << "		if( temp > valueMax ) {\n";
		_solve << "			indexMax = j;\n";
		_solve << "			valueMax = temp;\n";
		_solve << "		}\n";
		_solve << "	}\n";
		_solve << "	if( indexMax > i ) {\n";
		ExportForLoop loop2( k,0,dim );
		loop2 << "	" << _swap.getFullName() << " = A[i*" << toString( dim ) << "+" << k.getName() << "];\n";
		loop2 << "	A[i*" << toString( dim ) << "+" << k.getName() << "] = A[indexMax*" << toString( dim ) << "+" << k.getName() << "];\n";
		loop2 << "	A[indexMax*" << toString( dim ) << "+" << k.getName() << "] = " << _swap.getFullName() << ";\n";
		_solve.addStatement( loop2 );
		if( REUSE ) {
			_solve << "	" << intSwap.getFullName() << " = " << rk_perm.getFullName() << "[i];\n";
			_solve << "	" << rk_perm.getFullName() << "[i] = " << rk_perm.getFullName() << "[indexMax];\n";
			_solve << "	" << rk_perm.getFullName() << "[indexMax] = " << intSwap.getFullName() << ";\n";
		}
		_solve << "	}\n";
		_solve << "	" << _determinant.getFullName() << " *= A[i*" << toString( dim ) << "+i];\n";
		_solve << "	for( j=i+1; j < " << toString( dim ) << "; j++ ) {\n";
		_solve << "		A[j*" << toString( dim ) << "+i] = -A[j*" << toString( dim ) << "+i]/A[i*" << toString( dim ) << "+i];\n";
		_solve << "		for( k=i+1; k < " << toString( dim ) << "; k++ ) {\n";
		_solve << "			A[j*" << toString( dim ) << "+k] += A[j*" << toString( dim ) << "+i] * A[i*" << toString( dim ) << "+k];\n";
		_solve << "		}\n";
		_solve << "	}\n";
		_solve << "}\n";
		_solve << _determinant.getFullName() << " *= A[" << toString( (dim-1)*dim+(dim-1) ) << "];\n";
	}
	_solve << _determinant.getFullName() << " = " << absF << "(" << _determinant.getFullName() << ");\n";
	
	return SUCCESSFUL_RETURN;
}