自学教程：C++ ACADOERROR函数代码示例

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 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 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 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;}