76 eval =
new SCPevaluation( _userInteraction,objective_,dynamic_discretization_,constraint_,_isCP );
212 iter.
init( x_init, xa_init, p_init, u_init, w_init );
226 cout <<
"--> Computing initial linearization of NLP system ...\n";
235 cout <<
"<-- Computing initial linearization of NLP system done.\n";
237 int useRealtimeIterations;
270 int maxNumberOfSteps;
275 returnvalue =
step( x0_,p_ );
323 printf(
"START OF THE FEEDBACK STEP \n");
366 cout <<
"--> Solving banded QP ...\n";
376 cout <<
"<-- Solving banded QP done.\n";
409 cout <<
"--> Perform globalized SQP step ...\n";
432 cout <<
"<-- Perform globalized SQP step done.\n";
437 int terminateAtConvergence = 0;
496 cout <<
"--> Computing new linearization of NLP system ...\n";
518 cout <<
"<-- Computing new linearization of NLP system done.\n";
537 cout <<
"--> Computing or approximating Hessian matrix ...\n";
550 cout <<
"<-- Computing or approximating Hessian matrix done.\n";
591 cout <<
"SCPmethod::shiftVariables\n" );
606 return iter.
shift( timeShift, lastX, lastXA, lastP, lastU, lastW );
673 int hessianApproximation;
698 int sparseQPsolution;
705 cout <<
"--> Initializing banded QP solver ...\n";
721 cout <<
"<-- Initializing banded QP solver done.\n";
729 int globalizationStrategy;
750 cout <<
"--> Initial integration of dynamic system ...\n";
755 cout <<
"<-- Initial integration of dynamic system done.\n";
807 cout <<
"--> Initializing Hessian computations ...\n";
812 cout <<
"<-- Initializing Hessian computations done.\n";
816 int discretizationMode;
836 double KKTmultiplierRegularisation;
862 cout << setw( 6 ) << right << (int) foo(0, 0) <<
" | ";
864 cout << setw( 6 ) << right << (int) foo(0, 0) <<
" | ";
866 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) <<
" | ";
868 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) <<
" | ";
870 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) <<
" | ";
872 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) <<
" | ";
891 double KKTmultiplierRegularisation;
896 int discretizationMode;
912 <<
"Covergence achieved. Demanded KKT tolerance is " 914 <<
"." << endl << endl;
949 double damping = 1.0;
952 if( run1 == 5 )
break;
966 int useRealtimeIterations;
1063 cout <<
"SCPmethod::getFirstControl\n";
1148 for(
uint i=0; i<
N; ++i )
returnValue setLast(LogName _name, int lastValue, double time=-INFTY)
virtual returnValue apply(BlockMatrix &B, const BlockMatrix &x, const BlockMatrix &y)=0
virtual returnValue getDisturbances(VariablesGrid &w_) const
Implements a very rudimentary block sparse matrix class.
returnValue disableAutoInit()
returnValue addItem(LogName _name, const char *const _label=DEFAULT_LABEL)
virtual returnValue solve(BandedCP &cp)=0
Implements linesearch techniques to perform a globalized step of an SCPmethod for solving NLPs...
DVector getConstraintBlockDims() const
returnValue setReference(const VariablesGrid &ref)
Allows real time measurements based on the system's clock.
returnValue clearDynamicDiscretization()
virtual returnValue getVarianceCovariance(DMatrix &var)
returnValue set(OptionsName name, int value)
virtual returnValue setupLogging()
virtual returnValue reset()
returnValue checkForConvergence()
Implements an exact Hessian computation for obtaining second-order derivatives within NLPsolvers...
UserInteraction * userInteraction
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
BooleanType acadoIsNegative(double x, double TOL)
Stores and evaluates the constraints of optimal control problems.
Provides a time grid consisting of vector-valued optimization variables at each grid point...
virtual returnValue setRealTimeParameters(const DVector &DeltaX, const DVector &DeltaP=emptyConstVector)
Allows to pass back messages to the calling function.
virtual returnValue getParameters(VariablesGrid &p_) const
virtual returnValue getFirstControl(DVector &u0_) const
NLPsolver & operator=(const NLPsolver &rhs)
virtual returnValue evaluate(OCPiterate &iter, BandedCP &cp)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
BlockMatrix lambdaConstraint
returnValue init(const VariablesGrid *const _x, const VariablesGrid *const _xa, const VariablesGrid *const _p, const VariablesGrid *const _u, const VariablesGrid *const _w)
virtual returnValue evaluateLagrangeGradient(uint N, const OCPiterate &iter, const BandedCP &cp, BlockMatrix &nablaL)
returnValue stopClockAndPrintRuntimeProfile()
returnValue printIterate() const
#define CLOSE_NAMESPACE_ACADO
uint getNumConstraintBlocks() const
returnValue printIteration()
virtual returnValue getSensitivitiesXA(BlockMatrix &_sens) const
returnValue initializeHessianProjection()
Base class for discretizing a DifferentialEquation for use in optimal control algorithms.
virtual returnValue initHessian(BlockMatrix &B, uint N, const OCPiterate &iter)=0
virtual double getObjectiveValue() const
virtual returnValue getSensitivitiesP(BlockMatrix &_sens) const
virtual returnValue getSensitivitiesX(BlockMatrix &_sens) const
BlockMatrix lambdaDynamic
virtual returnValue prepareNextStep()
virtual double getObjectiveValue() const
virtual returnValue performStep(OCPiterate &iter, BandedCP &cp, SCPevaluation *eval)=0
Implements a constant Hessian as approximation of second-order derivatives within NLPsolvers...
virtual double getKKTtolerance(const OCPiterate &iter, const BandedCP &cp, double KKTmultiplierRegularisation=0.0)
returnValue setupRealTimeParameters(const DVector &x0_=emptyConstVector, const DVector &p_=emptyConstVector)
returnValue computeHessianMatrix(const BlockMatrix &oldLagrangeGradient, const BlockMatrix &newLagrangeGradient)
virtual returnValue getVarianceCovariance(DMatrix &var)
Implements a Gauss-Newton approximation as second-order derivatives within NLPsolvers.
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
uint getNumPoints() const
virtual returnValue getSensitivitiesW(BlockMatrix &_sens) const
virtual returnValue finalizeSolve(BandedCP &cp)
Implements a fullstep to perform a step of an SCPmethod for solving NLPs.
virtual returnValue step(const DVector &x0_=emptyConstVector, const DVector &p_=emptyConstVector)
double getHessianScaling() const
returnValue init(uint _nRows, uint _nCols)
BandedCPsolver * bandedCPsolver
SCPmethod & operator=(const SCPmethod &rhs)
returnValue checkForRealTimeMode(const DVector &x0_, const DVector &p_)
virtual returnValue print(std::ostream &stream=std::cout, const std::string &name=DEFAULT_LABEL, const std::string &startString=DEFAULT_START_STRING, const std::string &endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const std::string &colSeparator=DEFAULT_COL_SEPARATOR, const std::string &rowSeparator=DEFAULT_ROW_SEPARATOR) const
returnValue printLogRecord(std::ostream &_stream, int idx, LogPrintMode _mode=PRINT_ITEM_BY_ITEM) const
BooleanType isInRealTimeMode
virtual returnValue evaluateSensitivities(const OCPiterate &iter, BandedCP &cp)
NLPderivativeApproximation * derivativeApproximation
virtual returnValue getFirstControl(DVector &u0_) const =0
returnValue acadoPrintCopyrightNotice(const std::string &subpackage)
returnValue replot(PlotFrequency _frequency=PLOT_IN_ANY_CASE)
Encapsulates all user interaction for setting options, logging data and plotting results.
virtual returnValue getDifferentialStates(VariablesGrid &xd_) const
virtual returnValue performCurrentStep()
Implements a Gauss-Newton approximation with block BFGS updates as second-order derivatives within NL...
void rhs(const real_t *x, real_t *f)
Implements different sequential convex programming methods for solving NLPs.
Implements BFGS updates for approximating second-order derivatives within NLPsolvers.
virtual returnValue initScaling(BlockMatrix &B, const BlockMatrix &x, const BlockMatrix &y)=0
virtual returnValue start()
Base class for different algorithms for solving nonlinear programming (NLP) problems.
virtual returnValue getSensitivitiesU(BlockMatrix &_sens) const
BooleanType hasLSQobjective() const
virtual returnValue shift(double timeShift=-1.0, DVector lastX=emptyVector, DVector lastXA=emptyVector, DVector lastP=emptyVector, DVector lastU=emptyVector, DVector lastW=emptyVector)
virtual returnValue init(const OCPiterate &iter)
virtual returnValue getControls(VariablesGrid &u_) const
virtual NLPsolver * clone() const
virtual returnValue init(VariablesGrid *x_init, VariablesGrid *xa_init, VariablesGrid *p_init, VariablesGrid *u_init, VariablesGrid *w_init)
virtual returnValue stop()
virtual returnValue printRuntimeProfile() const
DVector getVector(uint pointIdx) const
virtual returnValue feedbackStep(const DVector &x0_, const DVector &p_=emptyConstVector)
virtual returnValue getAnySensitivities(BlockMatrix &_sens, uint idx) const
Allows to setup and store user-specified log records of algorithmic information.
Base class for different ways to evaluate functions and derivatives within an SCPmethod for solving N...
#define BEGIN_NAMESPACE_ACADO
int addLogRecord(LogRecord &_record)
uint getNumConstraints() const
virtual returnValue setReference(const VariablesGrid &ref)
returnValue print() const
BooleanType needToReevaluate
virtual returnValue getAlgebraicStates(VariablesGrid &xa_) const
virtual returnValue shiftVariables(double timeShift=-1.0, DVector lastX=emptyVector, DVector lastXA=emptyVector, DVector lastP=emptyVector, DVector lastU=emptyVector, DVector lastW=emptyVector)
Solves banded conic programs arising in optimal control using condensing.
virtual returnValue solve(const DVector &x0_=emptyConstVector, const DVector &p_=emptyConstVector)
virtual returnValue prepareSolve(BandedCP &cp)
virtual returnValue init(const OCPiterate &iter_)=0
Stores and evaluates the objective function of optimal control problems.
#define ACADOERROR(retval)
BooleanType hasPerformedStep
returnValue getLast(LogName _name, DMatrix &lastValue) const
virtual returnValue getTime(double &_elapsedTime)