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 OptimizationAlgorithmBase::initializeAlgebraicStates( const char* fileName , BooleanType autoinit)
{
VariablesGrid tmp = fopen( fileName,"r" );
if ( tmp.isEmpty() == BT_TRUE )
return RET_FILE_CAN_NOT_BE_OPENED;
return initializeAlgebraicStates(tmp,autoinit);
}
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 Constraint::add( const ConstraintComponent& component ) {
Vector tmp_ub(grid.getNumPoints());
Vector tmp_lb(grid.getNumPoints());
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);
}
}
return add( tmp_lb, component.getExpression(), tmp_ub );
}
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;
}
returnValue Actuator::delayActuatorInput( VariablesGrid& _u,
VariablesGrid& _p
)
{
if ( hasDeadTime( ) == BT_FALSE )
{
// store last signal
Vector tmp = _u.getLastVector( );
if ( _p.isEmpty( ) == BT_FALSE )
tmp.append( _p.getLastVector( ) );
lastSignal.init( tmp );
lastSignal.setTime( 0,_u.getLastTime( ) );
return SUCCESSFUL_RETURN;
}
else
{
double startTime = _u.getFirstTime( );
double endTime = _u.getLastTime( );
// determine variables grid of delayed input
VariablesGrid uDelayed, pDelayed;
if ( getDelayedInputGrids( _u,_p, uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_DELAYING_INPUTS_FAILED );
// store last signal
lastSignal = uDelayed.getTimeSubGrid( uDelayed.getFloorIndex( endTime ),uDelayed.getLastIndex( ) );
if ( _p.isEmpty( ) == BT_FALSE )
lastSignal.appendValues( pDelayed.getTimeSubGrid( pDelayed.getFloorIndex( endTime ),pDelayed.getLastIndex( ) ) );
/* printf("u:\n");
_u.print();
printf("p:\n");
_p.print();
printf("uDelayed:\n");
uDelayed.print();
printf("pDelayed:\n");
pDelayed.print();*/
//
// printf("last:\n");
// lastSignal.print();
// crop delayed signal to current horizon
_u = uDelayed.getTimeSubGrid( uDelayed.getFloorIndex( startTime ),uDelayed.getFloorIndex( endTime ) );
if ( _p.isEmpty( ) == BT_FALSE )
_p = pDelayed.getTimeSubGrid( pDelayed.getFloorIndex( startTime ),pDelayed.getFloorIndex( endTime ) );
// printf("u:\n");
// _u.print();
// printf("p:\n");
// _p.print();
return SUCCESSFUL_RETURN;
}
}
returnValue Controller::initializeAlgebraicStates( const VariablesGrid& _xa_init
)
{
if ( controlLaw != 0 )
controlLaw->initializeAlgebraicStates( _xa_init.getVector(0) );
return SUCCESSFUL_RETURN;
}
VariablesGrid VariablesGrid::getTimeSubGrid( uint startIdx,
uint endIdx
) const
{
VariablesGrid newVariablesGrid;
if ( ( startIdx >= getNumPoints( ) ) || ( endIdx >= getNumPoints( ) ) )
return newVariablesGrid;
if ( startIdx > endIdx )
return newVariablesGrid;
for( uint i=startIdx; i<=endIdx; ++i )
newVariablesGrid.addMatrix( *(values[i]),getTime( i ) );
return newVariablesGrid;
}
VariablesGrid VariablesGrid::getValuesSubGrid( uint startIdx,
uint endIdx
) const
{
VariablesGrid newVariablesGrid;
if ( ( startIdx >= getNumValues( ) ) || ( endIdx >= getNumValues( ) ) )
return newVariablesGrid;
if ( startIdx > endIdx )
return newVariablesGrid;
for( uint i=0; i<getNumPoints( ); ++i )
newVariablesGrid.addMatrix( values[i]->getRows( startIdx,endIdx ),getTime( i ) );
return newVariablesGrid;
}
returnValue VariablesGrid::appendTimes( const VariablesGrid& arg,
MergeMethod _mergeMethod
)
{
// nothing to do for empty grids
if ( arg.getNumPoints( ) == 0 )
return SUCCESSFUL_RETURN;
if ( getNumPoints( ) == 0 )
{
*this = arg;
return SUCCESSFUL_RETURN;
}
// consistency check
if ( acadoIsGreater( arg.getFirstTime( ),getLastTime( ) ) == BT_FALSE )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( acadoIsEqual( getLastTime( ),arg.getFirstTime( ) ) == BT_FALSE )
{
// simply append
for( uint i=0; i<arg.getNumPoints( ); ++i )
addMatrix( *(arg.values[i]),arg.getTime( i ) );
}
else
{
// if last and first time point coincide, merge as specified
switch ( _mergeMethod )
{
case MM_KEEP:
break;
case MM_REPLACE:
setVector( getLastIndex(),arg.getFirstVector( ) );
break;
case MM_DUPLICATE:
addMatrix( *(arg.values[0]),arg.getTime( 0 ) );
break;
}
// simply append all remaining points
for( uint i=1; i<arg.getNumPoints( ); ++i )
addMatrix( *(arg.values[i]),arg.getTime( i ) );
}
return SUCCESSFUL_RETURN;
}
returnValue SimulationEnvironment::initializeAlgebraicStates( const VariablesGrid& _xa_init )
{
if ( controller != 0 )
controller->initializeAlgebraicStates( _xa_init );
if ( process != 0 )
process->initializeAlgebraicStates( _xa_init.getVector(0) );
return SUCCESSFUL_RETURN;
}
returnValue MultiObjectiveAlgorithm::evaluateObjectives( VariablesGrid &xd_ ,
VariablesGrid &xa_ ,
VariablesGrid &p_ ,
VariablesGrid &u_ ,
VariablesGrid &w_ ,
Expression **arg ){
int run1;
Grid tmp_grid;
ocp->getGrid( tmp_grid );
VariablesGrid *_xd = 0;
VariablesGrid *_xa = 0;
VariablesGrid *_p = 0;
VariablesGrid *_u = 0;
VariablesGrid *_w = 0;
if( xd_.isEmpty() == BT_FALSE ) _xd = new VariablesGrid(xd_);
if( xa_.isEmpty() == BT_FALSE ) _xa = new VariablesGrid(xa_);
if( p_.isEmpty() == BT_FALSE ) _p = new VariablesGrid(p_ );
if( u_.isEmpty() == BT_FALSE ) _u = new VariablesGrid(u_ );
if( w_.isEmpty() == BT_FALSE ) _w = new VariablesGrid(w_ );
Objective *obj;
for( run1 = 0; run1 < m; run1++ ){
obj = new Objective( tmp_grid );
obj->addMayerTerm(*arg[run1]);
OCPiterate xx( _xd, _xa, _p, _u, _w );
obj->evaluate( xx );
obj->getObjectiveValue( result(count,run1) );
delete obj;
}
count++;
if( _xd != 0 ) delete _xd;
if( _xa != 0 ) delete _xa;
if( _p != 0 ) delete _p ;
if( _u != 0 ) delete _u ;
if( _w != 0 ) delete _w ;
return SUCCESSFUL_RETURN;
}
returnValue GaussianNoise::step( VariablesGrid& _w
)
{
if ( getStatus( ) != BS_READY )
return ACADOERROR( RET_BLOCK_NOT_READY );
if ( getDim( ) != _w.getNumValues( ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
if ( w.getNumPoints( ) != _w.getNumPoints( ) )
w.init( getDim(),_w.getNumPoints( ) );
for( uint i=0; i<_w.getNumPoints( ); ++i )
for( uint j=0; j<getDim( ); ++j )
w(i,j) = getGaussianRandomNumber( mean(j),variance(j) );
_w = w;
return SUCCESSFUL_RETURN;
}
returnValue ClippingFunctionality::clipSignals( VariablesGrid& _u,
VariablesGrid& _p
)
{
// consistency checks
if ( _u.getNumValues( ) != getNumControlLimits( ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
if ( _p.getNumValues( ) != getNumParameterLimits( ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
// limit controls
for( uint i=0; i<getNumControlLimits( ); ++i )
{
for( uint j=0; j<_u.getNumPoints( ); ++j )
{
if ( _u( j,i ) < lowerLimitControls( i ) )
_u( j,i ) = lowerLimitControls( i );
if ( _u( j,i ) > upperLimitControls( i ) )
_u( j,i ) = upperLimitControls( i );
}
}
// limit parameters
for( uint i=0; i<getNumParameterLimits( ); ++i )
{
for( uint j=0; j<_p.getNumPoints( ); ++j )
{
if ( _p( j,i ) < lowerLimitParameters( i ) )
_p( j,i ) = lowerLimitParameters( i );
if ( _p( j,i ) > upperLimitParameters( i ) )
_p( j,i ) = upperLimitParameters( i );
}
}
return SUCCESSFUL_RETURN;
}
returnValue GnuplotWindow::obtainPlotDataString( VariablesGrid& _dataGrid,
std::string& _plotDataString
) const
{
stringstream ss;
_dataGrid.sprint( ss );
_plotDataString = ss.str();
return SUCCESSFUL_RETURN;
}
int main( ){
USING_NAMESPACE_ACADO
DifferentialState phi, omega; // the states of the pendulum
Parameter l, alpha ; // its length and the friction
const double g = 9.81 ; // the gravitational constant
DifferentialEquation f ; // the model equations
Function h ; // the measurement function
VariablesGrid measurements; // read the measurements
measurements = readFromFile( "data.txt" ); // from a file.
// --------------------------------------
OCP ocp(measurements.getTimePoints()); // construct an OCP
h << phi ; // the state phi is measured
ocp.minimizeLSQ( h, measurements ) ; // fit h to the data
f << dot(phi ) == omega ; // a symbolic implementation
f << dot(omega) == -(g/l) *sin(phi ) // of the model
- alpha*omega ; // equations
ocp.subjectTo( f ); // solve OCP s.t. the model,
ocp.subjectTo( 0.0 <= alpha <= 4.0 ); // the bounds on alpha
ocp.subjectTo( 0.0 <= l <= 2.0 ); // and the bounds on l.
// --------------------------------------
GnuplotWindow window;
window.addSubplot( phi , "The angle phi", "time [s]", "angle [rad]" );
window.addSubplot( omega, "The angular velocity dphi" );
window.addData( 0, measurements(0) );
ParameterEstimationAlgorithm algorithm(ocp); // the parameter estimation
algorithm << window;
algorithm.solve(); // algorithm solves the problem.
return 0;
}
returnValue PIDcontroller::init( double startTime,
const Vector &x0_,
const Vector &p_,
const VariablesGrid& _yRef
)
{
if ( x0_.getDim( ) != getNumInputs( ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
// Use reference trajectory if it is defined,
// otherwise set default reference to zero
Vector xRef( x0_.getDim() );
if ( _yRef.getNumPoints( ) > 0 )
{
if ( _yRef.getNumValues( ) != getNumInputs( ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
xRef = _yRef.getVector( 0 );
}
else
{
xRef.setZero( );
}
// initialize control and parameter signals
u.init( getNumOutputs() );
u.setZero( );
p = p_;
lastError = xRef - x0_;
setStatus( BS_READY );
return SUCCESSFUL_RETURN;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// DEFINE A VARIABLES GRID:
// ------------------------
Grid dataGrid( 0.0, 5.0, 6 );
VariablesGrid data;
data.init( 2, dataGrid );
data( 0, 0 ) = 0.0; data( 0, 1 ) = 1.0 ;
data( 1, 0 ) = 0.2; data( 1, 1 ) = 0.8 ;
data( 2, 0 ) = 0.4; data( 2, 1 ) = 0.7 ;
data( 3, 0 ) = 0.6; data( 3, 1 ) = 0.65 ;
data( 4, 0 ) = 0.8; data( 4, 1 ) = 0.625;
data( 5, 0 ) = 1.0; data( 5, 1 ) = 0.613;
// CONSTRUCT A CURVE INTERPOLATING THE DATA:
// -----------------------------------------
Curve c1, c2;
c1.add( data, IM_CONSTANT );
c2.add( data, IM_LINEAR );
// PLOT CURVES ON GIVEN GRID:
// --------------------------
GnuplotWindow window;
window.addSubplot( c1, 0.0,5.0, "Constant data Interpolation" );
window.addSubplot( c2, 0.0,5.0, "Linear data Interpolation" );
window.plot();
return 0;
}
