returnValue Integrator::setForwardSeed( const int &order,
const DVector &xSeed,
const DVector &pSeed,
const DVector &uSeed,
const DVector &wSeed ){
int run1;
if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
DVector tmpX;
DVector components = rhs->getDifferentialStateComponents();
dP = pSeed;
dU = uSeed;
dW = wSeed;
if( xSeed.getDim() != 0 ){
tmpX.init( components.getDim() );
for( run1 = 0; run1 < (int) components.getDim(); run1++ )
tmpX(run1) = xSeed((int) components(run1));
}
return setProtectedForwardSeed( tmpX, pSeed, uSeed, wSeed, order );
}
returnValue OCP::subjectTo( const DVector& _lb, const Expression& _expr, const DVector& _ub )
{
ASSERT(_lb.getDim() == _expr.getDim() && _lb.getDim() == _ub.getDim());
constraint->add(_lb, _expr, _ub);
return SUCCESSFUL_RETURN;
}
BooleanType RungeKuttaExport::checkSymmetry( const DVector& _cc ) {
if( _cc.getDim() <= 1 ) return BT_FALSE;
BooleanType symmetry = BT_TRUE;
uint i;
for( i = 0; i < _cc.getDim(); i++ ) {
int tmp = acadoRoundAway(1.0 - _cc(i) - _cc(_cc.getDim()-1-i));
if( symmetry ) symmetry = (tmp == 0);
}
return symmetry;
}
returnValue RungeKuttaExport::initializeButcherTableau( const DMatrix& _AA, const DVector& _bb, const DVector& _cc ) {
if( _cc.isEmpty() || !_AA.isSquare() || _AA.getNumRows() != _bb.getDim() || _bb.getDim() != _cc.getDim() ) return RET_INVALID_OPTION;
numStages = _cc.getDim();
is_symmetric = checkSymmetry( _cc );
// std::cout << "Symmetry of the chosen method: " << is_symmetric << "\n";
AA = _AA;
bb = _bb;
cc = _cc;
return SUCCESSFUL_RETURN;
}
returnValue Integrator::evaluateTransition( const double time, DVector &xd, const DVector &xa,
const DVector &p, const DVector &u, const DVector &w ){
ASSERT( transition != 0 );
EvaluationPoint z( *transition, xd.getDim(), xa.getDim(), p.getDim(), u.getDim(), w.getDim() );
z.setT ( time );
z.setX ( xd );
z.setXA( xa );
z.setP ( p );
z.setU ( u );
z.setW ( w );
xd = transition->evaluate( z );
return SUCCESSFUL_RETURN;
}
returnValue Integrator::integrate( const Grid &t_ ,
const DVector &x0 ,
const DVector &xa ,
const DVector &p ,
const DVector &u ,
const DVector &w ){
int run1;
returnValue returnvalue;
if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
DVector tmpX;
DVector components = rhs->getDifferentialStateComponents();
const int N = components.getDim();
if( x0.getDim() != 0 ){
tmpX.init( components.getDim() );
for( run1 = 0; run1 < (int) components.getDim(); run1++ )
tmpX(run1) = x0((int) components(run1));
}
// tmpX.print( "integrator x0" );
// u.print( "integrator u0" );
// p.print( "integrator p0" );
returnvalue = evaluate( tmpX, xa, p, u, w, t_ );
if( returnvalue != SUCCESSFUL_RETURN )
return returnvalue;
xE.init(rhs->getDim());
xE.setZero();
DVector tmp(rhs->getDim());
getProtectedX(&tmp);
for( run1 = 0; run1 < N; run1++ )
xE((int) components(run1)) = tmp(run1);
for( run1 = N; run1 < N + ma; run1++ )
xE(run1) = tmp(run1);
if( transition != 0 )
returnvalue = evaluateTransition( t_.getLastTime(), xE, xa, p, u, w );
return returnvalue;
}
returnValue Integrator::getBackwardSensitivities( DVector &DX,
DVector &DP ,
DVector &DU ,
DVector &DW ,
int order ) const{
int run2;
returnValue returnvalue;
DVector tmpX ( rhs->getDim() );
DX.setZero();
DP.setZero();
DU.setZero();
DW.setZero();
returnvalue = getProtectedBackwardSensitivities( tmpX, DP, DU, DW, order );
DVector components = rhs->getDifferentialStateComponents();
for( run2 = 0; run2 < (int) components.getDim(); run2++ )
DX((int) components(run2)) = tmpX(run2);
for( run2 = 0; run2 < (int) dPb.getDim(); run2++ )
DP(run2) += dPb(run2);
for( run2 = 0; run2 < (int) dUb.getDim(); run2++ )
DU(run2) += dUb(run2);
for( run2 = 0; run2 < (int) dWb.getDim(); run2++ )
DW(run2) += dWb(run2);
return returnvalue;
}
returnValue Integrator::setBackwardSeed( const int &order,
const DVector &seed ){
dXb = seed;
uint run1;
DVector tmp( seed.getDim() );
DVector components = rhs->getDifferentialStateComponents();
if( seed.getDim() != 0 ){
tmp.init( components.getDim() );
for( run1 = 0; run1 < components.getDim(); run1++ )
tmp(run1) = seed((int) components(run1));
}
return setProtectedBackwardSeed( tmp, order );
}
returnValue Integrator::diffTransitionForward( DVector &DX,
const DVector &DP,
const DVector &DU,
const DVector &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 Integrator::diffTransitionBackward( DVector &DX,
DVector &DP,
DVector &DU,
DVector &DW,
int &order ){
ASSERT( transition != 0 );
if( order != 1 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
EvaluationPoint z( *transition, DX.getDim(), 0, DP.getDim(), DU.getDim(), DW.getDim() );
transition->AD_backward( DX, z );
DX = z.getX();
DP = z.getP();
DU = z.getU();
DW = z.getW();
return SUCCESSFUL_RETURN;
}
OCP::OCP( const double &tStart_, const double &tEnd_, const DVector& _numSteps )
: MultiObjectiveFunctionality(),
grid(new Grid()), objective(new Objective()), constraint(new Constraint())
{
if( _numSteps.getDim() <= 0 ) ACADOERROR( RET_INVALID_ARGUMENTS );
DVector 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);
}
BooleanType GnuplotWindow::getMouseEvent( double &mouseX, double &mouseY )
{
DVector tmp;
if (tmp.read( "mouse.dat" ) != SUCCESSFUL_RETURN)
return BT_FALSE;
mouseX = tmp( tmp.getDim()-2 );
mouseY = tmp( tmp.getDim()-1 );
if ( system("rm mouse.dat") )
return BT_FALSE;
return BT_TRUE;
}
returnValue VariablesGrid::setVector( uint pointIdx,
const DVector& _values
)
{
if ( pointIdx >= getNumPoints( ) )
return ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
if ( _values.getDim( ) != getNumRows( pointIdx ) )
return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
for( uint j=0; j<getNumRows( ); ++j )
operator()( pointIdx,j ) = _values( j );
return SUCCESSFUL_RETURN;
}
returnValue Integrator::integrateSensitivities( ){
uint run1;
returnValue returnvalue;
if( ( nBDirs > 0 || nBDirs2 > 0 ) && transition != 0 ){
int order;
if( nBDirs2 > 0 ) order = 2;
else order = 1;
returnvalue = diffTransitionBackward( dXb, dPb, dUb, dWb, order );
setBackwardSeed( order, dXb );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
returnvalue = evaluateSensitivities();
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
if( nBDirs > 0 || nBDirs2 > 0 ) return SUCCESSFUL_RETURN;
int order = 1;
if( nFDirs2 > 0 ) order = 2;
DMatrix tmp( rhs->getDim(), 1 );
returnvalue = getProtectedForwardSensitivities(&tmp,order);
DVector components = rhs->getDifferentialStateComponents();
dX.init(rhs->getDim()-ma);
dX.setZero();
for( run1 = 0; run1 < components.getDim(); run1++ )
dX((int) components(run1)) = tmp(run1,0);
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
if( transition != 0 )
returnvalue = diffTransitionForward( dX, dP, dU, dW, order );
return returnvalue;
}
returnValue Actuator::setParameterDeadTimes( const DVector& _deadTimes
)
{
if ( _deadTimes.getDim( ) != getNP( ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( _deadTimes.getMin( ) < 0.0 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
if ( deadTimes.getDim( ) == 0 )
return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
for( uint i=0; i<getNP(); ++i )
deadTimes( getNU()+i ) = _deadTimes( i );
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 );
DVector components = rhs->getDifferentialStateComponents();
DVector tmpX ( components.getDim(), x0 );
DVector tmpXA( rhs->getNXA() , xa );
DVector tmpP ( rhs->getNP () , p );
DVector tmpU ( rhs->getNU () , u );
DVector tmpW ( rhs->getNW () , w );
return integrate( t_, tmpX, tmpXA, tmpP, tmpU, tmpW );
}
returnValue OCP::subjectTo( int _index, const DVector& _lb, const Expression& _expr, const DVector& _ub )
{
ASSERT(_index >= AT_START);
cout << _lb.getDim() << " " << _expr.getDim() << endl;
ASSERT(_lb.getDim() == _expr.getDim());
ASSERT(_lb.getDim() == _ub.getDim());
if (_index == AT_START)
{
for (unsigned el = 0; el < _lb.getDim(); ++el)
ACADO_TRY( constraint->add(0, _lb( el ), _expr( el ), _ub( el )) );
}
else if (_index == AT_END)
{
for (unsigned el = 0; el < _lb.getDim(); ++el)
ACADO_TRY(constraint->add(grid->getLastIndex(), _lb( el ), _expr( el ), _ub( el )) );
}
else
for (unsigned el = 0; el < _lb.getDim(); ++el)
constraint->add(_index, _lb( el ), _expr( el ), _ub( el ));
return SUCCESSFUL_RETURN;
}
returnValue SIMexport::exportAcadoHeader( const std::string& _dirName,
const std::string& _fileName,
const std::string& _realString,
const std::string& _intString,
int _precision
) const
{
string moduleName;
get(CG_MODULE_NAME, moduleName);
int qpSolver;
get(QP_SOLVER, qpSolver);
int useSinglePrecision;
get(USE_SINGLE_PRECISION, useSinglePrecision);
string fileName;
fileName = _dirName + "/" + _fileName;
map<string, pair<string, string> > options;
DVector nMeasV = getNumMeas();
DVector nOutV = getDimOutputs();
options[ "ACADO_N" ] = make_pair(toString( getN() ), "Number of control/estimation intervals.");
options[ "ACADO_NX" ] = make_pair(toString( getNX() ), "Number of differential variables.");
options[ "ACADO_NXD" ] = make_pair(toString( getNDX() ), "Number of differential derivative variables.");
options[ "ACADO_NXA" ] = make_pair(toString( getNXA() ), "Number of algebraic variables.");
options[ "ACADO_NU" ] = make_pair(toString( getNU() ), "Number of control variables.");
options[ "ACADO_NOD" ] = make_pair(toString( getNOD() ), "Number of online data values.");
options[ "ACADO_NUMOUT" ] = make_pair(toString( nOutV.getDim() ), "Number of output functions.");
if( !nMeasV.isEmpty() && !nOutV.isEmpty() ) {
std::ostringstream acado_nout;
ExportVariable( "ACADO_NOUT",nOutV,STATIC_CONST_INT ).exportDataDeclaration(acado_nout);
std::ostringstream acado_nmeas;
ExportVariable( "ACADO_NMEAS",nMeasV,STATIC_CONST_INT ).exportDataDeclaration(acado_nmeas);
options[ "ACADO_OUTPUTS_DEFINED" ] = make_pair("\n" + acado_nout.str() + acado_nmeas.str(), "Dimension and measurements of the output functions per shooting interval.");
}
//
// ACADO variables and workspace
//
ExportStatementBlock variablesBlock;
stringstream variables;
if (collectDataDeclarations(variablesBlock, ACADO_VARIABLES) != SUCCESSFUL_RETURN)
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
variablesBlock.exportCode(variables, _realString, _intString, _precision);
ExportStatementBlock workspaceBlock;
stringstream workspace;
if (collectDataDeclarations(workspaceBlock, ACADO_WORKSPACE) != SUCCESSFUL_RETURN)
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
workspaceBlock.exportCode(workspace, _realString, _intString, _precision);
ExportStatementBlock functionsBlock;
stringstream functions;
if (collectFunctionDeclarations( functionsBlock ) != SUCCESSFUL_RETURN)
return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
functionsBlock.exportCode(functions, _realString);
ExportCommonHeader ech(fileName, "", _realString, _intString, _precision);
ech.configure( moduleName, useSinglePrecision, false, (QPSolverName)qpSolver,
options, variables.str(), workspace.str(), functions.str());
return ech.exportCode();
}
returnValue WeightGeneration::generateWeights( const int &n ,
DVector &weight ,
DMatrix &Weights ,
const DVector &weightsLB ,
const DVector &weightsUB ,
DVector &formers ,
const int &layer ,
int &lastOne ,
int ¤tOne,
double &step
) const{
int run1, run2;
if( n == layer ){
//printf("case1: n = %d layer = %d \n", n, layer );
double weight_test = 1.0;
for( run1 = 0; run1 < (int) weight.getDim()-1; run1++ )
weight_test -= weight(run1);
if( ( weight_test >= weightsLB(layer) - 100.0*EPS ) &&
( weight_test <= weightsUB(layer) + 100.0*EPS ) ){
if( currentOne >= 0 ){
DVector tmp( formers.getDim()+1 );
for( run1 = 0; run1 < (int) formers.getDim(); run1++ )
tmp(run1) = formers(run1);
tmp(formers.getDim()) = lastOne;
formers = tmp;
lastOne = currentOne;
currentOne = -1;
}
else{
DVector tmp( formers.getDim()+1 );
for( run1 = 0; run1 < (int) formers.getDim(); run1++ )
tmp(run1) = formers(run1);
tmp(formers.getDim()) = Weights.getNumCols();
formers = tmp;
}
weight(n) = weight_test;
DMatrix tmp( weight.getDim(), Weights.getNumCols()+1 );
for( run1 = 0; run1 < (int) Weights.getNumRows(); run1++ ){
for( run2 = 0; run2 < (int) Weights.getNumCols(); run2++ )
tmp(run1,run2) = Weights(run1,run2);
}
for( run2 = 0; run2 < (int) weight.getDim(); run2++ )
tmp(run2,Weights.getNumCols()) = weight(run2);
Weights = tmp;
}
}
else{
//printf("case2: n = %d layer = %d \n", n, layer );
double weight_test = weightsLB(n);
while( weight_test <= weightsUB(n) + 100.0*EPS ){
if( n == layer-2 ) currentOne = Weights.getNumCols()+1;
weight(n) = weight_test;
WeightGeneration child;
child.generateWeights( n+1, weight, Weights, weightsLB, weightsUB, formers, layer, lastOne, currentOne, step );
weight_test += (weightsUB(n)-weightsLB(n))*step;
}
}
return SUCCESSFUL_RETURN;
}
