returnValue allocateDoublePointerFromFile( FILE *file , double **x, int &nRows, int &nCols ){
if( !file ) return ACADOERROR(RET_FILE_CAN_NOT_BE_OPENED);
int nEntries ;
int maxEntries ;
int checkEntry ;
char *myChar ;
returnValue returnvalue;
int offset ;
if( x == 0 ) return ACADOERROR(RET_INVALID_ARGUMENTS);
*x = 0;
nRows = 0 ;
nCols = -1 ;
returnvalue = SUCCESSFUL_RETURN;
while( !feof(file) ){
nRows++;
nEntries = 0;
maxEntries = 1;
myChar = (char*)calloc(maxEntries,sizeof(char));
while( fscanf(file,"%c",&myChar[nEntries]) != EOF ){
if( myChar[nEntries] == LINE_SEPARATOR ) break;
nEntries++;
if( nEntries >= maxEntries ){
maxEntries += maxEntries;
myChar = (char*)realloc(myChar,maxEntries*sizeof(char));
}
}
if( nCols == -1 ) offset = 0;
else offset = (nRows-1)*nCols;
checkEntry = AcadoIoReadLine( myChar, nEntries, offset, x );
free(myChar);
if( checkEntry > 0 ){
if( nCols == -1 ) nCols = checkEntry;
if( nCols != checkEntry ) return ACADOERROR(RET_FILE_HAS_NO_VALID_ENTRIES);
}
else{
nRows--;
}
}
if( nRows <= 0 || nCols <= 0 ) return ACADOWARNING(RET_FILE_HAS_NO_VALID_ENTRIES);
if( fclose(file) != 0 ) return ACADOWARNING(RET_FILE_CAN_NOT_BE_CLOSED);
if( returnvalue != SUCCESSFUL_RETURN ) ACADOWARNING(returnvalue);
return SUCCESSFUL_RETURN;
}
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 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 allocateDoublePointerFromFile( FILE *file, double **x, int &dim ){
if( !file ) return ACADOERROR(RET_FILE_CAN_NOT_BE_OPENED);
int nEntries = 0;
int maxEntries = 1;
if( x == 0 ) return ACADOERROR(RET_INVALID_ARGUMENTS);
*x = 0;
char *myChar = (char*)calloc(maxEntries,sizeof(char));
while( fscanf(file,"%c",&myChar[nEntries]) != EOF ){
nEntries++;
if( nEntries >= maxEntries ){
maxEntries += maxEntries;
myChar = (char*)realloc(myChar,maxEntries*sizeof(char));
}
}
returnValue returnvalue = SUCCESSFUL_RETURN ;
if( fclose(file) != 0 ) returnvalue = RET_FILE_CAN_NOT_BE_CLOSED;
dim = AcadoIoReadLine( myChar, nEntries, 0, x );
free(myChar);
if( dim <= 0 ) returnvalue = RET_FILE_HAS_NO_VALID_ENTRIES;
if( returnvalue != SUCCESSFUL_RETURN ) ACADOWARNING(returnvalue);
return SUCCESSFUL_RETURN;
}
DifferentialEquation& DifferentialEquation::addAlgebraic( const double &arg ){
det = DET_DAE;
if( fabs(arg) < EPS ) return *this;
ACADOWARNING( RET_INFEASIBLE_ALGEBRAIC_CONSTRAINT );
return *this;
}
returnValue ObjectiveElement::setBackwardSeed( BlockMatrix *seed, int order ){
if( order == 1 ){
if( seed != 0 ){
if( bSeed != 0 ) delete bSeed;
bSeed = new BlockMatrix(*seed);
}
else{
if( bSeed != 0 ) delete bSeed;
bSeed = 0;
}
return SUCCESSFUL_RETURN;
}
if( order == 2 ){
if( seed != 0 ){
if( bSeed2 != 0 ) delete bSeed2;
bSeed2 = new BlockMatrix(*seed);
}
else{
if( bSeed2 != 0 ) delete bSeed2;
bSeed2 = 0;
}
return SUCCESSFUL_RETURN;
}
return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
}
DifferentialEquation& DifferentialEquation::operator<<( const int& arg ){
if( arg != 0 ){
ACADOWARNING(RET_INVALID_USE_OF_FUNCTION);
return *this;
}
det = DET_DAE;
return *this;
}
returnValue Constraint::getBounds( const OCPiterate& iter ) {
returnValue returnvalue;
returnvalue = BoxConstraint::getBounds( iter );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOWARNING(returnvalue);
if( point_constraints != 0 )
if( point_constraints[0] != 0 )
returnvalue = point_constraints[0]->getBounds( iter );
return returnvalue;
}
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;
}
returnValue ObjectiveElement::getBackwardSensitivities( BlockMatrix *D, int order ){
ASSERT( D != 0 );
if( order == 1 ){
D[0] = dBackward;
return SUCCESSFUL_RETURN;
}
if( order == 2 ){
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
}
returnValue CondensingExport::setLevenbergMarquardt( double _levenbergMarquardt
)
{
if ( _levenbergMarquardt < 0.0 )
{
ACADOWARNING( RET_INVALID_ARGUMENTS );
levenbergMarquardt = 0.0;
}
else
{
levenbergMarquardt = _levenbergMarquardt;
}
return SUCCESSFUL_RETURN;
}
returnValue OptimizationAlgorithmBase::getDisturbances( const char* fileName ) const{
returnValue returnvalue;
if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
VariablesGrid xx;
returnvalue = nlpSolver->getDisturbances( xx );
if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
FILE *file = fopen(fileName,"w");
file << xx;
fclose(file);
return SUCCESSFUL_RETURN;
}
returnValue Constraint::getBounds( const OCPiterate& iter ){
returnValue returnvalue;
returnvalue = BoxConstraint::getBounds( iter );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOWARNING(returnvalue);
if( point_constraints != 0 )
{
for( uint i=0; i<grid.getNumPoints(); ++i )
if( point_constraints[i] != 0 )
returnvalue = point_constraints[i]->getBounds( iter );
}
return returnvalue;
}
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;
}
double SimulationEnvironment::determineComputationalDelay( double controllerRuntime
) const
{
int simulateComputationalDelay;
get( SIMULATE_COMPUTATIONAL_DELAY,simulateComputationalDelay );
if ( (BooleanType)simulateComputationalDelay == BT_TRUE )
{
ACADOWARNING( RET_COMPUTATIONAL_DELAY_NOT_SUPPORTED );
return 0.0;
double computationalDelayFactor, computationalDelayOffset;
get( COMPUTATIONAL_DELAY_FACTOR,computationalDelayFactor );
get( COMPUTATIONAL_DELAY_OFFSET,computationalDelayOffset );
return acadoMax( 0.0, controllerRuntime*computationalDelayFactor + computationalDelayOffset );
}
else
return 0.0;
}
returnValue OptimizationAlgorithmBase::getAlgebraicStates( VariablesGrid &xa_ ) const{
if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
return nlpSolver->getAlgebraicStates( xa_ );
}
returnValue ObjectiveElement::setForwardSeed( BlockMatrix *xSeed_,
BlockMatrix *xaSeed_,
BlockMatrix *pSeed_,
BlockMatrix *uSeed_,
BlockMatrix *wSeed_,
int order ){
if( order == 1 ){
if( xSeed_ != 0 ){
if( xSeed != 0 ) delete xSeed;
xSeed = new BlockMatrix(*xSeed_);
}
else{
if( xSeed != 0 ) delete xSeed;
xSeed = 0;
}
if( xaSeed_ != 0 ){
if( xaSeed != 0 ) delete xaSeed;
xaSeed = new BlockMatrix(*xaSeed_);
}
else{
if( xaSeed != 0 ) delete xaSeed;
xaSeed = 0;
}
if( pSeed_ != 0 ){
if( pSeed != 0 ) delete pSeed;
pSeed = new BlockMatrix(*pSeed_);
}
else{
if( pSeed != 0 ) delete pSeed;
pSeed = 0;
}
if( uSeed_ != 0 ){
if( uSeed != 0 ) delete uSeed;
uSeed = new BlockMatrix(*uSeed_);
}
else{
if( uSeed != 0 ) delete uSeed;
uSeed = 0;
}
if( wSeed_ != 0 ){
if( wSeed != 0 ) delete wSeed;
wSeed = new BlockMatrix(*wSeed_);
}
else{
if( wSeed != 0 ) delete wSeed;
wSeed = 0;
}
return SUCCESSFUL_RETURN;
}
if( order == 2 ){
if( xSeed_ != 0 ){
if( xSeed2 != 0 ) delete xSeed2;
xSeed2 = new BlockMatrix(*xSeed_);
}
else{
if( xSeed2 != 0 ) delete xSeed2;
xSeed2 = 0;
}
if( xaSeed_ != 0 ){
if( xaSeed2 != 0 ) delete xaSeed2;
xaSeed2 = new BlockMatrix(*xaSeed_);
}
else{
if( xaSeed2 != 0 ) delete xaSeed2;
xaSeed2 = 0;
}
if( pSeed_ != 0 ){
if( pSeed2 != 0 ) delete pSeed2;
pSeed2 = new BlockMatrix(*pSeed_);
}
else{
if( pSeed2 != 0 ) delete pSeed2;
pSeed2 = 0;
}
if( uSeed_ != 0 ){
if( uSeed2 != 0 ) delete uSeed2;
uSeed2 = new BlockMatrix(*uSeed_);
}
else{
if( uSeed2 != 0 ) delete uSeed2;
uSeed2 = 0;
}
if( wSeed_ != 0 ){
if( wSeed2 != 0 ) delete wSeed2;
wSeed2 = new BlockMatrix(*wSeed_);
}
else{
if( wSeed2 != 0 ) delete wSeed2;
wSeed2 = 0;
}
return SUCCESSFUL_RETURN;
}
return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
}
returnValue OptimizationAlgorithmBase::getParameters( VariablesGrid &p_ ) const
{
if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
return nlpSolver->getParameters( p_ );
}
returnValue OptimizationAlgorithmBase::getDisturbances( VariablesGrid &w_ ) const{
if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
return nlpSolver->getDisturbances( w_ );
}
double OptimizationAlgorithmBase::getObjectiveValue() const{
if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
return nlpSolver->getObjectiveValue();
}
returnValue ConjugateGradientMethod::solve( double *b ){
// CONSISTENCY CHECKS:
// -------------------
if( dim <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
if( nDense <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
if( A == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
int run1, run2;
double *aux = new double[dim];
double auxR, auxR2, norm1;
double alpha, beta;
// INITIALISE X:
// --------------
for( run1 = 0; run1 < dim; run1++ )
x[run1] = 0.0;
// RESET THE COUNTER IF TOO LARGE:
// -------------------------------
if( pCounter > dim ) pCounter = dim-1;
// APPLY THE PRECONDITIONER ON b:
// ------------------------------
applyPreconditioner( b );
// COMPUTE WARM START INITIALIZATION FOR X BASED ON PREVIOUS
// DECOMPOSITIONS:
// ----------------------------------------------------------
for( run1 = 0; run1 < pCounter; run1++ ){
alpha = scalarProduct( p[run1], r )/norm2[run1];
for( run2 = 0; run2 < dim; run2++ ){
x[run2] += alpha*p[run1][run2];
}
}
// COMPUTE INITIAL RESIDUUM:
// -------------------------
multiply( x, aux );
for( run1 = 0; run1 < dim; run1++ )
r[run1] -= aux[run1];
for( run1 = 0; run1 < dim; run1++ )
p[pCounter][run1] = r[run1];
while( pCounter <= 2*dim-1 ){
norm1 = 0.0;
for( run1 = 0; run1 < dim; run1++ ){
if( r[run1] >= 0 && norm1 < r[run1] ) norm1 = r[run1];
if( r[run1] <= 0 && norm1 < -r[run1] ) norm1 = -r[run1];
}
if( printLevel == HIGH )
acadoPrintf("STEP NUMBER %d, WEIGHTED NORM = %.16e \n", pCounter, norm1 );
if( norm1 < TOL ) break;
auxR = scalarProduct( r,r );
multiply( p[pCounter], aux );
norm2[pCounter] = scalarProduct( p[pCounter], aux );
alpha = auxR/norm2[pCounter];
for( run1 = 0; run1 < dim; run1++ ){
x [run1] += alpha*p[pCounter][run1];
r [run1] -= alpha*aux[run1];
}
auxR2 = scalarProduct( r,r );
beta = auxR2/auxR;
for( run1 = 0; run1 < dim; run1++ )
p[pCounter+1][run1] = r[run1] + beta*p[pCounter][run1];
pCounter++;
}
delete[] aux ;
if( pCounter >= 2*dim ){
if( printLevel == MEDIUM || printLevel == HIGH )
return ACADOWARNING( RET_LINEAR_SYSTEM_NUMERICALLY_SINGULAR );
else return RET_LINEAR_SYSTEM_NUMERICALLY_SINGULAR;
}
return SUCCESSFUL_RETURN;
}
returnValue LSQEndTerm::evaluateSensitivitiesGN( BlockMatrix *GNhessian ){
int run2, run3, run4;
const int N = grid.getNumPoints();
const int nh = fcn.getDim();
if( bSeed != 0 ){
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ||
xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
double *bseed = new double [nh];
double **J = new double*[nh];
for( run2 = 0; run2 < nh; run2++ )
J[run2] = new double[fcn.getNumberOfVariables() +1];
if( bSeed->getNumRows( 0, 0 ) != 1 ) return ACADOWARNING( RET_WRONG_DEFINITION_OF_SEEDS );
Matrix bseed_;
bSeed->getSubBlock( 0, 0, bseed_);
dBackward.init( 1, 5*N );
Matrix Dx ( 1, nx );
Matrix Dxa( 1, na );
Matrix Dp ( 1, np );
Matrix Du ( 1, nu );
Matrix Dw ( 1, nw );
Dx .setZero();
Dxa.setZero();
Dp .setZero();
Du .setZero();
Dw .setZero();
for( run2 = 0; run2 < nh; run2++ ) bseed[run2] = 0;
for( run2 = 0; run2 < nh; run2++ ){
for(run3 = 0; (int) run3 < fcn.getNumberOfVariables() +1; run3++ )
J[run2][run3] = 0.0;
bseed[run2] = 1.0;
fcn.AD_backward( 0, bseed, J[run2] );
bseed[run2] = 0.0;
for( run3 = 0; run3 < nx; run3++ ){
Dx( 0, run3 ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx; run3 < nx+na; run3++ ){
Dxa( 0, run3-nx ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx+na; run3 < nx+na+np; run3++ ){
Dp( 0, run3-nx-na ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx+na+np; run3 < nx+na+np+nu; run3++ ){
Du( 0, run3-nx-na-np ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx+na+np+nu; run3 < nx+na+np+nu+nw; run3++ ){
Dw( 0, run3-nx-na-np-nu ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
}
if( nx > 0 ) dBackward.setDense( 0, N-1, Dx );
if( na > 0 ) dBackward.setDense( 0, 2*N-1, Dxa );
if( np > 0 ) dBackward.setDense( 0, 3*N-1, Dp );
if( nu > 0 ) dBackward.setDense( 0, 4*N-1, Du );
if( nw > 0 ) dBackward.setDense( 0, 5*N-1, Dw );
// COMPUTE GAUSS-NEWTON HESSIAN APPROXIMATION IF REQUESTED:
// --------------------------------------------------------
if( GNhessian != 0 ){
const int nnn = nx+na+np+nu+nw;
Matrix tmp( nh, nnn );
for( run3 = 0; run3 < nnn; run3++ ){
for( run2 = 0; run2 < nh; run2++ ){
tmp( run2, run3 ) = 0.0;
for( run4 = 0; run4 < nh; run4++ ){
tmp( run2, run3 ) += S.operator()(run2,run4)*J[run4][y_index[run3]];
}
}
}
Matrix tmp2;
int i,j;
int *Sidx = new int[6];
int *Hidx = new int[5];
Sidx[0] = 0;
Sidx[1] = nx;
Sidx[2] = nx+na;
Sidx[3] = nx+na+np;
Sidx[4] = nx+na+np+nu;
Sidx[5] = nx+na+np+nu+nw;
Hidx[0] = N-1;
Hidx[1] = 2*N-1;
Hidx[2] = 3*N-1;
Hidx[3] = 4*N-1;
Hidx[4] = 5*N-1;
for( i = 0; i < 5; i++ ){
for( j = 0; j < 5; j++ ){
tmp2.init(Sidx[i+1]-Sidx[i],Sidx[j+1]-Sidx[j]);
tmp2.setZero();
for( run3 = Sidx[i]; run3 < Sidx[i+1]; run3++ )
for( run4 = Sidx[j]; run4 < Sidx[j+1]; run4++ )
for( run2 = 0; run2 < nh; run2++ )
tmp2(run3-Sidx[i],run4-Sidx[j]) += J[run2][y_index[run3]]*tmp(run2,run4);
if( tmp2.getDim() != 0 ) GNhessian->addDense(Hidx[i],Hidx[j],tmp2);
}
}
delete[] Sidx;
delete[] Hidx;
}
// --------------------------------------------------------
for( run2 = 0; run2 < nh; run2++ )
delete[] J[run2];
delete[] J;
delete[] bseed;
return SUCCESSFUL_RETURN;
}
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
