C++ VariablesGrid类代码示例

VariablesGrid VariablesGrid::getTimeSubGrid(	double startTime,
												double endTime
												) const
{
    uint startIdx = getCeilIndex( startTime );
	uint endIdx   = getFloorIndex( endTime );

	VariablesGrid newVariablesGrid;

	if ( ( isInInterval( startTime ) == BT_FALSE ) || ( isInInterval( endTime ) == BT_FALSE ) )
		return newVariablesGrid;
	
	if ( ( startIdx >= getNumPoints( ) ) || ( endIdx >= getNumPoints( ) ) )
		return newVariablesGrid;

// 	if ( startIdx > endIdx )
// 		return newVariablesGrid;
	
	// add all matrices in interval (constant interpolation)
	if ( ( hasTime( startTime ) == BT_FALSE ) && ( startIdx > 0 ) )
		newVariablesGrid.addMatrix( *(values[ startIdx-1 ]),startTime );
	
	for( uint i=startIdx; i<=endIdx; ++i )
		newVariablesGrid.addMatrix( *(values[i]),getTime( i ) );
	
	if ( hasTime( endTime ) == BT_FALSE )
		newVariablesGrid.addMatrix( *(values[ endIdx ]),endTime );

    return newVariablesGrid;
}

returnValue Controller::getCurrentReference(	double tStart,
												VariablesGrid& _yRef
												) const
{
	double tEnd = tStart + controlLaw->getLengthPredictionHorizon( );

	// if no external reference trajectory is given, evaluate internal one
	if ( _yRef.isEmpty( ) == BT_TRUE )
	{
		if ( referenceTrajectory != 0 )
			referenceTrajectory->getReference( tStart,tEnd, _yRef );
	}

	// if prediction shall not be used, only use first value
	int useReferencePrediction = 0;
	get( USE_REFERENCE_PREDICTION,useReferencePrediction );
	
	if ( (BooleanType)useReferencePrediction == BT_FALSE )
	{
		Vector firstVector = _yRef.getFirstVector( );
		Grid predictionGrid( tStart,tEnd );
		_yRef.init( firstVector,predictionGrid );
	}

	return SUCCESSFUL_RETURN;
}

returnValue PeriodicReferenceTrajectory::getReference(	double tStart,
														double tEnd,
														VariablesGrid& _yRef
														) const
{
	if ( acadoIsStrictlyGreater( tStart,tEnd ) == BT_TRUE )
		return ACADOERROR( RET_INVALID_ARGUMENTS );

	double T = yRef.getLastTime() - yRef.getFirstTime();         // cycle duration
	int nStart = (int)floor( (double) (tStart/T+100.0*EPS) );  // cycle number at start
	int nEnd   = (int)floor( (double) (tEnd  /T-100.0*EPS) );  // cycle number at end

	if ( nStart == nEnd )
	{
		_yRef = (yRef.getTimeSubGrid( tStart-T*(double)nStart,tEnd-T*(double)nStart )).shiftTimes( T*(double)nStart );
	}
	else
	{
		_yRef = (yRef.getTimeSubGrid( tStart-T*(double)nStart,yRef.getLastTime() )).shiftTimes( T*(double)nStart );
		
		for( int i=nStart+1; i<nEnd; ++i )
			_yRef.appendTimes( VariablesGrid(yRef).shiftTimes( T*(double)i ),MM_KEEP );
		
		_yRef.appendTimes( (yRef.getTimeSubGrid( yRef.getFirstTime(),tEnd-T*(double)nEnd )).shiftTimes( T*(double)nEnd ) );
	}
	
	return SUCCESSFUL_RETURN;
}

/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO

    // DEFINE A RIGHT-HAND-SIDE:
    // -------------------------
    DifferentialState         x;
    AlgebraicState            z;
    Parameter               p,q;

    DifferentialEquation f;

    f << dot(x) == -p*x*x*z  ;
    f <<     0  ==  q*q - z*z;


    // DEFINE AN INTEGRATOR:
    // ---------------------

	IntegratorBDF integrator(f);

	integrator.set( INTEGRATOR_PRINTLEVEL, HIGH );

	
    // DEFINE INITIAL VALUES:
    // ----------------------

    double x0   =  1.0;
    double z0   =  1.000000;

    double pp[2] = { 1.0, 1.0 };

    double t0   = 0.0 ;
    double tend = 0.2 ;


    // START THE INTEGRATION:
    // ----------------------

	//integrator.freezeAll();
    integrator.integrate( t0, tend, &x0, &z0, pp );


    // GET THE RESULTS
    // ---------------

    VariablesGrid differentialStates;
    VariablesGrid algebraicStates   ;

    integrator.getX ( differentialStates );
    integrator.getXA( algebraicStates    );

	differentialStates.print( "x" );
	algebraicStates.print( "z" );


    return 0;
}

returnValue OptimizationAlgorithmBase::initializeDisturbances( const char* fileName)
{
	VariablesGrid tmp = fopen( fileName,"r" );
	
	if ( tmp.isEmpty() == BT_TRUE )
		return RET_FILE_CAN_NOT_BE_OPENED;
	
    return initializeDisturbances(tmp);
}

returnValue Constraint::add( const int index_, const ConstraintComponent& component ) {

    Vector tmp_ub(grid.getNumPoints());
    Vector tmp_lb(grid.getNumPoints());

    ASSERT_RETURN( index_ < (int) grid.getNumPoints() ).addMessage("\n >>>  The constraint component can not be set as the associated discretization point is not in the time horizon.  <<< \n\n");

    uint run1;

    if( component.hasLBgrid() == 0 ) {

        for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
            if( (component.getLB()).getDim() == 1 )
                tmp_lb(run1) = (component.getLB()).operator()(0);
            else {
                if( (component.getLB()).getDim() <= run1 )
                    return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
                tmp_lb(run1) = (component.getLB()).operator()(run1);
            }
        }
    }
    else {

        VariablesGrid LBgrid = component.getLBgrid();

        for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
            Vector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
            tmp_lb(run1) = tmp(0);
        }
    }


    if( component.hasUBgrid() == 0 ) {
        for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
            if( (component.getUB()).getDim() == 1 )
                tmp_ub(run1) = (component.getUB()).operator()(0);
            else {
                if( (component.getUB()).getDim() <= run1 )
                    return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
                tmp_ub(run1) = (component.getUB()).operator()(run1);
            }
        }
    }
    else {

        VariablesGrid UBgrid = component.getUBgrid();

        for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
            Vector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
            tmp_ub(run1) = tmp(0);
        }
    }

    ACADO_TRY( add( index_, tmp_lb(index_), component.getExpression(), tmp_ub(index_) ) );

    return SUCCESSFUL_RETURN;
}

returnValue SimulationEnvironment::initializeAlgebraicStates( const char* fileName )
{
	VariablesGrid tmp = fopen( fileName,"r" );
	
	if ( tmp.isEmpty( ) == BT_TRUE )
		return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
	
	return initializeAlgebraicStates( tmp );
}

returnValue OCP::minimizeLSQ( const Function      &h,
                              const char*  rFilename  ){

    VariablesGrid r = readFromFile( rFilename );

    if( r.isEmpty() == BT_TRUE )
        return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );

    return minimizeLSQ( h,r );
}

/* identitical to same function within the class Process! */
returnValue Actuator::checkInputConsistency(	const VariablesGrid& _u,
												const VariablesGrid& _p
												) const
{
	if ( _u.getNumPoints( ) < 2 )
		return ACADOERROR( RET_INVALID_ARGUMENTS );

	if ( _u.getNumRows( ) != getNU( ) )
		return ACADOERROR( RET_CONTROL_DIMENSION_MISMATCH );

	if ( _p.isEmpty( ) == BT_TRUE )
	{
		if ( getNP( ) > 0 )
			return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
	}
	else
	{
		if ( _p.getNumPoints( ) < 2 )
			return ACADOERROR( RET_INVALID_ARGUMENTS );

		if ( _p.getNumRows( ) != getNP( ) )
			return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
	
		if ( acadoIsEqual( _u.getFirstTime( ),_p.getFirstTime( ) ) == BT_FALSE )
			return ACADOERROR( RET_INVALID_ARGUMENTS );
	
		if ( acadoIsEqual( _u.getLastTime( ),_p.getLastTime( ) ) == BT_FALSE )
			return ACADOERROR( RET_INVALID_ARGUMENTS );
	}

	return SUCCESSFUL_RETURN;
}

returnValue Sensor::addSensorNoise(	VariablesGrid& _y
									) const
{
	if ( hasNoise( ) == BT_FALSE )
		return SUCCESSFUL_RETURN;

	// generate current noise
	VariablesGrid currentNoise;

	if ( generateNoise( _y.getFirstTime(),_y.getLastTime(),currentNoise ) != SUCCESSFUL_RETURN )
		return ACADOERROR( RET_GENERATING_NOISE_FAILED );

	// determine common grid
	Grid commonGrid, tmpGrid;
	_y.getGrid( commonGrid );
	currentNoise.getGrid( tmpGrid );
	commonGrid.merge( tmpGrid,MM_KEEP );
	
	// adapt input grids and add noise
	_y.refineGrid( commonGrid );
	currentNoise.refineGrid( commonGrid );
	_y += currentNoise.getValuesSubGrid( 0,getNY()-1 );

	return SUCCESSFUL_RETURN;
}

returnValue Sensor::getDelayedOutputGrid(	const VariablesGrid& _y,
											VariablesGrid& _yDelayed
											) const
{
	// determine common time grid for delayed outputs:
	Grid delayedOutputTimeGrid = lastSignal.getTimePoints( );

	// make sure that last time instant of horizon lies within the grid
	if ( acadoIsEqual( lastSignal.getLastTime(),_y.getLastTime( ) ) == BT_FALSE )
		delayedOutputTimeGrid.addTime( _y.getLastTime( ) );

	// add grids of all delayed output components
	for( uint i=0; i<getNY( ); ++i )
		delayedOutputTimeGrid.merge( _y.getTimePoints( ).shiftTimes( deadTimes(i) ),MM_REPLACE );

	VariablesGrid tmp;

	// setup common variables grid for delayed inputs
	_yDelayed.init( );

	for( uint i=0; i<getNY( ); ++i )
	{
		tmp = lastSignal( i );
		tmp.merge( _y( i ).shiftTimes( deadTimes(i) ),MM_REPLACE,BT_FALSE );
		tmp.refineGrid( delayedOutputTimeGrid );

		_yDelayed.appendValues( tmp );
	}

	return SUCCESSFUL_RETURN;
}

returnValue OptimizationAlgorithmBase::getParameters( Vector &p_  ) const
{
	if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );

	VariablesGrid tmp;

	returnValue returnvalue = nlpSolver->getParameters( tmp );
	if ( returnvalue != SUCCESSFUL_RETURN )
		return returnvalue;

	p_ = tmp.getVector( 0 );

	return SUCCESSFUL_RETURN;
}

VariablesGrid VariablesGrid::operator[](	const uint pointIdx
												) const
{
    ASSERT( values != 0 );
	if ( pointIdx >= getNumPoints( ) )
	{
		ACADOERROR( RET_INVALID_ARGUMENTS );
		return VariablesGrid();
	}

	VariablesGrid pointGrid;
	pointGrid.addMatrix( *(values[pointIdx]),getTime( pointIdx ) );

    return pointGrid;
}

returnValue PIDcontroller::step(	double currentTime,
									const Vector& _x,
									const Vector& _p,
									const VariablesGrid& _yRef
									)
{
	if ( getStatus( ) != BS_READY )
		return ACADOERROR( RET_BLOCK_NOT_READY );

	if ( _x.getDim( ) != getNumInputs( ) )
		return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );


	/* 1) Use reference trajectory if it is defined */
	// set default reference to zero
	Vector xRef( _x.getDim() );

	if ( _yRef.getNumPoints( ) > 0 )
	{
		if ( _yRef.getNumValues( ) != getNumInputs( ) )
			return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );

		xRef = _yRef.getVector( 0 );
	}
	else
	{
		xRef.setZero( );
	}


	/* 2) Determine PID control action. */
	if ( getNumOutputs( ) > 0 )
	{
		if ( determineControlAction( xRef-_x,u ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
	}
	else
		u.init();

	p = _p;


	/* 3) Call output transformator. */
	if ( clipSignals( u,p ) != SUCCESSFUL_RETURN )
		return ACADOERROR( RET_OUTPUTTRANSFORMATOR_STEP_FAILED );

	return SUCCESSFUL_RETURN;
}

returnValue Curve::discretize( const Grid &discretizationGrid, VariablesGrid &result ) const{

    uint        run1       ;
    returnValue returnvalue;
    Vector      tmp        ;

    result.init( dim, discretizationGrid );

    for( run1 = 0; run1 < discretizationGrid.getNumPoints(); run1++ ){
        returnvalue = evaluate( discretizationGrid.getTime(run1), tmp );
        if( returnvalue != SUCCESSFUL_RETURN )
            return returnvalue;
        result.setVector(run1,tmp);
    }
    return SUCCESSFUL_RETURN;
}