Tudelft stable

acado/code_generation/export_gauss_newton_cn2.cpp

@@ -687,21 +687,27 @@ returnValue ExportGaussNewtonCN2::setupConstraintsEvaluation( void )
 		// Optionally store derivatives
 		if ( pacEvHx.isGiven() == false )
 		{
-			loopPac.addStatement(
-					pacEvHx.makeRowVector().
-					getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX)
-					== conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
-			);
+			for (unsigned j1 = 0; j1 < dimPacH; ++j1) {
+			    for (unsigned j2 = 0; j2 < NX; ++j2) {
+			        loopPac.addStatement(
+			                pacEvHx.getElement(runPac * dimPacH + j1, j2) ==
+			                        conValueOut.getCol(derOffset + j1*NX+j2)
+			        );
+			    }
+			}
 
 			derOffset = derOffset + dimPacH * NX;
 		}
 		if (pacEvHu.isGiven() == false )
 		{
-			loopPac.addStatement(
-					pacEvHu.makeRowVector().
-					getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU)
-					== conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
-			);
+            for (unsigned j1 = 0; j1 < dimPacH; ++j1) {
+                for (unsigned j2 = 0; j2 < NU; ++j2) {
+                    loopPac.addStatement(
+                            pacEvHu.getElement(runPac * dimPacH + j1, j2) ==
+                                    conValueOut.getCol(derOffset + j1*NU+j2)
+                    );
+                }
+            }
 
 			derOffset = derOffset + dimPacH * NU;
 		}

acado/code_generation/export_gauss_newton_generic.cpp

@@ -56,6 +56,25 @@ returnValue ExportGaussNewtonGeneric::setup( )
 
 	setupAuxiliaryFunctions();
 
+	int useArrivalCost;
+	get(CG_USE_ARRIVAL_COST, useArrivalCost);
+	if (useArrivalCost == YES) {
+	    ExportVariable evRet("ret", 1, 1, INT, ACADO_LOCAL, true);
+
+	    ExportVariable evReset("reset", 1, 1, INT, ACADO_LOCAL, true);
+	    evReset.setDoc("Reset S_{AC}. Set it to 1 to initialize arrival cost calculation, "
+	            "and later should set it to 0.");
+
+	    updateArrivalCost.init("updateArrivalCost", evReset);
+	    updateArrivalCost.doc("Use this function to update the arrival cost.");
+	    updateArrivalCost.setReturnValue( evRet );
+	    updateArrivalCost << (evRet == 0);
+
+	    acWL.setup("WL", NX, NX, REAL, ACADO_VARIABLES);
+	    acWL.setDoc("Arrival cost term: Cholesky decomposition, lower triangular, "
+	                " of the inverse of the state noise covariance matrix.");
+	}
+
 	return SUCCESSFUL_RETURN;
 }
 
@@ -129,6 +148,8 @@ returnValue ExportGaussNewtonGeneric::getFunctionDeclarations(	ExportStatementBl
 	declarations.addDeclaration( evaluateStageCost );
 	declarations.addDeclaration( evaluateTerminalCost );
 
+    declarations.addDeclaration( updateArrivalCost );
+
 	return SUCCESSFUL_RETURN;
 }
 
@@ -188,6 +209,12 @@ returnValue ExportGaussNewtonGeneric::getCode(	ExportStatementBlock& code
 	code.addFunction( getKKT );
 	code.addFunction( getObjective );
 
+	int useArrivalCost;
+	get(CG_USE_ARRIVAL_COST, useArrivalCost);
+	if (useArrivalCost == YES) {
+	    code.addFunction( updateArrivalCost );
+	}
+
 	return SUCCESSFUL_RETURN;
 }
 
@@ -404,7 +431,11 @@ returnValue ExportGaussNewtonGeneric::setupObjectiveEvaluation( void )
 	ExportVariable qq, rr;
 	qq.setup("stageq", NX, 1, REAL, ACADO_LOCAL);
 	rr.setup("stager", NU, 1, REAL, ACADO_LOCAL);
-	setStagef.setup("setStagef", qq, rr, index);
+
+
+    ExportVariable SlxCall = objSlx.isGiven() == true ? objSlx : ExportVariable("Slx", NX, 1, REAL, ACADO_LOCAL);
+    ExportVariable SluCall = objSlu.isGiven() == true ? objSlu : ExportVariable("Slu", NU, 1, REAL, ACADO_LOCAL);
+	setStagef.setup("setStagef", qq, rr, SlxCall, SluCall, index);
 
 	if (Q2.isGiven() == false)
 		setStagef.addStatement(
@@ -417,6 +448,7 @@ returnValue ExportGaussNewtonGeneric::setupObjectiveEvaluation( void )
 				qq == Q2 * Dy.getRows(index * NY, (index + 1) * NY)
 		);
 	}
+    setStagef.addStatement( qq += SlxCall );
 	setStagef.addLinebreak();
 
 	if (R2.isGiven() == false)
@@ -429,6 +461,7 @@ returnValue ExportGaussNewtonGeneric::setupObjectiveEvaluation( void )
 				rr == R2 * Dy.getRows(index * NY, (index + 1) * NY)
 		);
 	}
+	setStagef.addStatement( rr += SluCall );
 
 	//
 	// Setup necessary QP variables
@@ -687,19 +720,27 @@ returnValue ExportGaussNewtonGeneric::setupConstraintsEvaluation( void )
 		// Optionally store derivatives
 		if (pacEvHx.isGiven() == false)
 		{
-			loopPac.addStatement(
-					pacEvHx.makeRowVector().getCols(runPac * dimPacH * NX, (runPac + 1) * dimPacH * NX) ==
-							conValueOut.getCols(derOffset, derOffset + dimPacH * NX )
-			);
+		    for (unsigned j1 = 0; j1 < dimPacH; ++j1) {
+		        for (unsigned j2 = 0; j2 < NX; ++j2) {
+		            loopPac.addStatement(
+		                    pacEvHx.getElement(runPac * dimPacH + j1, j2) ==
+		                            conValueOut.getCol(derOffset + j1*NX+j2)
+		            );
+		        }
+		    }
 
 			derOffset = derOffset + dimPacH * NX;
 		}
 		if (pacEvHu.isGiven() == false )
 		{
-			loopPac.addStatement(
-					pacEvHu.makeRowVector().getCols(runPac * dimPacH * NU, (runPac + 1) * dimPacH * NU) ==
-							conValueOut.getCols(derOffset, derOffset + dimPacH * NU )
-			);
+		    for (unsigned j1 = 0; j1 < dimPacH; ++j1) {
+		        for (unsigned j2 = 0; j2 < NU; ++j2) {
+		            loopPac.addStatement(
+		                    pacEvHu.getElement(runPac * dimPacH + j1, j2) ==
+		                            conValueOut.getCol(derOffset + j1*NU+j2)
+		            );
+		        }
+		    }
 		}
 
 		// Add loop to the function.
@@ -856,6 +897,16 @@ returnValue ExportGaussNewtonGeneric::setupMultiplicationRoutines( )
 
 returnValue ExportGaussNewtonGeneric::setupEvaluation( )
 {
+    int gradientUp;
+    get( LIFTED_GRADIENT_UPDATE, gradientUp );
+    bool gradientUpdate = (bool) gradientUp;
+
+    int variableObjS;
+    get(CG_USE_VARIABLE_WEIGHTING_MATRIX, variableObjS);
+    int sensitivityProp;
+    get( DYNAMIC_SENSITIVITY, sensitivityProp );
+    bool adjoint = ((ExportSensitivityType) sensitivityProp == BACKWARD || ((ExportSensitivityType) sensitivityProp == INEXACT && gradientUpdate));
+
 	////////////////////////////////////////////////////////////////////////////
 	//
 	// Setup preparation phase
@@ -914,19 +965,37 @@ returnValue ExportGaussNewtonGeneric::setupEvaluation( )
 	feedback.addStatement( DyN -= yN );
 	feedback.addLinebreak();
 
-	for (unsigned i = 0; i < N; ++i)
-		feedback.addFunctionCall(setStagef, qpq.getAddress(i * NX), qpr.getAddress(i * NU), ExportIndex( i ));
+	for (unsigned i = 0; i < N; ++i) {
+        ExportArgument SlxCall =
+                objSlx.isGiven() == true || (variableObjS == false && !adjoint) ? objSlx : objSlx.getAddress(i * NX, 0);
+        ExportArgument SluCall =
+                objSlu.isGiven() == true || (variableObjS == false && !adjoint) ? objSlu : objSlu.getAddress(i * NU, 0);
+
+		feedback.addFunctionCall(setStagef, qpq.getAddress(i * NX), qpr.getAddress(i * NU), SlxCall, SluCall, ExportIndex( i ));
+	}
 	feedback.addLinebreak();
+    ExportVariable SlxCall =
+                objSlx.isGiven() == true || (variableObjS == false && !adjoint) ? objSlx : objSlx.getRows(N * NX, (N + 1) * NX);
 	feedback.addStatement( qpqf == QN2 * DyN );
+    feedback.addStatement( qpqf += SlxCall );
+
 	feedback.addLinebreak();
 
-	//
-	// Arrival cost in the MHE case
-	//
-	if (initialStateFixed() == false)
+//	//
+//	// Arrival cost in the MHE case
+//	//
+//	if (initialStateFixed() == false)
+//	{
+//		// It is assumed this is the shifted version from the previous time step!
+//		feedback.addStatement( DxAC == xAC - x.getRow( 0 ).getTranspose() );
+//	}
+
+	if (SAC.getDim() > 0)
 	{
-		// It is assumed this is the shifted version from the previous time step!
-		feedback.addStatement( DxAC == xAC - x.getRow( 0 ).getTranspose() );
+	    // Include arrival cost
+	    feedback.addStatement( DxAC == x.getRow( 0 ).getTranspose() - xAC );
+        feedback.addStatement( Q1.getRows(0,NX) += SAC );
+	    feedback.addStatement( qpq.getRows(0,NX) += SAC*DxAC );
 	}
 
 	//
@@ -943,11 +1012,11 @@ returnValue ExportGaussNewtonGeneric::setupEvaluation( )
 	feedback.addStatement( x.makeColVector() += qpx );
 	feedback.addStatement( u.makeColVector() += qpu );
 
-	if (initialStateFixed() == false)
-	{
-		// This is the arrival cost for the next time step!
-		feedback.addStatement( xAC == x.getRow( 1 ).getTranspose() + DxAC );
-	}
+//	if (initialStateFixed() == false)
+//	{
+//		// This is the arrival cost for the next time step!
+//		feedback.addStatement( xAC == x.getRow( 1 ).getTranspose() + DxAC );
+//	}
 
 	////////////////////////////////////////////////////////////////////////////
 	//

acado/code_generation/export_gauss_newton_hpmpc.cpp

@@ -71,6 +71,9 @@ returnValue ExportGaussNewtonHpmpc::getDataDeclarations(	ExportStatementBlock& d
 	if (status != SUCCESSFUL_RETURN)
 		return status;
 
+    int hardcodeConstraintValues;
+    get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
+
 	declarations.addDeclaration(x0, dataStruct);
 
 	if (Q1.isGiven() == true)
@@ -102,6 +105,14 @@ returnValue ExportGaussNewtonHpmpc::getDataDeclarations(	ExportStatementBlock& d
 
 	declarations.addDeclaration(nIt, dataStruct);
 
+    if (hardcodeConstraintValues == NO) {
+        declarations.addDeclaration(evLbValues, dataStruct);
+        declarations.addDeclaration(evUbValues, dataStruct);
+
+        declarations.addDeclaration(evLbAValues, dataStruct);
+        declarations.addDeclaration(evUbAValues, dataStruct);
+    }
+
 	return SUCCESSFUL_RETURN;
 }
 
@@ -507,8 +518,8 @@ returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void )
 
 
 
-//	int hardcodeConstraintValues;
-//	get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
+    int hardcodeConstraintValues;
+    get(CG_HARDCODE_CONSTRAINT_VALUES, hardcodeConstraintValues);
 
 	evaluateConstraints.setup("evaluateConstraints");
 
@@ -567,11 +578,22 @@ returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void )
 	qpLb.setup("qpLb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
 	qpUb.setup("qpUb", N * NU + N * NX, 1, REAL, ACADO_WORKSPACE);
 
-	evLbValues.setup("evLbValues", lbValues, STATIC_CONST_REAL, ACADO_LOCAL);
-	evUbValues.setup("evUbValues", ubValues, STATIC_CONST_REAL, ACADO_LOCAL);
+	if( hardcodeConstraintValues == YES ) {
+	    evLbValues.setup("evLbValues", lbValues, STATIC_CONST_REAL, ACADO_LOCAL);
+	    evUbValues.setup("evUbValues", ubValues, STATIC_CONST_REAL, ACADO_LOCAL);
 
-	evaluateConstraints.addVariable( evLbValues );
-	evaluateConstraints.addVariable( evUbValues );
+	    evaluateConstraints.addVariable( evLbValues );
+	    evaluateConstraints.addVariable( evUbValues );
+	}
+	else {
+	    evLbValues.setup("lbValues", N * NU + N * NX, 1, REAL, ACADO_VARIABLES);
+	    evLbValues.setDoc( "Lower bounds values." );
+	    evUbValues.setup("ubValues", N * NU + N * NX, 1, REAL, ACADO_VARIABLES);
+	    evUbValues.setDoc( "Upper bounds values." );
+
+	    initialize.addStatement( evLbValues == lbValues );
+	    initialize.addStatement( evUbValues == ubValues );
+	}
 
 	evaluateConstraints.addStatement( qpLb.getRows(0, N * NU) == evLbValues.getRows(0, N * NU) - u.makeColVector() );
 	evaluateConstraints.addStatement( qpUb.getRows(0, N * NU) == evUbValues.getRows(0, N * NU) - u.makeColVector() );
@@ -649,11 +671,22 @@ returnValue ExportGaussNewtonHpmpc::setupConstraintsEvaluation( void )
 	lbAValues.append( pocLbStack[ N ] );
 	ubAValues.append( pocUbStack[ N ] );
 
-	ExportVariable evLbAValues("lbAValues", lbAValues, STATIC_CONST_REAL);
-	ExportVariable evUbAValues("ubAValues", ubAValues, STATIC_CONST_REAL);
+	if( hardcodeConstraintValues == YES || qpDimHtot == 0 ) {
+	    evLbAValues.setup("lbAValues", lbAValues, STATIC_CONST_REAL);
+	    evUbAValues.setup("ubAValues", ubAValues, STATIC_CONST_REAL);
 
-	evaluateConstraints.addVariable( evLbAValues );
-	evaluateConstraints.addVariable( evUbAValues );
+	    evaluateConstraints.addVariable( evLbAValues );
+	    evaluateConstraints.addVariable( evUbAValues );
+	}
+	else {
+	    evLbAValues.setup("lbAValues", qpDimHtot, 1, REAL, ACADO_VARIABLES);
+	    evLbAValues.setDoc( "Lower affine bounds values." );
+	    evUbAValues.setup("ubAValues", qpDimHtot, 1, REAL, ACADO_VARIABLES);
+	    evUbAValues.setDoc( "Upper affine bounds values." );
+
+	    initialize.addStatement( evLbAValues == lbAValues );
+	    initialize.addStatement( evUbAValues == ubAValues );
+	}
 
 	//
 	// Evaluate path constraints

acado/code_generation/export_gauss_newton_hpmpc.hpp

@@ -195,7 +195,8 @@ class ExportGaussNewtonHpmpc : public ExportNLPSolver
 	ExportVariable qpq, qpqf, qpr;
 	ExportVariable qpx, qpu;
 
-	ExportVariable evLbValues, evUbValues;
+    ExportVariable evLbValues, evUbValues;
+    ExportVariable evLbAValues, evUbAValues;
 	ExportVariable qpLb, qpUb;
 
 	ExportVariable qpLbA, qpUbA;

acado/code_generation/export_gauss_newton_qpdunes.cpp

@@ -107,6 +107,8 @@ returnValue ExportGaussNewtonQpDunes::getDataDeclarations(	ExportStatementBlock&
 	declarations.addDeclaration(qpUb, dataStruct);
 	declarations.addDeclaration(lbValues, dataStruct);
 	declarations.addDeclaration(ubValues, dataStruct);
+    declarations.addDeclaration(evLbAValues, dataStruct);
+    declarations.addDeclaration(evUbAValues, dataStruct);
 	declarations.addDeclaration(qpC, dataStruct);
 	declarations.addDeclaration(qpc, dataStruct);
 
@@ -734,8 +736,8 @@ returnValue ExportGaussNewtonQpDunes::setupConstraintsEvaluation( void )
 
 	if (getNumComplexConstraints() == 0)
 		return SUCCESSFUL_RETURN;
-	else if(hardcodeConstraintValues == YES)
-		return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
+//	else if(hardcodeConstraintValues == NO)
+//		return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
 
 	unsigned dimLbA  = N * dimPacH;
 	unsigned dimConA = dimLbA * (NX + NU);
@@ -778,20 +780,30 @@ returnValue ExportGaussNewtonQpDunes::setupConstraintsEvaluation( void )
 	{
 		if ( dimPacH )
 		{
-			lbAValues.append( lbPathConValues.block(i * NX, 0, NX, 1) );
-			ubAValues.append( ubPathConValues.block(i * NX, 0, NX, 1) );
+			lbAValues.append( lbPathConValues.block(i * dimPacH, 0, dimPacH, 1) );
+			ubAValues.append( ubPathConValues.block(i * dimPacH, 0, dimPacH, 1) );
 		}
 		lbAValues.append( pocLbStack[ i ] );
 		ubAValues.append( pocUbStack[ i ] );
 	}
 	lbAValues.append( pocLbStack[ N ] );
 	ubAValues.append( pocUbStack[ N ] );
 
-	ExportVariable evLbAValues("lbAValues", lbAValues, STATIC_CONST_REAL);
-	ExportVariable evUbAValues("ubAValues", ubAValues, STATIC_CONST_REAL);
+	if (hardcodeConstraintValues == YES  || dimLbA == 0) {
+	    evLbAValues.setup("lbAValues", lbAValues, STATIC_CONST_REAL);
+	    evUbAValues.setup("ubAValues", ubAValues, STATIC_CONST_REAL);
 
-	evaluateConstraints.addVariable( evLbAValues );
-	evaluateConstraints.addVariable( evUbAValues );
+	    evaluateConstraints.addVariable( evLbAValues );
+	    evaluateConstraints.addVariable( evUbAValues );
+	} else {
+        evLbAValues.setup("lbAValues", dimLbA, 1, REAL, ACADO_VARIABLES);
+        evLbAValues.setDoc( "Lower affine bounds values." );
+        evUbAValues.setup("ubAValues", dimLbA, 1, REAL, ACADO_VARIABLES);
+        evUbAValues.setDoc( "Upper affine bounds values." );
+
+        initialize.addStatement( evLbAValues == lbAValues );
+        initialize.addStatement( evUbAValues == ubAValues );
+	}
 
 	//
 	// Evaluate path constraints

acado/code_generation/export_gauss_newton_qpdunes.hpp

@@ -163,6 +163,7 @@ class ExportGaussNewtonQpDunes : public ExportNLPSolver
 	ExportVariable qpLb, qpUb;
 
 	ExportVariable lbValues, ubValues;
+	ExportVariable evLbAValues, evUbAValues;
 
 	ExportVariable qpA;
 	ExportVariable qpLbA, qpUbA;