uint ModelData::addOutput( const std::string& output, const std::string& diffs_output, const uint dim,
const Grid& grid, const std::string& colInd, const std::string& rowPtr ){
DVector colIndV, rowPtrV;
colIndV.read( colInd.c_str() );
rowPtrV.read( rowPtr.c_str() );
if( rowPtrV.getDim() != (dim+1) ) {
return ACADOERROR( RET_INVALID_OPTION );
}
colInd_outputs.push_back( colIndV );
rowPtr_outputs.push_back( rowPtrV );
return addOutput( output, diffs_output, dim, grid );
}
returnValue ModelData::setModel( const std::string& fileName, const std::string& _rhs_ODE, const std::string& _diffs_ODE )
{
if( differentialEquation.getNumDynamicEquations() == 0 )
{
externModel = fileName;
rhs_name = _rhs_ODE;
diffs_name = _diffs_ODE;
export_rhs = BT_FALSE;
}
else
{
return ACADOERROR( RET_INVALID_OPTION );
}
return SUCCESSFUL_RETURN;
}
returnValue LogRecord::setLast( uint _name,
LogRecordItemType _type,
const Matrix& value,
double time
)
{
LogRecordItem* item = find( _name,_type );
// checks if item exists
if ( ( item != 0 ) && ( item->isWriteProtected( ) == BT_FALSE ) )
{
if ( item->setValue( frequency,value,time ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_LOG_RECORD_CORRUPTED );
}
return SUCCESSFUL_RETURN;
}
returnValue QPsolver_qpOASES::solve( double* H,
double* A,
double* g,
double* lb,
double* ub,
double* lbA,
double* ubA,
uint maxIter
)
{
if ( qp == 0 )
return ACADOERROR( RET_INITIALIZE_FIRST );
/* call to qpOASES, using hotstart if possible and desired */
numberOfSteps = maxIter;
qpOASES::returnValue returnvalue;
qpStatus = QPS_SOLVING;
//printf( "nV: %d, nC: %d \n",qp->getNV(),qp->getNC() );
if ( (bool)qp->isInitialised( ) == false )
{
returnvalue = qp->init( H,g,A,lb,ub,lbA,ubA,numberOfSteps,0 );
}
else
{
int performHotstart = 0;
get( HOTSTART_QP,performHotstart );
if ( (bool)performHotstart == true )
{
returnvalue = qp->hotstart( H,g,A,lb,ub,lbA,ubA,numberOfSteps,0 );
}
else
{
/* if no hotstart is desired, reset QP and use cold start */
qp->reset( );
returnvalue = qp->init( H,g,A,lb,ub,lbA,ubA,numberOfSteps,0 );
}
}
setLast( LOG_NUM_QP_ITERATIONS, numberOfSteps );
/* update QP status and determine return value */
return updateQPstatus( returnvalue );
}
returnValue ModelData::setMeasurements( const Vector& numberMeasurements ){
int i;
if( outputExpressions.size() != numberMeasurements.getDim() && outputNames.size() != numberMeasurements.getDim() ) {
return ACADOERROR( RET_INVALID_OPTION );
}
outputGrids.clear();
num_meas.clear();
for( i = 0; i < (int)numberMeasurements.getDim(); i++ ) {
Grid nextGrid( 0.0, 1.0, (int)numberMeasurements(i) + 1 );
outputGrids.push_back( nextGrid );
uint numOuts = (int) ceil((double)outputGrids[i].getNumIntervals()/((double) N) - 10.0*EPS);
num_meas.push_back( numOuts );
}
return SUCCESSFUL_RETURN;
}
returnValue Integrator::integrate( const Grid &t_,
double *x0,
double *xa,
double *p ,
double *u ,
double *w ){
if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
Vector components = rhs->getDifferentialStateComponents();
Vector tmpX ( components.getDim(), x0 );
Vector tmpXA( rhs->getNXA() , xa );
Vector tmpP ( rhs->getNP () , p );
Vector tmpU ( rhs->getNU () , u );
Vector tmpW ( rhs->getNW () , w );
return integrate( t_, tmpX, tmpXA, tmpP, tmpU, tmpW );
}
returnValue Actuator::init( double _startTime,
const Vector& _startValueU,
const Vector& _startValueP
)
{
Vector tmp;
if ( _startValueU.isEmpty( ) == BT_FALSE )
tmp.append( _startValueU );
if ( _startValueP.isEmpty( ) == BT_FALSE )
tmp.append( _startValueP );
if ( TransferDevice::init( _startTime,tmp ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_ACTUATOR_INIT_FAILED );
return SUCCESSFUL_RETURN;
}
uint ModelData::addOutput( const std::string& output, const std::string& diffs_output, const uint dim, const Grid& grid ){
if( outputExpressions.size() == 0 && differentialEquation.getNumDynamicEquations() == 0 ) {
outputNames.push_back( output );
diffs_outputNames.push_back( diffs_output );
dim_outputs.push_back( dim );
outputGrids.push_back( grid );
uint numOuts = (int) ceil((double)grid.getNumIntervals());
num_meas.push_back( numOuts );
}
else {
return ACADOERROR( RET_INVALID_OPTION );
}
return dim_outputs.size();
}
returnValue Sensor::setOutputNoise( const Noise& _noise,
double _noiseSamplingTime
)
{
if ( _noise.getDim( ) != getNY( ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
for( uint i=0; i<getNY( ); ++i )
{
if ( additiveNoise[i] != 0 )
delete additiveNoise[i];
additiveNoise[i] = _noise.clone( i );
}
noiseSamplingTimes.setAll( _noiseSamplingTime );
return SUCCESSFUL_RETURN;
}
returnValue Integrator::diffTransitionForward( Vector &DX,
const Vector &DP,
const Vector &DU,
const Vector &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;
}
returnValue PlotWindow::getDataGrids( const VariablesGrid* const variablesGrid,
VariableType& _type,
VariablesGrid& _dataGrid,
Grid& _discretizationGrid
)
{
_dataGrid.init();
_discretizationGrid.init( );
_type = variablesGrid->getType( );
InterpolationMode mode = IM_LINEAR;
if ( ( _type == VT_CONTROL ) || ( _type == VT_PARAMETER ) )
mode = IM_CONSTANT;
switch ( mode )
{
case IM_CONSTANT:
_discretizationGrid.addTime( 0.0 );
for( uint i=0; i<variablesGrid->getNumPoints( )-1; ++i )
{
_dataGrid.addVector( variablesGrid->getVector(i),variablesGrid->getTime(i) );
_dataGrid.addVector( variablesGrid->getVector(i),variablesGrid->getTime(i+1) );
_discretizationGrid.addTime( (double)_dataGrid.getNumPoints() );
}
_dataGrid.addVector( variablesGrid->getLastVector(),variablesGrid->getLastTime() );
_discretizationGrid.addTime( (double)_dataGrid.getNumPoints() );
break;
case IM_LINEAR:
_dataGrid = *variablesGrid;
break;
default:
return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
}
_dataGrid.setType( _type );
return SUCCESSFUL_RETURN;
}
VariablesGrid VariablesGrid::operator()( const uint rowIdx
) const
{
ASSERT( values != 0 );
if ( rowIdx >= getNumRows( ) )
{
ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
return VariablesGrid();
}
Grid tmpGrid;
getGrid( tmpGrid );
VariablesGrid rowGrid( 1,tmpGrid,getType( ) );
for( uint run1 = 0; run1 < getNumPoints(); run1++ )
rowGrid( run1,0 ) = values[run1]->operator()( rowIdx,0 );
return rowGrid;
}
returnValue SCPmethod::getAnySensitivities( BlockMatrix& _sens,
uint idx
) const
{
if ( idx > 4 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
uint N = bandedCP.dynGradient.getNumRows();
Matrix tmp;
_sens.init( N,1 );
for( uint i=0; i<N; ++i )
{
bandedCP.dynGradient.getSubBlock( i,idx,tmp );
_sens.setDense( i,0,tmp );
}
return SUCCESSFUL_RETURN;
}
returnValue CFunction::AD_backward( int number, double *seed, double *df ){
uint run1;
ASSERT( number < (int) maxAlloc );
if( cFcnDBackward != NULL ){
double *f = new double[dim];
cFcnDBackward( number, xStore[number], seed, f, df, user_data );
for( run1 = 0; run1 < nn; run1++ )
seedStore[number][run1] = seed[run1];
delete[] f;
return SUCCESSFUL_RETURN;
}
return ACADOERROR(RET_INVALID_USE_OF_FUNCTION);
}
returnValue Sensor::setOutputNoise( uint idx,
const Noise& _noise,
double _noiseSamplingTime
)
{
if ( ( idx >= getNY( ) ) || ( _noise.getDim( ) != 1 ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( additiveNoise[idx] != 0 )
delete additiveNoise[idx];
additiveNoise[idx] = _noise.clone( );
if ( ( idx > 0 ) && ( acadoIsEqual( _noiseSamplingTime, noiseSamplingTimes(0) ) == BT_FALSE ) )
ACADOWARNING( RET_NO_DIFFERENT_NOISE_SAMPLING_FOR_DISCRETE );
noiseSamplingTimes.setAll( _noiseSamplingTime ); // should be changed later
return SUCCESSFUL_RETURN;
}
returnValue Integrator::diffTransitionBackward( Vector &DX,
Vector &DP,
Vector &DU,
Vector &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;
}
returnValue Objective::evaluateSensitivitiesGN( BlockMatrix &hessian ){
returnValue returnvalue;
uint run1;
hessian.setZero();
if( nMayer != 0 )
return ACADOERROR(RET_GAUSS_NEWTON_APPROXIMATION_NOT_SUPPORTED);
for( run1 = 0; run1 < nLSQ; run1++ ){
returnvalue = lsqTerm[run1]->evaluateSensitivitiesGN( &hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
}
for( run1 = 0; run1 < nEndLSQ; run1++ ){
returnvalue = lsqEndTerm[run1]->evaluateSensitivitiesGN( &hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
}
return SUCCESSFUL_RETURN;
}
returnValue VectorspaceElement::printToFile( const char* const filename,
const char* const name,
const char* const startString,
const char* const endString,
uint width,
uint precision,
const char* const colSeparator,
const char* const rowSeparator
) const
{
FILE* file = fopen( filename,"w+" );
if ( file == 0 )
return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
printToFile( file, name,startString,endString,width,precision,colSeparator,rowSeparator );
fclose( file );
return SUCCESSFUL_RETURN;
}
returnValue DenseQPsolver::solveCP( DenseCP *cp )
{
ASSERT( cp != 0 );
if( cp->isQP() == BT_FALSE )
return ACADOERROR( RET_QP_SOLVER_CAN_ONLY_SOLVE_QP );
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// QUICK HACK: SOLVE CALL SHOULD AVOID PASSING THE MAX-ITER ARGUMENT !!!
const uint maxIter = 1000;
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
returnValue returnvalue;
returnvalue = solve( &cp->H, &cp->A, &cp->g, &cp->lb, &cp->ub, &cp->lbA, &cp->ubA, maxIter );
if( returnvalue != SUCCESSFUL_RETURN )
return returnvalue;
// GET THE PRIMAL AND DUAL SOLUTION FROM THE QP SOLVER AND
// STORE THEM IN THE RIGHT FORMAT:
// -------------------------------------------------------
Vector xOpt, yOpt;
getPrimalSolution( xOpt );
getDualSolution ( yOpt );
// printf( "DeltaU0 = [ %e, %e ]\n", xOpt(4+0),xOpt(4+1) );
// cp->lb.print("lb");
cp->setQPsolution( xOpt, yOpt );
return SUCCESSFUL_RETURN;
}
returnValue ModelData::setModel( const DifferentialEquation& _f )
{
if( rhs_name.isEmpty() && NX2 == 0 ) {
differentialEquation = _f;
Expression rhs;
differentialEquation.getExpression( rhs );
NX2 = rhs.getDim() - differentialEquation.getNXA();
if( NDX == 0 ) NDX = differentialEquation.getNDX();
NXA = differentialEquation.getNXA();
if( NU == 0 ) NU = differentialEquation.getNU();
if( NP == 0 ) NP = differentialEquation.getNP();
model_dimensions_set = BT_TRUE;
export_rhs = BT_TRUE;
}
else {
return ACADOERROR( RET_INVALID_OPTION );
}
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::getFirstControl( Vector& u0_ ) const
{
#ifdef SIM_DEBUG
acadoPrintf( "SCPmethod::getFirstControl\n" );
#endif
if( iter.u == 0 )
return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
u0_ = iter.u->getVector( 0 );
if ( hasPerformedStep == BT_FALSE )
{
Vector deltaU0( getNU() );
bandedCPsolver->getFirstControl( deltaU0 );
u0_ += deltaU0;
}
return SUCCESSFUL_RETURN;
}
returnValue OCP::subjectTo( const TimeHorizonElement index_, const ConstraintComponent& component ){
uint i;
switch( index_ ){
case AT_START:
for( i = 0; i < component.getDim(); i++ )
ACADO_TRY( constraint.add( 0,component(i) ) );
return SUCCESSFUL_RETURN;
case AT_END:
for( i = 0; i < component.getDim(); i++ )
ACADO_TRY( constraint.add( grid.getLastIndex(),component(i) ) );
return SUCCESSFUL_RETURN;
default:
return ACADOERROR(RET_UNKNOWN_BUG);
}
return SUCCESSFUL_RETURN;
}
OCP::OCP( const double &tStart_, const double &tEnd_, const Vector& _numSteps )
:MultiObjectiveFunctionality(){
if( _numSteps.getDim() <= 0 ) ACADOERROR( RET_INVALID_ARGUMENTS );
Vector 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);
}
returnValue PointConstraint::add( const double lb_, const Expression& arg, const double ub_ ){
if( fcn == 0 )
return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
// CHECK FOR A SIMPLE BOUND:
// -------------------------
VariableType varType = arg.getVariableType( );
int component = arg.getComponent (0);
if( arg.isVariable() == BT_TRUE ){
if( varType != VT_INTERMEDIATE_STATE ){
nb++;
var = (VariableType*)realloc(var , nb*sizeof(VariableType));
index = (int* )realloc(index, nb*sizeof(int ));
blb = (double* )realloc(blb , nb*sizeof(double ));
bub = (double* )realloc(bub , nb*sizeof(double ));
var [nb-1] = varType ;
index[nb-1] = component;
blb [nb-1] = lb_ ;
bub [nb-1] = ub_ ;
}
}
// ADD THE ARGUMENT TO THE FUNCTION TO BE EVALUATED:
// -------------------------------------------------
fcn[0] << arg;
lb[0] = (double*)realloc(lb[0],fcn[0].getDim()*sizeof(double));
ub[0] = (double*)realloc(ub[0],fcn[0].getDim()*sizeof(double));
ub[0][fcn[0].getDim()-1] = ub_;
lb[0][fcn[0].getDim()-1] = lb_;
return SUCCESSFUL_RETURN;
}
returnValue TreeProjection::loadIndices( SymbolicIndexList *indexList ){
returnValue returnvalue = SUCCESSFUL_RETURN;
if( argument == NULL ){
ACADOERROR(RET_INTERMEDIATE_STATE_HAS_NO_ARGUMENT);
ASSERT( 1 == 0 );
}
if( indexList->addNewElement( VT_INTERMEDIATE_STATE, vIndex ) == BT_TRUE ){
returnvalue = argument->loadIndices( indexList );
indexList->addOperatorPointer( argument, vIndex );
}
if( name.isEmpty() == BT_TRUE ){
name = "acadoWorkspace.acado_aux";
}
return returnvalue;
}
returnValue VariablesGrid::appendValues( const VariablesGrid& arg )
{
// setup new grid if current grid is empty
if ( getNumPoints( ) == 0 )
{
*this = arg;
return SUCCESSFUL_RETURN;
}
if ( getNumPoints( ) != arg.getNumPoints( ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
Vector tmp1,tmp2;
for( uint i=0; i<getNumPoints(); ++i )
{
values[i]->appendRows( *(arg.values[i]) );
values[i]->appendSettings( *(arg.values[i]) );
}
return SUCCESSFUL_RETURN;
}
returnValue Plotting::replot( PlotFrequency _frequency
)
{
if ( _frequency == PLOT_NEVER )
return SUCCESSFUL_RETURN;
PlotWindow* window = plotCollection.first;
while ( window != 0 )
{
// update plot data and ...
getPlotWindow( *window );
// ... plot the window
if ( window->replot( _frequency ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_PLOTTING_FAILED );
window = window->getNext( );
}
return SUCCESSFUL_RETURN;
}
returnValue PlotWindow::getExpressionDataGrids( const Expression* const expression,
VariableType& _type,
VariablesGrid& _dataGrid,
Grid& _discretizationGrid
)
{
OutputFcn f;
VariablesGrid loggedX,loggedXA,loggedP,loggedU,loggedW;
_type = expression->getVariableType( );
f << *expression;
plotDataRecord.getLast( LOG_DIFFERENTIAL_STATES,loggedX );
plotDataRecord.getLast( LOG_ALGEBRAIC_STATES,loggedXA );
plotDataRecord.getLast( LOG_PARAMETERS,loggedP );
plotDataRecord.getLast( LOG_CONTROLS,loggedU );
plotDataRecord.getLast( LOG_DISTURBANCES,loggedW );
if ( loggedP.isEmpty() == BT_FALSE )
loggedP.refineGrid( loggedX );
if ( loggedU.isEmpty() == BT_FALSE )
loggedU.refineGrid( loggedX );
if ( loggedW.isEmpty() == BT_FALSE )
loggedW.refineGrid( loggedX );
returnValue returnvalue = f.evaluate( &loggedX,&loggedXA,&loggedP,&loggedU,&loggedW, &_dataGrid );
if( returnvalue != SUCCESSFUL_RETURN )
return ACADOERROR( returnvalue );
VariablesGrid tmp;
plotDataRecord.getLast( LOG_DISCRETIZATION_INTERVALS,tmp );
tmp.getGrid( _discretizationGrid );
return SUCCESSFUL_RETURN;
}
inline returnValue PointConstraint::computeForwardSensitivityBlock( int offset, int offset2, DMatrix *seed ){
if( seed == 0 ) return SUCCESSFUL_RETURN;
int run1, run2;
returnValue returnvalue;
const int nc = fcn[0].getDim();
double* dresult1 = new double[nc ];
double* fseed1 = new double[fcn[0].getNumberOfVariables()+1];
DMatrix tmp( nc, seed->getNumCols() );
for( run1 = 0; run1 < (int) seed->getNumCols(); run1++ ){
for( run2 = 0; run2 <= fcn[0].getNumberOfVariables(); run2++ )
fseed1[run2] = 0.0;
for( run2 = 0; run2 < (int) seed->getNumRows(); run2++ )
fseed1[y_index[0][offset+run2]] = seed->operator()(run2,run1);
returnvalue = fcn[0].AD_forward( 0, fseed1, dresult1 );
if( returnvalue != SUCCESSFUL_RETURN ){
delete[] dresult1;
delete[] fseed1 ;
return ACADOERROR(returnvalue);
}
for( run2 = 0; run2 < nc; run2++ )
tmp( run2, run1 ) = dresult1[run2];
}
dForward.setDense( 0, offset2, tmp );
delete[] dresult1;
delete[] fseed1 ;
return SUCCESSFUL_RETURN;
}
returnValue VariablesGrid::getIntegral( InterpolationMode mode,
Vector& value
) const
{
value.setZero();
switch( mode )
{
case IM_CONSTANT:
for( uint i=0; i<getNumIntervals( ); ++i )
{
for( uint j=0; j<getNumValues( ); ++j )
{
//value(j) += ( getIntervalLength( i ) / getIntervalLength( ) ) * operator()( i,j );
value(j) += getIntervalLength( i ) * operator()( i,j );
}
}
break;
case IM_LINEAR:
for( uint i=0; i<getNumIntervals( ); ++i )
{
for( uint j=0; j<getNumValues( ); ++j )
{
//value(j) += ( getIntervalLength( i ) / getIntervalLength( ) ) * ( operator()( i,j ) + operator()( i+1,j ) ) / 2.0;
value(j) += getIntervalLength( i ) * ( operator()( i,j ) + operator()( i+1,j ) ) / 2.0;
}
}
break;
default:
return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
}
return SUCCESSFUL_RETURN;
}
