41 const std::string& _commonHeaderName
48 return ACADOERRORTEXT(
RET_INVALID_OPTION,
"Impossible to perform full condensing, when the initial state is not fixed. You can use regular condensing instead." );
54 LOG(
LVL_DEBUG ) <<
"Solver: setup initialization... " << endl;
58 LOG(
LVL_DEBUG ) <<
"Solver: setup variables... " << endl;
62 LOG(
LVL_DEBUG ) <<
"Solver: setup multiplication routines... " << endl;
66 LOG(
LVL_DEBUG ) <<
"Solver: setup model simulation... " << endl;
70 LOG(
LVL_DEBUG ) <<
"Solver: setup objective evaluation... " << endl;
74 LOG(
LVL_DEBUG ) <<
"Solver: setup condensing... " << endl;
78 LOG(
LVL_DEBUG ) <<
"Solver: setup constraints... " << endl;
82 LOG(
LVL_DEBUG ) <<
"Solver: setup evaluation... " << endl;
86 LOG(
LVL_DEBUG ) <<
"Solver: setup auxiliary functions... " << endl;
168 code.
addStatement(
"/******************************************************************************/\n" );
172 code.
addStatement(
"/******************************************************************************/\n" );
265 return (
NX +
N * NU);
326 unsigned indexX =
getNY();
398 tmpFxCall, tmpFuCall, objSCall,
447 int hardcodeConstraintValues;
450 int hessianApproximation;
463 DVector lbValuesMatrix( numBounds );
464 DVector ubValuesMatrix( numBounds );
467 for(
unsigned run1 = 0; run1 <
NX; ++run1)
473 for(
unsigned run1 = 0; run1 <
N; ++run1)
474 for(
unsigned run2 = 0; run2 <
NU; ++run2)
481 if (hardcodeConstraintValues ==
YES)
535 unsigned numPathCon = N *
dimPacH;
536 unsigned numPointCon =
dimPocH;
543 for(
unsigned i = 0; i <
xBoundsIdx.size(); ++i)
560 unsigned blk = conIdx / NX + 1;
563 unsigned blkRow = (
col * (2 * N -
col - 1) / 2 + blk - 1) * NX + conIdx %
NX;
575 DMatrix vXBoundIndices(1, numStateBounds);
576 for (
unsigned i = 0; i < numStateBounds; ++i)
598 lCol.
addStatement( blkRow == (col * (2 * N - col - 1) / 2 + blk - 1) * NX + conIdx % NX );
600 A.
getSubMatrix(row, row + 1, offsetBounds + col * NU, offsetBounds + (col + 1) * NU ) ==
E.
getRow( blkRow ) );
617 if (hardcodeConstraintValues ==
YES)
654 unsigned rowOffset = numStateBounds;
692 getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX)
696 derOffset = derOffset + dimPacH *
NX;
702 getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU)
706 derOffset = derOffset + dimPacH *
NU;
775 for (
unsigned row = 0;
row < N - 1; ++
row)
807 rowOffset + (iRow + 1) * dimPacH,
808 colOffset + iCol * NU,
809 colOffset + (iCol + 1) * NU)
814 for (
unsigned row = 0;
row < N - 1; ++
row)
818 unsigned blk = (
col * (2 * N -
col - 1) / 2 +
row);
819 unsigned row2 =
row + 1;
851 eLoopJ.
addStatement( blk == (col * (2 * N - col - 1) / 2 + row) );
887 for (
unsigned i = 0; i <
N; ++i)
892 rowOffset + i * dimPacH,
893 rowOffset + (i + 1) * dimPacH,
895 colOffset + (i + 1) * NU)
901 rowOffset + i * dimPacH,
902 rowOffset + (i + 1) * dimPacH,
904 colOffset + (i + 1) * NU)
906 i * dimPacH,(i + 1) * dimPacH, 0, NU)
944 for (
unsigned i = 0; i <
N; ++i)
993 for (
unsigned i = 0, intRowOffset = 0; i < N + 1; ++i)
1026 (intRowOffset + dim) * NX)
1034 (intRowOffset + dim) * NU)
1036 dim + dim * NX + dim * NU));
1040 intRowOffset += dim;
1046 for (
unsigned row = 0, intRowOffset = 0;
row < N + 1; ++
row)
1052 string(
"Evaluating multiplications of constraint functions on node: #" ) +
toString(
row ) );
1089 A.
getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim, 0, NX) ==
1102 eLoop.
addStatement( iBlk == (iCol * (2 * N - iCol - 1) / 2 +
row-1) );
1104 A.
getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
1105 colOffset + iCol * NU, colOffset + (iCol + 1) * NU) ==
1118 A.
getSubMatrix(rowOffset + intRowOffset, rowOffset + intRowOffset + dim,
1119 colOffset +
row * NU, colOffset + (
row + 1) * NU) ==
1135 intRowOffset += dim;
1202 LOG(
LVL_DEBUG ) <<
"Setup condensing: H00, H10" << endl;
1265 LOG(
LVL_DEBUG ) <<
"Setup condensing: H11 and E" << endl;
1331 int offset =
col * (2 * N -
col + 1) / 2;
1350 for (
unsigned row = N - 1; col <
row; --
row)
1363 S1Call,
E.
getAddress((offset + row - col - 1) * NX),
1398 cLoop.
addStatement( offset == col * (2 *
N + 1 - col) / 2 );
1427 S1Call,
E.
getAddress((offset + row - col - 1) * NX),
1457 LOG(
LVL_DEBUG ) <<
"---> Copy H11 lower part" << endl;
1462 for (
unsigned ii = 0; ii <
N; ++ii)
1463 for(
unsigned jj = 0; jj < ii; ++jj)
1469 for (
unsigned ii = 0; ii <
N; ++ii)
1505 LOG(
LVL_DEBUG ) <<
"Setup condensing: create Dx0, Dy and DyN" << endl;
1518 bool gradientUpdate = (bool) gradientUp;
1522 int sensitivityProp;
1528 for(
unsigned run1 = 0; run1 <
N; ++run1)
1540 for(
unsigned run1 = 0; run1 <
N; run1++ )
1606 for (
unsigned i = 0; i <
N; ++i)
1615 for (
unsigned i = N - 1; 0 < i; --i)
1673 LOG(
LVL_DEBUG ) <<
"Setup condensing: create expand routine" << endl;
1704 int hessianApproximation;
1707 if( secondOrder || adjoint ) {
1712 for (
uint j = 0; j <
NX; j++ ) {
1728 for (
int i = N - 1; i >= 1; i--) {
1729 for (
uint j = 0; j <
NX; j++ ) {
1766 bool boxConIsFinite =
false;
1776 boxConIsFinite =
true;
1779 if (boxConIsFinite ==
false || i == 0)
1782 for (
unsigned j = 0; j < lbBox.
getDim(); ++j)
1794 x0.
setDoc( (std::string)
"Current state feedback vector." );
1828 ExportVariable R22, Dy1, RDy1, Q22, QDy1,
E1, U1, U2, H101, w11, w12, w13;
1882 H.
getSubMatrix(offset + iRow *
NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
1883 H.
getSubMatrix(offset + iCol * NU, offset + (iCol + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU).
getTranspose()
1890 H.
getSubMatrix(0,
NX, offset + iCol * NU, offset + (iCol + 1) * NU) ==
1961 H.
getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) ==
1999 H.
getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU) ==
2003 H.
getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iRow * NU, offset + (iRow + 1) * NU) += mRegH11
2016 H.
getSubMatrix(offset + iRow * NU, offset + (iRow + 1) * NU, offset + iCol * NU, offset + (iCol + 1) * NU) +=
2026 bool gradientUpdate = (bool) gradientUp;
2028 int hessianApproximation;
2031 int sensitivityProp;
2034 if( secondOrder || adjoint ) {
2070 int hessianRegularization;
2081 retSim.
setDoc(
"Status of the integration module. =0: OK, otherwise the error code.");
2102 tmp.
setDoc(
"Status code of the qpOASES QP solver." );
2108 feedback.
doc(
"Feedback/estimation step of the RTI scheme." );
2115 s << tmp.
getName() <<
" = " << solve.
getName() <<
"( );" << endl;
2132 getKKT.
doc(
"Get the KKT tolerance of the current iterate." );
2133 kkt.
setDoc(
"The KKT tolerance value." );
2178 std::string sourceFile, headerFile, solverDefine;
2184 sourceFile = folderName +
"/" + moduleName +
"_qpoases_interface.cpp";
2185 headerFile = folderName +
"/" + moduleName +
"_qpoases_interface.hpp";
2186 solverDefine =
"QPOASES_HEADER";
2191 sourceFile = folderName +
"/" + moduleName +
"_qpoases3_interface.c";
2192 headerFile = folderName +
"/" + moduleName +
"_qpoases3_interface.h";
2193 solverDefine =
"QPOASES3_HEADER";
2199 "For condensed solution only qpOASES and qpOASES3 QP solver are supported");
2202 int useSinglePrecision;
2211 int maxNumQPiterations;
2218 if ( qpInterface == 0 )
2250 if ( qpInterface != 0 )
2258 int sparseQPsolution;
virtual returnValue setupCondensing()
virtual returnValue setupMultiplicationRoutines()
#define LOG(level)
Just define a handy macro for getting the logger.
Lowest level, the debug level.
unsigned getNumQPvars() const
ExportVariable conValueOut
ExportVariable getRow(const ExportIndex &idx) const
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportFunction condensePrep
ExportFunction shiftStates
ExportFunction preparation
ExportVariable getTranspose() const
virtual returnValue setupObjectiveEvaluation(void)
bool initialStateFixed() const
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
Allows to pass back messages to the calling function.
ExportVariable conValueIn
ExportFunction setObjR1R2
DVector getUpperBounds(uint pointIdx) const
returnValue addComment(const std::string &_comment)
HessianRegularizationMode
double getLowerBound(uint pointIdx, uint valueIdx) const
virtual returnValue configure(const std::string &_prefix, const std::string &_solverDefine, const int nvmax, const int ncmax, const int nwsrmax, const std::string &_printLevel, bool _useSinglePrecision, const std::string &_commonHeader, const std::string &_namespace, const std::string &_primalSolution, const std::string &_dualSolution, const std::string &_sigma, bool _hotstartQP, bool _externalCholesky, const std::string &_qpH, const std::string &_qpR, const std::string &_qpg, const std::string &_qpA, const std::string &_qplb, const std::string &_qpub, const std::string &_qplbA, const std::string &_qpubA)
ExportVariable objEvFxEnd
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
virtual returnValue setupConstraintsEvaluation(void)
const DMatrix & getGivenMatrix() const
Allows to export code of a for-loop.
string toString(T const &value)
#define CLOSE_NAMESPACE_ACADO
double getUpperBound(uint pointIdx, uint valueIdx) const
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.
ExportFunction condenseFdb
Base class for export of NLP/OCP solvers.
ExportAcadoFunction evaluateStageCost
A class for generating the glue code for interfacing qpOASES.
virtual returnValue setup()
Defines a matrix-valued variable that can be passed as argument to exported functions.
ExportAcadoFunction evaluateTerminalCost
ExportFunction regularization
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 setupEvaluation()
virtual returnValue setDoc(const std::string &_doc)
virtual returnValue setupInitialization()
std::string commonHeaderName
ExportFunction modelSimulation
virtual ExportFunction & doc(const std::string &_doc)
ExportFunction macATw1QDy
virtual bool isDefined() const
ExportFunction expansionStep2
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
ExportFunction getObjective
virtual returnValue setupQPInterface()
ExportVariable makeRowVector() const
ExportVariable objValueIn
ExportFunction initializeNodes
double levenbergMarquardt
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
virtual returnValue exportCode()
bool performsSingleShooting() const
virtual returnValue setupSimulation(void)
std::vector< std::shared_ptr< ExportAcadoFunction > > evaluatePointConstraints
Encapsulates all user interaction for setting options, logging data and plotting results.
ExportFunction setObjQ1Q2
Allows to export code of an arbitrary function.
virtual uint getDim() const
ExportFunction shiftControls
returnValue addStatement(const ExportStatement &_statement)
virtual unsigned getNumStateBounds() const
virtual bool isGiven() const
std::string getFullName() const
returnValue addLinebreak(uint num=1)
ExportFunction expansionStep
ExportAcadoFunction evaluatePathConstraints
GenericVector & append(const GenericVector &_arg)
bool performFullCondensing() const
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
virtual ExportFunction & acquire(ExportIndex &obj)
ExportFunction & addVariable(const ExportVariable &_var)
GenericVector< double > DVector
BooleanType acadoIsFinite(double x, double TOL)
uint getNumPoints() const
ExportVariable objSEndTerm
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
std::string getName() const
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
returnValue setupAuxiliaryFunctions()
virtual ExportFunction & release(const ExportIndex &obj)
unsigned getNumComplexConstraints(void)
DVector getLowerBounds(uint pointIdx) const
ExportFunction setObjQN1QN2
ExportGaussNewtonCN2(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
#define BEGIN_NAMESPACE_ACADO
BooleanType isFinite(const T &_value)
ExportFunction macQSbarW2
returnValue addFunction(const ExportFunction &_function)
virtual returnValue setupVariables()
A class for generating the glue code for interfacing qpOASES.
ExportFunction macS1TSbar
std::vector< unsigned > xBoundsIdx
virtual returnValue getCode(ExportStatementBlock &code)
Allows to export code for a block of statements.
ExportFunction initialize
ExportFunction mult_BT_T1
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
ExportFunction evaluateObjective
ExportVariable objValueOut
ExportFunction macBTW1_R1
ExportVariable getCol(const ExportIndex &idx) const
ExportFunction & addIndex(const ExportIndex &_index)
ExportFunction regularizeHessian
#define ACADOERROR(retval)
Defines a matrix-valued variable to be used for exporting code.
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
#define ACADOERRORTEXT(retval, text)
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)
ExportVariable makeColVector() const
std::string getName() const