Beispiel #1
0
returnValue Integrator::setForwardSeed(	const int    &order,
										const DVector &xSeed,
										const DVector &pSeed,
										const DVector &uSeed,
										const DVector &wSeed  ){


    int run1;
    if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );

    DVector tmpX;
    DVector components = rhs->getDifferentialStateComponents();

    dP = pSeed;
    dU = uSeed;
    dW = wSeed;

    if( xSeed.getDim() != 0 ){

        tmpX.init( components.getDim() );
        for( run1 = 0; run1 < (int) components.getDim(); run1++ )
             tmpX(run1) = xSeed((int) components(run1));
    }

    return setProtectedForwardSeed( tmpX, pSeed, uSeed, wSeed, order );
}
Beispiel #2
0
returnValue OCP::subjectTo( const DVector& _lb, const Expression& _expr, const DVector& _ub )
{
	ASSERT(_lb.getDim() == _expr.getDim() && _lb.getDim() == _ub.getDim());
	constraint->add(_lb, _expr, _ub);

	return SUCCESSFUL_RETURN;
}
Beispiel #3
0
BooleanType RungeKuttaExport::checkSymmetry( const DVector& _cc ) {

	if( _cc.getDim() <= 1 ) return BT_FALSE;
	BooleanType symmetry = BT_TRUE;
	uint i;
	for( i = 0; i < _cc.getDim(); i++ ) {
		int tmp = acadoRoundAway(1.0 - _cc(i) - _cc(_cc.getDim()-1-i));
		if( symmetry ) symmetry = (tmp == 0);
	}
	return symmetry;
}
Beispiel #4
0
returnValue RungeKuttaExport::initializeButcherTableau( const DMatrix& _AA, const DVector& _bb, const DVector& _cc ) {

	if( _cc.isEmpty() || !_AA.isSquare() || _AA.getNumRows() != _bb.getDim() || _bb.getDim() != _cc.getDim() ) return RET_INVALID_OPTION;

	numStages = _cc.getDim();
	is_symmetric = checkSymmetry( _cc );
//	std::cout << "Symmetry of the chosen method: " << is_symmetric << "\n";
	AA = _AA;
	bb = _bb;
	cc = _cc;

	return SUCCESSFUL_RETURN;
}
Beispiel #5
0
returnValue Integrator::evaluateTransition( const double time, DVector &xd, const DVector &xa,
                                            const DVector &p, const DVector &u, const DVector &w ){

    ASSERT( transition != 0 );
    EvaluationPoint z( *transition, xd.getDim(), xa.getDim(), p.getDim(), u.getDim(), w.getDim() );
    z.setT ( time );
    z.setX ( xd   );
    z.setXA( xa   );
    z.setP ( p    );
    z.setU ( u    );
    z.setW ( w    );
    xd = transition->evaluate( z );
    return SUCCESSFUL_RETURN;
}
Beispiel #6
0
returnValue Integrator::integrate(	const Grid   &t_  ,
									const DVector &x0  ,
									const DVector &xa  ,
									const DVector &p   ,
									const DVector &u   ,
									const DVector &w    ){

    int run1;
    returnValue returnvalue;
    if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );

    DVector tmpX;

    DVector components = rhs->getDifferentialStateComponents();

    const int N = components.getDim();

    if( x0.getDim() != 0 ){
        tmpX.init( components.getDim() );
        for( run1 = 0; run1 < (int) components.getDim(); run1++ )
            tmpX(run1) = x0((int) components(run1));
    }


// 	tmpX.print( "integrator x0" );
// 	u.print( "integrator u0" );
// 	p.print( "integrator p0" );
    returnvalue = evaluate( tmpX, xa, p, u, w, t_ );

    if( returnvalue != SUCCESSFUL_RETURN )
        return returnvalue;

    xE.init(rhs->getDim());
    xE.setZero();

    DVector tmp(rhs->getDim());
    getProtectedX(&tmp);

    for( run1 = 0; run1 < N; run1++ )
        xE((int) components(run1)) = tmp(run1);

    for( run1 = N; run1 < N + ma; run1++ )
        xE(run1) = tmp(run1);

    if( transition != 0 )
        returnvalue = evaluateTransition( t_.getLastTime(), xE, xa, p, u, w );

    return returnvalue;
}
Beispiel #7
0
returnValue Integrator::getBackwardSensitivities(	DVector &DX,
													DVector &DP ,
													DVector &DU ,
													DVector &DW ,
													int    order   ) const{

    int run2;
    returnValue returnvalue;

    DVector tmpX ( rhs->getDim() );

    DX.setZero();
    DP.setZero();
    DU.setZero();
    DW.setZero();

    returnvalue = getProtectedBackwardSensitivities( tmpX, DP, DU, DW, order );
    DVector components = rhs->getDifferentialStateComponents();

    for( run2 = 0; run2 < (int) components.getDim(); run2++ )
        DX((int) components(run2)) = tmpX(run2);

    for( run2 = 0; run2 < (int) dPb.getDim(); run2++ )
        DP(run2) += dPb(run2);

    for( run2 = 0; run2 < (int) dUb.getDim(); run2++ )
        DU(run2) += dUb(run2);

    for( run2 = 0; run2 < (int) dWb.getDim(); run2++ )
        DW(run2) += dWb(run2);

    return returnvalue;
}
Beispiel #8
0
returnValue Integrator::setBackwardSeed(	const int    &order,
											const DVector &seed   ){

    dXb = seed;

    uint run1;
    DVector tmp( seed.getDim() );
    DVector components = rhs->getDifferentialStateComponents();

    if( seed.getDim() != 0 ){
        tmp.init( components.getDim() );
        for( run1 = 0; run1 < components.getDim(); run1++ )
             tmp(run1) = seed((int) components(run1));
    }
    return setProtectedBackwardSeed( tmp, order );
}
Beispiel #9
0
returnValue Integrator::diffTransitionForward(       DVector &DX,
                                               const DVector &DP,
                                               const DVector &DU,
                                               const DVector &DW,
                                               const int    &order ){

    ASSERT( transition != 0 );
    EvaluationPoint z( *transition, DX.getDim(), 0, DP.getDim(), DU.getDim(), DW.getDim() );
    z.setX ( DX );
    z.setP ( DP );
    z.setU ( DU );
    z.setW ( DW );

    if( order == 1 ) DX = transition->AD_forward( z );

    if( order != 1 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );

    return SUCCESSFUL_RETURN;
}
Beispiel #10
0
returnValue Integrator::diffTransitionBackward( DVector &DX,
                                                DVector &DP,
                                                DVector &DU,
                                                DVector &DW,
                                                int    &order ){

    ASSERT( transition != 0 );
    if( order != 1 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );

    EvaluationPoint z( *transition, DX.getDim(), 0, DP.getDim(), DU.getDim(), DW.getDim() );

    transition->AD_backward( DX, z );

    DX = z.getX();
    DP = z.getP();
    DU = z.getU();
    DW = z.getW();

    return SUCCESSFUL_RETURN;
}
Beispiel #11
0
OCP::OCP( const double &tStart_, const double &tEnd_, const DVector& _numSteps )
    : MultiObjectiveFunctionality(),
      grid(new Grid()), objective(new Objective()), constraint(new Constraint())
{
	if( _numSteps.getDim() <= 0 ) ACADOERROR( RET_INVALID_ARGUMENTS );
      
	DVector times( _numSteps.getDim()+1 );
	times(0) = tStart_;
	
	double totalSteps = 0;
	uint i;
	for( i = 0; i < _numSteps.getDim(); i++ ) {
		totalSteps += _numSteps(i);
	}
	double h = (tEnd_ - tStart_)/totalSteps;
	for( i = 0; i < _numSteps.getDim(); i++ ) {
		times(i+1) = times(i) + h*_numSteps(i);
	}
	
    setupGrid( times );
    modelData.setIntegrationGrid(*grid, totalSteps);
}
Beispiel #12
0
BooleanType GnuplotWindow::getMouseEvent( double &mouseX, double &mouseY )
{
    DVector tmp;
    if (tmp.read( "mouse.dat" ) != SUCCESSFUL_RETURN)
    	return BT_FALSE;

    mouseX = tmp( tmp.getDim()-2 );
    mouseY = tmp( tmp.getDim()-1 );

	if ( system("rm mouse.dat") )
		return BT_FALSE;

    return BT_TRUE;
}
Beispiel #13
0
returnValue VariablesGrid::setVector(	uint pointIdx,
										const DVector& _values
										)
{
	if ( pointIdx >= getNumPoints( ) )
		return ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );

	if ( _values.getDim( ) != getNumRows( pointIdx ) )
		return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
	
	for( uint j=0; j<getNumRows( ); ++j )
		operator()( pointIdx,j ) = _values( j );
	
	return SUCCESSFUL_RETURN;
}
Beispiel #14
0
returnValue Integrator::integrateSensitivities( ){

    uint run1;
    returnValue returnvalue;


    if( ( nBDirs > 0 || nBDirs2 > 0 ) && transition != 0 ){

        int order;
        if( nBDirs2 > 0 ) order = 2;
        else              order = 1;

        returnvalue = diffTransitionBackward( dXb, dPb, dUb, dWb, order );

        setBackwardSeed( order, dXb );

        if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
    }

    returnvalue = evaluateSensitivities();

    if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);


    if( nBDirs > 0 || nBDirs2 > 0 ) return SUCCESSFUL_RETURN;

    int order = 1;
    if( nFDirs2 > 0 ) order = 2;

    DMatrix tmp( rhs->getDim(), 1 );
    returnvalue = getProtectedForwardSensitivities(&tmp,order);

    DVector components = rhs->getDifferentialStateComponents();

    dX.init(rhs->getDim()-ma);
    dX.setZero();

    for( run1 = 0; run1 < components.getDim(); run1++ )
        dX((int) components(run1)) = tmp(run1,0);

    if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);

    if( transition != 0 )
        returnvalue = diffTransitionForward( dX, dP, dU, dW, order );

    return returnvalue;
}
Beispiel #15
0
returnValue Actuator::setParameterDeadTimes(	const DVector& _deadTimes
                                           )
{
    if ( _deadTimes.getDim( ) != getNP( ) )
        return ACADOERROR( RET_INVALID_ARGUMENTS );

    if ( _deadTimes.getMin( ) < 0.0 )
        return ACADOERROR( RET_INVALID_ARGUMENTS );

    if ( deadTimes.getDim( ) == 0 )
        return ACADOERROR( RET_MEMBER_NOT_INITIALISED );

    for( uint i=0; i<getNP(); ++i )
        deadTimes( getNU()+i ) = _deadTimes( i );

    return SUCCESSFUL_RETURN;
}
Beispiel #16
0
returnValue Integrator::integrate(	const Grid  &t_,
									double      *x0,
									double      *xa,
									double      *p ,
									double      *u ,
									double      *w  ){

    if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
    DVector components = rhs->getDifferentialStateComponents();

    DVector tmpX ( components.getDim(), x0 );
    DVector tmpXA( rhs->getNXA()      , xa );
    DVector tmpP ( rhs->getNP ()      , p  );
    DVector tmpU ( rhs->getNU ()      , u  );
    DVector tmpW ( rhs->getNW ()      , w  );

    return integrate( t_, tmpX, tmpXA, tmpP, tmpU, tmpW );
}
Beispiel #17
0
returnValue OCP::subjectTo( int _index, const DVector& _lb, const Expression& _expr, const DVector& _ub )
{
	ASSERT(_index >= AT_START);
	cout << _lb.getDim() << " " << _expr.getDim() << endl;
	ASSERT(_lb.getDim() == _expr.getDim());
	ASSERT(_lb.getDim() == _ub.getDim());

	if (_index == AT_START)
	{
		for (unsigned el = 0; el < _lb.getDim(); ++el)
			ACADO_TRY( constraint->add(0, _lb( el ), _expr( el ), _ub( el )) );
	}
	else if (_index == AT_END)
	{
		for (unsigned el = 0; el < _lb.getDim(); ++el)
			ACADO_TRY(constraint->add(grid->getLastIndex(), _lb( el ), _expr( el ), _ub( el )) );
	}
	else
		for (unsigned el = 0; el < _lb.getDim(); ++el)
			constraint->add(_index, _lb( el ), _expr( el ), _ub( el ));

	return SUCCESSFUL_RETURN;
}
Beispiel #18
0
returnValue SIMexport::exportAcadoHeader(	const std::string& _dirName,
											const std::string& _fileName,
											const std::string& _realString,
											const std::string& _intString,
											int _precision
											) const
{
	string moduleName;
	get(CG_MODULE_NAME, moduleName);

	int qpSolver;
	get(QP_SOLVER, qpSolver);

	int useSinglePrecision;
	get(USE_SINGLE_PRECISION, useSinglePrecision);

	string fileName;
	fileName = _dirName + "/" + _fileName;


	map<string, pair<string, string> > options;

	DVector nMeasV = getNumMeas();
	DVector nOutV = getDimOutputs();

	options[ "ACADO_N" ]   = make_pair(toString( getN() ),   "Number of control/estimation intervals.");
	options[ "ACADO_NX" ]  = make_pair(toString( getNX() ),  "Number of differential variables.");
	options[ "ACADO_NXD" ] = make_pair(toString( getNDX() ), "Number of differential derivative variables.");
	options[ "ACADO_NXA" ] = make_pair(toString( getNXA() ), "Number of algebraic variables.");
	options[ "ACADO_NU" ]  = make_pair(toString( getNU() ),  "Number of control variables.");
	options[ "ACADO_NOD" ]  = make_pair(toString( getNOD() ),  "Number of online data values.");
	options[ "ACADO_NUMOUT" ]  = make_pair(toString( nOutV.getDim() ),  "Number of output functions.");

	if( !nMeasV.isEmpty() && !nOutV.isEmpty() ) {
		std::ostringstream acado_nout;
		ExportVariable( "ACADO_NOUT",nOutV,STATIC_CONST_INT ).exportDataDeclaration(acado_nout);
		std::ostringstream acado_nmeas;
		ExportVariable( "ACADO_NMEAS",nMeasV,STATIC_CONST_INT ).exportDataDeclaration(acado_nmeas);
		options[ "ACADO_OUTPUTS_DEFINED" ]  = make_pair("\n" + acado_nout.str() + acado_nmeas.str(),  "Dimension and measurements of the output functions per shooting interval.");
	}

	//
	// ACADO variables and workspace
	//
	ExportStatementBlock variablesBlock;
	stringstream variables;

	if (collectDataDeclarations(variablesBlock, ACADO_VARIABLES) != SUCCESSFUL_RETURN)
		return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
	variablesBlock.exportCode(variables, _realString, _intString, _precision);

	ExportStatementBlock workspaceBlock;
	stringstream workspace;

	if (collectDataDeclarations(workspaceBlock, ACADO_WORKSPACE) != SUCCESSFUL_RETURN)
		return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
	workspaceBlock.exportCode(workspace, _realString, _intString, _precision);

	ExportStatementBlock functionsBlock;
	stringstream functions;

	if (collectFunctionDeclarations( functionsBlock ) != SUCCESSFUL_RETURN)
		return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
	functionsBlock.exportCode(functions, _realString);

	ExportCommonHeader ech(fileName, "", _realString, _intString, _precision);
	ech.configure( moduleName, useSinglePrecision, false, (QPSolverName)qpSolver,
			options, variables.str(), workspace.str(), functions.str());

	return ech.exportCode();
}
Beispiel #19
0
returnValue WeightGeneration::generateWeights( const  int    &n         ,
                                               DVector        &weight    ,
                                               DMatrix        &Weights   ,
                                               const  DVector &weightsLB ,
                                               const  DVector &weightsUB ,
                                               DVector        &formers   ,
                                               const int     &layer     ,
                                               int           &lastOne   ,
                                               int           &currentOne,
                                               double        &step
                                             ) const{


   int run1, run2;

    if( n == layer ){

        //printf("case1:  n = %d  layer = %d  \n", n, layer );

        double weight_test = 1.0;
        for( run1 = 0; run1 < (int) weight.getDim()-1; run1++ )
            weight_test -= weight(run1);

        if( ( weight_test >= weightsLB(layer) - 100.0*EPS ) &&
            ( weight_test <= weightsUB(layer) + 100.0*EPS )    ){

            if( currentOne >= 0 ){

                DVector tmp( formers.getDim()+1 );

                for( run1 = 0; run1 < (int) formers.getDim(); run1++ )
                    tmp(run1) = formers(run1);

                tmp(formers.getDim()) = lastOne;
                formers = tmp;
                lastOne = currentOne;
                currentOne = -1;
            }
            else{

                DVector tmp( formers.getDim()+1 );

                for( run1 = 0; run1 < (int) formers.getDim(); run1++ )
                    tmp(run1) = formers(run1);

                tmp(formers.getDim()) = Weights.getNumCols();
                formers = tmp;
            }

             weight(n) = weight_test;

             DMatrix tmp( weight.getDim(), Weights.getNumCols()+1 );

             for( run1 = 0; run1 < (int) Weights.getNumRows(); run1++ ){
                 for( run2 = 0; run2 < (int) Weights.getNumCols(); run2++ )
                     tmp(run1,run2) = Weights(run1,run2);
             }

             for( run2 = 0; run2 < (int) weight.getDim(); run2++ )
                     tmp(run2,Weights.getNumCols()) = weight(run2);

             Weights = tmp;
        }
    }
    else{

        //printf("case2:  n = %d  layer = %d  \n", n, layer );

        double weight_test = weightsLB(n);

        while( weight_test <= weightsUB(n) + 100.0*EPS ){

            if( n == layer-2 ) currentOne = Weights.getNumCols()+1;

            weight(n) = weight_test;

            WeightGeneration child;
            child.generateWeights( n+1, weight, Weights, weightsLB, weightsUB, formers, layer, lastOne, currentOne, step );

            weight_test += (weightsUB(n)-weightsLB(n))*step;
        }
    }

    return SUCCESSFUL_RETURN;
}