returnValue ExportGaussNewtonForces::setupEvaluation( ) { //////////////////////////////////////////////////////////////////////////// // // Setup preparation phase // //////////////////////////////////////////////////////////////////////////// preparation.setup("preparationStep"); preparation.doc( "Preparation step of the RTI scheme." ); ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true); retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code."); preparation.setReturnValue(retSim, false); preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n"; preparation.addFunctionCall( evaluateObjective ); preparation.addFunctionCall( evaluateConstraints ); //////////////////////////////////////////////////////////////////////////// // // Setup feedback phase // //////////////////////////////////////////////////////////////////////////// ExportVariable stateFeedback("stateFeedback", NX, 1, REAL, ACADO_LOCAL); ExportVariable returnValueFeedbackPhase("retVal", 1, 1, INT, ACADO_LOCAL, true); returnValueFeedbackPhase.setDoc( "Status code of the FORCES QP solver." ); feedback.setup("feedbackStep" ); feedback.doc( "Feedback/estimation step of the RTI scheme." ); feedback.setReturnValue( returnValueFeedbackPhase ); feedback.addStatement( // cond[ 0 ].getRows(0, NX) == x0 - x.getRow( 0 ).getTranspose() cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose() ); feedback.addLinebreak(); // Calculate objective residuals feedback << (Dy -= y) << (DyN -= yN); feedback.addLinebreak(); for (unsigned i = 0; i < N; ++i) feedback.addFunctionCall(setStagef, objGradients[ i ], ExportIndex( i )); feedback.addStatement( objGradients[ N ] == QN2 * DyN ); feedback.addLinebreak(); // // Configure output variables // std::vector< ExportVariable > vecQPVars; vecQPVars.clear(); vecQPVars.resize(N + 1); for (unsigned i = 0; i < N; ++i) vecQPVars[ i ].setup(string("out") + toString(i + 1), NX + NU, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix); vecQPVars[ N ].setup(string("out") + toString(N + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix); // // In case warm starting is enabled, give an initial guess, based on the old solution // int hotstartQP; get(HOTSTART_QP, hotstartQP); if ( hotstartQP ) { std::vector< ExportVariable > zInit; zInit.clear(); zInit.resize(N + 1); for (unsigned i = 0; i < N; ++i) { string name = "z_init_"; name = name + (i < 10 ? "0" : "") + toString( i ); zInit[ i ].setup(name, NX + NU, 1, REAL, FORCES_PARAMS, false, qpObjPrefix); } string name = "z_init_"; name = name + (N < 10 ? "0" : "") + toString( N ); zInit[ N ].setup(name, NX, 1, REAL, FORCES_PARAMS, false, qpObjPrefix); // TODO This should be further investigated. // // 1) Just use the old solution // // for (unsigned blk = 0; blk < N + 1; blk++) // feedback.addStatement(zInit[ blk ] == vecQPVars[ blk ] ); // // 2) Initialization by shifting // // for (unsigned blk = 0; blk < N - 1; blk++) // feedback.addStatement( zInit[ blk ] == vecQPVars[blk + 1] ); // for (unsigned el = 0; el < NX; el++) // feedback.addStatement( zInit[N - 1].getElement(el, 0) == vecQPVars[ N ].getElement(el, 0) ); } // // Call a QP solver // NOTE: we need two prefixes: // 1. module prefix // 2. structure instance prefix // ExportFunction solveQP; solveQP.setup("solve"); feedback << returnValueFeedbackPhase.getFullName() << " = " << qpModuleName << "_" << solveQP.getName() << "( " << "&" << qpObjPrefix << "_" << "params" << ", " << "&" << qpObjPrefix << "_" << "output" << ", " << "&" << qpObjPrefix << "_" << "info" << " );\n"; feedback.addLinebreak(); // // Here we have to add the differences.... // ExportVariable stageOut("stageOut", 1, NX + NU, REAL, ACADO_LOCAL); ExportIndex index( "index" ); acc.setup("accumulate", stageOut, index); acc.addStatement( x.getRow( index ) += stageOut.getCols(0, NX) ); acc.addLinebreak(); acc.addStatement( u.getRow( index ) += stageOut.getCols(NX, NX + NU) ); acc.addLinebreak(); for (unsigned i = 0; i < N; ++i) feedback.addFunctionCall(acc, vecQPVars[ i ], ExportIndex( i )); feedback.addLinebreak(); feedback.addStatement( x.getRow( N ) += vecQPVars[ N ].getTranspose() ); feedback.addLinebreak(); //////////////////////////////////////////////////////////////////////////// // // TODO Setup evaluation of KKT // //////////////////////////////////////////////////////////////////////////// ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true); getKKT.setup( "getKKT" ); getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." ); // kkt.setDoc( "The KKT tolerance value." ); kkt.setDoc( "1e-15." ); getKKT.setReturnValue( kkt ); getKKT.addStatement( kkt == 1e-15 ); return SUCCESSFUL_RETURN; }
returnValue ExportGaussNewtonBlockForces::setupEvaluation( ) { //////////////////////////////////////////////////////////////////////////// // // Preparation phase // //////////////////////////////////////////////////////////////////////////// preparation.setup( "preparationStep" ); preparation.doc( "Preparation step of the RTI scheme." ); ExportVariable retSim("ret", 1, 1, INT, ACADO_LOCAL, true); retSim.setDoc("Status of the integration module. =0: OK, otherwise the error code."); preparation.setReturnValue(retSim, false); ExportIndex index("index"); preparation.addIndex( index ); preparation << retSim.getFullName() << " = " << modelSimulation.getName() << "();\n"; preparation.addFunctionCall( evaluateObjective ); if( regularizeHessian.isDefined() ) preparation.addFunctionCall( regularizeHessian ); preparation.addFunctionCall( evaluateConstraints ); preparation.addLinebreak(); preparation << (Dy -= y) << (DyN -= yN); preparation.addLinebreak(); ExportForLoop condensePrepLoop( index, 0, getNumberOfBlocks() ); condensePrepLoop.addFunctionCall( condensePrep, index ); preparation.addStatement( condensePrepLoop ); preparation.addFunctionCall( evaluateAffineConstraints ); preparation.addStatement( objHessians[getNumberOfBlocks()] == QN1 ); DMatrix mReg = eye<double>( getNX() ); mReg *= levenbergMarquardt; preparation.addStatement( objHessians[getNumberOfBlocks()] += mReg ); preparation.addLinebreak(); preparation.addStatement( objGradients[ getNumberOfBlocks() ] == QN2 * DyN ); int variableObjS; get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS); ExportVariable SlxCall = objSlx.isGiven() == true || variableObjS == false ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX); preparation.addStatement( objGradients[ getNumberOfBlocks() ] += SlxCall ); preparation.addLinebreak(); //////////////////////////////////////////////////////////////////////////// // // Feedback phase // //////////////////////////////////////////////////////////////////////////// ExportVariable tmp("tmp", 1, 1, INT, ACADO_LOCAL, true); tmp.setDoc( "Status code of the qpOASES QP solver." ); feedback.setup("feedbackStep"); feedback.doc( "Feedback/estimation step of the RTI scheme." ); feedback.setReturnValue( tmp ); feedback.addIndex( index ); if (initialStateFixed() == true) { feedback.addStatement( cond[ 0 ] == x0 - x.getRow( 0 ).getTranspose() ); } feedback.addLinebreak(); // // Configure output variables // std::vector< ExportVariable > vecQPVars; vecQPVars.clear(); vecQPVars.resize(getNumberOfBlocks() + 1); for (unsigned i = 0; i < getNumberOfBlocks(); ++i) vecQPVars[ i ].setup(string("out") + toString(i + 1), getNumBlockVariables(), 1, REAL, FORCES_OUTPUT, false, qpObjPrefix); vecQPVars[ getNumberOfBlocks() ].setup(string("out") + toString(getNumberOfBlocks() + 1), NX, 1, REAL, FORCES_OUTPUT, false, qpObjPrefix); // // In case warm starting is enabled, give an initial guess, based on the old solution // int hotstartQP; get(HOTSTART_QP, hotstartQP); if ( hotstartQP ) { return ACADOERROR(RET_NOT_IMPLEMENTED_YET); } // // Call a QP solver // NOTE: we need two prefixes: // 1. module prefix // 2. structure instance prefix // ExportFunction solveQP; solveQP.setup("solve"); solveQP.setName( "solve" ); feedback << tmp.getFullName() << " = " << qpModuleName << "_" << solveQP.getName() << "( " << "&" << qpObjPrefix << "_" << "params" << ", " << "&" << qpObjPrefix << "_" << "output" << ", " << "&" << qpObjPrefix << "_" << "info" << " , NULL);\n"; feedback.addLinebreak(); for (unsigned i = 0; i < getNumberOfBlocks(); ++i) { feedback.addFunctionCall( expand, vecQPVars[i], ExportIndex(i) ); } feedback.addStatement( x.getRow( N ) += vecQPVars[ getNumberOfBlocks() ].getTranspose() ); feedback.addLinebreak(); //////////////////////////////////////////////////////////////////////////// // // Setup evaluation of the KKT tolerance // //////////////////////////////////////////////////////////////////////////// ExportVariable kkt("kkt", 1, 1, REAL, ACADO_LOCAL, true); getKKT.setup( "getKKT" ); getKKT.doc( "Get the KKT tolerance of the current iterate. Under development." ); // kkt.setDoc( "The KKT tolerance value." ); kkt.setDoc( "1e-15." ); getKKT.setReturnValue( kkt ); getKKT.addStatement( kkt == 1e-15 ); return SUCCESSFUL_RETURN; }