38 #include <acado/code_generation/templates/templates.hpp> 45 const std::string& _commonHeaderName
58 LOG(
LVL_DEBUG ) <<
"Solver: setup initialization... " << endl;
62 initializeForces.
setName(
"initializeForces" );
125 code.
addStatement(
"/******************************************************************************/\n" );
129 code.
addStatement(
"/******************************************************************************/\n" );
173 return (
N + 1) *
NX +
N *
NU;
196 int forceDiagHessian;
201 unsigned dimHRows =
NX +
NU;
202 unsigned dimHCols =
NX +
NU;
203 unsigned dimHNRows =
NX;
204 unsigned dimHNCols =
NX;
205 if (
objS.
isGiven() ==
true || forceDiagHessian ==
true)
220 for (
unsigned i = 0; i <
N; ++i)
227 for (
unsigned i = 0; i <
N; ++i)
276 unsigned indexX =
getNY();
310 if (tmpFx.isGiven() ==
true)
312 if (variableObjS ==
YES)
331 if (variableObjS ==
YES)
365 if (tmpFu.isGiven() ==
true)
367 if (variableObjS ==
YES)
386 if (variableObjS ==
YES)
437 tmpFxCall, tmpFuCall, objSCall,
469 if (tmpFxEnd.isGiven() ==
true)
501 for (
unsigned el = 0; el <
NX; ++el)
531 for (
unsigned el = 0; el <
NU; ++el)
553 if (
diagH ==
false) {
594 for (
unsigned i = 0; i <
N; ++i)
602 for (
unsigned el = 0; el <
NX; ++el)
689 for (
unsigned j = 0; j < lbTmp.
getDim(); ++j)
718 for (
unsigned j = 0; j < lbTmp.
getDim(); ++j)
739 for (
unsigned i = 0; i < N + 1; ++i)
745 int hardcodeConstraintValues;
748 if (!hardcodeConstraintValues && numBounds > 0)
762 for (
unsigned i = 0; i < N + 1; ++i) {
765 if( hardcodeConstraintValues ) {
782 for (
unsigned i = 0; i < N + 1; ++i)
785 if( hardcodeConstraintValues ) {
817 for (
unsigned i = 0; i <
N; ++i)
851 for (
unsigned i = start; i <
N; ++i)
865 for (
unsigned i = 0; i < dNum; ++i)
874 stagedNew == zeros<double>(NX, 1) -
d.
getRows(index * NX, (index + 1) * NX)
883 for (
unsigned i = 1; i < dNum; ++i)
889 for (
unsigned i = 0; i < dNum; ++i)
903 x0.
setDoc(
"Current state feedback vector." );
925 retSim.
setDoc(
"Status of the integration module. =0: OK, otherwise the error code.");
940 returnValueFeedbackPhase.
setDoc(
"Status code of the FORCES QP solver." );
942 feedback.
doc(
"Feedback/estimation step of the RTI scheme." );
955 for (
unsigned i = 0; i <
N; ++i)
963 std::vector< ExportVariable > vecQPVars;
966 vecQPVars.resize(N + 1);
967 for (
unsigned i = 0; i <
N; ++i)
979 std::vector< ExportVariable > zInit;
983 for (
unsigned i = 0; i <
N; ++i)
985 string name =
"z_init_";
986 name = name + (i < 10 ?
"0" :
"") +
toString( i );
989 string name =
"z_init_";
990 name = name + (N < 10 ?
"0" :
"") +
toString( N );
1018 solveQP.
setup(
"solve");
1022 << returnValueFeedbackPhase.
getFullName() <<
" = " 1026 <<
"&" <<
qpObjPrefix <<
"_" <<
"info" <<
" , NULL);\n";
1035 acc.
setup(
"accumulate", stageOut, index);
1042 for (
unsigned i = 0; i <
N; ++i)
1057 getKKT.
doc(
"Get the KKT tolerance of the current iterate. Under development." );
1091 tmp1.getDataStructString(),
1106 string outFile = folderName +
"/acado_forces_generator.m";
1110 int maxNumQPiterations;
1117 if ( maxNumQPiterations <= 0 )
1151 outFile = folderName +
"/acado_forces_generator.py";
#define LOG(level)
Just define a handy macro for getting the logger.
Lowest level, the debug level.
std::vector< ExportVariable > cond
virtual returnValue setupEvaluation()
ExportFunction evaluateObjective
ExportVariable getRow(const ExportIndex &idx) const
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
virtual returnValue setupVariables()
ExportFunction shiftStates
ExportFunction & setName(const std::string &_name)
ExportVariable getTranspose() const
bool initialStateFixed() const
virtual returnValue setupObjectiveEvaluation(void)
std::vector< std::vector< unsigned > > conLBIndices
ExportVariable & setup(const std::string &_name, uint _nRows=1, uint _nCols=1, ExportType _type=REAL, ExportStruct _dataStruct=ACADO_LOCAL, bool _callItByValue=false, const std::string &_prefix=std::string())
bool isGiven(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
std::vector< ExportVariable > conUB
Allows to pass back messages to the calling function.
Generator of the FORCES interface, the to, be called from MATLAB.
DVector getUpperBounds(uint pointIdx) const
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportVariable objEvFxEnd
ExportFunction preparation
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
unsigned getNumLowerBounds() const
const DMatrix & getGivenMatrix() const
Allows to export code of a for-loop.
string toString(T const &value)
unsigned getNumUpperBounds() const
std::vector< unsigned > conABDimensions
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
#define CLOSE_NAMESPACE_ACADO
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
Defines a scalar-valued index variable to be used for exporting code.
std::vector< std::vector< double > > conLBValues
Base class for export of NLP/OCP solvers.
ExportAcadoFunction evaluateStageCost
Defines a matrix-valued variable that can be passed as argument to exported functions.
ExportAcadoFunction evaluateTerminalCost
ExportFunction & setup(const std::string &_name="defaultFunctionName", const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)
virtual returnValue setDoc(const std::string &_doc)
virtual returnValue setupInitialization()
std::vector< ExportVariable > objGradients
std::string commonHeaderName
ExportFunction modelSimulation
virtual ExportFunction & doc(const std::string &_doc)
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
ExportFunction setObjR1R2
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
ExportFunction getObjective
ExportGaussNewtonForces(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
ExportFunction setObjQ1Q2
ExportFunction setObjQN1QN2
std::vector< ExportVariable > conLB
virtual returnValue getCode(ExportStatementBlock &code)
ExportVariable objValueIn
ExportFunction initializeNodes
double levenbergMarquardt
A class for configuration and export for interface to the FORCES QP solver.
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
virtual returnValue setupMultiplicationRoutines()
virtual returnValue setupSimulation(void)
Encapsulates all user interaction for setting options, logging data and plotting results.
Allows to export code of an arbitrary function.
virtual returnValue setup()
virtual uint getDim() const
ExportFunction shiftControls
returnValue addStatement(const ExportStatement &_statement)
std::string getFullName() const
returnValue addLinebreak(uint num=1)
std::vector< ExportVariable > conC
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
ExportFunction evaluateConstraints
std::shared_ptr< ExportForcesInterface > qpInterface
ExportFunction & addVariable(const ExportVariable &_var)
BooleanType acadoIsFinite(double x, double TOL)
uint getNumPoints() const
virtual returnValue setupQPInterface()
std::vector< std::vector< unsigned > > conUBIndices
ExportVariable objSEndTerm
std::string getName() const
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
returnValue setupAuxiliaryFunctions()
DVector getLowerBounds(uint pointIdx) const
std::vector< ExportVariable > objHessians
#define BEGIN_NAMESPACE_ACADO
BooleanType isFinite(const T &_value)
USING_NAMESPACE_ACADO void output(const char *name, const Expression &e)
ExportFunction conSetGxGu
returnValue addFunction(const ExportFunction &_function)
virtual returnValue setupConstraintsEvaluation(void)
GenericVector< T > getDiag() const
unsigned getNumQPvars() const
std::shared_ptr< ExportForcesGenerator > qpGenerator
Allows to export code for a block of statements.
ExportFunction initialize
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
ExportVariable objValueOut
ExportFunction & addIndex(const ExportIndex &_index)
std::vector< std::vector< double > > conUBValues
std::string getDataStructString() const
Defines a matrix-valued variable to be used for exporting code.
returnValue addFunctionCall(const std::string &_fName, const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)