ExportVariable DiscreteTimeExport::getAuxVariable() const
{
ExportVariable max;
if( NX1 > 0 ) {
max = lin_input.getGlobalExportVariable();
}
if( NX2 > 0 ) {
if( rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
max = rhs.getGlobalExportVariable();
}
if( diffs_rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
max = diffs_rhs.getGlobalExportVariable();
}
}
if( NX3 > 0 ) {
if( rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
max = rhs3.getGlobalExportVariable();
}
if( diffs_rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
max = diffs_rhs3.getGlobalExportVariable();
}
}
return max;
}
returnValue ExportGaussElim::setupSolveReuseComplete( ExportFunction& _solveReuse, ExportVariable& _bPerm ) {
ExportIndex run1( "i" );
ExportIndex run2( "j" );
ExportIndex tmp_index1( "index1" );
ExportIndex tmp_index2( "index2" );
ExportVariable tmp( "tmp_var", 1, 1, _bPerm.getType(), ACADO_LOCAL, true );
_solveReuse.addIndex( run1 );
_solveReuse.addIndex( run2 );
_solveReuse.addIndex( tmp_index1 );
_solveReuse.addIndex( tmp_index2 );
_solveReuse.addDeclaration(tmp);
uint run3;
if (nRightHandSides <= 0)
return ACADOERROR(RET_INVALID_OPTION);
ExportForLoop loop1( run1, 0, dim );
loop1 << run2.getName() << " = " << rk_perm.getFullName() << "[" << run1.getName() << "]*" << toString(nRightHandSides) << ";\n";
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
loop1 << _bPerm.get( run1,run3 ) << " = b[" << run2.getName() << "+" << toString(run3) << "];\n";
}
_solveReuse.addStatement( loop1 );
ExportForLoop loop2( run2, 1, dim ); // row run2
loop2.addStatement( tmp_index1 == run2*nRightHandSides );
ExportForLoop loop3( run1, 0, run2 ); // column run1
loop3.addStatement( tmp_index2 == run1*nRightHandSides );
loop3.addStatement( tmp == A.getElement(run2,run1) );
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
// loop3.addStatement( _bPerm.getElement( run2,run3 ) += tmp * _bPerm.getElement( run1,run3 ) );
loop3 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] += " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index2.getName() << "+" << toString(run3) << "];\n";
}
loop2.addStatement( loop3 );
_solveReuse.addStatement( loop2 );
// Solve the upper triangular system of equations:
ExportForLoop loop4( run1, dim-1, -1, -1 );
loop4.addStatement( tmp_index1 == run1*nRightHandSides );
ExportForLoop loop5( run2, dim-1, run1, -1 );
loop5.addStatement( tmp_index2 == run2*nRightHandSides );
loop5.addStatement( tmp == A.getElement( run1,run2 ) );
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
// loop5.addStatement( _bPerm.getElement( run1,run3 ) -= tmp * _bPerm.getElement( run2,run3 ) );
loop5 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] -= " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index2.getName() << "+" << toString(run3) << "];\n";
}
loop4.addStatement( loop5 );
loop4 << tmp.getName() << " = 1.0/A[" << run1.getName() << "*" << toString(dim+1) << "];\n";
for( run3 = 0; run3 < nRightHandSides; run3++ ) {
// loop4 << _bPerm.get( run1,run3 ) << " = " << _bPerm.get( run1,run3 ) << "*" << tmp.getName() << ";\n";
loop4 << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "] = " << tmp.getName() << "*" << _bPerm.getFullName() << "[" << tmp_index1.getName() << "+" << toString(run3) << "];\n";
}
_solveReuse.addStatement( loop4 );
_solveReuse.addStatement( b == _bPerm );
return SUCCESSFUL_RETURN;
}
ExportVariable ExportVariable::clone() const
{
ExportVariable ret;
if( !isNull() )
ret.assignNode( (*this)->clone() );
return ret;
}
ExportVariable ExportVariable::operator()( const String& _name
) const
{
ExportVariable tmp = deepcopy( *this );
tmp.setName( _name );
return tmp;
}
ExportVariable ExplicitRungeKuttaExport::getAuxVariable() const
{
ExportVariable max;
max = rhs.getGlobalExportVariable();
if( diffs_rhs.getGlobalExportVariable().getDim() > max.getDim() ) {
max = diffs_rhs.getGlobalExportVariable();
}
return max;
}
ExportVariable ThreeSweepsERKExport::getAuxVariable() const
{
ExportVariable max;
max = rhs.getGlobalExportVariable();
if( diffs_rhs.getGlobalExportVariable().getDim() > max.getDim() ) {
max = diffs_rhs.getGlobalExportVariable();
}
if( diffs_sweep3.getGlobalExportVariable().getDim() > max.getDim() ) {
max = diffs_sweep3.getGlobalExportVariable();
}
return max;
}
ExportVariable LiftedERKExport::getAuxVariable() const
{
ExportVariable max;
max = rhs.getGlobalExportVariable();
if( diffs_rhs.getGlobalExportVariable().getDim() > max.getDim() ) {
max = diffs_rhs.getGlobalExportVariable();
}
if( alg_rhs.getGlobalExportVariable().getDim() > max.getDim() ) {
max = alg_rhs.getGlobalExportVariable();
}
return max;
}
returnValue ExportGaussElim::setupSolveReuse( ExportFunction& _solveReuse, ExportFunction& _solveTriangular, ExportVariable& _bPerm ) {
uint run1, run2;
if (nRightHandSides > 0)
return ACADOERROR(RET_INVALID_OPTION);
for( run1 = 0; run1 < dim; run1++ ) {
_solveReuse << _bPerm.get( run1,0 ) << " = b[" << rk_perm.getFullName() << "[" << toString( run1 ) << "]];\n";
}
for( run2 = 1; run2 < dim; run2++ ) { // row run2
for( run1 = 0; run1 < run2; run1++ ) { // column run1
_solveReuse << _bPerm.get( run2,0 ) << " += A[" << toString( run2*dim+run1 ) << "]*" << _bPerm.getFullName() << "[" << toString( run1 ) << "];\n";
}
_solveReuse.addLinebreak();
}
_solveReuse.addLinebreak();
_solveReuse.addFunctionCall( _solveTriangular, A, _bPerm );
_solveReuse.addStatement( b == _bPerm );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupSolveReuseTranspose( ExportFunction& _solveReuse, ExportVariable& _bPerm ) {
ExportIndex run1( "i" );
ExportIndex run2( "j" );
ExportVariable tmp( "tmp_var", 1, 1, _bPerm.getType(), ACADO_LOCAL, true );
_solveReuse.addIndex( run1 );
_solveReuse.addIndex( run2 );
_solveReuse.addDeclaration(tmp);
_solveReuse.addStatement( _bPerm == b_trans );
ExportForLoop loop2( run2, 0, dim ); // row run2
ExportForLoop loop3( run1, 0, run2 ); // column run1
loop3.addStatement( _bPerm.getRow(run2) -= A.getElement(run1,run2)*_bPerm.getRow(run1) );
loop2.addStatement( loop3 );
loop2 << tmp.getName() << " = 1.0/A[" << run2.getName() << "*" << toString(dim+1) << "];\n";
loop2.addStatement( _bPerm.getRow(run2) == _bPerm.getRow(run2)*tmp );
_solveReuse.addStatement( loop2 );
// Solve the upper triangular system of equations:
ExportForLoop loop4( run1, dim-1, -1, -1 );
ExportForLoop loop5( run2, dim-1, run1, -1 );
loop5.addStatement( _bPerm.getRow(run1) += A.getElement(run2,run1)*_bPerm.getRow(run2) );
loop4.addStatement( loop5 );
_solveReuse.addStatement( loop4 );
// The permutation now happens HERE!
ExportForLoop loop1( run1, 0, dim );
loop1 << run2.getName() << " = " << rk_perm.getFullName() << "[" << run1.getName() << "];\n";
loop1.addStatement( b_trans.getRow(run2) == _bPerm.getRow(run1) );
_solveReuse.addStatement( loop1 );
return SUCCESSFUL_RETURN;
}
returnValue ThreeSweepsERKExport::setup( )
{
int sensGen;
get( DYNAMIC_SENSITIVITY,sensGen );
if ( (ExportSensitivityType)sensGen != SYMMETRIC ) ACADOERROR( RET_INVALID_OPTION );
// NOT SUPPORTED: since the forward sweep needs to be saved
if( !equidistantControlGrid() ) ACADOERROR( RET_INVALID_OPTION );
// NOT SUPPORTED: since the adjoint derivatives could be 'arbitrarily bad'
if( !is_symmetric ) ACADOERROR( RET_INVALID_OPTION );
LOG( LVL_DEBUG ) << "Preparing to export ThreeSweepsERKExport... " << endl;
// export RK scheme
uint numX = NX*(NX+1)/2.0;
uint numU = NU*(NU+1)/2.0;
uint rhsDim = NX + NX + NX*(NX+NU) + numX + NX*NU + numU;
inputDim = rhsDim + NU + NOD;
const uint rkOrder = getNumStages();
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
ExportVariable Ah ( "A*h", DMatrix( AA )*=h );
ExportVariable b4h( "b4*h", DMatrix( bb )*=h );
rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
rk_eta = ExportVariable( "rk_eta", 1, inputDim );
// seed_backward.setup( "seed", 1, NX );
int useOMP;
get(CG_USE_OPENMP, useOMP);
ExportStruct structWspace;
structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
rk_ttt.setup( "rk_ttt", 1, 1, REAL, structWspace, true );
uint timeDep = 0;
if( timeDependant ) timeDep = 1;
rk_xxx.setup("rk_xxx", 1, inputDim+timeDep, REAL, structWspace);
uint numK = NX*(NX+NU)+numX+NX*NU+numU;
rk_kkk.setup("rk_kkk", rkOrder, numK, REAL, structWspace);
rk_forward_sweep.setup("rk_sweep1", 1, grid.getNumIntervals()*rkOrder*NX, REAL, structWspace);
rk_backward_sweep.setup("rk_sweep2", 1, grid.getNumIntervals()*rkOrder*NX, REAL, structWspace);
if ( useOMP )
{
ExportVariable auxVar;
auxVar = diffs_rhs.getGlobalExportVariable();
auxVar.setName( "odeAuxVar" );
auxVar.setDataStruct( ACADO_LOCAL );
diffs_rhs.setGlobalExportVariable( auxVar );
}
ExportIndex run( "run1" );
// setup INTEGRATE function
integrate = ExportFunction( "integrate", rk_eta, reset_int );
integrate.setReturnValue( error_code );
rk_eta.setDoc( "Working array to pass the input values and return the results." );
reset_int.setDoc( "The internal memory of the integrator can be reset." );
rk_index.setDoc( "Number of the shooting interval." );
error_code.setDoc( "Status code of the integrator." );
integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
integrate.addIndex( run );
integrate.addStatement( rk_ttt == DMatrix(grid.getFirstTime()) );
if( inputDim > rhsDim ) {
// initialize sensitivities:
// integrate.addStatement( rk_eta.getCols( NX,2*NX ) == seed_backward );
DMatrix idX = eye<double>( NX );
DMatrix zeroXU = zeros<double>( NX,NU );
integrate.addStatement( rk_eta.getCols( 2*NX,NX*(2+NX) ) == idX.makeVector().transpose() );
integrate.addStatement( rk_eta.getCols( NX*(2+NX),NX*(2+NX+NU) ) == zeroXU.makeVector().transpose() );
integrate.addStatement( rk_eta.getCols( NX*(2+NX+NU),rhsDim ) == zeros<double>( 1,numX+NX*NU+numU ) );
// FORWARD SWEEP FIRST
integrate.addStatement( rk_xxx.getCols( NX,NX+NU+NOD ) == rk_eta.getCols( rhsDim,inputDim ) );
}
integrate.addLinebreak( );
// integrator loop: FORWARD SWEEP
ExportForLoop loop = ExportForLoop( run, 0, grid.getNumIntervals() );
for( uint run1 = 0; run1 < rkOrder; run1++ )
{
loop.addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) + Ah.getRow(run1)*rk_kkk.getCols( 0,NX ) );
// save forward trajectory
loop.addStatement( rk_forward_sweep.getCols( run*rkOrder*NX+run1*NX,run*rkOrder*NX+(run1+1)*NX ) == rk_xxx.getCols( 0,NX ) );
if( timeDependant ) loop.addStatement( rk_xxx.getCol( NX+NU+NOD ) == rk_ttt + ((double)cc(run1))/grid.getNumIntervals() );
loop.addFunctionCall( getNameRHS(),rk_xxx,rk_kkk.getAddress(run1,0) );
}
loop.addStatement( rk_eta.getCols( 0,NX ) += b4h^rk_kkk.getCols( 0,NX ) );
loop.addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
// end of integrator loop: FORWARD SWEEP
integrate.addStatement( loop );
if( inputDim > rhsDim ) {
// BACKWARD SWEEP NEXT
integrate.addStatement( rk_xxx.getCols( 2*NX,2*NX+NU+NOD ) == rk_eta.getCols( rhsDim,inputDim ) );
}
// integrator loop: BACKWARD SWEEP
ExportForLoop loop2 = ExportForLoop( run, 0, grid.getNumIntervals() );
for( uint run1 = 0; run1 < rkOrder; run1++ )
{
// load forward trajectory
loop2.addStatement( rk_xxx.getCols( 0,NX ) == rk_forward_sweep.getCols( (grid.getNumIntervals()-run)*rkOrder*NX-(run1+1)*NX,(grid.getNumIntervals()-run)*rkOrder*NX-run1*NX ) );
loop2.addStatement( rk_xxx.getCols( NX,2*NX ) == rk_eta.getCols( NX,2*NX ) + Ah.getRow(run1)*rk_kkk.getCols( 0,NX ) );
// save backward trajectory
loop2.addStatement( rk_backward_sweep.getCols( run*rkOrder*NX+run1*NX,run*rkOrder*NX+(run1+1)*NX ) == rk_xxx.getCols( NX,2*NX ) );
if( timeDependant ) loop2.addStatement( rk_xxx.getCol( 2*NX+NU+NOD ) == rk_ttt - ((double)cc(run1))/grid.getNumIntervals() );
loop2.addFunctionCall( getNameDiffsRHS(),rk_xxx,rk_kkk.getAddress(run1,0) );
}
loop2.addStatement( rk_eta.getCols( NX,2*NX ) += b4h^rk_kkk.getCols( 0,NX ) );
loop2.addStatement( rk_ttt -= DMatrix(1.0/grid.getNumIntervals()) );
// end of integrator loop: BACKWARD SWEEP
integrate.addStatement( loop2 );
if( inputDim > rhsDim ) {
// THIRD SWEEP NEXT
integrate.addStatement( rk_xxx.getCols( rhsDim,inputDim ) == rk_eta.getCols( rhsDim,inputDim ) );
}
// integrator loop: THIRD SWEEP
ExportForLoop loop3 = ExportForLoop( run, 0, grid.getNumIntervals() );
for( uint run1 = 0; run1 < rkOrder; run1++ )
{
// load forward trajectory
loop3.addStatement( rk_xxx.getCols( 0,NX ) == rk_forward_sweep.getCols( run*rkOrder*NX+run1*NX,run*rkOrder*NX+(run1+1)*NX ) );
// load backward trajectory
loop3.addStatement( rk_xxx.getCols( NX,2*NX ) == rk_backward_sweep.getCols( (grid.getNumIntervals()-run)*rkOrder*NX-(run1+1)*NX,(grid.getNumIntervals()-run)*rkOrder*NX-run1*NX ) );
loop3.addStatement( rk_xxx.getCols( 2*NX,rhsDim ) == rk_eta.getCols( 2*NX,rhsDim ) + Ah.getRow(run1)*rk_kkk.getCols( 0,NX*(NX+NU)+numX+NX*NU+numU ) );
if( timeDependant ) loop3.addStatement( rk_xxx.getCol( inputDim ) == rk_ttt + ((double)cc(run1))/grid.getNumIntervals() );
loop3.addFunctionCall( diffs_sweep3.getName(),rk_xxx,rk_kkk.getAddress(run1,0) );
}
loop3.addStatement( rk_eta.getCols( 2*NX,rhsDim ) += b4h^rk_kkk.getCols( 0,NX*(NX+NU)+numX+NX*NU+numU ) );
loop3.addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
// end of integrator loop: THIRD SWEEP
integrate.addStatement( loop3 );
integrate.addStatement( error_code == 0 );
LOG( LVL_DEBUG ) << "done" << endl;
return SUCCESSFUL_RETURN;
}
returnValue ExportExactHessianCN2::setupObjectiveEvaluation( void )
{
evaluateObjective.setup("evaluateObjective");
int gradientUp;
get( LIFTED_GRADIENT_UPDATE, gradientUp );
bool gradientUpdate = (bool) gradientUp;
//
// A loop the evaluates objective and corresponding gradients
//
ExportIndex runObj( "runObj" );
ExportForLoop loopObjective( runObj, 0, N );
evaluateObjective.addIndex( runObj );
unsigned offset = performFullCondensing() == true ? 0 : NX;
if( evaluateStageCost.getFunctionDim() > 0 ) {
loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
loopObjective.addLinebreak( );
// Evaluate the objective function
loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
loopObjective.addLinebreak( );
ExportVariable tmpFxx, tmpFxu, tmpFuu;
tmpFxx.setup("tmpFxx", NX, NX, REAL, ACADO_LOCAL);
tmpFxu.setup("tmpFxu", NX, NU, REAL, ACADO_LOCAL);
tmpFuu.setup("tmpFuu", NU, NU, REAL, ACADO_LOCAL);
//
// Optional computation of Q1, Q2
//
ExportVariable tmpEH;
tmpEH.setup("tmpEH", NX+NU, NX+NU, REAL, ACADO_LOCAL);
setObjQ1Q2.setup("addObjTerm", tmpFxx, tmpFxu, tmpFuu, tmpEH);
setObjQ1Q2.addStatement( tmpEH.getSubMatrix(0,NX,0,NX) += tmpFxx );
setObjQ1Q2.addStatement( tmpEH.getSubMatrix(0,NX,NX,NX+NU) += tmpFxu );
setObjQ1Q2.addStatement( tmpEH.getSubMatrix(NX,NX+NU,0,NX) += tmpFxu.getTranspose() );
setObjQ1Q2.addStatement( tmpEH.getSubMatrix(NX,NX+NU,NX,NX+NU) += tmpFuu );
loopObjective.addFunctionCall(
setObjQ1Q2, objValueOut.getAddress(0, 1+NX+NU), objValueOut.getAddress(0, 1+NX+NU+NX*NX),
objValueOut.getAddress(0, 1+NX+NU+NX*(NX+NU)), objS.getAddress(runObj*(NX+NU), 0) );
ExportVariable tmpDx, tmpDu, tmpDF;
tmpDx.setup("tmpDx", NX, 1, REAL, ACADO_LOCAL);
tmpDu.setup("tmpDu", NU, 1, REAL, ACADO_LOCAL);
tmpDF.setup("tmpDF", NX+NU, 1, REAL, ACADO_LOCAL);
setObjR1R2.setup("addObjLinearTerm", tmpDx, tmpDu, tmpDF);
setObjR1R2.addStatement( tmpDx == tmpDF.getRows(0,NX) );
setObjR1R2.addStatement( tmpDu == tmpDF.getRows(NX,NX+NU) );
int sensitivityProp;
get( DYNAMIC_SENSITIVITY, sensitivityProp );
bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD);
if( gradientUpdate || adjoint ) {
loopObjective.addStatement( objValueOut.getCols(1,1+NX+NU) += objg.getRows(runObj*(NX+NU),(runObj+1)*(NX+NU)).getTranspose() );
}
loopObjective.addFunctionCall(
setObjR1R2, QDy.getAddress(runObj * NX), g.getAddress(offset+runObj * NU, 0), objValueOut.getAddress(0, 1) );
loopObjective.addLinebreak( );
}
else {
DMatrix Du(NU,1); Du.setAll(0);
DMatrix Dx(NX,1); Dx.setAll(0);
loopObjective.addStatement( g.getRows(offset+runObj*NU, offset+runObj*NU+NU) == Du );
loopObjective.addStatement( QDy.getRows(runObj*NX, runObj*NX+NX) == Dx );
}
evaluateObjective.addStatement( loopObjective );
//
// Evaluate the quadratic Mayer term
//
if( evaluateTerminalCost.getFunctionDim() > 0 ) {
evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
// Evaluate the objective function, last node.
evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
evaluateObjective.addLinebreak( );
ExportVariable tmpFxxEnd;
tmpFxxEnd.setup("tmpFxxEnd", NX, NX, REAL, ACADO_LOCAL);
//
// Optional computation of QN1
//
ExportVariable tmpEH_N;
tmpEH_N.setup("tmpEH_N", NX, NX, REAL, ACADO_LOCAL);
setObjQN1QN2.setup("addObjEndTerm", tmpFxxEnd, tmpEH_N);
setObjQN1QN2.addStatement( tmpEH_N == tmpFxxEnd );
evaluateObjective.addFunctionCall(
setObjQN1QN2, objValueOut.getAddress(0, 1+NX), objSEndTerm );
evaluateObjective.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == objValueOut.getCols(1,1+NX).getTranspose() );
evaluateObjective.addLinebreak( );
}
else {
DMatrix hess(NX,NX); hess.setAll(0);
evaluateObjective.addStatement(objSEndTerm == hess);
DMatrix Dx(NX,1); Dx.setAll(0);
evaluateObjective.addStatement( QDy.getRows(N * NX, (N + 1) * NX) == Dx );
}
return SUCCESSFUL_RETURN;
}
returnValue LiftedERKExport::setup( )
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
ExportStruct structWspace;
structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
int sensGen;
get( DYNAMIC_SENSITIVITY,sensGen );
if ( (ExportSensitivityType)sensGen != FORWARD && (ExportSensitivityType)sensGen != NO_SENSITIVITY && (ExportSensitivityType)sensGen != INEXACT ) ACADOERROR( RET_INVALID_OPTION );
int liftMode = 1;
// liftMode == 1 --> EXACT LIFTING
// liftMode == 2 --> INEXACT LIFTING
if( (ExportSensitivityType)sensGen == INEXACT ) liftMode = 2;
bool DERIVATIVES = ((ExportSensitivityType)sensGen != NO_SENSITIVITY);
LOG( LVL_DEBUG ) << "Preparing to export ExplicitRungeKuttaExport... " << endl;
// setup linear solver:
int solverType;
userInteraction->get( LINEAR_ALGEBRA_SOLVER,solverType );
if ( solver )
delete solver;
solver = 0;
switch( (LinearAlgebraSolver) solverType ) {
case GAUSS_LU:
solver = new ExportGaussElim( userInteraction,commonHeaderName );
solver->init( NXA, NX+NU+1 );
solver->setReuse( true ); // IFTR method
solver->setup();
rk_auxSolver = solver->getGlobalExportVariable( 1 );
break;
default:
return ACADOERROR( RET_INVALID_OPTION );
}
rk_A = ExportVariable( "rk_A", NXA, NXA, REAL, structWspace );
rk_b = ExportVariable( "rk_b", NXA, 1+NX+NU, REAL, structWspace );
// export RK scheme
uint rhsDim = NX*(NX+NU+1);
uint zDim = NXA*(NX+NU+1);
inputDim = (NX+NXA)*(NX+NU+1) + NU + NOD;
if( !DERIVATIVES ) return ACADOERROR( RET_INVALID_OPTION );
const uint rkOrder = getNumStages();
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
ExportVariable Ah ( "A*h", DMatrix( AA )*=h );
ExportVariable b4h( "b4*h", DMatrix( bb )*=h );
rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
rk_eta = ExportVariable( "rk_eta", 1, inputDim );
rk_ttt.setup( "rk_ttt", 1, 1, REAL, structWspace, true );
uint timeDep = 0;
if( timeDependant ) timeDep = 1;
rk_xxx.setup("rk_xxx", 1, inputDim+NXA+timeDep, REAL, structWspace);
rk_kkk.setup("rk_kkk", rkOrder, rhsDim, REAL, structWspace);
rk_zzz.setup("rk_zzz", getN()*grid.getNumIntervals()*rkOrder, NXA, REAL, structWspace);
rk_zTemp.setup("rk_zTemp", 1, NXA*(1+NX+NXA+NU), REAL, structWspace);
// if( liftMode == 1 ) {
rk_diffZ.setup("rk_diffZ", getN()*grid.getNumIntervals()*rkOrder*NXA, NX+NU, REAL, structWspace);
rk_delta.setup( "rk_delta", 1, NX+NU, REAL, ACADO_WORKSPACE );
rk_prev.setup( "rk_prev", getN(), NX+NU, REAL, ACADO_WORKSPACE );
// }
if ( useOMP )
{
ExportVariable auxVar;
auxVar = getAuxVariable();
auxVar.setName( "odeAuxVar" );
auxVar.setDataStruct( ACADO_LOCAL );
rhs.setGlobalExportVariable( auxVar );
diffs_rhs.setGlobalExportVariable( auxVar );
}
ExportIndex run( "run1" );
ExportIndex shoot_index( "shoot_index" );
ExportIndex k_index( "k_index" );
// setup INTEGRATE function
integrate = ExportFunction( "integrate", rk_eta, rk_index ); // you need the rk_index for LIFTING !
integrate.setReturnValue( error_code );
rk_eta.setDoc( "Working array to pass the input values and return the results." );
reset_int.setDoc( "The internal memory of the integrator can be reset." );
rk_index.setDoc( "Number of the shooting interval." );
error_code.setDoc( "Status code of the integrator." );
integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
integrate.addIndex( run );
integrate.addIndex( shoot_index );
integrate.addIndex( k_index );
ExportVariable det( "det", 1, 1, REAL, ACADO_LOCAL, true );
integrate.addDeclaration( det );
integrate << shoot_index.getFullName() << " = " << rk_index.getFullName() << ";\n";
ExportVariable numInt( "numInts", 1, 1, INT );
if( !equidistantControlGrid() ) {
integrate.addStatement( std::string( "int numSteps[" ) + toString( numSteps.getDim() ) + "] = {" + toString( numSteps(0) ) );
uint i;
for( i = 1; i < numSteps.getDim(); i++ ) {
integrate.addStatement( std::string( ", " ) + toString( numSteps(i) ) );
}
integrate.addStatement( std::string( "};\n" ) );
integrate.addStatement( std::string( "int " ) + numInt.getName() + " = numSteps[" + rk_index.getName() + "];\n" );
}
integrate.addStatement( rk_ttt == DMatrix(grid.getFirstTime()) );
if( DERIVATIVES ) {
// initialize sensitivities:
DMatrix idX = eye<double>( NX );
DMatrix zeroXU = zeros<double>( NX,NU );
integrate.addStatement( rk_eta.getCols( NX+NXA,NXA+NX*(1+NX) ) == idX.makeVector().transpose() );
integrate.addStatement( rk_eta.getCols( (NX+NXA)*(1+NX),(NX+NXA)*(1+NX)+NX*NU ) == zeroXU.makeVector().transpose() );
}
if( inputDim > (rhsDim+zDim) ) {
integrate.addStatement( rk_xxx.getCols( NXA+rhsDim+zDim,NXA+inputDim ) == rk_eta.getCols( rhsDim+zDim,inputDim ) );
}
// if( liftMode == 1 ) {
integrate.addStatement( rk_delta.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) - rk_prev.getSubMatrix(shoot_index,shoot_index+1,0,NX) );
integrate.addStatement( rk_delta.getCols( NX,NX+NU ) == rk_eta.getCols( rhsDim+zDim,rhsDim+zDim+NU ) - rk_prev.getSubMatrix(shoot_index,shoot_index+1,NX,NX+NU) );
integrate.addStatement( rk_prev.getSubMatrix(shoot_index,shoot_index+1,0,NX) == rk_eta.getCols( 0,NX ) );
integrate.addStatement( rk_prev.getSubMatrix(shoot_index,shoot_index+1,NX,NX+NU) == rk_eta.getCols( rhsDim+zDim,rhsDim+zDim+NU ) );
// }
integrate.addLinebreak( );
// integrator loop
ExportForLoop loop;
if( equidistantControlGrid() ) {
loop = ExportForLoop( run, 0, grid.getNumIntervals() );
}
else {
loop = ExportForLoop( run, 0, 1 );
loop.addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
}
loop.addStatement( k_index == (shoot_index*grid.getNumIntervals()+run) );
for( uint run1 = 0; run1 < rkOrder; run1++ )
{
loop.addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) + Ah.getRow(run1)*rk_kkk.getCols( 0,NX ) );
loop.addStatement( rk_xxx.getCols( NX,NX*(1+NX) ) == rk_eta.getCols( NX+NXA,NXA+NX*(1+NX) ) + Ah.getRow(run1)*rk_kkk.getCols( NX,NX*(1+NX) ) );
loop.addStatement( rk_xxx.getCols( NX*(1+NX),rhsDim ) == rk_eta.getCols( (NX+NXA)*(1+NX),(NX+NXA)*(1+NX)+NX*NU ) + Ah.getRow(run1)*rk_kkk.getCols( NX*(1+NX),rhsDim ) );
if( timeDependant ) loop.addStatement( rk_xxx.getCol( NXA+inputDim ) == rk_ttt + ((double)cc(run1))/grid.getNumIntervals() );
// update algebraic variables based on previous SQP step:
// if( liftMode == 1 ) {
loop.addStatement( rk_zzz.getRow(k_index*rkOrder+run1) += rk_delta*rk_diffZ.getRows( k_index*rkOrder*NXA+run1*NXA,k_index*rkOrder*NXA+run1*NXA+NXA ).getTranspose() );
// }
// evaluate the right algebraic variables and equations:
loop.addStatement( rk_xxx.getCols( rhsDim,rhsDim+NXA ) == rk_zzz.getRow(k_index*rkOrder+run1) );
if( liftMode == 2 ) {
for( uint i = 0; i < NXA; i++ ) {
for( uint j = 0; j < NX; j++ ) {
loop.addStatement( rk_xxx.getCol( rhsDim+2*NXA+i*NX+j ) == rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,j ) );
}
for( uint j = 0; j < NU; j++ ) {
loop.addStatement( rk_xxx.getCol( rhsDim+NXA*(2+NX)+i*NU+j ) == rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,NX+j ) );
}
}
}
loop.addFunctionCall( alg_rhs.getName(),rk_xxx,rk_zTemp );
// JACOBIAN FACTORIZATION ALGEBRAIC EQUATIONS
if( liftMode == 1 ) { // EXACT
for( uint i = 0; i < NXA; i++ ) {
for( uint j = 0; j < NXA; j++ ) {
loop.addStatement( rk_A.getElement(i,j) == rk_zTemp.getCol(NXA*(1+NX)+i*NXA+j) );
}
}
loop.addStatement( det.getFullName() + " = " + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
}
else { // INEXACT
if( run1 == 0 ) {
loop.addStatement( std::string("if(") + run.getName() + " == 0) {\n" );
for( uint i = 0; i < NXA; i++ ) {
for( uint j = 0; j < NXA; j++ ) {
loop.addStatement( rk_A.getElement(i,j) == rk_zTemp.getCol(NXA*(1+NX)+i*NXA+j) );
}
}
loop.addStatement( det.getFullName() + " = " + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
loop.addStatement( std::string("}\n") );
}
}
for( uint i = 0; i < NXA; i++ ) {
loop.addStatement( rk_b.getElement(i,0) == 0.0 - rk_zTemp.getCol(i) );
for( uint j = 0; j < NX; j++ ) {
loop.addStatement( rk_b.getElement(i,1+j) == 0.0 - rk_zTemp.getCol(NXA+i*NX+j) );
}
for( uint j = 0; j < NU; j++ ) {
loop.addStatement( rk_b.getElement(i,1+NX+j) == 0.0 - rk_zTemp.getCol(NXA*(1+NX+NXA)+i*NU+j) );
}
}
// NEWTON STEP ON THE ALGEBRAIC EQUATIONS
loop.addFunctionCall( solver->getNameSolveReuseFunction(),rk_A.getAddress(0,0),rk_b.getAddress(0,0),rk_auxSolver.getAddress(0,0) );
loop.addStatement( rk_zzz.getRow(k_index*rkOrder+run1) += rk_b.getCol( 0 ).getTranspose() );
loop.addStatement( rk_xxx.getCols( rhsDim+NXA,rhsDim+2*NXA ) == rk_b.getCol( 0 ).getTranspose() );
for( uint i = 0; i < NXA; i++ ) {
for( uint j = 0; j < NX; j++ ) {
if( liftMode == 1 ) loop.addStatement( rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,j ) == rk_b.getElement(i,1+j) );
else loop.addStatement( rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,j ) += rk_b.getElement(i,1+j) );
loop.addStatement( rk_xxx.getCol( rhsDim+2*NXA+i*NX+j ) == rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,j ) );
}
for( uint j = 0; j < NU; j++ ) {
if( liftMode == 1 ) loop.addStatement( rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,NX+j ) == rk_b.getElement(i,1+NX+j) );
else loop.addStatement( rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,NX+j ) += rk_b.getElement(i,1+NX+j) );
loop.addStatement( rk_xxx.getCol( rhsDim+NXA*(2+NX)+i*NU+j ) == rk_diffZ.getElement( k_index*rkOrder*NXA+run1*NXA+i,NX+j ) );
}
}
loop.addFunctionCall( getNameDiffsRHS(),rk_xxx,rk_kkk.getAddress(run1,0) );
}
loop.addStatement( rk_eta.getCols( 0,NX ) += b4h^rk_kkk.getCols(0,NX) );
loop.addStatement( rk_eta.getCols( NX+NXA,NXA+NX*(1+NX) ) += b4h^rk_kkk.getCols(NX,NX*(1+NX)) );
loop.addStatement( rk_eta.getCols( (NX+NXA)*(1+NX),(NX+NXA)*(1+NX)+NX*NU ) += b4h^rk_kkk.getCols(NX*(1+NX),rhsDim) );
loop.addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
// end of integrator loop
if( !equidistantControlGrid() ) {
loop.addStatement( "}\n" );
// loop.unrollLoop();
}
integrate.addStatement( loop );
integrate.addStatement( error_code == 0 );
LOG( LVL_DEBUG ) << "done" << endl;
return SUCCESSFUL_RETURN;
}
returnValue ExportExactHessianQpDunes::setupObjectiveEvaluation( void )
{
evaluateObjective.setup("evaluateObjective");
//
// A loop the evaluates objective and corresponding gradients
//
ExportIndex runObj( "runObj" );
ExportForLoop loopObjective( runObj, 0, N );
evaluateObjective.addIndex( runObj );
// Interface variable to qpDUNES
qpH.setup("qpH", N * (NX + NU) * (NX + NU) + NX * NX, 1, REAL, ACADO_WORKSPACE); // --> to be used only after regularization to pass to qpDUNES
qpg.setup("qpG", N * (NX + NU) + NX, 1, REAL, ACADO_WORKSPACE);
// LM regularization preparation
ExportVariable evLmX = zeros<double>(NX, NX);
ExportVariable evLmU = zeros<double>(NU, NU);
if (levenbergMarquardt > 0.0)
{
DMatrix lmX = eye<double>( NX );
lmX *= levenbergMarquardt;
DMatrix lmU = eye<double>( NU );
lmU *= levenbergMarquardt;
evLmX = lmX;
evLmU = lmU;
}
ExportVariable stagef;
stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
ExportVariable stageH;
stageH.setup("stageH", NX + NU, NX + NU, REAL, ACADO_LOCAL);
if( evaluateStageCost.getFunctionDim() > 0 ) {
loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
loopObjective.addLinebreak( );
// Evaluate the objective function
loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
loopObjective.addLinebreak( );
ExportVariable tmpFxx, tmpFxu, tmpFuu;
tmpFxx.setup("tmpFxx", NX, NX, REAL, ACADO_LOCAL);
tmpFxu.setup("tmpFxu", NX, NU, REAL, ACADO_LOCAL);
tmpFuu.setup("tmpFuu", NU, NU, REAL, ACADO_LOCAL);
setStageH.setup("addObjTerm", tmpFxx, tmpFxu, tmpFuu, stageH);
setStageH.addStatement( stageH.getSubMatrix(0,NX,0,NX) += tmpFxx + evLmX );
setStageH.addStatement( stageH.getSubMatrix(0,NX,NX,NX+NU) += tmpFxu );
setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,0,NX) += tmpFxu.getTranspose() );
setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,NX,NX+NU) += tmpFuu + evLmU );
loopObjective.addFunctionCall(
setStageH, objValueOut.getAddress(0, 1+NX+NU), objValueOut.getAddress(0, 1+NX+NU+NX*NX),
objValueOut.getAddress(0, 1+NX+NU+NX*(NX+NU)), objS.getAddress(runObj*(NX+NU), 0) );
ExportVariable tmpDF;
tmpDF.setup("tmpDF", NX+NU, 1, REAL, ACADO_LOCAL);
setStagef.setup("addObjLinearTerm", tmpDF, stagef);
setStagef.addStatement( stagef == tmpDF.getRows(0,NX+NU) );
loopObjective.addFunctionCall(
setStagef, objValueOut.getAddress(0, 1), qpg.getAddress(runObj * (NX+NU)) );
loopObjective.addLinebreak( );
}
else {
if(levenbergMarquardt > 0.0) {
setStageH.setup("addObjTerm", stageH);
setStageH.addStatement( stageH.getSubMatrix(0,NX,0,NX) += evLmX );
setStageH.addStatement( stageH.getSubMatrix(NX,NX+NU,NX,NX+NU) += evLmU );
loopObjective.addFunctionCall( setStageH, objS.getAddress(runObj*(NX+NU), 0) );
}
DMatrix D(NX+NU,1); D.setAll(0);
loopObjective.addStatement( qpg.getRows(runObj*(NX+NU), runObj*(NX+NU)+NX+NU) == D );
}
evaluateObjective.addStatement( loopObjective );
//
// Evaluate the quadratic Mayer term
//
if( evaluateTerminalCost.getFunctionDim() > 0 ) {
evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
// Evaluate the objective function, last node.
evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
evaluateObjective.addLinebreak( );
evaluateObjective.addStatement( objSEndTerm.makeRowVector() == objValueOut.getCols(1+NX,1+NX+NX*NX) + evLmX.makeRowVector() );
evaluateObjective.addStatement( qpg.getRows(N * NX, (N + 1) * NX) == objValueOut.getCols(1,1+NX).getTranspose() );
evaluateObjective.addLinebreak( );
}
else {
if(levenbergMarquardt > 0.0) {
evaluateObjective.addStatement( objSEndTerm == evLmX );
}
else {
DMatrix hess(NX,NX); hess.setAll(0);
evaluateObjective.addStatement( objSEndTerm == hess );
}
DMatrix Dx(NX,1); Dx.setAll(0);
evaluateObjective.addStatement( qpg.getRows(N*NX, (N+1)*NX) == Dx );
}
return SUCCESSFUL_RETURN;
}
returnValue CondensingExport::setupMultiplicationRoutines( )
{
ExportVariable QQ = deepcopy( Q );
QQ.setDataStruct( ACADO_LOCAL );
ExportVariable QC1( "QC1", getNX(),getNX() );
ExportVariable QE1( "QE1", getNX(),getNU()*getN() );
ExportVariable Dx1( "Dx1", getNX(),1 );
ExportVariable QDx1("QDx1", getNX(),1 );
ExportVariable Du1( "Du1", getNU(),1 );
ExportVariable RDu1("RDu1", getNU(),1 );
ExportVariable Gx ( "Gx", getNX(),getNX() );
ExportVariable Gu ( "Gu", getNX(),getNU() );
ExportVariable C1 ( "C1", getNX(),getNX() );
ExportVariable E1 ( "E1", getNX(),getNU()*getN() );
ExportVariable d1 ( "d1", getNX(),1 );
ExportVariable u1 ( "u1", getNU(),1 );
ExportVariable C ( "C", getNX()*getN(),getNX() );
ExportVariable QC ( "QC", getNX()*getN(),getNX() );
ExportVariable E ( "E", getNX()*getN(),getNU()*getN() );
ExportVariable QE ( "QE", getNX()*getN(),getNU()*getN() );
ExportVariable QDx( "QDx", getNX()*getN(),1 );
ExportVariable g0 ( "g0", getNX(),1 );
ExportVariable g1 ( "g1", getNU()*getN(),1 );
ExportVariable H00( "H00", getNX(),getNX() );
ExportVariable H01( "H01", getNX(),getNU()*getN() );
ExportVariable H11( "H11", getNU()*getN(),getNU()*getN() );
multiplyQC1.setup( "multiplyQC1", QQ,C1,QC1 );
multiplyQC1.addStatement( QC1 == QQ*C1 );
multiplyQE1.setup( "multiplyQE1", QQ,E1,QE1 );
multiplyQE1.addStatement( QE1 == QQ*E1 );
multiplyQDX1.setup( "multiplyQDX1", QQ,Dx1,QDx1 );
multiplyQDX1.addStatement( QDx1 == QQ*Dx1 );
multiplyRDU1.setup( "multiplyRDU1", R,Du1,RDu1 );
multiplyRDU1.addStatement( RDu1 == R*Du1 );
multiplyQC2.setup( "multiplyQC2", QQF,C1,QC1 );
multiplyQC2.addStatement( QC1 == QQF*C1 );
multiplyQE2.setup( "multiplyQE2", QQF,E1,QE1 );
multiplyQE2.addStatement( QE1 == QQF*E1 );
multiplyQDX2.setup( "multiplyQDX2", QQF,Dx1,QDx1 );
multiplyQDX2.addStatement( QDx1 == QQF*Dx1 );
multiplyC.setup( "multiplyC", Gx,C1,C1("C1_new") );
multiplyC.addStatement( C1("C1_new") == Gx*C1 );
multiplyE.setup( "multiplyE", Gx,E1,E1("E1_new") );
multiplyE.addStatement( E1("E1_new") == Gx*E1 );
if ( performsSingleShooting() == BT_FALSE )
{
multiplyCD1.setup( "multiplyCD1", Gx,d1,d1("d1_new") );
multiplyCD1.addStatement( d1("d1_new") == Gx*d1 );
multiplyEU1.setup( "multiplyEU1", Gu,u1,d1("d1_new") );
multiplyEU1.addStatement( d1("d1_new") += Gu*u1 );
}
if ( performsFullCondensing() == BT_FALSE )
{
multiplyG0.setup( "multiplyG0", C,QDx,g0 );
multiplyG0.addStatement( g0 == (C^QDx) );
}
multiplyG1.setup( "multiplyG1", E,QDx,g1 );
multiplyG1.addStatement( g1 == (E^QDx) );
if ( performsFullCondensing() == BT_FALSE )
{
multiplyH00.setup( "multiplyH00", C,QC,H00 );
multiplyH00.addStatement( H00 == (C^QC) );
}
multiplyH01.setup( "multiplyH01", C,QE,H01 );
multiplyH01.addStatement( H01 == (C^QE) );
multiplyH11.setup( "multiplyH11", E,QE,H11 );
multiplyH11.addStatement( (H11 == (E^QE)) );
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussNewtonForces::setupObjectiveEvaluation( void )
{
evaluateObjective.setup("evaluateObjective");
int variableObjS;
get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
int forceDiagHessian;
get(CG_FORCE_DIAGONAL_HESSIAN, forceDiagHessian);
diagH = false;
diagHN = false;
unsigned dimHRows = NX + NU;
unsigned dimHCols = NX + NU;
unsigned dimHNRows = NX;
unsigned dimHNCols = NX;
if (objS.isGiven() == true || forceDiagHessian == true)
if (objS.getGivenMatrix().isDiagonal())
{
diagH = true;
dimHCols = 1;
}
if (objSEndTerm.isGiven() == true || forceDiagHessian == true)
if (objSEndTerm.getGivenMatrix().isDiagonal() == true)
{
diagHN = true;
dimHNCols = 1;
}
objHessians.resize(N + 1);
for (unsigned i = 0; i < N; ++i)
{
objHessians[ i ].setup(string("H") + toString(i + 1), dimHRows, dimHCols, REAL, FORCES_PARAMS, false, qpObjPrefix);
}
objHessians[ N ].setup(string("H") + toString(N + 1), dimHNRows, dimHNCols, REAL, FORCES_PARAMS, false, qpObjPrefix);
objGradients.resize(N + 1);
for (unsigned i = 0; i < N; ++i)
{
objGradients[ i ].setup(string("f") + toString(i + 1), NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
}
objGradients[ N ].setup(string("f") + toString(N + 1), NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix);
//
// LM regularization preparation
//
ExportVariable evLmX = zeros<double>(NX, NX);
ExportVariable evLmU = zeros<double>(NU, NU);
if (levenbergMarquardt > 0.0)
{
DMatrix lmX = eye<double>( NX );
lmX *= levenbergMarquardt;
DMatrix lmU = eye<double>( NU );
lmU *= levenbergMarquardt;
evLmX = lmX;
evLmU = lmU;
}
//
// Main loop that calculates Hessian and gradients
//
ExportIndex runObj( "runObj" );
ExportForLoop loopObjective( runObj, 0, N );
evaluateObjective.addIndex( runObj );
loopObjective.addStatement( objValueIn.getCols(0, getNX()) == x.getRow( runObj ) );
loopObjective.addStatement( objValueIn.getCols(NX, NX + NU) == u.getRow( runObj ) );
loopObjective.addStatement( objValueIn.getCols(NX + NU, NX + NU + NOD) == od.getRow( runObj ) );
loopObjective.addLinebreak( );
// Evaluate the objective function
loopObjective.addFunctionCall(evaluateStageCost, objValueIn, objValueOut);
// Stack the measurement function value
loopObjective.addStatement(
Dy.getRows(runObj * NY, (runObj + 1) * NY) == objValueOut.getTranspose().getRows(0, getNY())
);
loopObjective.addLinebreak( );
// Optionally compute derivatives
unsigned indexX = getNY();
// unsigned indexG = indexX;
ExportVariable tmpObjS, tmpFx, tmpFu;
ExportVariable tmpFxEnd, tmpObjSEndTerm;
tmpObjS.setup("tmpObjS", NY, NY, REAL, ACADO_LOCAL);
if (objS.isGiven() == true)
tmpObjS = objS;
tmpFx.setup("tmpFx", NY, NX, REAL, ACADO_LOCAL);
if (objEvFx.isGiven() == true)
tmpFx = objEvFx;
tmpFu.setup("tmpFu", NY, NU, REAL, ACADO_LOCAL);
if (objEvFu.isGiven() == true)
tmpFu = objEvFu;
tmpFxEnd.setup("tmpFx", NYN, NX, REAL, ACADO_LOCAL);
if (objEvFxEnd.isGiven() == true)
tmpFxEnd = objEvFxEnd;
tmpObjSEndTerm.setup("tmpObjSEndTerm", NYN, NYN, REAL, ACADO_LOCAL);
if (objSEndTerm.isGiven() == true)
tmpObjSEndTerm = objSEndTerm;
//
// Optional computation of Q1, Q2
//
if (Q1.isGiven() == false)
{
ExportVariable tmpQ1, tmpQ2;
tmpQ1.setup("tmpQ1", NX, NX, REAL, ACADO_LOCAL);
tmpQ2.setup("tmpQ2", NX, NY, REAL, ACADO_LOCAL);
setObjQ1Q2.setup("setObjQ1Q2", tmpFx, tmpObjS, tmpQ1, tmpQ2);
setObjQ1Q2.addStatement( tmpQ2 == (tmpFx ^ tmpObjS) );
setObjQ1Q2.addStatement( tmpQ1 == tmpQ2 * tmpFx );
if (tmpFx.isGiven() == true)
{
if (variableObjS == YES)
{
loopObjective.addFunctionCall(
setObjQ1Q2,
tmpFx, objS.getAddress(runObj * NY, 0),
Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
);
}
else
{
loopObjective.addFunctionCall(
setObjQ1Q2,
tmpFx, objS,
Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
);
}
}
else
{
if (variableObjS == YES)
{
if (objEvFx.isGiven() == true)
loopObjective.addFunctionCall(
setObjQ1Q2,
objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
);
}
else
{
loopObjective.addFunctionCall(
setObjQ1Q2,
objValueOut.getAddress(0, indexX), objS,
Q1.getAddress(runObj * NX, 0), Q2.getAddress(runObj * NX, 0)
);
}
indexX += objEvFx.getDim();
}
loopObjective.addLinebreak( );
}
if (R1.isGiven() == false)
{
ExportVariable tmpR1, tmpR2;
tmpR1.setup("tmpR1", NU, NU, REAL, ACADO_LOCAL);
tmpR2.setup("tmpR2", NU, NY, REAL, ACADO_LOCAL);
setObjR1R2.setup("setObjR1R2", tmpFu, tmpObjS, tmpR1, tmpR2);
setObjR1R2.addStatement( tmpR2 == (tmpFu ^ tmpObjS) );
setObjR1R2.addStatement( tmpR1 == tmpR2 * tmpFu );
if (tmpFu.isGiven() == true)
{
if (variableObjS == YES)
{
loopObjective.addFunctionCall(
setObjR1R2,
tmpFu, objS.getAddress(runObj * NY, 0),
R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
);
}
else
{
loopObjective.addFunctionCall(
setObjR1R2,
tmpFu, objS,
R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
);
}
}
else
{
if (variableObjS == YES)
{
loopObjective.addFunctionCall(
setObjR1R2,
objValueOut.getAddress(0, indexX), objS.getAddress(runObj * NY, 0),
R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
);
}
else
{
loopObjective.addFunctionCall(
setObjR1R2,
objValueOut.getAddress(0, indexX), objS,
R1.getAddress(runObj * NU, 0), R2.getAddress(runObj * NU, 0)
);
}
}
loopObjective.addLinebreak( );
}
indexX = getNY();
ExportArgument tmpFxCall = tmpFx;
if (tmpFx.isGiven() == false)
{
tmpFxCall = objValueOut.getAddress(0, indexX);
indexX += objEvFx.getDim();
}
ExportArgument tmpFuCall = tmpFu;
if (tmpFu.isGiven() == false)
{
tmpFuCall = objValueOut.getAddress(0, indexX);
}
ExportArgument objSCall = variableObjS == true ? objS.getAddress(runObj * NY, 0) : objS;
if (S1.isGiven() == false)
{
ExportVariable tmpS1;
ExportVariable tmpS2;
tmpS1.setup("tmpS1", NX, NU, REAL, ACADO_LOCAL);
tmpS2.setup("tmpS2", NX, NY, REAL, ACADO_LOCAL);
setObjS1.setup("setObjS1", tmpFx, tmpFu, tmpObjS, tmpS1);
setObjS1.addVariable( tmpS2 );
setObjS1.addStatement( tmpS2 == (tmpFx ^ tmpObjS) );
setObjS1.addStatement( tmpS1 == tmpS2 * tmpFu );
loopObjective.addFunctionCall(
setObjS1,
tmpFxCall, tmpFuCall, objSCall,
S1.getAddress(runObj * NX, 0)
);
}
evaluateObjective.addStatement( loopObjective );
//
// Evaluate the quadratic Mayer term
//
evaluateObjective.addStatement( objValueIn.getCols(0, NX) == x.getRow( N ) );
evaluateObjective.addStatement( objValueIn.getCols(NX, NX + NOD) == od.getRow( N ) );
// Evaluate the objective function
evaluateObjective.addFunctionCall(evaluateTerminalCost, objValueIn, objValueOut);
evaluateObjective.addLinebreak( );
evaluateObjective.addStatement( DyN.getTranspose() == objValueOut.getCols(0, NYN) );
evaluateObjective.addLinebreak();
if (QN1.isGiven() == false)
{
indexX = getNYN();
ExportVariable tmpQN1, tmpQN2;
tmpQN1.setup("tmpQN1", NX, NX, REAL, ACADO_LOCAL);
tmpQN2.setup("tmpQN2", NX, NYN, REAL, ACADO_LOCAL);
setObjQN1QN2.setup("setObjQN1QN2", tmpFxEnd, tmpObjSEndTerm, tmpQN1, tmpQN2);
setObjQN1QN2.addStatement( tmpQN2 == (tmpFxEnd ^ tmpObjSEndTerm) );
setObjQN1QN2.addStatement( tmpQN1 == tmpQN2 * tmpFxEnd );
if (tmpFxEnd.isGiven() == true)
evaluateObjective.addFunctionCall(
setObjQN1QN2,
tmpFxEnd, objSEndTerm,
QN1.getAddress(0, 0), QN2.getAddress(0, 0)
);
else
evaluateObjective.addFunctionCall(
setObjQN1QN2,
objValueOut.getAddress(0, indexX), objSEndTerm,
QN1.getAddress(0, 0), QN2.getAddress(0, 0)
);
evaluateObjective.addLinebreak( );
}
//
// Hessian setup
//
ExportVariable stageH;
ExportIndex index( "index" );
stageH.setup("stageH", dimHRows, dimHCols, REAL, ACADO_LOCAL);
setStageH.setup("setStageH", stageH, index);
if (Q1.isGiven() == false)
{
if (diagH == false)
setStageH.addStatement(
stageH.getSubMatrix(0, NX, 0, NX) == Q1.getSubMatrix(index * NX, (index + 1) * NX, 0, NX) + evLmX
);
else
for (unsigned el = 0; el < NX; ++el)
setStageH.addStatement(
stageH.getElement(el, 0) == Q1.getElement(index * NX + el, el)
);
}
else
{
setStageH << index.getFullName() << " = " << index.getFullName() << ";\n";
if (diagH == false)
{
setStageH.addStatement(
stageH.getSubMatrix(0, NX, 0, NX) == Q1 + evLmX
);
}
else
{
setStageH.addStatement(
stageH.getRows(0, NX) == Q1.getGivenMatrix().getDiag() + evLmX.getGivenMatrix().getDiag()
);
}
}
setStageH.addLinebreak();
if (R1.isGiven() == false)
{
if (diagH == false)
setStageH.addStatement(
stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1.getSubMatrix(index * NU, (index + 1) * NU, 0, NU) + evLmU
);
else
for (unsigned el = 0; el < NU; ++el)
setStageH.addStatement(
stageH.getElement(NX + el, 0) == R1.getElement(index * NU + el, el)
);
}
else
{
if (diagH == false)
{
setStageH.addStatement(
stageH.getSubMatrix(NX, NX + NU, NX, NX + NU) == R1 + evLmU
);
}
else
{
setStageH.addStatement(
stageH.getRows(NX, NX + NU) == R1.getGivenMatrix().getDiag() + evLmU.getGivenMatrix().getDiag()
);
}
}
setStageH.addLinebreak();
if (diagH == false) {
if (S1.isGiven() == false)
{
setStageH.addStatement(
stageH.getSubMatrix(0, NX, NX, NX + NU) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU)
);
setStageH.addStatement(
stageH.getSubMatrix(NX, NX + NU, 0, NX) == S1.getSubMatrix(index * NX, (index + 1) * NX, 0, NU).getTranspose()
);
}
else if(S1.getGivenMatrix().isZero() == false)
{
setStageH.addStatement(
stageH.getSubMatrix(0, NX, NX, NX + NU) == S1
);
setStageH.addStatement(
stageH.getSubMatrix(NX, NX + NU, 0, NX) == S1.getTranspose()
);
}
}
if (Q1.isGiven() == true && R1.isGiven() == true && S1.isGiven() == true)
{
initialize <<
setStageH.getName() << "( " << objHessians[ 0 ].getFullName() << ", " << "0" << " );\n";
initialize.addLinebreak();
if (diagHN == false)
{
initialize.addStatement(
objHessians[ N ] == QN1 + evLmX
);
}
else
{
initialize.addStatement(
objHessians[ N ] == QN1.getGivenMatrix().getDiag() + evLmX.getGivenMatrix().getDiag()
);
}
}
else
{
for (unsigned i = 0; i < N; ++i)
evaluateObjective.addFunctionCall(setStageH, objHessians[ i ], ExportIndex(i));
evaluateObjective.addLinebreak();
if (diagHN == false)
evaluateObjective.addStatement(
objHessians[ N ] == QN1 + evLmX
);
else
for (unsigned el = 0; el < NX; ++el)
evaluateObjective.addStatement(
objHessians[ N ].getElement(el, 0) == QN1.getElement(el, el) + evLmX.getElement(el, el)
);
}
//
// Gradient setup
//
ExportVariable stagef;
stagef.setup("stagef", NX + NU, 1, REAL, ACADO_LOCAL);
setStagef.setup("setStagef", stagef, index);
if (Q2.isGiven() == false)
setStagef.addStatement(
stagef.getRows(0, NX) == Q2.getSubMatrix(index * NX, (index + 1) * NX, 0, NY) *
Dy.getRows(index * NY, (index + 1) * NY)
);
else
{
setStagef << index.getFullName() << " = " << index.getFullName() << ";\n";
setStagef.addStatement(
stagef.getRows(0, NX) == Q2 *
Dy.getRows(index * NY, (index + 1) * NY)
);
}
setStagef.addLinebreak();
if (R2.isGiven() == false)
setStagef.addStatement(
stagef.getRows(NX, NX + NU) == R2.getSubMatrix(index * NU, (index + 1) * NU, 0, NY) *
Dy.getRows(index * NY, (index + 1) * NY)
);
else
{
setStagef.addStatement(
stagef.getRows(NX, NX + NU) == R2 *
Dy.getRows(index * NY, (index + 1) * NY)
);
}
return SUCCESSFUL_RETURN;
}
returnValue ExportGaussElim::setupFactorization( ExportFunction& _solve, ExportVariable& _swap, ExportVariable& _determinant, const string& absF ) {
uint run1, run2, run3;
ExportIndex i( "i" );
_solve.addIndex( i );
ExportIndex j( "j" );
ExportIndex k( "k" );
ExportVariable indexMax( "indexMax", 1, 1, INT, ACADO_LOCAL, true );
ExportVariable intSwap( "intSwap", 1, 1, INT, ACADO_LOCAL, true );
ExportVariable valueMax( "valueMax", 1, 1, REAL, ACADO_LOCAL, true );
ExportVariable temp( "temp", 1, 1, REAL, ACADO_LOCAL, true );
if( !UNROLLING ) {
_solve.addIndex( j );
_solve.addIndex( k );
_solve.addDeclaration( indexMax );
if( REUSE ) _solve.addDeclaration( intSwap );
_solve.addDeclaration( valueMax );
_solve.addDeclaration( temp );
}
// initialise rk_perm (the permutation vector)
if( REUSE ) {
ExportForLoop loop1( i,0,dim );
loop1 << rk_perm.get( 0,i ) << " = " << i.getName() << ";\n";
_solve.addStatement( loop1 );
}
_solve.addStatement( _determinant == 1 );
if( UNROLLING || dim <= 5 ) {
// Start the factorization:
for( run1 = 0; run1 < (dim-1); run1++ ) {
// Search for pivot in column run1:
for( run2 = run1+1; run2 < dim; run2++ ) {
// add the test (if or else if):
stringstream test;
if( run2 == (run1+1) ) {
test << "if(";
} else {
test << "else if(";
}
test << absF << "(A[" << toString( run2*dim+run1 ) << "]) > " << absF << "(A[" << toString( run1*dim+run1 ) << "])";
for( run3 = run1+1; run3 < dim; run3++ ) {
if( run3 != run2) {
test << " && " << absF << "(A[" << toString( run2*dim+run1 ) << "]) > " << absF << "(A[" << toString( run3*dim+run1 ) << "])";
}
}
test << ") {\n";
_solve.addStatement( test.str() );
// do the row swaps:
// for A:
for( run3 = 0; run3 < dim; run3++ ) {
_solve.addStatement( _swap == A.getSubMatrix( run1,run1+1,run3,run3+1 ) );
_solve.addStatement( A.getSubMatrix( run1,run1+1,run3,run3+1 ) == A.getSubMatrix( run2,run2+1,run3,run3+1 ) );
_solve.addStatement( A.getSubMatrix( run2,run2+1,run3,run3+1 ) == _swap );
}
if( REUSE ) { // rk_perm also needs to be updated if it needs to be possible to reuse the factorization
_solve.addStatement( intSwap == rk_perm.getCol( run1 ) );
_solve.addStatement( rk_perm.getCol( run1 ) == rk_perm.getCol( run2 ) );
_solve.addStatement( rk_perm.getCol( run2 ) == intSwap );
}
_solve.addStatement( "}\n" );
}
// potentially needed row swaps are done
_solve.addLinebreak();
// update of the next rows:
for( run2 = run1+1; run2 < dim; run2++ ) {
_solve << "A[" << toString( run2*dim+run1 ) << "] = -A[" << toString( run2*dim+run1 ) << "]/A[" << toString( run1*dim+run1 ) << "];\n";
_solve.addStatement( A.getSubMatrix( run2,run2+1,run1+1,dim ) += A.getSubMatrix( run2,run2+1,run1,run1+1 ) * A.getSubMatrix( run1,run1+1,run1+1,dim ) );
_solve.addLinebreak();
}
_solve.addStatement( _determinant == _determinant*A.getSubMatrix(run1,run1+1,run1,run1+1) );
_solve.addLinebreak();
}
_solve.addStatement( _determinant == _determinant*A.getSubMatrix(dim-1,dim,dim-1,dim) );
_solve.addLinebreak();
}
else { // without UNROLLING:
_solve << "for( i=0; i < (" << toString( dim-1 ) << "); i++ ) {\n";
_solve << " indexMax = i;\n";
_solve << " valueMax = " << absF << "(A[i*" << toString( dim ) << "+i]);\n";
_solve << " for( j=(i+1); j < " << toString( dim ) << "; j++ ) {\n";
_solve << " temp = " << absF << "(A[j*" << toString( dim ) << "+i]);\n";
_solve << " if( temp > valueMax ) {\n";
_solve << " indexMax = j;\n";
_solve << " valueMax = temp;\n";
_solve << " }\n";
_solve << " }\n";
_solve << " if( indexMax > i ) {\n";
ExportForLoop loop2( k,0,dim );
loop2 << " " << _swap.getFullName() << " = A[i*" << toString( dim ) << "+" << k.getName() << "];\n";
loop2 << " A[i*" << toString( dim ) << "+" << k.getName() << "] = A[indexMax*" << toString( dim ) << "+" << k.getName() << "];\n";
loop2 << " A[indexMax*" << toString( dim ) << "+" << k.getName() << "] = " << _swap.getFullName() << ";\n";
_solve.addStatement( loop2 );
if( REUSE ) {
_solve << " " << intSwap.getFullName() << " = " << rk_perm.getFullName() << "[i];\n";
_solve << " " << rk_perm.getFullName() << "[i] = " << rk_perm.getFullName() << "[indexMax];\n";
_solve << " " << rk_perm.getFullName() << "[indexMax] = " << intSwap.getFullName() << ";\n";
}
_solve << " }\n";
_solve << " " << _determinant.getFullName() << " *= A[i*" << toString( dim ) << "+i];\n";
_solve << " for( j=i+1; j < " << toString( dim ) << "; j++ ) {\n";
_solve << " A[j*" << toString( dim ) << "+i] = -A[j*" << toString( dim ) << "+i]/A[i*" << toString( dim ) << "+i];\n";
_solve << " for( k=i+1; k < " << toString( dim ) << "; k++ ) {\n";
_solve << " A[j*" << toString( dim ) << "+k] += A[j*" << toString( dim ) << "+i] * A[i*" << toString( dim ) << "+k];\n";
_solve << " }\n";
_solve << " }\n";
_solve << "}\n";
_solve << _determinant.getFullName() << " *= A[" << toString( (dim-1)*dim+(dim-1) ) << "];\n";
}
_solve << _determinant.getFullName() << " = " << absF << "(" << _determinant.getFullName() << ");\n";
return SUCCESSFUL_RETURN;
}
returnValue ExplicitRungeKuttaExport::setup( )
{
int sensGen;
get( DYNAMIC_SENSITIVITY,sensGen );
if ( (ExportSensitivityType)sensGen != FORWARD && (ExportSensitivityType)sensGen != NO_SENSITIVITY ) ACADOERROR( RET_INVALID_OPTION );
bool DERIVATIVES = ((ExportSensitivityType)sensGen != NO_SENSITIVITY);
LOG( LVL_DEBUG ) << "Preparing to export ExplicitRungeKuttaExport... " << endl;
// export RK scheme
uint rhsDim = NX*(NX+NU+1);
if( !DERIVATIVES ) rhsDim = NX;
inputDim = NX*(NX+NU+1) + NU + NOD;
if( !DERIVATIVES ) inputDim = NX + NU + NOD;
const uint rkOrder = getNumStages();
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
ExportVariable Ah ( "A*h", DMatrix( AA )*=h );
ExportVariable b4h( "b4*h", DMatrix( bb )*=h );
rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
rk_eta = ExportVariable( "rk_eta", 1, inputDim );
int useOMP;
get(CG_USE_OPENMP, useOMP);
ExportStruct structWspace;
structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
rk_ttt.setup( "rk_ttt", 1, 1, REAL, structWspace, true );
uint timeDep = 0;
if( timeDependant ) timeDep = 1;
rk_xxx.setup("rk_xxx", 1, inputDim+timeDep, REAL, structWspace);
rk_kkk.setup("rk_kkk", rkOrder, rhsDim, REAL, structWspace);
if ( useOMP )
{
ExportVariable auxVar;
auxVar = getAuxVariable();
auxVar.setName( "odeAuxVar" );
auxVar.setDataStruct( ACADO_LOCAL );
rhs.setGlobalExportVariable( auxVar );
diffs_rhs.setGlobalExportVariable( auxVar );
}
ExportIndex run( "run1" );
// setup INTEGRATE function
if( equidistantControlGrid() ) {
integrate = ExportFunction( "integrate", rk_eta, reset_int );
}
else {
integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
}
integrate.setReturnValue( error_code );
rk_eta.setDoc( "Working array to pass the input values and return the results." );
reset_int.setDoc( "The internal memory of the integrator can be reset." );
rk_index.setDoc( "Number of the shooting interval." );
error_code.setDoc( "Status code of the integrator." );
integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
integrate.addIndex( run );
ExportVariable numInt( "numInts", 1, 1, INT );
if( !equidistantControlGrid() ) {
integrate.addStatement( std::string( "int numSteps[" ) + toString( numSteps.getDim() ) + "] = {" + toString( numSteps(0) ) );
uint i;
for( i = 1; i < numSteps.getDim(); i++ ) {
integrate.addStatement( std::string( ", " ) + toString( numSteps(i) ) );
}
integrate.addStatement( std::string( "};\n" ) );
integrate.addStatement( std::string( "int " ) + numInt.getName() + " = numSteps[" + rk_index.getName() + "];\n" );
}
integrate.addStatement( rk_ttt == DMatrix(grid.getFirstTime()) );
if( DERIVATIVES ) {
// initialize sensitivities:
DMatrix idX = eye<double>( NX );
DMatrix zeroXU = zeros<double>( NX,NU );
integrate.addStatement( rk_eta.getCols( NX,NX*(1+NX) ) == idX.makeVector().transpose() );
integrate.addStatement( rk_eta.getCols( NX*(1+NX),NX*(1+NX+NU) ) == zeroXU.makeVector().transpose() );
}
if( inputDim > rhsDim ) {
integrate.addStatement( rk_xxx.getCols( rhsDim,inputDim ) == rk_eta.getCols( rhsDim,inputDim ) );
}
integrate.addLinebreak( );
// integrator loop
ExportForLoop loop;
if( equidistantControlGrid() ) {
loop = ExportForLoop( run, 0, grid.getNumIntervals() );
}
else {
loop = ExportForLoop( run, 0, 1 );
loop.addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
}
for( uint run1 = 0; run1 < rkOrder; run1++ )
{
loop.addStatement( rk_xxx.getCols( 0,rhsDim ) == rk_eta.getCols( 0,rhsDim ) + Ah.getRow(run1)*rk_kkk );
if( timeDependant ) loop.addStatement( rk_xxx.getCol( inputDim ) == rk_ttt + ((double)cc(run1))/grid.getNumIntervals() );
loop.addFunctionCall( getNameDiffsRHS(),rk_xxx,rk_kkk.getAddress(run1,0) );
}
loop.addStatement( rk_eta.getCols( 0,rhsDim ) += b4h^rk_kkk );
loop.addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
// end of integrator loop
if( !equidistantControlGrid() ) {
loop.addStatement( "}\n" );
// loop.unrollLoop();
}
integrate.addStatement( loop );
integrate.addStatement( error_code == 0 );
LOG( LVL_DEBUG ) << "done" << endl;
return SUCCESSFUL_RETURN;
}
returnValue ExportNLPSolver::setConstraints(const OCP& _ocp)
{
////////////////////////////////////////////////////////////////////////////
//
// Extract box constraints
//
////////////////////////////////////////////////////////////////////////////
Grid grid;
Constraint constraints;
_ocp.getGrid( grid );
_ocp.getConstraint( constraints );
VariablesGrid ugrid(NU, grid);
VariablesGrid xgrid(NX, grid);
OCPiterate tmp;
tmp.init(&xgrid, 0, 0, &ugrid, 0);
constraints.getBounds( tmp );
BooleanType isFinite = BT_FALSE;
Vector lbTmp;
Vector ubTmp;
//
// Extract box constraints on inputs
//
for (unsigned i = 0; i < tmp.u->getNumPoints(); ++i)
{
lbTmp = tmp.u->getLowerBounds( i );
ubTmp = tmp.u->getUpperBounds( i );
if ( (ubTmp - lbTmp).isPositive() == BT_FALSE )
return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Some lower bounds are bigger than upper bounds?");
if ( (lbTmp.isFinite( ) == BT_TRUE) || (ubTmp.isFinite( ) == BT_TRUE) )
isFinite = BT_TRUE;
}
if (isFinite == BT_TRUE)
uBounds = *(tmp.u);
else
uBounds.init();
//
// Extract box constraints on states
//
isFinite = BT_FALSE;
xBoundsIdx.clear();
for (unsigned i = 0; i < tmp.x->getNumPoints(); ++i)
{
lbTmp = tmp.x->getLowerBounds( i );
ubTmp = tmp.x->getUpperBounds( i );
if ( (ubTmp - lbTmp).isPositive() == BT_FALSE )
return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Some lower bounds are bigger than upper bounds?");
if ( (lbTmp.isFinite( ) == BT_TRUE) || (ubTmp.isFinite( ) == BT_TRUE) )
isFinite = BT_TRUE;
// This is maybe not necessary
if (isFinite == BT_FALSE || i == 0)
continue;
for (unsigned j = 0; j < lbTmp.getDim(); ++j)
{
if ( ( acadoIsFinite( ubTmp( j ) ) == BT_TRUE ) || ( acadoIsFinite( lbTmp( j ) ) == BT_TRUE ) )
{
xBoundsIdx.push_back(i * lbTmp.getDim() + j);
}
}
}
if ( isFinite == BT_TRUE )
xBounds = *(tmp.x);
else
xBounds.init();
////////////////////////////////////////////////////////////////////////////
//
// Intermezzo - reset static counters
//
////////////////////////////////////////////////////////////////////////////
DifferentialState dummy0;
Control dummy1;
dummy0.clearStaticCounters();
dummy1.clearStaticCounters();
DifferentialState vX( NX );
Control vU( NU );
////////////////////////////////////////////////////////////////////////////
//
// Extract path constraints; pac prefix
//
////////////////////////////////////////////////////////////////////////////
conAuxVar.setName( "conAuxVar" );
conAuxVar.setDataStruct( ACADO_WORKSPACE );
Function pacH;
constraints.getPathConstraints(pacH, pacLBMatrix, pacUBMatrix);
dimPacH = pacH.getDim();
if (dimPacH != 0)
{
Expression expPacH, expPacHx, expPacHu;
pacH.getExpression( expPacH );
expPacHx = forwardDerivative(expPacH, vX);
expPacHu = forwardDerivative(expPacH, vU);
Function pacHx, pacHu;
pacHx << expPacHx;
pacHu << expPacHu;
// Set dimension of residual
pacEvH.setup("evH", N * dimPacH, 1, REAL, ACADO_WORKSPACE);
// Check derivative of path constraints w.r.t. x
if (pacHx.getNX() == 0 && pacHx.getNU() == 0 && pacHx.getNP() == 0)
{
EvaluationPoint epPacHx( pacHx );
Vector v = pacHx.evaluate( epPacHx );
if (v.isZero() == BT_FALSE)
{
// Hard-code derivative evaluation
Matrix m;
m.init(dimPacH, NX);
for (unsigned j = 0; j < dimPacH; ++j)
for (unsigned k = 0; k < NX; ++k)
m(j, k) = v(j * NX + k);
pacEvHx.setup("evHx", m, REAL, ACADO_WORKSPACE);
}
}
else
{
pacH << expPacHx;
pacEvHx.setup("evHx", N * dimPacH, NX, REAL, ACADO_WORKSPACE);
}
// Check derivative of path constraints w.r.t. u
if (pacHu.getNX() == 0 && pacHu.getNU() == 0 && pacHu.getNP() == 0)
{
EvaluationPoint epPacHu( pacHu );
Vector v = pacHu.evaluate( epPacHu );
if (v.isZero() == BT_FALSE)
{
// Hard-code derivative evaluation
Matrix m;
m.init(dimPacH, NU);
for (unsigned j = 0; j < dimPacH; ++j)
for (unsigned k = 0; k < NU; ++k)
m(j, k) = v(j * NU + k);
pacEvHu.setup("evHu", m, REAL, ACADO_WORKSPACE);
}
}
else
{
pacH << expPacHu;
pacEvHu.setup("evHu", N * dimPacH, NU, REAL, ACADO_WORKSPACE);
}
if (performsSingleShooting() == BT_FALSE)
{
pacEvHxd.setup("evHxd", dimPacH, 1, REAL, ACADO_WORKSPACE);
}
conAuxVar.setup("conAuxVar", pacH.getGlobalExportVariableSize(), 1, REAL, ACADO_WORKSPACE);
conValueIn.setup("conValueIn", 1, NX + 0 + NU + NP, REAL, ACADO_WORKSPACE);
conValueOut.setup("conValueOut", 1, pacH.getDim(), REAL, ACADO_WORKSPACE);
evaluatePathConstraints.init(pacH, "evaluatePathConstraints", NX, 0, NU);
evaluatePathConstraints.setGlobalExportVariable( conAuxVar );
}
////////////////////////////////////////////////////////////////////////////
//
// Extract point constraints; poc prefix
//
////////////////////////////////////////////////////////////////////////////
Function pocH;
Expression expPocH, expPocHx, expPocHu;
Matrix pocLBMatrix, pocUBMatrix;
evaluatePointConstraints.resize(N + 1);
unsigned dimPocHMax = 0;
pocLbStack.resize(N + 1);
pocUbStack.resize(N + 1);
// Setup the point constraints
for (unsigned i = 0; i < N + 1; ++i)
{
// Get the point constraint
constraints.getPointConstraint(i, pocH, pocLBMatrix, pocUBMatrix);
// Extract and stack the point constraint if it exists
if ( pocH.getDim() )
{
if (pocH.getNU() > 0 && i == N)
{
return ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "The terminal (point) constraint must not depend on controls.");
}
// Extract the function expression and stack its Jacobians w.r.t.
// x and u
pocH.getExpression( expPocH );
// XXX AFAIK, this is not bullet-proof!
// if (expPocH.getVariableType() != VT_INTERMEDIATE_STATE)
if (expPocH.getVariableType() != VT_UNKNOWN && expPocH.getVariableType() != VT_INTERMEDIATE_STATE)
continue;
expPocHx = forwardDerivative(expPocH, vX);
pocH << expPocHx;
if (i < N)
{
expPocHu = forwardDerivative(expPocH, vU);
pocH << expPocHu;
}
// Stack the new function
evaluatePointConstraints[ i ] = std::tr1::shared_ptr< ExportAcadoFunction >(new ExportAcadoFunction);
String pocFName;
pocFName = "evaluatePointConstraint";
pocFName << String( i );
if (i < N)
{
evaluatePointConstraints[ i ]->init(pocH, pocFName, NX, 0, NU);
}
else
{
evaluatePointConstraints[ i ]->init(pocH, pocFName, NX, 0, 0);
}
// Determine the maximum function dimension
if ( dimPocHMax < (unsigned)pocH.getDim() )
{
dimPocHMax = pocH.getDim();
}
// TODO This is too specific for condensing, thus should be moved to condensing class.
// Stack the lower and upper bounds
pocLB.append( pocLBMatrix.getRow( 0 ) );
pocUB.append( pocUBMatrix.getRow( 0 ) );
pocLbStack[ i ] = pocLBMatrix.getRow( 0 );
pocUbStack[ i ] = pocUBMatrix.getRow( 0 );
}
}
// std::cout << "lb dim: " << pocLB.getDim() << std::endl;
// std::cout << "ub dim: " << pocUB.getDim() << std::endl;
dimPocH = pocLB.getDim();
if ( dimPocH != 0 )
{
unsigned pocAuxVarDim = 0;
ExportVariable pocAuxVarTemp;
for (unsigned i = 0; i < evaluatePointConstraints.size(); ++i)
{
if ( !evaluatePointConstraints[ i ] )
continue;
pocAuxVarTemp = evaluatePointConstraints[ i ]->getGlobalExportVariable();
pocAuxVarDim = pocAuxVarDim < pocAuxVarTemp.getDim() ? pocAuxVarTemp.getDim() : pocAuxVarDim;
evaluatePointConstraints[ i ]->setGlobalExportVariable( conAuxVar );
}
int conAuxVarDim =
(conAuxVar.getDim() < pocAuxVarDim) ? pocAuxVarDim : conAuxVar.getDim();
conAuxVar.setup("conAuxVar", conAuxVarDim, 1, REAL, ACADO_WORKSPACE);
conValueIn.setup("conValueIn", 1, NX + 0 + NU + NP, REAL, ACADO_WORKSPACE);
unsigned conValueOutDim =
(dimPocHMax < conValueOut.getDim()) ? conValueOut.getDim() : dimPocHMax;
conValueOut.setup("conValueOut", 1, conValueOutDim, REAL, ACADO_WORKSPACE);
pocEvH.setup("pocEvH", dimPocH, 1, REAL, ACADO_WORKSPACE);
pocEvHx.setup("pocEvHx", dimPocH, NX, REAL, ACADO_WORKSPACE);
// For this guy we actually need less... but no worry for now
pocEvHu.setup("pocEvHu", dimPocH, NU, REAL, ACADO_WORKSPACE);
// Setup one more variable for MS:
if (performsSingleShooting() == BT_FALSE)
{
pocEvHxd.setup("pocEvHxd", dimPocH, 1, REAL, ACADO_WORKSPACE);
}
}
return SUCCESSFUL_RETURN;
}
returnValue AuxiliaryFunctionsExport::setup( )
{
String fileName( "auxiliary_functions.c" );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Preparing to export %s... ",fileName.getName() );
ExportVariable x ( "x", N+1,NX, REAL,ACADO_VARIABLES );
ExportVariable u ( "u", N,NU, REAL,ACADO_VARIABLES );
ExportVariable p ( "p", 1,NP, REAL,ACADO_VARIABLES );
ExportVariable xRef( "xRef", N+1,NX, REAL,ACADO_VARIABLES );
ExportVariable uRef( "uRef", N,NU, REAL,ACADO_VARIABLES );
getAcadoVariablesX.setup( "getAcadoVariablesX" );
getAcadoVariablesX.setReturnValue( x,BT_TRUE );
getAcadoVariablesU.setup( "getAcadoVariablesU" );
getAcadoVariablesU.setReturnValue( u,BT_TRUE );
getAcadoVariablesXRef.setup( "getAcadoVariablesXRef" );
getAcadoVariablesXRef.setReturnValue( xRef,BT_TRUE );
getAcadoVariablesURef.setup( "getAcadoVariablesURef" );
getAcadoVariablesURef.setReturnValue( uRef,BT_TRUE );
printStates.setup( "printStates" );
printStates.addStatement( ExportPrintf(x.getTranspose()) );
printControls.setup( "printControls" );
printControls.addStatement( ExportPrintf(u.getTranspose()) );
int operatingSystem;
get( OPERATING_SYSTEM,operatingSystem );
getTime.setup( "getTime" );
ExportVariable currentTime( "current_time",(Matrix)0.0 );
currentTime.callByValue();
if ( (OperatingSystem)operatingSystem == OS_WINDOWS )
{
getTime.addStatement( "LARGE_INTEGER counter, frequency;\n" );
getTime.addStatement( "QueryPerformanceFrequency(&frequency);\n" );
getTime.addStatement( "QueryPerformanceCounter(&counter);\n" );
getTime.addStatement( "current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);\n" );
}
else
{
// OS_UNIX
getTime.addStatement( "struct timeval theclock;\n" );
getTime.addStatement( "gettimeofday( &theclock,0 );\n" );
getTime.addStatement( "current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;\n" );
}
getTime.setReturnValue( currentTime );
printHeader.setup( "printHeader" );
printHeader.addStatement( " printf(\"\\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\\nCopyright (C) 2008-2011 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.\\nDeveloped within the Optimization in Engineering Center (OPTEC) under\\nsupervision of Moritz Diehl. All rights reserved.\\n\\nACADO Toolkit is distributed under the terms of the GNU Lesser\\nGeneral Public License 3 in the hope that it will be useful,\\nbut WITHOUT ANY WARRANTY; without even the implied warranty of\\nMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\\nGNU Lesser General Public License for more details.\\n\\n\" );\n" );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
returnValue DiscreteTimeExport::getCode( ExportStatementBlock& code
)
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
if ( useOMP ) {
ExportVariable max = getAuxVariable();
max.setName( "auxVar" );
max.setDataStruct( ACADO_LOCAL );
if( NX2 > 0 ) {
rhs.setGlobalExportVariable( max );
diffs_rhs.setGlobalExportVariable( max );
}
if( NX3 > 0 ) {
rhs3.setGlobalExportVariable( max );
diffs_rhs3.setGlobalExportVariable( max );
}
getDataDeclarations( code, ACADO_LOCAL );
stringstream s;
s << "#pragma omp threadprivate( "
<< max.getFullName() << ", "
<< rk_xxx.getFullName();
if( NX1 > 0 ) {
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev1.getFullName();
s << ", " << rk_diffsNew1.getFullName();
}
if( NX2 > 0 || NXA > 0 ) {
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev2.getFullName();
s << ", " << rk_diffsNew2.getFullName();
}
if( NX3 > 0 ) {
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev3.getFullName();
s << ", " << rk_diffsNew3.getFullName();
s << ", " << rk_diffsTemp3.getFullName();
}
s << " )" << endl << endl;
code.addStatement( s.str().c_str() );
}
if( NX1 > 0 ) {
code.addFunction( lin_input );
code.addStatement( "\n\n" );
}
if( NX2 > 0 ) {
code.addFunction( rhs );
code.addStatement( "\n\n" );
code.addFunction( diffs_rhs );
code.addStatement( "\n\n" );
}
if( NX3 > 0 ) {
code.addFunction( rhs3 );
code.addStatement( "\n\n" );
code.addFunction( diffs_rhs3 );
code.addStatement( "\n\n" );
}
if( !equidistantControlGrid() ) {
ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
code.addDeclaration( numStepsV );
code.addLinebreak( 2 );
}
double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
code.addComment(std::string("Fixed step size:") + toString(h));
code.addFunction( integrate );
return SUCCESSFUL_RETURN;
}
