Exemplo n.º 1
0
returnValue ExportCholeskySolver::getCode( ExportStatementBlock& code )
{
	code.addFunction( chol );
	code.addFunction( solve );

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 2
0
returnValue ThreeSweepsERKExport::getCode(	ExportStatementBlock& code
											)
{
	int useOMP;
	get(CG_USE_OPENMP, useOMP);
	if ( useOMP )
	{
		getDataDeclarations( code, ACADO_LOCAL );

		code << "#pragma omp threadprivate( "
				<< getAuxVariable().getFullName()  << ", "
				<< rk_xxx.getFullName() << ", "
				<< rk_ttt.getFullName() << ", "
				<< rk_kkk.getFullName() << ", "
				<< rk_forward_sweep.getFullName() << ", "
				<< rk_backward_sweep.getFullName()
				<< " )\n\n";
	}

	int sensGen;
	get( DYNAMIC_SENSITIVITY,sensGen );
	if( exportRhs ) {
		code.addFunction( rhs );
		code.addFunction( diffs_rhs );
		code.addFunction( diffs_sweep3 );
	}

	double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
	code.addComment(std::string("Fixed step size:") + toString(h));
	code.addFunction( integrate );

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 3
0
returnValue DiscreteTimeExport::getCode(	ExportStatementBlock& code
										)
{
	if( NX1 > 0 ) {
		code.addFunction( lin_input );
		code.addStatement( "\n\n" );
	}

	if( NX2 > 0 ) {
		code.addFunction( rhs );
		code.addStatement( "\n\n" );
		code.addFunction( diffs_rhs );
		code.addStatement( "\n\n" );
	}

	if( !equidistantControlGrid() ) {
		ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
		code.addDeclaration( numStepsV );
		code.addLinebreak( 2 );
	}
	double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
	code.addComment(String("Fixed step size:") << String(h));

	code.addFunction( integrate );

	if( NX2 != NX ) {
		prepareFullRhs();
		code.addFunction( fullRhs );
	}

	return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonBlockForces::getCode(	ExportStatementBlock& code
											)
{
	setupQPInterface();
	code.addStatement( *qpInterface );

	code.addFunction( cleanup );
	code.addFunction( shiftQpData );

	code.addFunction( evaluateConstraints );
	code.addFunction( evaluateAffineConstraints );

	return ExportGaussNewtonCN2::getCode( code );
}
returnValue ExportCholeskyDecomposition::getCode(	ExportStatementBlock& code
													)
{
	if (A.getDim() != 0)
		return code.addFunction( fcn );

	return SUCCESSFUL_RETURN;
}
returnValue ExportCholeskyDecomposition::getFunctionDeclarations(	ExportStatementBlock& declarations
																	) const
{
	if (A.getDim() != 0)
		return declarations.addFunction( fcn );

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 7
0
returnValue LiftedERKExport::getCode(	ExportStatementBlock& code
										)
{
	// 	int printLevel;
	// 	get( PRINTLEVEL,printLevel );
	//
	// 	if ( (PrintLevel)printLevel >= HIGH )
	// 		acadoPrintf( "--> Exporting %s... ",fileName.getName() );

	int useOMP;
	get(CG_USE_OPENMP, useOMP);
	if ( useOMP )
	{
		getDataDeclarations( code, ACADO_LOCAL );

		code << "#pragma omp threadprivate( "
				<< getAuxVariable().getFullName()  << ", "
				<< rk_xxx.getFullName() << ", "
				<< rk_ttt.getFullName() << ", "
				<< rk_kkk.getFullName()
				<< " )\n\n";
	}

	int sensGen;
	get( DYNAMIC_SENSITIVITY,sensGen );
	if( exportRhs ) {
		code.addFunction( rhs );
		code.addFunction( diffs_rhs );
		code.addFunction( alg_rhs );
		solver->getCode( code ); // solver for the algebraic equations
	}
	else {
		return ACADOERRORTEXT( RET_NOT_YET_IMPLEMENTED, "Externally defined dynamics not yet supported for the lifted ERK integrator!");
	}

	double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
	code.addComment(std::string("Fixed step size:") + toString(h));
	code.addFunction( integrate );


	// 	if ( (PrintLevel)printLevel >= HIGH )
	// 		acadoPrintf( "done.\n" );

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 8
0
returnValue ExplicitRungeKuttaExport::getCode(	ExportStatementBlock& code
                                             )
{
// 	int printLevel;
// 	get( PRINTLEVEL,printLevel );
//
// 	if ( (PrintLevel)printLevel >= HIGH )
// 		acadoPrintf( "--> Exporting %s... ",fileName.getName() );

    int useOMP;
    get(CG_USE_OPENMP, useOMP);
    if ( useOMP )
    {
        getDataDeclarations( code, ACADO_LOCAL );

        code << "#pragma omp threadprivate( "
             << getAuxVariable().getFullName()  << ", "
             << rk_xxx.getFullName() << ", "
             << rk_ttt.getFullName() << ", "
             << rk_kkk.getFullName()
             << " )\n\n";
    }

    int sensGen;
    get( DYNAMIC_SENSITIVITY,sensGen );
    if( exportRhs ) {
        code.addFunction( rhs );
        code.addFunction( diffs_rhs );
    }

    double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
    code.addComment(std::string("Fixed step size:") + toString(h));
    code.addFunction( integrate );


// 	if ( (PrintLevel)printLevel >= HIGH )
// 		acadoPrintf( "done.\n" );

    return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::getCode(	ExportStatementBlock& code
											)
{

	if (nRightHandSides > 0) {
		if( !REUSE ) return ACADOERROR(RET_INVALID_OPTION);

		setupFactorization( solve, rk_swap, determinant, string("fabs") );
		code.addFunction( solve );

		setupSolveReuseComplete( solveReuse, rk_bPerm );
		code.addFunction( solveReuse );

		if( TRANSPOSE ) {
			setupSolveReuseTranspose( solveReuseTranspose, rk_bPerm_trans );
			code.addFunction( solveReuseTranspose );
		}
	}
	else {
		setupSolveUpperTriangular( solveTriangular );
		code.addFunction( solveTriangular );

		setupSolve( solve, solveTriangular, rk_swap, determinant, string("fabs") );
		code.addFunction( solve );

		if( REUSE ) { // Also export the extra function which reuses the factorization of the matrix A

			setupSolveReuse( solveReuse, solveTriangular, rk_bPerm );
			code.addFunction( solveReuse );
		}
//		if( TRANSPOSE ) return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
		if( TRANSPOSE ) {
			setupSolveReuseTranspose( solveReuseTranspose, rk_bPerm_trans );
			code.addFunction( solveReuseTranspose );
		}
	}

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 10
0
returnValue AuxiliaryFunctionsExport::getCode(	ExportStatementBlock& code
												)
{
// 	int printLevel;
// 	get( PRINTLEVEL,printLevel );
// 
// 	if ( (PrintLevel)printLevel >= HIGH ) 
// 		acadoPrintf( "--> Exporting %s... ",fileName.getName() );

	code.addFunction( getAcadoVariablesX );
	code.addFunction( getAcadoVariablesU );
	code.addFunction( getAcadoVariablesXRef );
	code.addFunction( getAcadoVariablesURef );
	
	code.addFunction( printStates );
	code.addFunction( printControls );
	code.addFunction( getTime );
	code.addFunction( printHeader );

// 	if ( (PrintLevel)printLevel >= HIGH ) 
// 		acadoPrintf( "done.\n" );

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 11
0
returnValue CondensingExport::getCode(	ExportStatementBlock& code
										)
{
// 	int printLevel;
// 	get( PRINTLEVEL,printLevel );
// 
// 	if ( (PrintLevel)printLevel >= HIGH ) 
// 		acadoPrintf( "--> Exporting %s... ",fileName.getName() );

	code.addFunction( multiplyQC1 );
	code.addFunction( multiplyQE1 );
	code.addFunction( multiplyQDX1 );
	code.addFunction( multiplyRDU1 );
	code.addFunction( multiplyQC2 );
	code.addFunction( multiplyQE2 );
	code.addFunction( multiplyQDX2 );
	code.addFunction( multiplyC );
	code.addFunction( multiplyE );
	code.addFunction( multiplyCD1 );
	code.addFunction( multiplyEU1 );
	code.addFunction( multiplyG0 );
	code.addFunction( multiplyG1 );
	code.addFunction( multiplyH00 );
	code.addFunction( multiplyH01 );
	code.addFunction( multiplyH11 );

	code.addFunction( condense1 );
	code.addFunction( condense2 );
	code.addFunction( expand );
	code.addFunction( setupQP );

	code.addFunction( getObjectiveValue );

// 	if ( (PrintLevel)printLevel >= HIGH ) 
// 		acadoPrintf( "done.\n" );

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 12
0
returnValue DiscreteTimeExport::getCode(	ExportStatementBlock& code
										)
{
	int useOMP;
	get(CG_USE_OPENMP, useOMP);
	if ( useOMP ) {
		ExportVariable max = getAuxVariable();
		max.setName( "auxVar" );
		max.setDataStruct( ACADO_LOCAL );
		if( NX2 > 0 ) {
			rhs.setGlobalExportVariable( max );
			diffs_rhs.setGlobalExportVariable( max );
		}
		if( NX3 > 0 ) {
			rhs3.setGlobalExportVariable( max );
			diffs_rhs3.setGlobalExportVariable( max );
		}

		getDataDeclarations( code, ACADO_LOCAL );

		stringstream s;
		s << "#pragma omp threadprivate( "
				<< max.getFullName() << ", "
				<< rk_xxx.getFullName();
		if( NX1 > 0 ) {
			if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev1.getFullName();
			s << ", " << rk_diffsNew1.getFullName();
		}
		if( NX2 > 0 || NXA > 0 ) {
			if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev2.getFullName();
			s << ", " << rk_diffsNew2.getFullName();
		}
		if( NX3 > 0 ) {
			if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev3.getFullName();
			s << ", " << rk_diffsNew3.getFullName();
			s << ", " << rk_diffsTemp3.getFullName();
		}
		s << " )" << endl << endl;
		code.addStatement( s.str().c_str() );
	}

	if( NX1 > 0 ) {
		code.addFunction( lin_input );
		code.addStatement( "\n\n" );
	}

	if( NX2 > 0 ) {
		code.addFunction( rhs );
		code.addStatement( "\n\n" );
		code.addFunction( diffs_rhs );
		code.addStatement( "\n\n" );
	}

	if( NX3 > 0 ) {
		code.addFunction( rhs3 );
		code.addStatement( "\n\n" );
		code.addFunction( diffs_rhs3 );
		code.addStatement( "\n\n" );
	}

	if( !equidistantControlGrid() ) {
		ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
		code.addDeclaration( numStepsV );
		code.addLinebreak( 2 );
	}
	double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
	code.addComment(std::string("Fixed step size:") + toString(h));

	code.addFunction( integrate );

	return SUCCESSFUL_RETURN;
}
returnValue ExportIRK4StageSimplifiedNewton::getCode(	ExportStatementBlock& code
                                                    )
{
    if( eig.isEmpty() || transf1.isEmpty() || transf2.isEmpty() || fabs(stepsize) <= ZERO_EPS ) return ACADOERROR(RET_INVALID_OPTION);

    if( TRANSPOSE ) return ACADOERROR( RET_NOT_YET_IMPLEMENTED );

    setupFactorization( solve_complex, rk_swap_complex, determinant_complex, string("cabs") );
    code.addFunction( solve_complex );

    if( REUSE ) { // Also export the extra function which reuses the factorization of the matrix A
        setupSolveReuseComplete( solveReuse_complex, rk_bPerm_complex );
        code.addFunction( solveReuse_complex );
    }

    ExportVariable eig_var( 1.0/stepsize*eig );

    // SETUP solve_full
    ExportVariable det_complex1( "det_complex1", 1, 1, REAL, ACADO_LOCAL, true );
    ExportVariable det_complex2( "det_complex2", 1, 1, REAL, ACADO_LOCAL, true );
    solve_full.addDeclaration( det_complex1 );
    solve_full.addDeclaration( det_complex2 );
    ExportIndex i( "i" );
    solve_full.addIndex(i);

    // form the real and complex linear system matrices
    solve_full.addStatement( A_mem_complex1 == A_full );
    if( implicit ) {
        ExportForLoop loop1( i, 0, dim*dim );
        loop1 << A_mem_complex1.getFullName() << "[i] += (" << eig_var.get(0,0) << "+" << eig_var.get(0,1) << "*I)*" << I_full.getFullName() << "[i];\n";
        solve_full.addStatement( loop1 );
    }
    else {
        ExportForLoop loop1( i, 0, dim );
        loop1 << A_mem_complex1.getFullName() << "[i*" << toString(dim) << "+i] -= (" << eig_var.get(0,0) << "+" << eig_var.get(0,1) << "*I);\n";
        solve_full.addStatement( loop1 );
    }

    solve_full.addStatement( A_mem_complex2 == A_full );
    if( implicit ) {
        ExportForLoop loop1( i, 0, dim*dim );
        loop1 << A_mem_complex2.getFullName() << "[i] += (" << eig_var.get(1,0) << "+" << eig_var.get(1,1) << "*I)*" << I_full.getFullName() << "[i];\n";
        solve_full.addStatement( loop1 );
    }
    else {
        ExportForLoop loop1( i, 0, dim );
        loop1 << A_mem_complex2.getFullName() << "[i*" << toString(dim) << "+i] -= (" << eig_var.get(1,0) << "+" << eig_var.get(1,1) << "*I);\n";
        solve_full.addStatement( loop1 );
    }

    // factorize the real and complex linear systems
    solve_full.addFunctionCall(getNameSolveComplexFunction(),A_mem_complex1,rk_perm_full.getAddress(0,0));
    solve_full.addFunctionCall(getNameSolveComplexFunction(),A_mem_complex2,rk_perm_full.getAddress(1,0));

    code.addFunction( solve_full );

    // SETUP solveReuse_full
    if( REUSE ) {
        solveReuse_full.addIndex(i);

        // transform the right-hand side
        transformRightHandSide( solveReuse_full, i );

        // solveReuse the real and complex linear systems
        solveReuse_full.addFunctionCall(getNameSolveComplexReuseFunction(),A_mem_complex1,b_mem_complex1,rk_perm_full.getAddress(0,0));
        solveReuse_full.addFunctionCall(getNameSolveComplexReuseFunction(),A_mem_complex2,b_mem_complex2,rk_perm_full.getAddress(1,0));

        // transform back to the solution
        transformSolution( solveReuse_full, i );

        code.addFunction( solveReuse_full );
    }

    return SUCCESSFUL_RETURN;
}
Exemplo n.º 14
0
returnValue ExportGaussNewtonCN2::getCode(	ExportStatementBlock& code
											)
{
	setupQPInterface();

	code.addLinebreak( 2 );
	code.addStatement( "/******************************************************************************/\n" );
	code.addStatement( "/*                                                                            */\n" );
	code.addStatement( "/* ACADO code generation                                                      */\n" );
	code.addStatement( "/*                                                                            */\n" );
	code.addStatement( "/******************************************************************************/\n" );
	code.addLinebreak( 2 );

	int useOMP;
	get(CG_USE_OPENMP, useOMP);
	if ( useOMP )
	{
		code.addDeclaration( state );
	}

	code.addFunction( modelSimulation );

	code.addFunction( evaluateLSQ );
	code.addFunction( evaluateLSQEndTerm );

	code.addFunction( setObjQ1Q2 );
	code.addFunction( setObjR1R2 );
	code.addFunction( setObjS1 );
	code.addFunction( setObjQN1QN2 );
	code.addFunction( evaluateObjective );

	code.addFunction( multGxGu );
	code.addFunction( moveGuE );

	code.addFunction( multBTW1 );
	code.addFunction( mac_S1T_E );
	code.addFunction( macBTW1_R1 );
	code.addFunction( multGxTGu );
	code.addFunction( macQEW2 );

	code.addFunction( macATw1QDy );
	code.addFunction( macBTw1 );
	code.addFunction( macS1TSbar );
	code.addFunction( macQSbarW2 );
	code.addFunction( macASbar );
	code.addFunction( expansionStep );

	code.addFunction( copyHTH );

	code.addFunction( multRDy );
	code.addFunction( multQDy );

	code.addFunction( multQN1Gu );

	code.addFunction( evaluatePathConstraints );

	for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
	{
		if (evaluatePointConstraints[ i ] == 0)
			continue;
		code.addFunction( *evaluatePointConstraints[ i ] );
	}

	code.addFunction( condensePrep );
	code.addFunction( condenseFdb );
	code.addFunction( expand );

	code.addFunction( preparation );
	code.addFunction( feedback );

	code.addFunction( initialize );
	code.addFunction( initializeNodes );
	code.addFunction( shiftStates );
	code.addFunction( shiftControls );
	code.addFunction( getKKT );
	code.addFunction( getObjective );

	return SUCCESSFUL_RETURN;
}
returnValue ExportExactHessianQpDunes::getCode(	ExportStatementBlock& code
														)
{
	setupQPInterface();
	code.addStatement( *qpInterface );

	code.addLinebreak( 2 );
	code.addStatement( "/******************************************************************************/\n" );
	code.addStatement( "/*                                                                            */\n" );
	code.addStatement( "/* ACADO code generation                                                      */\n" );
	code.addStatement( "/*                                                                            */\n" );
	code.addStatement( "/******************************************************************************/\n" );
	code.addLinebreak( 2 );

	int useOMP;
	get(CG_USE_OPENMP, useOMP);
	if ( useOMP )
	{
		code.addDeclaration( state );
	}

	code.addFunction( modelSimulation );

	code.addFunction( evaluateStageCost );
	code.addFunction( evaluateTerminalCost );
	code.addFunction( setObjQ1Q2 );
	code.addFunction( setObjR1R2 );
	code.addFunction( setObjQN1QN2 );
	code.addFunction( setStageH );
	code.addFunction( setStagef );
	code.addFunction( evaluateObjective );

	code.addFunction( regularizeHessian );

	code.addFunction( evaluatePathConstraints );

	for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
	{
		if (evaluatePointConstraints[ i ] == 0)
			continue;
		code.addFunction( *evaluatePointConstraints[ i ] );
	}

	code.addFunction( setStagePac );
	code.addFunction( evaluateConstraints );

	code.addFunction( acc );

	code.addFunction( preparation );
	code.addFunction( feedback );

	code.addFunction( initialize );
	code.addFunction( initializeNodes );
	code.addFunction( shiftStates );
	code.addFunction( shiftControls );
	code.addFunction( getKKT );
	code.addFunction( getObjective );

	code.addFunction( cleanup );
	code.addFunction( shiftQpData );

	return SUCCESSFUL_RETURN;
}
Exemplo n.º 16
0
returnValue ExportHouseholderQR::getCode(	ExportStatementBlock& code
											)
{
	unsigned run1, run2, run3;

	if( TRANSPOSE ) return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
	//
	// Solve the upper triangular system of equations:
	//
	for (run1 = nCols; run1 > (nCols - nBacksolves); run1--)
	{
		for (run2 = nCols - 1; run2 > (run1 - 1); run2--)
		{
			solveTriangular.addStatement(
					b.getRow(run1 - 1) -= A.getSubMatrix((run1 - 1), run1, run2, run2 + 1) * b.getRow(run2));
		}
		solveTriangular <<
				"b[" << toString((run1 - 1)) << "] = b["
				<< toString((run1 - 1)) << "]/A["
				<< toString((run1 - 1) * nCols + (run1 - 1)) << "];\n";
	}
	code.addFunction(solveTriangular);
	
	//
	// Main solver function
	//
	solve.addStatement( determinant == 1.0 );

	if( UNROLLING || nRows <= 5 )
	{
		// Start the factorization:
		for (run1 = 0; run1 < nCols; run1++)
		{
			for (run2 = run1; run2 < nRows; run2++)
			{
				solve.addStatement(
						rk_temp.getCol(run2)
								== A.getSubMatrix(run2, run2 + 1, run1,
										run1 + 1));
			}
			// calculate norm:
			solve.addStatement(rk_temp.getCol(nRows) ==
					rk_temp.getCols(run1, nRows) * rk_temp.getTranspose().getRows(run1, nRows));
			solve << rk_temp.getFullName() << "[" << toString(nRows) << "] = sqrt("
					<< rk_temp.getFullName() << "[" << toString(nRows)
					<< "]);\n";

			// update first element:
			solve << rk_temp.getFullName() << "[" << toString(run1) << "] += ("
					<< rk_temp.getFullName() << "[" << toString(run1)
					<< "] < 0 ? -1 : 1)*" << rk_temp.getFullName()
					<< "[" << toString(nRows) << "];\n";

			// calculate norm:
			solve.addStatement(rk_temp.getCol(nRows) ==
					rk_temp.getCols(run1, nRows) * rk_temp.getTranspose().getRows(run1, nRows));
			solve << rk_temp.getFullName() << "[" << toString(nRows) << "] = sqrt("
					<< rk_temp.getFullName() << "[" << toString(nRows)
					<< "]);\n";

			// normalization:
			for (run2 = run1; run2 < nRows; run2++)
			{
				solve << rk_temp.getFullName() << "[" << toString(run2) << "] = "
						<< rk_temp.getFullName() << "[" << toString(run2)
						<< "]/" << rk_temp.getFullName() << "["
						<< toString(nRows) << "];\n";
			}

			// update current column:
			solve.addStatement(
					rk_temp.getCol(nRows)
							== rk_temp.getCols(run1, nRows)
									* A.getSubMatrix(run1, nRows, run1,
											run1 + 1));
			solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
			solve.addStatement(
					A.getSubMatrix(run1, run1 + 1, run1, run1 + 1) -=
							rk_temp.getCol(run1) * rk_temp.getCol(nRows));

			solve.addStatement( determinant == determinant * A.getElement(run1, run1) );

			if (REUSE)
			{
				// replace zeros by results that can be reused:
				for (run2 = run1; run2 < nRows - 1; run2++)
				{
					solve.addStatement(
							A.getSubMatrix(run2 + 1, run2 + 2, run1, run1 + 1)
									== rk_temp.getCol(run2));
				}
			}

			// update following columns:
			for (run2 = run1 + 1; run2 < nCols; run2++)
			{
				solve.addStatement(
						rk_temp.getCol(nRows)
								== rk_temp.getCols(run1, nRows)
										* A.getSubMatrix(run1, nRows, run2,
												run2 + 1));
				solve <<  rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
				for (run3 = run1; run3 < nRows; run3++)
				{
					solve.addStatement(
							A.getSubMatrix(run3, run3 + 1, run2, run2 + 1) -=
									rk_temp.getCol(run3) * rk_temp.getCol(nRows));
				}
			}
			// update right-hand side:
			solve.addStatement(
					rk_temp.getCol(nRows)
							== rk_temp.getCols(run1, nRows)
									* b.getRows(run1, nRows));
			solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
			for (run3 = run1; run3 < nRows; run3++)
			{
				solve.addStatement( b.getRow(run3) -= rk_temp.getCol(run3) * rk_temp.getCol(nRows));
			}

			if (REUSE)
			{
				// store last element to be reused:
				solve.addStatement(
						rk_temp.getCol(run1) == rk_temp.getCol(nRows - 1));
			}
		}
	}
	else
	{
		ExportIndex i( "i" );
		ExportIndex j( "j" );
		ExportIndex k( "k" );

		solve.addIndex( i );
		solve.addIndex( j );
		solve.addIndex( k );

		solve << "for( i=0; i < " << toString( nCols ) << "; i++ ) {\n";
		solve << "	for( j=i; j < " << toString( nRows ) << "; j++ ) {\n";
		solve << "		" << rk_temp.getFullName() << "[j] = A[j*" << toString( nCols ) << "+i];\n";
		solve << "	}\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[i];\n";
		solve << "	for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
		solve << "		" << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*" << rk_temp.getFullName() << "[j];\n";
		solve << "	}\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] = sqrt(" << rk_temp.getFullName() << "[" << toString( nRows ) << "]);\n";
		// update first element:
		solve << "	" << rk_temp.getFullName() << "[i] += (" << rk_temp.getFullName() << "[i] < 0 ? -1 : 1)*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[i];\n";
		solve << "	for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
		solve << "		" << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*" << rk_temp.getFullName() << "[j];\n";
		solve << "	}\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] = sqrt(" << rk_temp.getFullName() << "[" << toString( nRows ) << "]);\n";
		solve << "	for( j=i; j < " << toString( nRows ) << "; j++ ) {\n";
		solve << "		" << rk_temp.getFullName() << "[j] = " << rk_temp.getFullName() << "[j]/" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
		solve << "	}\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*A[i*" << toString( nCols ) << "+i];\n";
		solve << "	for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
		solve << "		" << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*A[j*" << toString( nCols ) << "+i];\n";
		solve << "	}\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
		solve << "	A[i*" << toString( nCols ) << "+i] -= " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";

		solve << "	" << determinant.getFullName() << " *= " << "	A[i * " << toString( nCols ) << " + i];\n";

		if( REUSE ) {
			solve << "	for( j=i; j < (" << toString( nRows ) << "-1); j++ ) {\n";
			solve << "		A[(j+1)*" << toString( nCols ) << "+i] = " << rk_temp.getFullName() << "[j];\n";
			solve << "	}\n";
		}
		solve << "	for( j=i+1; j < " << toString( nCols ) << "; j++ ) {\n";
		solve << "		" << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*A[i*" << toString( nCols ) << "+j];\n";
		solve << "		for( k=i+1; k < " << toString( nRows ) << "; k++ ) {\n";
		solve << "			" << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[k]*A[k*" << toString( nCols ) << "+j];\n";
		solve << "		}\n";
		solve << "		" << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
		solve << "		for( k=i; k < " << toString( nRows ) << "; k++ ) {\n";
		solve << "			A[k*" << toString( nCols ) << "+j] -= " << rk_temp.getFullName() << "[k]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
		solve << "		}\n";
		solve << "	}\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*b[i];\n";
		solve << "	for( k=i+1; k < " << toString( nRows ) << "; k++ ) {\n";
		solve << "		" << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[k]*b[k];\n";
		solve << "	}\n";
		solve << "	" << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
		solve << "	for( k=i; k < " << toString( nRows ) << "; k++ ) {\n";
		solve << "		b[k] -= " << rk_temp.getFullName() << "[k]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
		solve << "	}\n";
		if( REUSE ) {
			solve << "	" << rk_temp.getFullName() << "[i] = " << rk_temp.getFullName() << "[" << toString( nRows-1 ) << "];\n";
		}
		solve << "}\n";
	}
	solve.addLinebreak();

	solve.addFunctionCall(solveTriangular, A, b);
	code.addFunction( solve );
	
    code.addLinebreak( 2 );
	if( REUSE ) { // Also export the extra function which reuses the factorization of the matrix A
		// update right-hand side:
		for( run1 = 0; run1 < nCols; run1++ ) {
			solveReuse.addStatement( rk_temp.getCol( nRows ) == A.getSubMatrix( run1+1,run1+2,run1,run1+1 )*b.getRow( run1 ) );
			for( run2 = run1+1; run2 < (nRows-1); run2++ ) {
				solveReuse.addStatement( rk_temp.getCol( nRows ) += A.getSubMatrix( run2+1,run2+2,run1,run1+1 )*b.getRow( run2 ) );
			}
			solveReuse.addStatement( rk_temp.getCol( nRows ) += rk_temp.getCol( run1 )*b.getRow( nRows-1 ) );
			solveReuse << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n" ;
			for( run3 = run1; run3 < (nRows-1); run3++ ) {
				solveReuse.addStatement( b.getRow( run3 ) -= A.getSubMatrix( run3+1,run3+2,run1,run1+1 )*rk_temp.getCol( nRows ) );
			}
			solveReuse.addStatement( b.getRow( nRows-1 ) -= rk_temp.getCol( run1 )*rk_temp.getCol( nRows ) );
		}
		solveReuse.addLinebreak();

		solveReuse.addFunctionCall( solveTriangular, A, b );
		code.addFunction( solveReuse );
	}
	
	return SUCCESSFUL_RETURN;
}
Exemplo n.º 17
0
returnValue ExportGaussNewtonForces::getCode(	ExportStatementBlock& code
														)
{
	setupQPInterface();
	code.addStatement( *qpInterface );

	code.addLinebreak( 2 );
	code.addStatement( "/******************************************************************************/\n" );
	code.addStatement( "/*                                                                            */\n" );
	code.addStatement( "/* ACADO code generation                                                      */\n" );
	code.addStatement( "/*                                                                            */\n" );
	code.addStatement( "/******************************************************************************/\n" );
	code.addLinebreak( 2 );

	int useOMP;
	get(CG_USE_OPENMP, useOMP);
	if ( useOMP )
	{
		code.addDeclaration( state );
	}

	code.addFunction( modelSimulation );

	code.addFunction( evaluateStageCost );
	code.addFunction( evaluateTerminalCost );
	code.addFunction( setObjQ1Q2 );
	code.addFunction( setObjR1R2 );
	code.addFunction( setObjS1 );
	code.addFunction( setObjQN1QN2 );
	code.addFunction( setStageH );
	code.addFunction( setStagef );
	code.addFunction( evaluateObjective );

	code.addFunction( conSetGxGu );
	code.addFunction( conSetd );
	code.addFunction( evaluateConstraints );

	code.addFunction( acc );

	code.addFunction( preparation );
	code.addFunction( feedback );

	code.addFunction( initialize );
	code.addFunction( initializeNodes );
	code.addFunction( shiftStates );
	code.addFunction( shiftControls );
	code.addFunction( getKKT );
	code.addFunction( getObjective );

	return SUCCESSFUL_RETURN;
}