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